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
Last revision Both sides next revision
lehrkraefte:blc:robotics:pid [2017/07/06 09:11]
Ivo Blöchliger 		lehrkraefte:blc:robotics:pid [2017/07/07 07:48]
Ivo Blöchliger
Line 1: Line 1:
-{{ :lehrkraefte:blc:robotics:regeltechnik.pdf |}} +===== Funktionierenden Line-Follower ===== 
- +<code c++ sketch.txt>
-<code txt>+
 // 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])   // TODO: bessere Umrechnung (z.B. [0,1]->[500,799] und [-1,0]->[-799,-500])
-  int l = links*799; +  int l = ((links>=0) ? 1 : -1)*(sqrt(abs(links)))*799; 
-  int r = rechts*799;+  int r = ((rechts>=0) ? 1 : -1)*(sqrt(abs(rechts)))*799;
   robot.motors.setPowers(l,r);   robot.motors.setPowers(l,r);
 } }
Line 23: Line 22:
  
 // 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 54:
     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     lastus = micros(); // Zeit merken
     float diff = (lastv-v)/zeit;  // Änderung pro Zeit     float diff = (lastv-v)/zeit;  // Änderung pro Zeit
Line 61: Line 63:
       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.motors.setPowers(0,0); // Motoren Power 0 (drehen sich aber noch ca. 5 Ticks weiter!) 
-  robot.buttons.waitClear();  // Warten bis Dfingerabdäröschti 
 } }
  
Line 86: Line 92:
     // Anpassen mit Menufunktion, choice ist 1, 2 oder 3. Anpassungsschritt proportional zur Grösse des Parameters     // 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.txt
  • Last modified: 2017/07/07 07:49
  • by Ivo Blöchliger