Advertisement
Velaxers

Line tracker code

Jan 21st, 2020
184
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #define IRR 7 //IR right
  2. #define IRL 8 //IR left
  3. #define USECHO A0 //ultrasonic echo
  4. #define USTRIG A1 //ultrasonic trig
  5. #define MRF 6 //motor right front
  6. #define MRB 9 //motor right back
  7. #define MLF 10 //motor left front
  8. #define MLB 11 //motor left back
  9. #define FAST 60 //fast motor speed (analog)
  10. #define SLOW 30 //slow motor speed (analog)
  11. #define right 1 //rotation right
  12. #define left 0 //rotation left
  13. #define ramp 30 // distance between robot and a ramp
  14. #define bottle 10 //distance between robot and a bottle
  15.  
  16. #define rotate90(Direction)\
  17. {\
  18.   if (Direction == 1)\
  19.   {\
  20.     motor(0,0,FAST,0);\
  21.     delay(1000);\
  22.   }\
  23.   else\
  24.   {\
  25.     motor(FAST,0,0,0);\
  26.     delay(1000);\
  27.   }\
  28. }
  29. #define motor(speedMRF, speedMRB, speedMLF, speedMLB)\
  30. {\
  31.    analogWrite(MRF, speedMRF);\
  32.    analogWrite(MRB, speedMRB);\
  33.    analogWrite(MLF, speedMLF);\
  34.    analogWrite(MLB, speedMLB);\
  35. }
  36. void setup() {
  37.   //IR SENSORS
  38.   pinMode (IRR, INPUT);
  39.   pinMode (IRL, INPUT);
  40.   //MOTORS
  41.   pinMode (MRF, OUTPUT);
  42.   pinMode (MRB, OUTPUT);
  43.   pinMode (MLF, OUTPUT);
  44.   pinMode (MLB, OUTPUT);
  45.   //ULTRASONIC SENSOR
  46.   pinMode (USECHO, INPUT);
  47.   pinMode (USTRIG, OUTPUT);
  48.   Serial.begin (9600);
  49.  
  50. }
  51. void loop(){
  52.   //IR Readings
  53.   int RIR = digitalRead(IRL);
  54.   int LIR = digitalRead(IRR);
  55.  
  56.   //Ultra Sonic;
  57.   unsigned long duration, distance;
  58.   digitalWrite(USTRIG, HIGH);
  59.   delayMicroseconds(10);
  60.   digitalWrite(USTRIG, LOW);
  61.   duration = pulseIn(USECHO, HIGH);
  62.   distance = (duration/2) / 29.1;
  63.  
  64.     Serial.print(distance);
  65.     Serial.print(" cm////");
  66.     Serial.print(RIR);
  67.     Serial.print("  ");
  68.     Serial.println(LIR);
  69.    
  70.   if (distance==bottle) //bottle
  71.   {
  72.     motor(0,SLOW,0,SLOW);
  73.     rotate90(right);
  74.     motor(SLOW,0,SLOW,0);
  75.     delay(2000);
  76.     rotate90(left);
  77.     motor(SLOW,0,SLOW,0);
  78.     delay(4000);
  79.     rotate90(left);
  80.     motor(SLOW,0,SLOW,0);
  81.     delay(2000);
  82.     rotate90(right);
  83.   }
  84.   else if (distance==ramp) //ramp
  85.   {
  86.     motor(FAST,0,FAST,0);
  87.     delay(5000);
  88.   }
  89.   else //no ramp nor bottle is infront of robot
  90.   {
  91.     if (RIR == 1 && LIR == 1)
  92.       {motor (FAST,0,FAST,0);}
  93.     else if (RIR==1 && LIR ==0)
  94.       {motor (SLOW,0,FAST,0);}
  95.     else if (RIR==0 && LIR==1)
  96.       {motor (FAST,0,SLOW,0);}
  97.     else if (RIR==0 && LIR==0)
  98.       {motor (0, SLOW, 0, SLOW);}
  99.   }
  100. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement