Projektlabor Robotik MINTgrün

class Routenplaner {
  ArrayList<colorSpot> blaufolge = new ArrayList<colorSpot>();
  ArrayList<colorSpot> punktfolge = new ArrayList<colorSpot>();
  //die Route wird durch linien zwischen aufeinanderfolgenden Punkten gezeichnet
  void zeichneRoute() { 
    for (int i=0;i<punktfolge.size()-1;i++) {
      stroke(255, 0, 0);
      line(punktfolge.get(i).x, punktfolge.get(i).y, punktfolge.get(i+1).x, punktfolge.get(i+1).y);
    }
    for (int i=0;i<blaufolge.size()-1;i++) {
      stroke(0, 0, 255);
      line(blaufolge.get(i).x, blaufolge.get(i).y, blaufolge.get(i+1).x, blaufolge.get(i+1).y);
    }
  }
  //die Route wird erweitert, falls sie nicht gestoppt ist, und wenn ein Laserpunkt in ausreichendem Abstand zum letzten erkannt wurde
  void erweitereRoute() {
    if (stop == false) {
      if (farbPunktFinder.laserposition.qualitaet== true) {
        if (punktfolge.size()<1) {
          punktfolge.add(farbPunktFinder.laserposition);
        }
        else {
         colorSpot letzterPunkt = punktfolge.get(punktfolge.size()-1);
          float distanz = sqrt(sq(letzterPunkt.x-farbPunktFinder.laserposition.x)+sq(letzterPunkt.y-farbPunktFinder.laserposition.y));
          if (distanz > 5) {
            punktfolge.add(farbPunktFinder.laserposition);
          }
        }
      }
    }
    if (farbPunktFinder.blauposition.qualitaet== true) {
        if (blaufolge.size()<1) {
          blaufolge.add(farbPunktFinder.blauposition);
        }
        else {
          colorSpot letzterPunkt = blaufolge.get(blaufolge.size()-1);
          float distanz = sqrt(sq(letzterPunkt.x-farbPunktFinder.blauposition.x)+sq(letzterPunkt.y-farbPunktFinder.blauposition.y));
          if (distanz > 5) {
            blaufolge.add(farbPunktFinder.blauposition);
          }
        }
      }
  }
 
  void loescheRoute (boolean relevant) {
    if (relevant){
    // die gesamte route wi gelöschtrd
    println("loesche");
    while (punktfolge.size ()>0) {
      punktfolge.remove(0);
    }
    }else{
      // die blau aufgezeichnete Linie wird gelöscht
      while (blaufolge.size ()>0) {
      blaufolge.remove(0);
      }
    }
  }
  //einzelne Punkte werden von der Route gelöscht
  void loeschePunkte() {
    println("klicke auf den zu loeschenden Punkt");
    for (int i=0;i<punktfolge.size();i++) {
      if (sqrt(sq(mouseX-punktfolge.get(i).x)+sq(mouseY-punktfolge.get(i).y))<8) {
 
        if (i<zaehler) {
          punktfolge.remove(i);
          println("loesche Punkt"+i);
          zaehler = zaehler-1;
        }
        else if (i==zaehler) {
          println("dieser Punkt wird gerade angefahren und kann daher nicht gelöscht werden");
 
        }
        else {
          punktfolge.remove(i);
          println("loesche Punkt"+i);
        }
      }
    }
  }
  //Ein Punkt wird an der Stelle hinzugefügt, an der die Maus ist.
  void fuegePunkteHinzu() {
    colorSpot mauspunkt = new colorSpot();
    mauspunkt.x=mouseX;
    mauspunkt.y=mouseY;
    mauspunkt.qualitaet=true;
    punktfolge.add(mauspunkt);
  }
}