module Roboter:

output          MOTOR_A_DIR(integer),   
                MOTOR_B_DIR(integer),
                MOTOR_C_DIR(integer),
                MOTOR_A_SPEED(integer),
                MOTOR_B_SPEED(integer),
                MOTOR_C_SPEED(integer);
constant        DEFAULT_LIGHT_THRESHHOLD = 40 : integer;

sensor          LIGHT_1_VALUE: integer; % Light detection
input           LIGHT_LOW_1;
input           LIGHT_HIGH_1;
output          SET_LIGHT_1_THRESHHOLD := DEFAULT_LIGHT_THRESHHOLD :integer;

sensor          LIGHT_3_VALUE: integer; % Light detection
input           LIGHT_LOW_3;
input           LIGHT_HIGH_3;
output          SET_LIGHT_3_THRESHHOLD := DEFAULT_LIGHT_THRESHHOLD :integer;

output          CPUTS (string);         % LCD Display
constant        MOTOR_OFF,              % Commands
                MOTOR_FWD, 
                MOTOR_REV, 
                MOTOR_BRAKE, 
                MAX_SPEED: integer;

function get_rot_pos(integer): integer;

constant        treshhold = 40  : integer,      % Black/White Treshold
                leftturn = 16   : integer,      % turn left constant
                rightturn = 16  : integer;      % turn right constant


var             value_of_light          : integer,      % Temp
                Rot_Pos := 0            : integer,      % Rotation
                Rot_Pos_Alt:= 0         : integer in    % Rotation
		 
                
signal  touch,				% Touch
	black1,				% Black Sensor 1
	white1,				% White Sensor 1
	black2,				% Black Sensor 2
	white2,				% White Sensor 2
        turn_left,                      % turn left
        turn_right,                     % turn right
        move_fwd,                       % forward
        move_rev,                       % back
        move_stop,                      % stop
        get_can,                        % close grip arms
        put_can,                        % open grip arms                        
        rot_fwd,                        % Rotation pulse forward
        rot_rev,                        % rotation pulse reverse
        rot_value       : integer,      % absolute value of rotation
        turn_left_90,                   % 90 left turn
        turn_right_90,                  % 90 right turn
        finished in                     % finished signal





[

%
% Eigenes Programm
%

loop

        await 1 tick;
end loop;               

||


% --------------------------------------------------------------
% Sensors
% --------------------------------------------------------------

loop
        % 
        % Sensor 1: Touch and Light
        % 

        value_of_light := ?LIGHT_1_VALUE;
        if value_of_light < 300 then 
                if (value_of_light <=treshhold) then
			emit black1;
		else
			emit white1
		end if;

        end if;
        if ((value_of_light > 300) and (value_of_light < 400)) then
                emit touch;
        end if;
        
        %
        % Sensor 2: Rotation
        %

        Rot_Pos := get_rot_pos(1);

        emit rot_value(Rot_Pos);

        if ( Rot_Pos < Rot_Pos_Alt ) then
                emit rot_fwd;
                Rot_Pos_Alt := Rot_Pos;
        end if;

        if ( Rot_Pos > Rot_Pos_Alt ) then
                emit rot_rev;
                Rot_Pos_Alt := Rot_Pos;
        end if; 

	%
	% Sensor 3: Licht 2
	%

	present LIGHT_LOW_3 then
		emit black2;
		emit CPUTS("BLAC2");
	end present;

	present LIGHT_HIGH_3 then
		emit white2;
		emit CPUTS("WHIT2");
	end present;

        await 1 tick;

        
end loop

||



% -------------------------------------------------------------
% Motor commands
% -------------------------------------------------------------
loop

        present turn_left then
                emit MOTOR_A_DIR(MOTOR_REV);
                emit MOTOR_C_DIR(MOTOR_BRAKE);
                emit MOTOR_A_SPEED(MAX_SPEED/2);
        end present;

        present move_stop then
                emit MOTOR_A_DIR(MOTOR_BRAKE);
                emit MOTOR_C_DIR(MOTOR_BRAKE);
                %sustain motor_speed (0);
        end present;

        present turn_left_90 then
                emit MOTOR_A_DIR(MOTOR_REV);
                emit MOTOR_C_DIR(MOTOR_FWD);
                emit MOTOR_A_SPEED(MAX_SPEED/2);
                emit MOTOR_C_SPEED(MAX_SPEED/2);
                await leftturn rot_fwd;
                emit MOTOR_A_DIR(MOTOR_BRAKE);
                emit MOTOR_C_DIR(MOTOR_BRAKE);
                emit finished;
        end present;

        present turn_right then
                emit MOTOR_A_DIR(MOTOR_FWD);
                emit MOTOR_C_DIR(MOTOR_BRAKE);
                emit MOTOR_A_SPEED(MAX_SPEED/2);
        end present;

        present turn_right_90 then
                emit MOTOR_A_DIR(MOTOR_FWD);
                emit MOTOR_C_DIR(MOTOR_REV);
                emit MOTOR_A_SPEED(MAX_SPEED/2);
                emit MOTOR_C_SPEED(MAX_SPEED/2);
                await rightturn rot_rev;
                emit MOTOR_A_DIR(MOTOR_BRAKE);
                emit MOTOR_C_DIR(MOTOR_BRAKE);
                emit finished;
        end present;

        present move_fwd then
                emit MOTOR_A_DIR(MOTOR_REV);
                emit MOTOR_C_DIR(MOTOR_REV);
                emit MOTOR_A_SPEED(MAX_SPEED);
                emit MOTOR_C_SPEED(MAX_SPEED);
        end present;

        present move_rev then
                emit MOTOR_A_DIR(MOTOR_FWD);
                emit MOTOR_C_DIR(MOTOR_FWD);
                emit MOTOR_A_SPEED(MAX_SPEED);
                emit MOTOR_C_SPEED(MAX_SPEED);
        end present;

        present get_can then
                emit MOTOR_A_DIR(MOTOR_BRAKE);
                emit MOTOR_C_DIR(MOTOR_BRAKE);
                emit MOTOR_B_DIR(MOTOR_REV);
		emit MOTOR_B_SPEED(MAX_SPEED);
		await 30 tick;
                emit MOTOR_B_SPEED(MAX_SPEED/2);
                await 100 tick;
                emit MOTOR_B_SPEED(MAX_SPEED);
                await 20 tick;
                emit MOTOR_B_DIR(MOTOR_BRAKE);
                emit finished;
        end present;

        present put_can then
                emit MOTOR_A_DIR(MOTOR_BRAKE);
                emit MOTOR_C_DIR(MOTOR_BRAKE);
                emit MOTOR_B_DIR(MOTOR_FWD);
                emit MOTOR_B_SPEED(MAX_SPEED);
                await 50 tick;
                emit MOTOR_B_SPEED(MAX_SPEED/2);
                await 100 tick;
                emit MOTOR_B_DIR(MOTOR_BRAKE);
                emit finished;
        end present;

        await 1 tick;
end loop

]
end signal
end var
end module