Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #define path_state 60 // Thresholds
- #define white_state 20
- #define speed_normal 85 // Speeds
- #define plus_correction 95
- #define minus_correction 65
- #define pause 50 // Wait time after sensor change
- task main()
- {
- SetSensorLight(IN_1);
- SetSensorLight(IN_2);
- SetSensorLight(IN_3);
- bool check = false; // If true = Stop, if false = keep going
- while(true)
- {
- OnFwd(OUT_AB,speed_normal); //20
- while(true)
- {
- if(IN_2 >= path_state)
- {
- if(IN_3 <= white_state && IN_1 <= white_state)
- {}
- else if(IN_1 >= white_state)
- {
- OnFwd(OUT_A, minus_correction);
- OnFwd(OUT_B, plus_correction);
- }
- else if(IN_3 >= white_state)
- {
- OnFwd(OUT_A, plus_correction);
- OnFwd(OUT_B, minus_correction);
- }
- else
- {
- ClearScreen();
- NumOut(60, 10, 1);
- break;
- }
- } //40
- if(IN_2 <= path_state)
- {
- if(IN_3 <= white_state && IN_1 <= white_state)
- {
- while(true)
- {
- Wait(pause);
- if(IN_2 >= path_state)
- {
- check = true;
- Off(OUT_AB);
- }
- else
- {
- NumOut(60, 20, 2);
- break;
- }
- }
- if(IN_1 >= white_state)
- {
- OnFwd(OUT_A, minus_correction);
- OnFwd(OUT_B, plus_correction); //60
- }
- else if(IN_3 >= white_state)
- {
- OnFwd(OUT_A, plus_correction);
- OnFwd(OUT_B, minus_correction);
- }
- else
- {
- NumOut(60, 30, 3);
- break;
- }
- }
- while(check)
- {
- // Do fancy shit with Lego Gun here
- }
- }
- }
- }
- }
Add Comment
Please, Sign In to add comment