Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #define IRR 7 //IR right
- #define IRL 8 //IR left
- #define USECHO A0 //ultrasonic echo
- #define USTRIG A1 //ultrasonic trig
- #define MRF 6 //motor right front
- #define MRB 9 //motor right back
- #define MLF 10 //motor left front
- #define MLB 11 //motor left back
- #define FAST 60 //fast motor speed (analog)
- #define SLOW 30 //slow motor speed (analog)
- #define right 1 //rotation right
- #define left 0 //rotation left
- #define ramp 30 // distance between robot and a ramp
- #define bottle 10 //distance between robot and a bottle
- #define rotate90(Direction)\
- {\
- if (Direction == 1)\
- {\
- motor(0,0,FAST,0);\
- delay(1000);\
- }\
- else\
- {\
- motor(FAST,0,0,0);\
- delay(1000);\
- }\
- }
- #define motor(speedMRF, speedMRB, speedMLF, speedMLB)\
- {\
- analogWrite(MRF, speedMRF);\
- analogWrite(MRB, speedMRB);\
- analogWrite(MLF, speedMLF);\
- analogWrite(MLB, speedMLB);\
- }
- void setup() {
- //IR SENSORS
- pinMode (IRR, INPUT);
- pinMode (IRL, INPUT);
- //MOTORS
- pinMode (MRF, OUTPUT);
- pinMode (MRB, OUTPUT);
- pinMode (MLF, OUTPUT);
- pinMode (MLB, OUTPUT);
- //ULTRASONIC SENSOR
- pinMode (USECHO, INPUT);
- pinMode (USTRIG, OUTPUT);
- Serial.begin (9600);
- }
- void loop(){
- //IR Readings
- int RIR = digitalRead(IRL);
- int LIR = digitalRead(IRR);
- //Ultra Sonic;
- unsigned long duration, distance;
- digitalWrite(USTRIG, HIGH);
- delayMicroseconds(10);
- digitalWrite(USTRIG, LOW);
- duration = pulseIn(USECHO, HIGH);
- distance = (duration/2) / 29.1;
- Serial.print(distance);
- Serial.print(" cm////");
- Serial.print(RIR);
- Serial.print(" ");
- Serial.println(LIR);
- if (distance==bottle) //bottle
- {
- motor(0,SLOW,0,SLOW);
- rotate90(right);
- motor(SLOW,0,SLOW,0);
- delay(2000);
- rotate90(left);
- motor(SLOW,0,SLOW,0);
- delay(4000);
- rotate90(left);
- motor(SLOW,0,SLOW,0);
- delay(2000);
- rotate90(right);
- }
- else if (distance==ramp) //ramp
- {
- motor(FAST,0,FAST,0);
- delay(5000);
- }
- else //no ramp nor bottle is infront of robot
- {
- if (RIR == 1 && LIR == 1)
- {motor (FAST,0,FAST,0);}
- else if (RIR==1 && LIR ==0)
- {motor (SLOW,0,FAST,0);}
- else if (RIR==0 && LIR==1)
- {motor (FAST,0,SLOW,0);}
- else if (RIR==0 && LIR==0)
- {motor (0, SLOW, 0, SLOW);}
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement