Advertisement
Guest User

Quick fix for " setMotors()"

a guest
Jul 31st, 2016
71
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 2.36 KB | None | 0 0
  1. #include <NewPing.h>
  2.  
  3. #define RLED 23
  4. #define GLED 13
  5. #define BLED 22
  6.  
  7. #define LIR A3
  8. #define CIR A1
  9. #define RIR A2
  10.  
  11. #define VDIV A0
  12.  
  13. #define LF 12
  14. #define LB 11
  15. #define RF 9
  16. #define RB 10
  17.  
  18. #define USTRIGGER 6
  19. #define USECHO 5
  20.  
  21. #define STOP_THRESH 2
  22.  
  23. NewPing sonar(USTRIGGER, USECHO);
  24. int LED[3] = {RLED, GLED, BLED};
  25. int IR[3] = {LIR, CIR, RIR};
  26. int MOT[2][2] = {{LF, LB}, {RF, RB}};
  27. void setup() {
  28.   //LEDS and IR
  29.   for (int i = 0; i < 3; i++) {
  30.     pinMode(LED[i], OUTPUT);
  31.     pinMode(IR[i], INPUT);
  32.   }
  33.  
  34.   //Voltage Divider, for checking battery
  35.   pinMode(VDIV, INPUT);
  36.  
  37.   //Serial, for outputting... stuff.
  38.   Serial.begin(9600);
  39. }
  40.  
  41. void loop() {
  42.  
  43.   for (int i = 0; i < 3; i++) {
  44.     //digitalWrite(LED[(i-1>=0) ? i-1 : 2], 0);
  45.     //digitalWrite(LED[i], 1);
  46.     //Serial.println(sonar.ping_cm());
  47.     //Serial.println(checkIR(IR[i]));
  48.    
  49.   }
  50.   delay(1000);
  51. }
  52.  
  53. //returns true if on white, false if on black/edge
  54. bool checkIR(int ir) {
  55.   return analogRead(ir) < 500;
  56. }
  57.  
  58. //returns true if the battery has enough juice
  59. bool checkBatt() {
  60.   return analogRead(VDIV) > 512;
  61. }
  62.  
  63.  
  64. //each motor between -255 and 255 (though best values are < 200)
  65. void setMotors(int left, int right) {
  66.  int dir;
  67.  int val[2] = {255, 255};
  68.   for (int i = 0; i < 2; i++) {
  69.     if (val[i] == 0) {
  70.       for (int j = 0; j < 2; j++) {
  71.         analogWrite(MOT[i][j], 0);
  72.       }
  73.     }
  74.     else {
  75.       dir = val[i] / abs(val[i]);
  76.       analogWrite(MOT[i][(1-dir)/2], 0);
  77.       analogWrite(MOT[i][(1+dir)/2], abs(val[i]));
  78.   }
  79.  }
  80. }
  81.  
  82. void followLineUntilJunction() {
  83.   int confirm = 0;
  84.   uint8_t reading;
  85.   unsigned long t = millis();
  86.  
  87.   while (true) {
  88.     if (millis() - t > 100) {
  89.       t = millis();
  90.  
  91.       reading = 0;
  92.       for (int i = 0; i < 3; i++) reading =+ checkIR(IR[i]) << i;
  93.      
  94.       if (reading == 0b000 || reading == 0b101 || reading == 0b111) {
  95.         if (confirm <= STOP_THRESH) {
  96.           confirm++;
  97.           continue;
  98.         }
  99.         else {
  100.           setMotors(0, 0);
  101.           return;
  102.         }
  103.       }
  104.       if (reading == 0b100 || reading == 0b110) {
  105.         confirm = 0;
  106.         setMotors(100, 0);
  107.         continue;
  108.       }
  109.       if (reading == 0b001 || reading == 0b011) {
  110.         confirm = 0;
  111.         setMotors(0, 100);
  112.         continue;
  113.       }
  114.       setMotors(75, 75);
  115.     }
  116.   }
  117. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement