lehrkraefte:blc:robotics:pid

Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Both sides previous revision Previous revision
Next revision 		Previous revision
lehrkraefte:blc:robotics:pid [2017/07/06 07:41]
Ivo Blöchliger 		lehrkraefte:blc:robotics:pid [2017/07/07 07:49] (current)
Ivo Blöchliger
Line 1: Line 1:
-{{ :lehrkraefte:blc:robotics:regeltechnik.pdf |}} +===== Funktionierenden Line-Follower ===== 
- +<code c++ sketch.txt>
-<code c++>+
 // Umrechnung vom Intervall [-1,1] auf [-799,799] // Umrechnung vom Intervall [-1,1] auf [-799,799]
 // Die Umrechnung braucht nicht linear zu sein! Die Motoren drehen schliesslich erst ab ca. Power 500 // Die Umrechnung braucht nicht linear zu sein! Die Motoren drehen schliesslich erst ab ca. Power 500
 void setFloatPower(float links, float rechts) { void setFloatPower(float links, float rechts) {
-  // TODO: bessere Umrechnung (z.B. [0,1]->[500,799] und [-1,0]->[-799,-500]) +  int l = ((links>=0) ? 1 : -1)*(sqrt(abs(links)))*799; 
-  int l = links*799; +  int r = ((rechts>=0) ? 1 : -1)*(sqrt(abs(rechts)))*799;
-  int r = rechts*799;+
   robot.motors.setPowers(l,r);   robot.motors.setPowers(l,r);
 } }
Line 23: Line 21:
  
 // Parameter P,I,D // Parameter P,I,D
-// TODO: bessere Werte finden +float pid[] = {0.9, 0.0, 0.03};
-float pid[] = {1.0, 0.0, 0.0};+
  
-void lineFollow() {+ 
 +/**  
 + *  Der Linie folgen (dir=-1 heisst schwarz ist links). 
 + */ 
 +int lineFollow(int dir) {
   // Diese Werte zuerst experimentell bestimmen!   // Diese Werte zuerst experimentell bestimmen!
-  int weiss = 600;  // Messwert auf dem weissen Klebeband +  int weiss = 380;  // Messwert auf dem weissen Klebeband 
-  int schwarz = 80;  // Messwert auf dem schwarzen Klebeband+  int schwarz = 30;  // Messwert auf dem schwarzen Klebeband
      
   // Sollwert für die Motoren (in [-1,1]), hängt auch vom Unterprogram setFloatPower ab!   // Sollwert für die Motoren (in [-1,1]), hängt auch vom Unterprogram setFloatPower ab!
   // D.h. wie schnell sollen diese drehen, wenn man genau auf der Linie fährt.   // D.h. wie schnell sollen diese drehen, wenn man genau auf der Linie fährt.
-  float soll = 0.5;+  float soll = 0.65;
  
   // Zeitpunkt letzter Messung   // Zeitpunkt letzter Messung
Line 52: Line 53:
     int wert = ir.measure();     int wert = ir.measure();
     // Auf Intervall [-1,1] umrechnen, v ist damit auch die Abweichung vom Sollwert 0.0     // Auf Intervall [-1,1] umrechnen, v ist damit auch die Abweichung vom Sollwert 0.0
-    float v = 0.0; // TODO: hier Formel aus wert, weiss und schwarz +    float v = dir*(2.0*(wert-schwarz)/(weiss-schwarz) - 1.0); 
-    float zeit = (micros()-lastus)/10000000.0;   // Zeitdifferenz in Sekunden+    float zeit = (micros()-lastus)/1000000.0;   // Zeitdifferenz in Sekunden 
 +    lastus = micros(); // Zeit merken
     float diff = (lastv-v)/zeit;  // Änderung pro Zeit     float diff = (lastv-v)/zeit;  // Änderung pro Zeit
     lastv = v;   // Messwert merken     lastv = v;   // Messwert merken
Line 60: Line 62:
       diff = 0.0;  // Differenz macht beim ersten Mal keinen Sinn       diff = 0.0;  // Differenz macht beim ersten Mal keinen Sinn
     }     }
-    float korrektur = pid[0]*v + pid[1]*summe + pid[2]*diff;+    float korrektur = pid[0]*v + pid[1]*summe - pid[2]*diff;
     float links=clip(soll+korrektur);     float links=clip(soll+korrektur);
     float rechts=clip(soll-korrektur);     float rechts=clip(soll-korrektur);
     setFloatPower(links, rechts);     setFloatPower(links, rechts);
     if (robot.buttons.get()!=0) { // Button gedrückt (nicht Null)?     if (robot.buttons.get()!=0) { // Button gedrückt (nicht Null)?
-      break; // Wiederholung abbrechen+      robot.motors.setPowers(0,0); 
 +      break;
     }     }
 +    
 +    //
 +    // TODO: Weitere Abbruch-Bedingungen formulieren
 +    //
 +    
     first = false;  // Ist nicht mehr das erste Mal     first = false;  // Ist nicht mehr das erste Mal
   }   }
-  robot.buttons.waitClear();  // Warten bis Dfingerabdäröschti 
 } }
  
Line 78: Line 85:
   char *msg[] = {"Set proportional", "Set integral", "Set differential"};   char *msg[] = {"Set proportional", "Set integral", "Set differential"};
   while (true) {   while (true) {
-    int choice = robot.menu.choice("setP", "setI", "setD", "abort");+    int choice = robot.menu.choice("1 setP", "2 setI", "3 setD", "4 abort");
     if (choice==4) {     if (choice==4) {
       break;       break;
     }     }
-    // Anpassen mit Menufunktion+    // Anpassen mit Menufunktion, choice ist 1, 2 oder 3. Anpassungsschritt proportional zur Grösse des Parameters
     pid[choice-1] = robot.menu.adjustFloat(msg[choice-1], pid[choice-1], abs(pid[choice-1])/20.0+0.001);     pid[choice-1] = robot.menu.adjustFloat(msg[choice-1], pid[choice-1], abs(pid[choice-1])/20.0+0.001);
 +  }
 } }
 +
  
 </code> </code>
  • lehrkraefte/blc/robotics/pid.1499319677.txt.gz
  • Last modified: 2017/07/06 07:41
  • by Ivo Blöchliger