Advertisement
GUPPYYYY

Line following code

Oct 30th, 2024
29
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <QTRSensors.h>
  2.  
  3.  
  4. QTRSensors qtr;
  5.  
  6. const uint8_t SensorCount = 4;
  7.  
  8. uint16_t sensorValues[SensorCount];
  9. const int In1 = A2;
  10.  
  11. const int In2 = A3;
  12.  
  13. const int In3 = A1;
  14.  
  15. const int In4 = A0;
  16.  
  17. const int EnA = 11;
  18. const int EnB = 3;
  19.  
  20. void setup()
  21.  
  22. {
  23.    qtr.setTypeRC();
  24.  
  25.   qtr.setSensorPins((const uint8_t[]){4, 5, 6, 7}, SensorCount);
  26.  
  27.   Serial.begin(9600);
  28.   pinMode(In1, OUTPUT);
  29.   pinMode(In2, OUTPUT);
  30.   pinMode(In3, OUTPUT);
  31.   pinMode(In4, OUTPUT);
  32.   pinMode(EnA, OUTPUT);
  33.   pinMode(EnB, OUTPUT);
  34.  
  35. }
  36.  
  37. void loop() {
  38.   qtr.read(sensorValues);
  39.     int leftValue = map(sensorValues[0], 0, 1500, 0, 20);
  40.     int leftCenterValue = map(sensorValues[1], 0, 1500, 0, 20);
  41.     int rightCenterValue = map(sensorValues[2], 0, 1500, 0, 20);
  42.     int rightValue = map(sensorValues[3], 0, 1500, 0, 20);
  43.    
  44.  
  45.  
  46.     if (leftValue - 3 > rightValue)
  47. {
  48.         goRight();
  49.     }
  50.     else if (leftValue < rightValue - 3)
  51.     {
  52.  
  53.         goLeft();
  54.     }
  55.  
  56. else
  57. {
  58. goStraight();
  59. }
  60.  
  61.  
  62. Serial.print(leftValue);
  63.  
  64.  Serial.print('\t');
  65.  
  66.  Serial.print(leftCenterValue);
  67.  
  68.   Serial.print('\t');
  69.  
  70.    Serial.print(rightCenterValue);
  71.  
  72.    Serial.print('\t');
  73.    Serial.print(rightValue);
  74.  
  75.    Serial.print('\t');
  76.  
  77.    Serial.println();
  78.  
  79.   delay(200);
  80.  
  81.  
  82.  
  83.   delay(250);
  84.   }
  85.    void goStraight()
  86.   {
  87.     digitalWrite(In1, HIGH);
  88.    digitalWrite(In2, LOW);
  89.     digitalWrite(In3, HIGH);
  90.     digitalWrite(In4, LOW);    
  91.     analogWrite(EnA, 100);
  92.     analogWrite(EnB, 100);
  93.     delay(50);
  94.    Stop();
  95.   }
  96.  
  97.   void Stop()
  98.   {
  99.     digitalWrite(In1, LOW);
  100.     digitalWrite(In2, LOW);  
  101.     digitalWrite(In3, LOW);
  102.     digitalWrite(In4, LOW);
  103.     analogWrite(EnA, 0);
  104.     analogWrite(EnB, 0);
  105.    }
  106.  
  107.   void goRight()
  108.    {
  109.     digitalWrite(In1, HIGH);
  110.     digitalWrite(In2, LOW);  
  111.     digitalWrite(In3, LOW);
  112.     digitalWrite(In4, HIGH);
  113.     analogWrite(EnA, 100);
  114.     analogWrite(EnB, 100);
  115.     delay(50);
  116.     Stop();
  117.     }
  118.   void goLeft()
  119.   {
  120.     digitalWrite(In1, LOW);
  121.     digitalWrite(In2, HIGH);  
  122.     digitalWrite(In3, HIGH);
  123.     digitalWrite(In4, LOW);
  124.     analogWrite(EnA, 100);
  125.     analogWrite(EnB, 100);
  126.     delay(50);
  127.     Stop();
  128.     }
  129.  
  130.  
  131.  
  132.  
  133.  
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement