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 2
- #define RBM 3
- void setup()
- {
- sw1_press();
- }
- void loop()
- {
- go_forward(100,500);
- turn_right(100,300);
- go_forward(100,250);
- timed_follow( 3000 );
- go_backward(100,050);
- turn_right(100,780);
- go_backward(100,800);
- turn_right(100,780);
- turn_left(100,300);
- timed_follow( 15000 );
- }
- void timed_follow( long int ms )
- {
- long int start = millis();
- while( millis() - start<ms )
- {
- int left = analogRead( LEFT_SENSOR );
- int right = analogRead( RIGHT_SENSOR );
- if( left < BLACK )
- {
- spin_left(100);
- }
- else if( right < BLACK )
- {
- spin_right(100);
- }
- 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 go_backward(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