Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <atx.h>
- #define BLACK 150
- #define LEFT_SENSOR 2
- #define RIGHT_SENSOR 3
- int state = 1;
- int ms;
- void setup()
- {
- lcd("pressSW1");
- sw1_press();
- beep();
- }
- void loop()
- {
- int left = analogRead(LEFT_SENSOR);
- int right = analogRead(RIGHT_SENSOR);
- if(left < 200 && right < 200)
- {
- beep();
- switch(state)
- {
- case 1:
- {
- go_backward(50);
- delay(250);
- spin_left(100);
- delay(380);
- break;
- }
- case 2:
- {
- go_backward(50);
- delay(250);
- spin_right(100);
- delay(400);
- break;
- }
- case 3:
- {
- go_backward(50);
- delay(250);
- spin_right(100);
- delay(400);
- break;
- }
- case 4:
- {
- go_backward(50);
- delay(250);
- spin_right(100);
- delay(400);
- break;
- }
- default:
- {
- motor_stop(ALL);
- sw1_press();
- }
- }
- state = state + 1;
- }
- else if(left < 200)
- {
- spin_left(20);
- }
- else if(right < 200)
- {
- spin_right(20);
- }
- else
- {
- go_forward(100);
- }
- }
- void spin_left(int t_power)
- {
- motor(0,t_power);
- motor(1,t_power);
- motor(2,-t_power);
- motor(3,-t_power);
- }
- void spin_right(int t_power)
- {
- motor(0,-t_power);
- motor(1,-t_power);
- motor(2,t_power);
- motor(3,t_power);
- }
- void go_forward(int f_power)
- {
- motor(0,f_power);
- motor(1,f_power);
- motor(2,f_power);
- motor(3,f_power);
- }
- void go_backward(int b_power)
- {
- motor(0,-b_power);
- motor(1,-b_power);
- motor(2,-b_power);
- motor(3,-b_power);
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement