SHARE
TWEET

robot care nu merge

a guest Apr 21st, 2017 56 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <QTRSensors.h>
  2.  
  3. //TODO: Inlocuiti x cu pinul la care este conectat pe Arduino
  4. const unsigned int MOTOR1A = 3;
  5. const unsigned int MOTOR1B = 5;
  6. const unsigned int MOTOR2A = 9;
  7. const unsigned int MOTOR2B = 6;
  8.  
  9. const unsigned int LINIE8 = A3;
  10. const unsigned int LINIE7 = A2;
  11. const unsigned int LINIE6 = A1;
  12. const unsigned int LINIE5 = A0;
  13. const unsigned int LINIE4 = 13;
  14. const unsigned int LINIE3 = 12;
  15. const unsigned int LINIE2 = 11;
  16. const unsigned int LINIE1 = 10;
  17. //ENDTODO
  18.  
  19. //PID
  20. const float KP = 0.5;
  21. const float KD = 2.0;
  22. const float KI = 0.0;
  23.  
  24. double last_p = 0;
  25. double integral = 0;
  26. //ENDPID
  27.  
  28. const int SPEED = 30;
  29. int motorTested = 1;
  30.  
  31. QTRSensorsRC qtr((unsigned char[]) {LINIE1, LINIE2, LINIE3, LINIE4, LINIE5, LINIE6, LINIE7, LINIE8}, 8);
  32. unsigned int senzori[8];
  33.  
  34. void setMotors(int a, int b)
  35. {
  36.     /*
  37.      * TODO: implementati controlul motoarelor
  38.      * a = valoarea pentru motorul stang
  39.      * b = valoarea pentru motorul drept
  40.      * Daca primeste valoare negativa, motorul se invarte inapoi.
  41.      * Pentru a roti motorul intr-o directie, setati unul din pini pe LOW si pe celalalt scrieti
  42.      * valoarea PWM. Aveti grija la semn, analogWrite primeste o valoare pozitiva (0 - 255).
  43.      */
  44.  
  45.   if(a <0)  
  46.   {
  47.    analogWrite(MOTOR1A, -a);
  48.    analogWrite(MOTOR1B, 0);
  49.   }
  50.   if(a >0)  
  51.   {
  52.     analogWrite(MOTOR1A, 0);
  53.     analogWrite(MOTOR1B, a);
  54.    
  55.   }
  56.   if(b <0)  
  57.   {
  58.     analogWrite(MOTOR2A, -b);
  59.     analogWrite(MOTOR2B, 0);
  60.   }
  61.   if(b >0)  
  62.   {
  63.     analogWrite(MOTOR2A, 0);
  64.     analogWrite(MOTOR2B, b);
  65.   }
  66. //  if(a == 0)
  67. //  {
  68. //     analogWrite(MOTOR1A, 0);
  69. //     analogWrite(MOTOR1B, 0);
  70. //  }
  71. //  if(b == 0)
  72. //  {
  73. //     analogWrite(MOTOR2A, 0);
  74. //     analogWrite(MOTOR2B, 0);
  75. //  }
  76.  
  77. }
  78.  
  79. void printLine()
  80. {
  81.     /*
  82.      * TODO: Cititi senzorii si trimiteti valorile citite prin interfata seriala.
  83.      * Pentru citire folositi qtr.read(senzori). Functia va citi senzorii care au fost initializati
  84.      * la declararea lui qtr si va pune valorile in vectorul senzori.
  85.      */
  86.    qtr.read(senzori);
  87.      
  88.  
  89. }
  90.  
  91. long getProportional()
  92. {
  93.       long proportional;
  94.     /*
  95.      * TODO: Functie ce va returna termenul proportional.
  96.      *
  97.      * Folositi functia qtr.readLine(senzori).
  98.      * Aceasta copiaza valorile citite in vectorul senzori si returneaza un int intre 0 si 7000,
  99.      * 0 insemnand ca linia este sub senzorul 1 si 7000 insemnand ca linia este sub senzorul 8.
  100.      *
  101.      * Termenul proportional este reprezentat de devierea de la conditia dorita. Se doreste ca
  102.      * linia sa se afle pe centru, deci functia va returna o valoare proportionala cu devierea
  103.      * liniei de la centru. Aceasta va fi negativa daca se afla in stanga si pozitiva daca se
  104.      * afla in dreapta.
  105.      */
  106.      proportional = qtr.readLine(senzori);
  107.    
  108.      proportional -= 3500;
  109.    proportional /= 10;
  110.    
  111.    
  112.     return proportional;
  113. }
  114.  
  115. double computePID()
  116. {
  117.     double proportional;
  118.     double derivative;
  119.     /*
  120.      * Aplicati algoritmul PID
  121.      * integral este o variabila globala definita mai sus.
  122.      * Folositi last_p pentru a calcula termenul derivat.
  123.      */
  124.    proportional =  getProportional();
  125.      derivative = proportional - last_p;
  126.    integral += proportional;
  127.    last_p = proportional;
  128.  
  129.      return KP * proportional + KI * integral + KD * derivative;
  130. }
  131.  
  132. void setup()
  133. {
  134.     Serial.begin(9600);
  135.  
  136.     pinMode(MOTOR1A, OUTPUT);
  137.     pinMode(MOTOR1B, OUTPUT);
  138.     pinMode(MOTOR2A, OUTPUT);
  139.     pinMode(MOTOR2B, OUTPUT);
  140.     pinMode(13, OUTPUT); //pinul cu led
  141.  
  142.     digitalWrite(13, HIGH);
  143.     for (int i = 0; i < 125; i++)  { //calibrare senzori
  144.          //semnalare calibrare prin led-ul legat la pinul 13
  145.         qtr.calibrate();
  146.         delay(20);
  147.        
  148.     }
  149.     digitalWrite(13, LOW);
  150. }
  151.  
  152.  
  153. void loop()
  154. {
  155. //  if(!motorTested) {
  156. //    setMotors(0,0);
  157. // 
  158. //  for(int i = 0 ; i < 20 ; i++) {
  159. //      printLine();
  160. //      delay(50);
  161. //  }
  162. //  setMotors(40, 0);
  163. //  delay(500);
  164. //  setMotors(0, 40);                  
  165. //  delay(500);
  166. //  setMotors(-40, 0);
  167. //  delay(500);
  168. //  setMotors(0, -40);
  169. //  delay(500);
  170. //
  171. //  motorTested = 1;
  172. //  setMotors(0,0);
  173. //  }
  174.  
  175.   float pid = computePID();
  176.  
  177. //  for(int i=0; i<8; i++)
  178. //  {
  179. //    Serial.print(senzori[i]);
  180. //    Serial.print(" ");
  181. //  }
  182.   Serial.println(qtr.readLine(senzori));
  183.   Serial.println(pid);
  184.   if(pid > 0) {
  185.     int a = constrain(SPEED + pid, 0, 255);
  186.     int b = constrain(SPEED - pid, 0, 255);
  187.     setMotors(a, b);
  188.   } else if(pid < 0) {
  189.     int a = constrain(SPEED + pid, 0, 255);
  190.     int b = constrain(SPEED - pid, 0, 255);
  191.     setMotors(a, b);
  192.   } else {
  193.     setMotors(SPEED, SPEED);
  194.   }
  195.  
  196.  
  197. }
RAW Paste Data
Pastebin PRO Summer Special!
Get 40% OFF on Pastebin PRO accounts!
Top