Projektlabor Robotik MINTgrün

Benutzer-Werkzeuge

Webseiten-Werkzeuge

Sie befinden sich hier: start » projektews1415 » melok
Zuletzt angesehen:
projektews1415:melok

Unterschiede

Hier werden die Unterschiede zwischen zwei Versionen gezeigt.

Link zu dieser Vergleichsansicht

Nächste Überarbeitung 		Vorhergehende Überarbeitung
projektews1415:melok [2015/03/30 14:22]
c.jaedicke angelegt 		projektews1415:melok [2016/01/21 12:45] (aktuell)
Zeile 238: Zeile 238:
 Ansonsten sind wir recht zufrieden, mit dem was wir geschafft haben. Zwar haben unsere Programmieraufgaben länger gedauert als nötig gewesen wäre, aber sie waren sehr lehrreich. Ansonsten sind wir gar nicht so schlecht im Zeitplan Ansonsten sind wir recht zufrieden, mit dem was wir geschafft haben. Zwar haben unsere Programmieraufgaben länger gedauert als nötig gewesen wäre, aber sie waren sehr lehrreich. Ansonsten sind wir gar nicht so schlecht im Zeitplan
 [{{ :​projektewise2014:​melok:​document.png |Gantt-Diagramm unseres Projekts, erstellt mit GanttProject. Die schwarzen Linien zeigen den Stand der einzelnen Schritte an.}}] [{{ :​projektewise2014:​melok:​document.png |Gantt-Diagramm unseres Projekts, erstellt mit GanttProject. Die schwarzen Linien zeigen den Stand der einzelnen Schritte an.}}]
 +
 +=====Code Motorsteuerung=====
 +<code c>
 +// melok motorsteuerung - von tim rogasch 2014-2015
 +//Variablen motorsteuerung
 +
 +const float radumfang = 0.377;
 +
 +float hall1Strecke = 0;
 +boolean hall1Richtung = true;
 +int hall1A = 0;
 +int hall1B = 0;
 +int hall1BCache = 5;  //Wert, den er sonst nie annimmt
 +unsigned long hall1Zeitcache;​
 +float hall1Speed = 0;
 +boolean hall1ErsteMessung = true;
 +float motor1Leistung = 0.0;
 +
 +float hall2Strecke = 0;
 +boolean hall2Richtung = true;
 +int hall2A = 0;
 +int hall2B = 0;
 +int hall2BCache = 5;  //Wert, den er sonst nie annimmt
 +unsigned long hall2Zeitcache;​
 +float hall2Speed = 0;
 +boolean hall2ErsteMessung = true;
 +float motor2Leistung = 0.0;
 +
 +//boolean motor1Richtung = true;
 +
 +
 +float sollGeschwindigkeit1 = 0;      //WICHTIGES ZEUG
 +float sollGeschwindigkeit2 = -0;      //WICHTIGES ZEUG
 +
 +unsigned long regelungsZeitcache;​
 +float regelung1Fehler = 0;
 +float regelung1Fehlercache = 0;
 +float regelung1Fehlersumme = 0;
 +float regelung2Fehler = 0;
 +float regelung2Fehlercache = 0;
 +float regelung2Fehlersumme = 0;
 +
 +//​Regelungswerte
 +const float kp  = 300;
 +const float ki   = 0.01; //0.01
 +const float kd  = 0.00;
 +
 +
 +//dose
 +boolean doseIst = false;
 +boolean doseSoll = false;
 +
 +
 +//Pins
 +int in1Pin = 7;
 +int in2Pin = 8;
 +int pwm1Pin = 9;
 +int in3Pin = 11;
 +int in4Pin = 12;
 +int pwm2Pin = 10;
 +
 +int mosfetPin = 6;
 +
 +
 +//serielle Kommunikation
 +
 +String inputString = ""; ​  //​string für seriellen input
 +boolean stringComplete = false; ​ // flag für kompletten string (zeilenumbruch erfolgt)
 +
 +
 +
 +void setup() {
 +  pinMode(in1Pin,​ OUTPUT); //IN1 Motor 1
 +  pinMode(in2Pin,​ OUTPUT); //IN2 Motor 1
 +  pinMode(pwm1Pin,​ OUTPUT); //PWM Motor 1
 +  pinMode(in3Pin,​ OUTPUT); //IN3 Motor 2
 +  pinMode(in4Pin,​ OUTPUT); //IN4 Motor 2
 +  pinMode(pwm2Pin,​ OUTPUT); //PWM Motor 2
 +
 +
 +  pinMode(2, INPUT); //Pin 2 ist ein Input (Hall1 A)
 +  pinMode(4, INPUT); //Pin 4 ist ein Input (Hall1 B)
 +
 +  pinMode(3, INPUT); //Pin 3 ist ein Input (Hall2 A)
 +  pinMode(5, INPUT); //Pin 5 ist ein Input (Hall2 B)
 +
 +  attachInterrupt(0,​ Hall1Change,​ CHANGE); //Interrupt 0 ist an Pin 2
 +  attachInterrupt(1,​ Hall2Change,​ CHANGE); //Interrupt 1 ist an Pin 3
 +
 +  pinMode(mosfetPin,​ OUTPUT); ​
 +
 +  regelungsZeitcache = micros();
 +
 +  //serial
 +  Serial.begin(9600);​
 +  inputString.reserve(200);​
 +}
 +
 +
 +void serialEvent() {
 +  while (Serial.available()) {
 +    // get the new byte:
 +    char inChar = (char)Serial.read();​
 +    if (inChar == '​\n'​) {                                      //​Zeilenumbruch
 +
 +//      Serial.println(inputString);​
 +      ​
 +      if (inputString.length() > 0) {
 +        ​
 +        String motor1wert;
 +        String motor2wert;
 +        String dosenwert;
 +        char motor1buf[32];​
 +        char motor2buf[32]; ​
 +        ​
 +        //hier zerlegen
 +
 +        //​motor1wert
 +        motor1wert = inputString.substring(0,​(inputString.indexOf(';'​)));​
 +        motor1wert.toCharArray(motor1buf,​ sizeof(motor1buf));​
 +        sollGeschwindigkeit1 = atof(motor1buf);​
 +
 +        //kürzen
 +        inputString = inputString.substring((inputString.indexOf(';'​) + 1));
 +
 +        //​motor2wert
 +        motor2wert = inputString.substring(0,​(inputString.indexOf(';'​)));​
 +        motor2wert.toCharArray(motor2buf,​ sizeof(motor2buf));​
 +        sollGeschwindigkeit2 = -(atof(motor2buf));​
 +
 +        //kürzen
 +        inputString = inputString.substring((inputString.indexOf(';'​) + 1));
 +
 +        //dosenwert
 +        dosenwert = inputString.substring(0,​1);​
 +        Serial.println(dosenwert);​
 +        if (dosenwert == "​1"​) {
 +          DoseEinschalten();​
 +        }
 +        else {
 +          DoseAbschalten();​
 +        }
 +        ​
 +      }
 +      //string leeren
 +      inputString = ""; ​
 +      Serial.flush();​
 +      StatusSenden();​
 +      ​
 +    ​
 +    }
 +    if(inChar != '​\n'​){
 +      inputString += inChar;
 +    }
 +  }
 +}
 +
 +
 +void loop() {    //​Hauptschleife
 +
 +  if (micros() > (regelungsZeitcache + 1000)) { //1000hz
 +    RegelungKanal1();​
 +    RegelungKanal2();​
 +    //    Serial.print(int(motor1Leistung));​
 +    //    Serial.print(";"​);​
 +    //    Serial.println(hall1Speed);​
 +    regelungsZeitcache = micros(); //Zeitcache neu schreiben
 +  }
 +
 +}
 +
 +
 +void StatusSenden() {
 +    Serial.print(hall1Strecke,​6);​
 +    Serial.print(';'​);​
 +    Serial.print(hall2Strecke,​6);​
 +    Serial.print('​\n'​);​
 +}
 +
 +
 +void DoseEinschalten() {
 +  if (doseIst == false) {
 +    analogWrite(mosfetPin,​255);​
 +    delay(80);  ​
 +    analogWrite(mosfetPin,​50);  ​
 +    doseIst = true;
 +  }
 +}
 +
 +void DoseAbschalten() {
 +  analogWrite(mosfetPin,​0); ​
 +  doseIst = false;
 +}
 +
 +
 +void RegelungKanal1()
 +{
 +  //​Erkennung,​ ob der Motor steht
 +  if (micros() > (hall1Zeitcache+100000)) {
 +    hall1Speed = 0;
 +    hall1ErsteMessung = true;
 +  }
 +  //Regelung
 +
 +  //Dieser Motor läuft "​rückwärts"​!
 +  // sollGeschwindigkeit1 = (-sollGeschwindigkeit1);​
 +
 +  regelung1Fehler = sollGeschwindigkeit1 - hall1Speed;
 +  if (regelung1Fehler > 50) {
 +    regelung1Fehler = 50;
 +  } //Deckeln
 +  if (regelung1Fehler < -50) {
 +    regelung1Fehler = -50;
 +  }  //Deckeln
 +  regelung1Fehlersumme = regelung1Fehlersumme + regelung1Fehler;​
 +  motor1Leistung = kp*regelung1Fehler + ki*(micros() - regelungsZeitcache)*regelung1Fehlersumme + kd/​(micros()-regelungsZeitcache) * (regelung1Fehler - regelung1Fehlercache);​ //​Reglergleichung
 +  regelung1Fehlercache = regelung1Fehler;​
 +  if (motor1Leistung > 255) {
 +    motor1Leistung = 255;
 +  }  //Deckeln
 +  if (motor1Leistung < -255) {
 +    motor1Leistung = -255;
 +  }  //Deckeln
 +  //Motor drehen lassen
 +  if (motor1Leistung > 0) {        //Vorwärts
 +    digitalWrite(in1Pin,​ HIGH); // High, Low = Vorwärts; Low, High = Rückwärts,​ Beide an = Bremse!
 +    digitalWrite(in2Pin,​ LOW);
 +  }
 +  if (motor1Leistung < 0) {        //​Rückwärts
 +    digitalWrite(in1Pin,​ LOW); // High, Low = Vorwärts; Low, High = Rückwärts,​ Beide an = Bremse
 +    digitalWrite(in2Pin,​ HIGH);
 +  }
 +  if (motor1Leistung == 0) {        //​Stillstand
 +    digitalWrite(in1Pin,​ HIGH); // High, Low = Vorwärts; Low, High = Rückwärts,​ Beide an = Bremse
 +    digitalWrite(in2Pin,​ HIGH);
 +  }
 +  analogWrite(pwm1Pin,​int(abs(motor1Leistung)));​
 +}
 +
 +void RegelungKanal2()
 +{
 +  //​Erkennung,​ ob der Motor steht
 +  if (micros() > (hall2Zeitcache+100000)) {
 +    hall2Speed = 0;
 +    hall2ErsteMessung = true;
 +  }
 +  //Regelung
 +  regelung2Fehler = sollGeschwindigkeit2 - hall2Speed;
 +  if (regelung2Fehler > 50) {
 +    regelung2Fehler = 50;
 +  } //Deckeln
 +  if (regelung2Fehler < -50) {
 +    regelung2Fehler = -50;
 +  }  //Deckeln
 +  regelung2Fehlersumme = regelung2Fehlersumme + regelung2Fehler;​
 +  motor2Leistung = kp*regelung2Fehler + ki*(micros() - regelungsZeitcache)*regelung2Fehlersumme + kd/​(micros()-regelungsZeitcache) * (regelung2Fehler - regelung2Fehlercache);​ //​Reglergleichung
 +  regelung2Fehlercache = regelung2Fehler;​
 +  if (motor2Leistung > 255) {
 +    motor2Leistung = 255;
 +  }  //Deckeln
 +  if (motor2Leistung < -255) {
 +    motor2Leistung = -255;
 +  }  //Deckeln
 +  //Motor drehen lassen
 +  if (motor2Leistung > 0) {        //Vorwärts
 +    digitalWrite(in3Pin,​ HIGH); // High, Low = Vorwärts; Low, High = Rückwärts,​ Beide an = Bremse!
 +    digitalWrite(in4Pin,​ LOW);
 +  }
 +  if (motor2Leistung < 0) {        //​Rückwärts
 +    digitalWrite(in3Pin,​ LOW); // High, Low = Vorwärts; Low, High = Rückwärts,​ Beide an = Bremse
 +    digitalWrite(in4Pin,​ HIGH);
 +  }
 +  if (motor2Leistung == 0) {        //​Stillstand
 +    digitalWrite(in3Pin,​ HIGH); // High, Low = Vorwärts; Low, High = Rückwärts,​ Beide an = Bremse
 +    digitalWrite(in4Pin,​ HIGH);
 +  }
 +  analogWrite(pwm2Pin,​int(abs(motor2Leistung)));​
 +}
 +
 +
 +
 +
 +void Hall1Change()
 +{
 +  //Variablen deklarieren
 +  boolean illegal = false;
 +  unsigned long zeitunterschied = 0;
 +  //Ausgänge des Hallsensors einlesen
 +  hall1A = digitalRead(2);​
 +  hall1B = digitalRead(4);​
 +  //​Illegalflag setzen
 +  //Was ist illegal? Nun, da ich hier die Gesetze schreibe:
 +  //Der Wert A ändert sich ohne eine Änderung des Wertes B. Dies passiert immer, wenn der Encoder prellt / schwingt oder an einem tatsaechlichen Wendepunkt.
 +    //In beiden Fällen muss hier die Geschwindigkeit null betragen.
 +  if (hall1B == hall1BCache) {
 +    illegal = true;
 +  }
 +  //Die erste Messung ist auch immer illegal!
 +  if (hall1B == hall1Zeitcache) {
 +    illegal = true;
 +    hall1ErsteMessung = false;
 +  }
 +  //​Drehrichtung ermitteln
 +  if (illegal == false) {
 +    if (hall1A ​ == true) {
 +      if (hall1B == true) {
 +        hall1Richtung = false;
 +      }
 +      else {
 +        hall1Richtung = true;
 +      }
 +    }
 +    else {           //A ist negativ
 +      if (hall1B == true) {
 +        hall1Richtung = true;
 +      }
 +      else {
 +        hall1Richtung = false;
 +      }
 +    }  ​
 +  }
 +  //​Geschwindigkeit ausrechnen, wenn die Messung nicht illegal war
 +  if (illegal == false) {
 +    zeitunterschied = micros() - hall1Zeitcache;​
 +    //​Serial.println(zeitunterschied);​
 +    //​(1/​(zeitunterschied*32*50))*pi*0,​12=236
 +    hall1Speed = 1/​(float(zeitunterschied)/​236);​ //​Vereinfacht
 +  }
 +  else {
 +    hall1Speed = 0;
 +  }     
 +  //bei Rückwärtsdrehung wird der Wert nun negativ!
 +  if (hall1Richtung == false) {
 +    hall1Speed = -(hall1Speed);​
 +  }
 +  //Den Cache neu beschreiben
 +  hall1BCache = hall1B;
 +  hall1Zeitcache = micros();
 +}
 +
 +void Hall2Change()
 +{
 +  //Variablen deklarieren
 +  boolean illegal = false;
 +  unsigned long zeitunterschied = 0;
 +  //Ausgänge des Hallsensors einlesen
 +  hall2A = digitalRead(3);​
 +  hall2B = digitalRead(5);​
 +  //​Illegalflag setzen
 +  //Was ist illegal? Nun, da ich hier die Gesetze schreibe:
 +  //Der Wert A ändert sich ohne eine Änderung des Wertes B. Dies passiert immer, wenn der Encoder prellt / schwingt oder an einem tatsaechlichen Wendepunkt.
 +    //In beiden Fällen muss hier die Geschwindigkeit null betragen.
 +  if (hall2B == hall2BCache) {
 +    illegal = true;
 +  }
 +  //Die erste Messung ist auch immer illegal!
 +  if (hall2B == hall2Zeitcache) {
 +    illegal = true;
 +    hall2ErsteMessung = false;
 +  }
 +  //​Drehrichtung ermitteln
 +  if (illegal == false) {
 +    if (hall2A ​ == true) {
 +      if (hall2B == true) {
 +        hall2Richtung = false;
 +      }
 +      else {
 +        hall2Richtung = true;
 +      }
 +    }
 +    else {           //A ist negativ
 +      if (hall2B == true) {
 +        hall2Richtung = true;
 +      }
 +      else {
 +        hall2Richtung = false;
 +      }
 +    }  ​
 +  }
 +  //​Geschwindigkeit ausrechnen, wenn die Messung nicht illegal war
 +  if (illegal == false) {
 +    zeitunterschied = micros() - hall2Zeitcache;​
 +    //​Serial.println(zeitunterschied);​
 +    //​(1/​(zeitunterschied*32*50))*pi*0,​12=236
 +    hall2Speed = 1/​(float(zeitunterschied)/​236);​ //​Vereinfacht
 +  }
 +  else {
 +    hall2Speed = 0;
 +  }     
 +  //bei Rückwärtsdrehung wird der Wert nun negativ!
 +  if (hall2Richtung == false) {
 +    hall2Speed = -(hall2Speed);​
 +  }
 +  //Den Cache neu beschreiben
 +  hall2BCache = hall2B;
 +  hall2Zeitcache = micros();
 +}
 +</​code>​
 +
 +===== Vorläufiger Code Wegfindung v2.0 =====
 +Ein erster unvollständiger Code wird durch noch einen unvollständigeren Code ersetzt
 +
 +==== Hauptschleife ====
 +
 +<code java>
 +//////////////////////////////////////////////////////////////////////////////////////// ​
 +/////Neu ausgearbeitete Wegfindung des Roboters melok, noch nicht abgeschlossen/////////​
 +////////////////////////////////////////////////////////////////////////////////////////​
 + 
 +  int radius = 25;
 +  CustomDraw c = new CustomDraw();​
 +  Border theborder = new Border();
 +  Robot melok = new Robot();
 +  float[] waypoints;
 +  ​
 +void setup() {
 +  size(800,​600);​
 +  background(255);​
 +  noLoop(); //for debugging ​
 +  ​
 +  String[] waypointStrg = loadStrings("​waypoints.txt"​);​ //importing waypoints from waypoints.txt,​ which is in sketch folder, Correct content: waypoint1x,​waypoint1y,​waypoint2x,​waypoint2y...
 +  waypoints = float(split(waypointStrg[0],​ ','​));​
 +  println(waypoints);​
 +
 +  theborder.build();​
 +  ​
 +  melok.position.set(100,​200);​
 +  melok.direction.set(1,​0);​
 +  melok.speed = (0);
 +  ​
 +}
 +
 +void draw() {
 +  ​
 +  melok.posdraw();​
 +  melok.dirdraw();​
 +  melok.settargetspeed();​
 +  ​
 +}
 +
 +//building the border
 +</​code>​
 +
 +==== Klasse Border ====
 +
 +<code java>
 +class Border {
 +  Borderpart start;
 +  Borderpart pointer;
 +
 +
 +  Border() {
 +    start = new Borderpart();​
 +    pointer = start;
 +  }
 +
 +  float distanceto(Robot roboter) {
 +    if (isCrossing(roboter)) {
 +      return roboter.position.dist(start.crossPoint(roboter));​
 +    } else {
 +      println("​EPIC ERROR!!!111oneone Robot is not facing any Border"​);​
 +      return 0;
 +    }
 +  }
 +  ​
 +  boolean isCrossing(Robot roboter){
 +    return start.borderisCrossed(roboter);​
 +    ​
 +  }
 +
 +  void build() {
 +
 +    for (int i = 0; i<= waypoints.length - 2; i+=2) {          //creating Borderparts for each pair of following waypoints
 +      if (i==0) {
 +        PVector temp = new PVector(waypoints[i],​ waypoints[i+1]); ​  //​some temp-vectors saving the positions of the waypoints
 +        PVector temp2 = new PVector(waypoints[i+2],​ waypoints[i+3]);​
 +        PVector tan = new PVector(temp2.x-temp.x,​ temp2.y-temp.y); ​ //STEP 1: creating PVector between to waypoints
 +        tan.setMag(radius); ​                                        //​STEP 2: set magnitude of PVector to radian of tolerance circles
 +        tan.rotate(PI/​2); ​                                          //​STEP 3: rotate PVector by 90 degrees (PI/2)
 +        pointer.left.beginpoint.set(temp); ​                         //STEP 4: set the beginpoints and endpoints to the coordinates of the waypoints
 +        pointer.left.beginpoint.add(tan); ​                          //​STEP 5 either add or subtract the tan-vector
 +        pointer.left.endpoint.set(temp2);​
 +        pointer.left.endpoint.add(tan);​
 +        pointer.right.beginpoint.set(temp);​
 +        pointer.right.beginpoint.sub(tan);​
 +        pointer.right.endpoint.set(temp2);​
 +        pointer.right.endpoint.sub(tan);​
 +        println(pointer.left.beginpoint);​
 +        c.ccircle(waypoints[i],​ waypoints[i+1],​ radius);
 +        pointer.borderdraw();​ //draw border
 +      } else if (i >= waypoints.length-2) {
 +
 +        c.ccircle(waypoints[i],​ waypoints[i+1],​ radius);
 +
 +        pointer.nextPart = new Borderpart();​
 +        pointer.nextPart.left.beginpoint.set(pointer.left.endpoint);​
 +        pointer.nextPart.left.endpoint.set(pointer.right.endpoint);​
 +
 +        pointer.nextPart.borderdraw();​
 +
 +        pointer = start;
 +      } else {
 +        // though there are parts of the border which connect the endpoints and the beginpoints of the other parts and which cant by calculated like this.
 +        // that's and the fact we don't have something like .previousPart are the reasons we have to calculate with pointer.nextPart and pointer.nectPart.nextPart
 +        pointer.nextPart = new Borderpart(); ​           //​initializing pointer.nextPart and pointer.nextPart.nextPart
 +        pointer.nextPart.nextPart = new Borderpart();​
 +
 +        //​calculating pointer.nextPart
 +
 +          PVector temp = new PVector(waypoints[i],​ waypoints[i+1]); ​  //​some temp-vectors saving the positions of the waypoints
 +        PVector temp2 = new PVector(waypoints[i+2],​ waypoints[i+3]);​
 +        PVector tan = new PVector(temp2.x-temp.x,​ temp2.y-temp.y); ​ //STEP 1: creating PVector between to waypoints
 +        tan.setMag(radius); ​                                        //​STEP 2: set magnitude of PVector to radian of tolerance circles
 +        tan.rotate(PI/​2); ​                                          //​STEP 3: rotate PVector by 90 degrees (PI/2)
 +        pointer.nextPart.nextPart.left.beginpoint.set(temp); ​                         //STEP 4: set the beginpoints and endpoints to the coordinates of the waypoints
 +        pointer.nextPart.nextPart.left.beginpoint.add(tan); ​                          //​STEP 5 either add or subtract the tan-vector
 +        pointer.nextPart.nextPart.left.endpoint.set(temp2);​
 +        pointer.nextPart.nextPart.left.endpoint.add(tan);​
 +        pointer.nextPart.nextPart.right.beginpoint.set(temp);​
 +        pointer.nextPart.nextPart.right.beginpoint.sub(tan);​
 +        pointer.nextPart.nextPart.right.endpoint.set(temp2);​
 +        pointer.nextPart.nextPart.right.endpoint.sub(tan);​
 +        //​calculating pointer.nextPart.nextPart
 +        //​borderparts are added at the outer curve side to prevent holes in the border
 +        //the Tangent of the other side is intentionally kept empty (but beginpoint and endpoint will be initialized (empty vectors are still (0,0) we either have to search a more intelligent way or make sure this will never be any problem))
 +        if (isLeftfour(pointer.left.beginpoint,​ pointer.left.endpoint,​ pointer.nextPart.nextPart.left.beginpoint,​ pointer.nextPart.nextPart.left.endpoint)) ​
 +        {
 +          pointer.nextPart.left.beginpoint.set(pointer.left.endpoint);​
 +          pointer.nextPart.left.endpoint.set(pointer.nextPart.nextPart.left.beginpoint);​
 +        } else {
 +          pointer.nextPart.right.beginpoint.set(pointer.right.endpoint);​
 +          pointer.nextPart.right.endpoint.set(pointer.nextPart.nextPart.right.beginpoint);​
 +        }
 +
 +        c.ccircle(waypoints[i],​ waypoints[i+1],​ radius);
 +        pointer.nextPart.borderdraw();​
 +        pointer.nextPart.nextPart.borderdraw();​
 +        //set the pointer tp pointer.nextPart.nextPart so we can start this again
 +        pointer = pointer.nextPart.nextPart;​
 +      }
 +      print(i);
 +      print(" ​ ");
 +      println(waypoints.length-i);​
 +    }
 +  }
 +}
 +</​code>​
 +
 +==== Klasse Borderpart ====
 +
 +<code java>
 +
 +class Borderpart {
 +  Borderpart nextPart; ​       //nextPart points to nect Part, which makes the Border to a linked list
 +  Tangent left;
 +  Tangent right;
 +
 +  Borderpart() {
 +    left = new Tangent();
 +    right = new Tangent();
 +  }
 +
 +  boolean borderpartisCrossed(Robot roboter) {
 +    println("​Checking if borderpartisCrossed"​);​
 +    boolean isCrossed;
 +    isCrossed = left.isCrossed(roboter)||right.isCrossed(roboter);​ //check, if left or right tangend will be crossed  ​
 +
 +    return isCrossed;
 +  }
 +
 +  boolean borderisCrossed(Robot roboter) {
 +    println("​Checking if borderisCrossed"​);​
 +    if (borderpartisCrossed(roboter)) {
 +      return true;
 +    } else {
 +      return nextPart.borderisCrossed(roboter);​
 +    }
 +  }
 +
 +  PVector crossPoint(Robot roboter) { //finds the first point, where borer is crossed by directional straight line
 +    if (borderpartisCrossed(roboter)) {
 +      PVector vector = new PVector(0, 0);
 +      if (left.isCrossed(roboter)) {
 +        return intersection(roboter.position.x,​ roboter.position.y,​ roboter.direction.x,​ roboter.direction.y,​ left.beginpoint.x,​ left.beginpoint.y,​ left.endpoint.x-left.beginpoint.x,​ left.endpoint.y-left.beginpoint.y);​
 +      } else {
 +        return intersection(roboter.position.x,​ roboter.position.y,​ roboter.direction.x,​ roboter.direction.y,​ right.beginpoint.x,​ right.beginpoint.y,​ right.endpoint.x-right.beginpoint.x,​ right.endpoint.y-right.beginpoint.y);​
 +      }
 +    } else {
 +      return nextPart.crossPoint(roboter);​
 +    }
 +  }
 +
 +  void borderdraw() { //draws border
 +    color(0);
 +    c.cline(left.beginpoint.x,​ left.beginpoint.y,​ left.endpoint.x,​ left.endpoint.y);​
 +    c.cline(right.beginpoint.x,​ right.beginpoint.y,​ right.endpoint.x,​ right.endpoint.y);​
 +  }
 +}
 +</​code>​
 +
 +==== Klasse Tangent ====
 +
 +<code java>
 +
 +class Tangent {
 +  PVector beginpoint; ​              //​Beginpoint of Tangent
 +  PVector endpoint; ​                //​Endpoint of Tangent
 +
 +    Tangent() {
 +    beginpoint = new PVector();
 +    endpoint = new PVector();
 +  }
 +
 +  boolean isCrossed(Robot roboter) { //checks if tangent will be crossed
 +    println("​Checking if tangent isCrossed"​);​
 +    if (endpoint == null|| beginpoint == null) {
 +      println("​Tangent is not crossed, because there is no tangent"​);​
 +      return false;
 +    } else {
 +        println("​Beginpoint"​ + beginpoint + "​Endpoint"​ + endpoint);
 +        stroke(255, 0, 0);
 +        c.cline(beginpoint.x,​ beginpoint.y,​ endpoint.x, endpoint.y);​
 +      PVector crosspoint = new PVector();
 +      stroke(100, 100, 100);
 +      crosspoint = intersection(roboter.position.x,​ roboter.position.y,​ roboter.direction.x,​ roboter.direction.y,​ beginpoint.x,​ beginpoint.y,​ endpoint.x-beginpoint.x,​ endpoint.y-beginpoint.y);​
 +      //Leider gibt es bei intersection() noch einen dicken Fehler(siehe dort), deshalb weren jetzt noch fehlerhafte Werte ausgespuckt
 +        println(crosspoint);​
 +        c.ccircle(crosspoint.x,​ crosspoint.y,​ 5);
 +      if ((beginpoint.x <= crosspoint.x && crosspoint.x <= endpoint.x)||(beginpoint.x >= crosspoint.x && crosspoint.x >= endpoint.x)) {
 +          stroke(0, 0, 255);
 +          c.ccircle(crosspoint.x,​ crosspoint.y,​ 5);
 +          println("​Tangent is crossed!"​);​
 +        return true;
 +      } else {
 +          println("​Tangent is not crossed."​);​
 +        return false;
 +      }
 +    }
 +  }
 +}
 +</​code>​
 +
 +==== Klasse Robot ====
 +<code java>
 +// Class of Robot
 +class Robot {
 +  PVector position;
 +  PVector direction;
 +  float speed;
 +  Boolean isSpraying;
 +
 +  float targedspeedleft;​
 +  float targedspeedright;​
 +
 +  int currentgoal;​
 +
 +  Robot() {
 +    position = new PVector(0, 0);
 +    direction = new PVector(0, 0);
 +    speed = 0;
 +    isSpraying = false;
 +    targedspeedleft = 0;
 +    targedspeedright = 0;
 +    currentgoal = 1;
 +  }
 +  void posdraw() {
 +    stroke(0, 255, 0);
 +    c.ccircle(position.x,​ position.y, 5);
 +  }
 +
 +  void dirdraw() {
 +    stroke(100, 100, 100);
 +    c.cline(position.x,​ position.y, position.x+50*direction.x,​ position.y+50*direction.y);​
 +  }
 +
 +  void settargetspeed() { //​main-method of robot
 +    goalcheck();​
 +    float distance = theborder.distanceto(melok);​
 +    float midspeed = sqrt(distance)/​100;​
 +    PVector desired = new PVector(waypoints[currentgoal],​ waypoints[currentgoal+1]);​
 +    desired.sub(position);​
 +    float anglechange = pow(PVector.angleBetween(desired,​ direction)/​3,​ 2);
 +
 +    if (isLefttwo(direction,​ desired)) {
 +      anglechange = -anglechange;​
 +    }
 +
 +    //​Icomplete,​ TODO: calculate targed-speed of the two wheels
 +  }
 +
 +  void goalcheck() { //checks f robot reched waypoint
 +    PVector waypoint = new PVector(waypoints[currentgoal],​ waypoints[currentgoal+1]);​
 +    if (position.dist(waypoint)<​radius) {
 +      currentgoal++;​
 +      theborder.start = theborder.start.nextPart;​
 +    }
 +  }
 +
 +  void receiveData() {
 +  }
 +  void sendData() {
 +  }
 +}
 +</​code>​
 +
 +==== Helpers ====
 +<​code>​
 +class CustomDraw {
 +
 +
 +  CustomDraw() {
 +  }
 +
 +  void cline(float Xpos1, float Ypos1, float Xpos2, float Ypos2) {
 +    line(Xpos1, Ypos1, Xpos2, Ypos2);
 +  }
 +  void ccircle(float x, float y, float r) {
 +    noFill();
 +    ellipse(x, y, 2*r, 2*r);
 +  }
 +}
 +
 +boolean isLefttwo(PVector heading, PVector target) { //​http://​forum.processing.org/​two/​discussion/​2463/​turn-left-or-right
 +  return heading.y * target.x > heading.x * target.y;
 +}
 +
 +boolean isLeftfour(PVector begin1, PVector end1, PVector begin2, PVector end2) { //frei nach http://​forum.processing.org/​two/​discussion/​2463/​turn-left-or-right
 +  PVector one = new PVector();
 +  one.set(end1);​
 +  one.sub(begin1);​
 +  PVector two = new PVector();
 +  two.set(end2);​
 +  two.sub(begin2);​
 +  return one.y * two.x > one.x * two.y;
 +}
 +
 +PVector intersection(float P1X, float P1Y, float D1X, float D1Y, float P2X, float P2Y, float D2X, float D2Y) { //this function calculates the Position of the crosspoint between two straight lines, input are the coordinates of the support vector and the irectional vector
 +  PVector intersection,​ P1, D1;
 +  intersection = new PVector(0, 0);
 +  P1 = new PVector(P1X,​ P1Y);
 +  D1 = new PVector(D1X,​ D1Y);
 +  println("​P1 " + P1 + "​D1"​ + D1);
 +  float t = (P2Y + (P1X-P2X)*(D2Y/​D2X) - P1Y) / (D1Y - D1X*(D2Y/​D2X)); ​    //​BUG! Leider habe ich hier einen großen Fehler entdeckt. Wenn eine Gerade senkrecht ist, ist die X-Koordinate ihres Richtungsvektors(D2X) null. also wird in diesem fall durch null geteilt. Dieser Fehler wird möglichst schnell behoben.
 +  println("​nennernenner"​+(D2Y/​D2X));​
 +  println("​Nenner"​ +(D1Y - D1X*(D2Y/​D2X)));​
 +  println("​t"​ + t );
 +  intersection = D1.get();
 +  intersection.mult(t);​
 +  intersection.add(P1);​
 +  return intersection;​
 +}
 +</​code>​
  
projektews1415/melok.1427718153.txt.gz · Zuletzt geändert: 2016/01/21 12:45 (Externe Bearbeitung)

Seiten-Werkzeuge

Donate Powered by PHP Valid HTML5 Valid CSS Driven by DokuWiki