SHARE
TWEET

Untitled

a guest Aug 13th, 2019 79 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. // --
  2. // The UNO definitions for which PINs control the motors
  3. // --
  4. #define LEFT_SPEED      5   // Controls the left motor speed via Pulse Width Modulation
  5. #define LEFT_FORWARD    7
  6. #define LEFT_BACKWARD   8
  7. #define LEFT_NONE       0
  8. #define RIGHT_SPEED     6   // Controls the right motor speed via Pulse Width Modulation
  9. #define RIGHT_FORWARD   11
  10. #define RIGHT_BACKWARD  9
  11. #define RIGHT_NONE      0
  12.  
  13. const int PINS_OUT[] = {
  14.   LEFT_SPEED,
  15.   LEFT_FORWARD,
  16.   LEFT_BACKWARD,
  17.   RIGHT_SPEED,
  18.   RIGHT_FORWARD,
  19.   RIGHT_BACKWARD,
  20. };
  21.  
  22. #define LT_RIGHT 10
  23. #define LT_MIDDLE 4
  24. #define LT_LEFT 2
  25.  
  26. const int PINS_IN[] = {
  27.   LT_RIGHT,
  28.   LT_MIDDLE,
  29.   LT_LEFT
  30. };
  31.  
  32. const int DIRECTIONS[] = {
  33.   LEFT_FORWARD,
  34.   LEFT_BACKWARD,
  35.   RIGHT_FORWARD,
  36.   RIGHT_BACKWARD
  37. };
  38.  
  39. #define DEFAULT_SPEED 160
  40.  
  41. #define ARRAY_LENGTH(X) (sizeof(X)/sizeof(X[0]))
  42.  
  43. void set_speed(int left_speed, int right_speed) {
  44.    analogWrite(LEFT_SPEED, left_speed);
  45.    analogWrite(RIGHT_SPEED, right_speed);
  46.  
  47. }
  48. void set_speed(int speed) {
  49.    set_speed(speed, speed);
  50. {
  51.  
  52. void set_direction(int left, int right);
  53. }
  54.  
  55.  for (int i = 0; i<ARRAY_LENGTH(DIRECTIONS); i++) {
  56.     digitalWrite(DIRECTIONS[i], LOW);
  57.  }
  58.  
  59.  if (left) {
  60.     digitalWrite(left, HIGH);
  61.  }
  62.   if (right) {
  63.      digitalWrite(right, HIGH);
  64.   }
  65. }
  66.  
  67. void set_direction_speed(int left, int right, int speed)
  68. {
  69.    set_direction(left, right);
  70.    set_speed(speed);
  71. }
  72.  
  73. void setup() {
  74.   // Set up all the PINS_OUT as OUTPUT pins.
  75.   for (int i = 0; i<ARRAY_LENGTH(PINS_OUT); i++) {
  76.     pinMode(PINS_OUT[i], OUTPUT);  
  77.   }
  78.  
  79.   // Set up input pins
  80.   for (int i = 0; i<ARRAY_LENGTH(PINS_IN); i++) {
  81.     pinMode(PINS_IN[i], INPUT);  
  82.   }
  83.  
  84.   set_speed(0);         // Make sure robot is not moving to start
  85. }
  86.  
  87. boolean line_track(int pin)
  88. {
  89.  
  90. return !digitalRead(pin);
  91.  
  92.  
  93.   void loop() {
  94.      boolean bRight = line_track(LT_RIGHT);
  95.      boolean bMiddle = lie_track(LT_MIDDLE);
  96.      boolean bLeft = line_track(LT_LEFT);
  97.  
  98.   if(bLeft && bRight) {
  99.    
  100.     set_speed(0);
  101.   } else {
  102.     if(bRight) {
  103.        Serial.println("R");
  104.         set_direction_speed(LEFT_FORWARD, RIGHT_BACKWARD, DEFAULT_SPEED);
  105.     }
  106.     else if(bLeft) {
  107.        Serial.println("L");
  108.        set_direction_speed(LEFT_BACKWARD, RIGHT_FORWARD, DEFAULT_SPEED);
  109.     }
  110.     else if(bMiddle){
  111.        Serial.println("M");
  112.        
  113.      set_direction_speed(LEFT_FORWARD, RIGHT_FORWARD, DEFAULT_SPEED);
  114.     }
  115.      else set_speed(0);
  116.    }
  117.   }
  118.     }
  119.     }
  120.    
  121.   }
  122.  
  123.  
  124.   }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top