Tzouraguy

RoboLab 3_2 full code

Nov 7th, 2020
1,215
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #define MAX_SPEED 250
  2. //Left Motor connection
  3. int enL = 11;
  4. int in1 = 12;
  5. int in2 = 13;
  6. //Right Motor connection
  7. int enR = 6;
  8. int in3 = 7;
  9. int in4 = 8;
  10. //Ultrasonic Sensor connection
  11. int trigPin 9;
  12. int echoPin 10;
  13.  
  14. int measurement;
  15.  
  16. void setup()
  17. {
  18.   //Assign DDR
  19.   pinMode(enL,OUTPUT);
  20.   pinMode(in1,OUTPUT);
  21.   pinMode(in2,OUTPUT);
  22.   pinMode(enR,OUTPUT);
  23.   pinMode(in3,OUTPUT);
  24.   pinMode(in4,OUTPUT);
  25.   pinMode(trigPin, OUTPUT);
  26.   pinMode(echoPin, INPUT);
  27.   //Set motor initial state as "off"
  28.   digitalWrite(enL,LOW);
  29.   digitalWrite(in1,LOW);
  30.   digitalWrite(in2,LOW);
  31.   digitalWrite(enR,LOW);
  32.   digitalWrite(in3,LOW);
  33.   digitalWrite(in4,LOW);
  34. }
  35.  
  36. /**
  37.  * Behaviour is implemented here
  38.  */
  39. void loop()
  40. {
  41.   measurement = measureDistance();
  42.   if(measurement > 200){
  43.     moveForeward();
  44.   } else if (measurement < 30) {
  45.     stop();
  46.   } else {
  47.     moveForeward(map(measurement,30,200,50,255));
  48.   }
  49. }
  50.  
  51. /**
  52.  * Sets drive to forward movement
  53.  */
  54. void moveForward()
  55. {
  56.   setL(250);
  57.   setR(250);
  58. }
  59.  
  60. /**
  61.  * Sets drive to forward movement with speed control
  62.  */
  63. void moveForward(int a)
  64. {
  65.   if(a > MAX_SPEED) {
  66.     setL(MAX_SPEED);
  67.     setR(MAX_SPEED);
  68.   } else if (a <= 0) {
  69.     stop();
  70.   } else {
  71.     setL(a);
  72.     setR(a);
  73.   }
  74. }
  75.  
  76. /**
  77.  * Sets drive to backward movement
  78.  */
  79. void moveBackward()
  80. {
  81.   setL(-250);
  82.   setR(-250);
  83. }
  84.  
  85. /**
  86.  * Sets drive to backward movement with speed control
  87.  */
  88. void moveBackward(int a)
  89. {
  90.   if(a > MAX_SPEED) {
  91.     setL(MAX_SPEED * (-1));
  92.     setR(MAX_SPEED * (-1));
  93.   } else if (a <= 0) {
  94.     stop();
  95.   } else {
  96.     setL(a *(-1));
  97.     setR(a *(-1));
  98.   }
  99. }
  100.  
  101. /**
  102.  * Sets drive to move counterclockwise
  103.  */
  104. void moveLeft()
  105. {
  106.   setL(50);
  107.   setR(250);
  108. }
  109.  
  110. /**
  111.  * Sets drive to move counterclockwise with speed control
  112.  */
  113. void moveLeft(int a)
  114. {
  115.   if(a > MAX_SPEED) {
  116.     setL(MAX_SPEED/2);
  117.     setR(MAX_SPEED);
  118.   } else if (a < MAX_SPEED * (-1)) {
  119.     setL(MAX_SPEED/-2);
  120.     setR(MAX_SPEED * (-1));
  121.   } else {
  122.     setL(a/2);
  123.     setR(map(abs(a),0,MAX_SPEED,MAX_SPEED/5,MAX_SPEED)*getSign(a));
  124.   }
  125. }
  126.  
  127. /**
  128.  * Sets drive to move clockwise
  129.  */
  130. void moveRight()
  131. {
  132.   setL(250);
  133.   setR(50);
  134. }
  135.  
  136. /**
  137.  * Sets drive to move counterclockwise with speed control
  138.  */
  139. void moveRight(int a)
  140. {
  141.   if(a > MAX_SPEED) {
  142.     setR(MAX_SPEED/2);
  143.     setL(MAX_SPEED);
  144.   } else if (a < MAX_SPEED * (-1)) {
  145.     setR(MAX_SPEED/-2);
  146.     setL(MAX_SPEED * (-1));
  147.   } else {
  148.     setR(a/2);
  149.     setL(map(abs(a),0,MAX_SPEED,MAX_SPEED/5,MAX_SPEED)*getSign(a));
  150.   }
  151. }
  152.  
  153. /**
  154.  * Sets drive to stop fully
  155.  */
  156. void stop()
  157. {
  158.   setL(0);
  159.   setR(0);
  160. }
  161.  
  162.  
  163. /**
  164.  * Left Motor control
  165.  */
  166. void setL(int a)
  167. {
  168.   digitalWrite(in1,(a>0)?HIGH:LOW);
  169.   digitalWrite(in2,(a<0)?HIGH:LOW);
  170.   analogWrite(enL,abs(a));
  171. }
  172.  
  173.  
  174. /**
  175.  * Right Motor control
  176.  */
  177. void setR(int a)
  178. {
  179.   digitalWrite(in3,(a>0)?HIGH:LOW);
  180.   digitalWrite(in4,(a<0)?HIGH:LOW);
  181.   analogWrite(enR,abs(a));
  182. }
  183.  
  184. /**
  185.  * Method to measure distance using the Ultrasonic Sensor
  186.  */
  187. int measureDistance(){
  188.   long duration;
  189.   int distance;
  190.   // Clears the trigPin
  191.   digitalWrite(trigPin, LOW);
  192.   delayMicroseconds(2);
  193.   // Sets the trigPin on HIGH state for 10 micro seconds
  194.   digitalWrite(trigPin, HIGH);
  195.   delayMicroseconds(10);
  196.   digitalWrite(trigPin, LOW);
  197.   // Reads the echoPin, returns the sound wave travel time in microseconds
  198.   duration = pulseIn(echoPin, HIGH);
  199.   // Calculating the distance
  200.   distance= duration*0.034/2;
  201.   return distance;
  202. }
  203.  
  204. /**
  205.  * Return sign of number
  206.  */
  207. int getSign(int a){
  208.   return a/abs(a);
  209. }
RAW Paste Data