Advertisement
Dani_info

line follower

Feb 25th, 2019
104
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <QTRSensors.h>
  2. #define NUM_SENSORS             8  // number of sensors used
  3. #define NUM_SAMPLES_PER_SENSOR  4  // average 4 analog samples per sensor reading
  4. #define EMITTER_PIN             2  // emitter is controlled by digital pin 2
  5.  
  6. QTRSensorsAnalog qtra((unsigned char[]) {
  7.   0, 1, 2, 3, 4, 5, 6, 7
  8. }, NUM_SENSORS, NUM_SAMPLES_PER_SENSOR, EMITTER_PIN);
  9. unsigned int sensorValues[NUM_SENSORS];
  10.  
  11. int MOTOR2_PIN1 = 3;
  12. int MOTOR2_PIN2 = 5;
  13. int MOTOR1_PIN1 = 6;
  14. int MOTOR1_PIN2 = 9;
  15.  
  16.  
  17.  
  18. void setup()
  19. {
  20.   Serial.begin(9600);
  21.  
  22.   delay(500);
  23.   pinMode(13, OUTPUT);
  24.   digitalWrite(13, HIGH);
  25.  
  26.   for (int i = 0; i < 400; i++)  // make the calibration take about 10 seconds
  27.   {
  28.     qtra.calibrate();       // reads all sensors 10 times at 2.5 ms per six sensors (i.e. ~25 ms per call)
  29.   }
  30.  
  31.   digitalWrite(13, LOW);
  32.  
  33.   for (int i = 0; i < NUM_SENSORS; i++)
  34.   {
  35.     Serial.print(qtra.calibratedMinimumOn[i]);
  36.     Serial.print(' ');
  37.   }
  38.   Serial.println();
  39.  
  40.   for (int i = 0; i < NUM_SENSORS; i++)
  41.   {
  42.     Serial.print(qtra.calibratedMaximumOn[i]);
  43.     Serial.print(' ');
  44.   }
  45.  
  46.   Serial.println();
  47.   Serial.println();
  48.   delay(1000);
  49.  
  50.   pinMode(MOTOR1_PIN1, OUTPUT);
  51.   pinMode(MOTOR1_PIN2, OUTPUT);
  52.   pinMode(MOTOR2_PIN1, OUTPUT);
  53.   pinMode(MOTOR2_PIN2, OUTPUT);
  54.  
  55. }
  56.  
  57. //float kp;
  58.  
  59. int Kp = 5;
  60. int Kd = 5;
  61. int pozitie_referinta;
  62. int eroare_pozitie;
  63. int eroare_pozitie_anterioara;
  64. int derivativ;
  65. int corectie_viteza;    //float corectie_viteza;
  66. int vitezaM1;
  67. int vitezaM2;
  68. int viteza_croaziera = 40;
  69.  
  70.  
  71. void loop()
  72. {
  73.  
  74.   pozitie_referinta = 35;
  75.  
  76.   unsigned int position = qtra.readLine(sensorValues) / 100;
  77.  
  78.   eroare_pozitie = pozitie_referinta - position;
  79.  
  80.   derivativ = eroare_pozitie - eroare_pozitie_anterioara;
  81.  
  82.   eroare_pozitie_anterioara = eroare_pozitie;
  83.  
  84.   corectie_viteza = Kp * eroare_pozitie + Kd * derivativ;
  85.  
  86.   vitezaM1 = viteza_croaziera - corectie_viteza;
  87.   vitezaM2 = viteza_croaziera + corectie_viteza;
  88.  
  89.   //go(vitezaM1, vitezaM2);
  90.  
  91.   Serial.print(vitezaM1);
  92.   Serial.print(" ");
  93.   Serial.println(vitezaM2);
  94.  
  95.   //Serial.println(eroare_pozitie);
  96.  
  97.   delay(250);
  98. }
  99.  
  100.  
  101. void go(int speedLeft, int speedRight)
  102. {
  103.   if (speedLeft > 0)
  104.   {
  105.     analogWrite(MOTOR1_PIN1, speedLeft);
  106.     analogWrite(MOTOR1_PIN2, 0);
  107.   }
  108.   else
  109.   {
  110.     analogWrite(MOTOR1_PIN1, 0);
  111.     analogWrite(MOTOR1_PIN2, -speedLeft);
  112.   }
  113.  
  114.   if (speedRight > 0)
  115.   {
  116.     analogWrite(MOTOR2_PIN1, speedRight);
  117.     analogWrite(MOTOR2_PIN2, 0);
  118.   }
  119.   else
  120.   {
  121.     analogWrite(MOTOR2_PIN1, 0);
  122.     analogWrite(MOTOR2_PIN2, -speedRight);
  123.   }
  124. }
Advertisement
RAW Paste Data Copied
Advertisement