%%%VORBEREITUNG
do_braindead_shortcircuit_evaluation (1);
COM_CloseNXT all;
clear all;
clf;
close all;


%%%VERINDUNGSAUFBAU
robo = COM_OpenNXT();
COM_SetDefaultNXT(robo);

%%%EINSTELLUNGEN
abstand=20; %in cm
faktor=8; %motorfaktor
AATL='brake';
%mpower=[-30 -50 -70];
%mtone=[440 880 1760];
%color=['b' 'g' 'r'];
motor = NXTMotor('BC');
motor.TachoLimit=0;
%zeit=zeros(length(mpower), 1);
OpenUltrasonic(SENSOR_4);

%%%UND LOS GEHT'S

	%%%STARTE MOTOR
	motor.ResetPosition();
	%NXT_PlayTone(mtone(k), 500)
	motor.ActionAtTachoLimit = AATL;
	motor.Power=0;
	motor.SendToNXT();
	
	while(1)
	%printf('sensor')
  	distanz=GetUltrasonic(SENSOR_4);
  	%printf('power')
  	mpower=int32(faktor*(distanz-abstand));
  	if mpower>=0
  	if mpower>=100;
  		%printf('too much')
  	motor.Power=int32(100);
  	else
  		%printf('ok')
  		motor.Power=mpower;
  	endif
  	else
  	if mpower<=-100
  		%printf('too less')
   	motor.Power=int32(-100);
   	else
   		%printf('ok')
   		motor.Power=mpower;
  	endif
  	endif	
  	motor.SendToNXT();
	endwhile
	%NXT_PlayTone(200, 500)

%%%WARTE AUF MOTOR(falls was schiefläuft)
%motor.WaitFor();



