Tzouraguy

RoboLab 3 full code

Nov 6th, 2020 (edited)
913
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 = 10;
  5. int in2 = 9;
  6. //Right Motor connection
  7. int enR = 6;
  8. int in3 = 5;
  9. int in4 = 3;
  10.  
  11. int i;
  12.  
  13. void setup()
  14. {
  15.   //Assign DDR
  16.   pinMode(enL,OUTPUT);
  17.   pinMode(in1,OUTPUT);
  18.   pinMode(in2,OUTPUT);
  19.   pinMode(enR,OUTPUT);
  20.   pinMode(in3,OUTPUT);
  21.   pinMode(in4,OUTPUT);
  22.   //Set initial state as "off"
  23.   digitalWrite(enL,LOW);
  24.   digitalWrite(in1,LOW);
  25.   digitalWrite(in2,LOW);
  26.   digitalWrite(enR,LOW);
  27.   digitalWrite(in3,LOW);
  28.   digitalWrite(in4,LOW);
  29. }
  30.  
  31. void loop()
  32. {
  33.   testMax();
  34.   testGradual();
  35. }
  36.  
  37. /**
  38.  * Sets drive to forward movement
  39.  */
  40. void moveForward()
  41. {
  42.   setL(250);
  43.   setR(250);
  44. }
  45.  
  46. /**
  47.  * Sets drive to forward movement with speed control
  48.  */
  49. void moveForward(int a)
  50. {
  51.   if(a > MAX_SPEED) {
  52.     setL(MAX_SPEED);
  53.     setR(MAX_SPEED);
  54.   } else if (a <= 0) {
  55.     stop();
  56.   } else {
  57.     setL(a);
  58.     setR(a);
  59.   }
  60. }
  61.  
  62. /**
  63.  * Sets drive to backward movement
  64.  */
  65. void moveBackward()
  66. {
  67.   setL(-250);
  68.   setR(-250);
  69. }
  70.  
  71. /**
  72.  * Sets drive to backward movement with speed control
  73.  */
  74. void moveBackward(int a)
  75. {
  76.   if(a > MAX_SPEED) {
  77.     setL(MAX_SPEED * (-1));
  78.     setR(MAX_SPEED * (-1));
  79.   } else if (a <= 0) {
  80.     stop();
  81.   } else {
  82.     setL(a *(-1));
  83.     setR(a *(-1));
  84.   }
  85. }
  86.  
  87. /**
  88.  * Sets drive to move counterclockwise
  89.  */
  90. void moveLeft()
  91. {
  92.   setL(50);
  93.   setR(250);
  94. }
  95.  
  96. /**
  97.  * Sets drive to move counterclockwise with speed control
  98.  */
  99. void moveLeft(int a)
  100. {
  101.   if(a > MAX_SPEED) {
  102.     setL(MAX_SPEED/2);
  103.     setR(MAX_SPEED);
  104.   } else if (a < MAX_SPEED * (-1)) {
  105.     setL(MAX_SPEED/-2);
  106.     setR(MAX_SPEED * (-1));
  107.   } else {
  108.     setL(a/2);
  109.     setR(map(abs(a),0,MAX_SPEED,MAX_SPEED/5,MAX_SPEED)*getSign(a));
  110.   }
  111. }
  112.  
  113. /**
  114.  * Sets drive to move clockwise
  115.  */
  116. void moveRight()
  117. {
  118.   setL(250);
  119.   setR(50);
  120. }
  121.  
  122. /**
  123.  * Sets drive to move counterclockwise with speed control
  124.  */
  125. void moveRight(int a)
  126. {
  127.   if(a > MAX_SPEED) {
  128.     setR(MAX_SPEED/2);
  129.     setL(MAX_SPEED);
  130.   } else if (a < MAX_SPEED * (-1)) {
  131.     setR(MAX_SPEED/-2);
  132.     setL(MAX_SPEED * (-1));
  133.   } else {
  134.     setR(a/2);
  135.     setL(map(abs(a),0,MAX_SPEED,MAX_SPEED/5,MAX_SPEED)*getSign(a));
  136.   }
  137. }
  138.  
  139. /**
  140.  * Sets drive to stop fully
  141.  */
  142. void stop()
  143. {
  144.   setL(0);
  145.   setR(0);
  146. }
  147.  
  148.  
  149. /**
  150.  * Left Motor control
  151.  */
  152. void setL(int a)
  153. {
  154.   digitalWrite(in1,(a>0)?HIGH:LOW);
  155.   digitalWrite(in2,(a<0)?HIGH:LOW);
  156.   analogWrite(enL,abs(a));
  157. }
  158.  
  159.  
  160. /**
  161.  * Right Motor control
  162.  */
  163. void setR(int a)
  164. {
  165.   digitalWrite(in3,(a>0)?HIGH:LOW);
  166.   digitalWrite(in4,(a<0)?HIGH:LOW);
  167.   analogWrite(enR,abs(a));
  168. }
  169.  
  170.  
  171. /**
  172.  * Test sequence of parameter-free actions
  173.  */
  174. void testMax()
  175. {
  176.   //Move Forwards
  177.   moveForward();
  178.   delay(4000);
  179.   stop();
  180.   delay(4000);
  181.   //Move Backwards
  182.   moveBackward();
  183.   delay(4000);
  184.   stop();
  185.   delay(4000);
  186.   //Turn Left
  187.   moveLeft();
  188.   delay(4000);
  189.   stop();
  190.   delay(4000);
  191.   //Turn Right
  192.   moveRight();
  193.   delay(4000);
  194.   stop();
  195.   delay(4000);
  196. }
  197.  
  198. /**
  199.  * Test sequence of parametered actions
  200.  */
  201. void testGradual()
  202. {
  203.   //Accelarate Forwards
  204.   for(i=0;i<=MAX_SPEED;i++) {
  205.     moveForward(i);
  206.     delay(10);
  207.   }
  208.   delay(4000);
  209.   stop();
  210.   delay(4000);
  211.   //Accelerate Backwards
  212.   for(i=0;i<=MAX_SPEED;i++) {
  213.     moveBackward(i);
  214.     delay(10);
  215.   }
  216.   delay(4000);
  217.   stop();
  218.   delay(4000);
  219.   //Turn Backwards-Left, then Forwards-Left
  220.   for(i=0-MAX_SPEED;i<=MAX_SPEED;i++) {
  221.     moveLeft(i);
  222.     delay(10);
  223.   }
  224.   delay(4000);
  225.   stop();
  226.   delay(4000);
  227.   //Turn Backwards-Right, then Forwards-Right
  228.   for(i=0-MAX_SPEED;i<=MAX_SPEED;i++) {
  229.     moveRight(i);
  230.     delay(10);
  231.   }
  232.   delay(4000);
  233.   stop();
  234.   delay(4000);
  235. }
  236.  
  237. /**
  238.  * Return sign of number
  239.  */
  240. int getSign(int a){
  241.   return a/abs(a);
  242. }
RAW Paste Data