Projektlabor Robotik MINTgrün

//Variablen, die die Lage des Roboters im Raum beschreiben
float x; // Position des Roboters in x-Richtung (pixel)
float y; // Position des Roboters in y-Richtung (pixel)
float angle; // Blickrichtung des Roboters (radian)
 
// Eigenschaften des Roboters, die beim Bau festgelegt wurden
float dRobot=100; //Abstand der Räder auf dem Roboter (in pixel)
float dWheel=20;  //Raddurchmesser (in pixel)
float stepAngle=2*PI/200; // Die Räder machen in einer Umdrehung (2*Pi radian) genau 200 Schritte (so wie unsere Stepper)
 
void setup() {
  //Einstellungen des Processing Grafikfensters
  size(512, 512); // setze die Größe des Fensters auf 512x512 Pixel
  frameRate(200); //versuche 200 Schritte in der Sekunde zu malen.
 
  //Setze den Roboter in die Mitte des Fensters
  x= width/2;  // setze den roboter in die mitte (horizontal)
  y= height/2; // setze den roboter in die mitte (vertikal)
  angle=0;     // lasse den Roboter nach rechts schauen
 
  //Male zum vergleich einen Kreis mit dem Durchmesser, den wir erwarten würden
  noStroke (); // lasse die Umrandung weg
  ellipse(x,y+dRobot/2,dRobot,dRobot); // Zeichne eine Ellipse mit dem Durchmesser dRobot, die ihren obersten Punkt an der aktuellen Roboterposition hat
  stroke(0); //male in Zukunft wieder alles mit Umrandungen
};
 
void draw() {
  stepleft();  //Simuliere eine Drehung des linken Rades um einen Winkel von "stepAngle"
  //stepright(); Zum Geradeausfahren müssen wir beide Räder gleichzeitig drehen
  point(x, y); //Male einen Punkt an die Stelle, wo der Roboterschwerpunkt ist
};
 
void stepleft() {
  //Blickrichtungsänderung in einem Schritt:
  angle=angle+(stepAngle*dWheel/2)/(dRobot*2*PI)*(2*PI); 
  // Herleitungsidee:
  // Die bei einer Radumdrehung von "stepAngle" radian auf dem Boden zurückgelegte Strecke des Rades ist (stepAngle*dWheel/2)
  // Der Umfang des Kreises, den das Rad entlangfährt, wenn das andere Rad stillsteht ist (dRobot*PI*2) (Der vom Rad gefahrene Kreis ist doppelt so groß wie der vom Schwepunkt zurückgelegte)
  // Die Winkeländerung für eine komplette Kreisfahrt ist (2*PI)
  // Wie groß ist der Anteil der bei einem Schritt zurückgelegten Strecke am gesamten Kreisumfang?
  // Wie groß muß dementsprechend die Blickrichtungsänderung sein?
 
  //Positionsänderung in einem Schritt:  
  x=x+ cos(angle)*(stepAngle*dWheel/2)/2; 
  y=y+ sin(angle)*(stepAngle*dWheel/2)/2; 
  // Herleitungsidee:
  // Das bewegte Rad legt bei einer Raddrehung um stepAngle eine Strecke von (stepAngle*dWheel/2) zurück
  // Der Roboterschwerpunkt liegt auf der Häfte der Strecke zwischen bewegtem und stillstehendem Rad
  // Der Schwerpunkt bewegt sich genau um die Hälfte der vom bewegten Rad zurückgelegten Strecke (Strahlensatz): (/2) 
 
  // Die Bewegung erfolgt in Blickrichtung (sin/cos(angle))
}
 
void stepright() {
  //Blickrichtungsänderung in einem Schritt:
  angle=angle-(stepAngle*dWheel/2)/(dRobot*2*PI)*(2*PI); //Nur das Vorzeichen des Schritts ist für die beiden Räder unterschiedlich  
 
  //Positionsänderung in einem Schritt:  
  x=x+ cos(angle)*(stepAngle*dWheel/2)/2;  //für beide Räder gleich
  y=y+ sin(angle)*(stepAngle*dWheel/2)/2;  //für beide Räder gleich
}