pseudocode

Research Track 1 - Assignment 1 
PSEUDO-CODE

1. Initialize global variables

SET a_th TO 2
SET d_th TO 0.4

SET straight_on_speed TO 75
SET kp TO 1
SET margin_error TO 1

SET max_angle_from_g TO 80
SET min_angle_from_g TO 10
SET dist_from_g TO 1
SET turn_from_g TO 90

SET refresh_rate TO 0.05

INITIALIZE print_flag TO 0

INITIALIZE instance R of class Robot

2. Functions

FUNCTION drive WITH (speed, seconds)
    SET R.motors[0].m0.power EQUALS TO speed
    SET R.motors[0].m1.power EQUALS TO speed
    CALL time.sleep WITH seconds
    SET R.motors[0].m0.power EQUALS TO 0
    SET R.motors[0].m1.power EQUALS TO 0
ENDFUNCTION

FUNCTION turn WITH (speed, seconds)
    SET R.motors[0].m0.power EQUALS TO speed
    SET R.motors[0].m1.power EQUALS TO -speed
    CALL time.sleep WITH seconds
    SET R.motors[0].m0.power EQUALS TO 0
    SET R.motors[0].m1.power EQUALS TO 0
ENDFUNCTION

FUNCTION find_token_silver
    SET dist TO 100
    FOR token IN CALL R.see
        IF token.info.marker_type IS MARKER_TOKEN_SILVER AND token.dist IS LESS THAN dist THEN
            SET dist EQUALS TO token.dist
        ENDIF

        SET rot_y EQUALS TO token.rot_y
    ENDFOR  

    IF dist IS EQUALS TO 100
        RETURN -1, -1
    ELSE
        RETURN dist, rot_y
    ENDIF
ENDFUNCTION

FUNCTION find_token_gold
    SET dist TO 100
    FOR token IN CALL R.see
        IF token.info.marker_type IS MARKER_TOKEN_GOLD AND token.dist IS LESS THAN dist THEN
            SET dist EQUALS TO token.dist
        ENDIF

        SET rot_y EQUALS TO token.rot_y
    ENDFOR  

    IF dist IS EQUALS TO 100
        RETURN -1, -1
    ELSE
        RETURN dist, rot_y
    ENDIF
ENDFUNCTION

FUNCTION turn_P WITH (grades)
    SET heading TO R.heading * 180 / math.pi

    IF heading IS BETWEEN -180 and 0 THEN
        SET heading TO CALL module WITH (heading)
    ELIF heading IS BETWEEN 0 and 180 THEN
        SET heading to 360 - heading
    ENDIF

    SET final TO heading + CALL module WITH (grades)

    IF final IS GREATER THAN 360 THEN
        SET final TO final - 360
        SET error TO 360 - heading + final
        SET final_flag TO 1
    ELSE 
        SET error TO final - heading
        SET error TO CALL module WITH (error)
    ENDIF

    WHILE error IS GREATER THAN margin_error 
        IF grades IS GREATER THAN 0 THEN 
            CALL turn WITH (-error * kp, refresh_rate)
        ELSE 
            CALL turn WITH (+error * kp, refresh_rate)
        ENDIF

        SET heading TO R.heading * 180 / math.pi
        IF heading IS BETWEEN -180 and 0 THEN
            SET heading TO CALL module WITH (heading)
        ELIF heading IS BETWEEN 0 and 180 THEN
            SET heading to 360 - heading
        ENDIF

        IF final_flag IS EQUALS TO 1 and heading IS GREATER THAN final THEN
            SET error TO 360 - heading + final
        ELSE 
            SET error TO final - heading
            SET error TO CALL module WITH (error)
        ENDIF

        IF error IS GREATER THAN 180 THEN
            SET error TO 360 - error
        ENDIF
ENDFUNCTION

FUNCTION move_behind
    SET dist_1 TO 100
    FOR token IN CALL R.see
        IF token.info.marker_type IS MARKER_TOKEN_GOLD AND token.dist IS LESS THAN dist_1 
            AND token.rot_y IS BETWEEN 75 AND 105 THEN
                SET dist_1 EQUALS TO token.dist
        ENDIF
    ENDFOR

    SET dist_2 TO 100
    FOR token IN CALL R.see
        IF token.info.marker_type IS MARKER_TOKEN_GOLD AND token.dist IS LESS THAN dist_2 
            AND token.rot_y IS BETWEEN -105 AND -75 THEN
                SET dist_2 EQUALS TO token.dist
        ENDIF
    ENDFOR

    IF dist_1 IS GREATER THAN dist_2 THEN
        CALL turn_P WITH (-180)
    ELSE 
        CALL turn_P WITH (+180)
    ENDIF

    CALL R.release
    CALL turn_P WITH (180)
ENDFUNCTION

FUNCTION module WITH (rot)
    IF rot IS LESS THAN 0 THEN 
        RETURN rot * -1
    ELSE
        RETURN rot
    ENDIF
ENDFUNCTION

3. Program

PRINT "Turning on the robot"
CALL time.sleep WITH (3)
PRINT "GO!"

WHILE 1
    SET dist_s, rot_s CALL find_token_silver

    SET dist_g, rot_g CALL find_token_gold
            
    SET rot_s_m CALL module WITH (rot_s)

    SET rot_g_m CALL module WITH (rot_g)
            
    IF dist_s IS EQUALS TO -1 THEN
        Print "I don't see any silver token... :("
    ELSE
        IF dist_s IS LESS THAN d_th AND rot_s_m IS LESS THAN a_th THEN
            CALL R.grab
            PRINT "Gotcha!"
            CALL move_behind
            SET print_flag TO 1
        ENDIF
    ENDIF

    IF dist_s IS LESS THAN dist_g AND rot_s_m IS LESS THAN 90 THEN
        IF print_flag IS EQUALS TO 0 
            PRINT "Silver token found"
            SET print_flag TO 2
        ELIF print_flag IS EQUALS TO 1
            SET print_flag TO 0
        ENDIF

        IF rot_s_m IS LESS THAN a_th THEN
            CALL drive WITH (+dist_s * straight_on_speed, refresh_rate)
        ELSE 
            IF rot_s IS LESS THAN 0 THEN
                CALL turn WITH (-rot_s_m, refresh_rate)
            ELSE
                CALL turn WITH (+rot_s_m, refresh_rate)
            ENDIF

            CONTINUE
        ENDIF
    ENDIF

    IF rot_g_m IS BETWEEN min_angle_from_g and max_angle_from_g THEN
        SET error TO 90 - rot_g_m

        IF rot_g IS LESS THAN 0 THEN
            CALL turn WITH (+error, refresh_rate)
        ELSE
            CALL turn WITH (-error, refresh_rate)
        ENDIF

        CONTINUE
    ENDIF

    SET dist_0 TO 100
    FOR token IN CALL R.see
        IF token.info.marker_type IS MARKER_TOKEN_GOLD AND token.dist IS LESS THAN dist_0 
            AND token.rot_y IS BETWEEN -min_angle_from_g AND min_angle_from_g THEN
                SET dist_0 EQUALS TO token.dist
        ENDIF
    ENDFOR

    IF dist_0 IS LESS THAN 1 THEN 
        SET dist_1 TO 100
        FOR token IN CALL R.see
            IF token.info.marker_type IS MARKER_TOKEN_GOLD AND token.dist IS LESS THAN dist_1 
                AND token.rot_y IS BETWEEN 80 AND 100 THEN
                    SET dist_1 EQUALS TO token.dist
            ENDIF
        ENDFOR

        IF dist_1 IS EQUALS TO 100 THEN
            PRINT "Error! I did not find any golden token on the right..."
        ENDIF

        SET dist_2 TO 100
        FOR token IN CALL R.see
            IF token.info.marker_type IS MARKER_TOKEN_GOLD AND token.dist IS LESS THAN dist_2 
                AND token.rot_y IS BETWEEN -100 AND -80 THEN
                    SET dist_2 EQUALS TO token.dist
            ENDIF
        ENDFOR

        IF dist_2 IS EQUALS TO 100 THEN
            PRINT "Error! I did not find any golden token on the left..."
        ENDIF

        IF dist_1 IS GREATER THAN dist_2 THEN
            PRINT "Turn right"
        ELSE 
            PRINT "Turn left"
        ENDIF

        WHILE dist_0 IS LESS THAN 3
            IF dist_1 IS GREATER THAN dist_2 THEN
                CALL turn WITH (+straight_on_speed / dist_0, refresh_rate)
            ELSE 
                CALL turn WITH (-straight_on_speed / dist_0, refresh_rate)
            ENDIF

            SET rot_g CALL find_token_gold
            SET rot_g_m CALL module WITH rot_g

            FOR token IN CALL R.see
                IF token.info.marker_type IS MARKER_TOKEN_GOLD AND token.dist IS LESS THAN dist_0 
                    AND token.rot_y IS BETWEEN -min_angle_from_g AND min_angle_from_g THEN
                        SET dist_0 EQUALS TO token.dist
                ENDIF
            ENDFOR
        ENDWHILE
    ENDIF

    CALL drive WITH (+straight_on_speed, refresh_rate)
ENDWHILE