Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <atx.h>
- #define BLACK 300
- #define CENTER_SENSOR 1
- #define LEFT_SENSOR 2
- #define RIGHT_SENSOR 3
- #define LFM 0
- #define LBM 1
- #define RFM 5
- #define RBM 4
- int state = 1;
- void setup()
- {
- sw1_press();
- }
- void loop()
- {
- int left = analogRead (LEFT_SENSOR);
- int right = analogRead (RIGHT_SENSOR);
- if(left < BLACK && right < BLACK)
- {
- if (CENTER_SENSOR > BLACK)
- {
- turn_right(1000,300);
- motor_stop(ALL);
- }
- if (left < 500 && 500 < BLACK && CENTER_SENSOR < 500)
- {
- motor_stop(ALL);
- }
- switch(state)
- {
- case 1:
- {
- turn_right(1000,1000);
- break;
- }
- case 2:
- {
- turn_right(1000,1000);
- break;
- }
- case 3:
- {
- turn_left(1000,1000);
- break;
- }
- default:
- {
- motor_stop(ALL);
- sw1_press()
- }
- }
- }
- else if (left < BLACK)
- {
- spin_left(100);
- }
- else if (right < BLACK)
- {
- spin_right(100);
- }
- else if (left > BLACK && right < BLACK && CENTER_SENSOR < BLACK)
- {
- turn_left(100,300);
- motor_stop(ALL);
- }
- else
- {
- motor_forward(30);
- }
- }
- void go_forward(int power,int ms)
- {
- motor(LFM,power);
- motor(LBM,power);
- motor(RFM,power);
- motor(RBM,power);
- delay(ms);
- motor_stop(ALL);
- }
- void motor_forward(int power)
- {
- motor(LFM,power);
- motor(LBM,power);
- motor(RFM,power);
- motor(RBM,power);
- }
- void turn_right(int power,int ms)
- {
- motor(LFM,power);
- motor(LBM,power);
- motor(RFM,-power);
- motor(RBM,-power);
- delay(ms);
- motor_stop(ALL);
- }
- void turn_left(int power,int ms)
- {
- motor(LFM,-power);
- motor(LBM,-power);
- motor(RFM,power);
- motor(RBM,power);
- delay(ms);
- motor_stop(ALL);
- }
- void spin_right(int power)
- {
- motor(LFM,power);
- motor(LBM,power);
- motor(RFM,-power);
- motor(RBM,-power);
- }
- void spin_left(int power)
- {
- motor(LFM,-power);
- motor(LBM,-power);
- motor(RFM,power);
- motor(RBM,power);
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement