Projektlabor Robotik MINTgrün

class Navigator {
  boolean messeEntfernung(int nummer) {
    // misst die Enfernung zwischen dem Roboter und dem momentan anzufahrenden Punkt
    if ((sqrt(sq(routenplaner.punktfolge.get(nummer).x-farbPunktFinder.blauposition.x)+sq(routenplaner.punktfolge.get(nummer).y-farbPunktFinder.blauposition.y)))<20) {
      return true;
    } 
    // der momentan anzufahrende Punkt wird mit zwei Ellipsen makiert
    else {
      stroke(255, 0, 0);
      ellipse(routenplaner.punktfolge.get(nummer).x, routenplaner.punktfolge.get(nummer).y, 5, 10 );
      ellipse(routenplaner.punktfolge.get(nummer).x, routenplaner.punktfolge.get(nummer).y, 10, 5 );
      return false;
    }
  }
  void sendeDaten(int nummer) {
    boolean sinnvoll;
    // es sollen nur dann ein Wert gesendet werden, wenn die richtigen LEDs gefunden wurden. 
    // wenn der Abstand zwischen den LEDs zu groß ist, kann das nicht stimmen
    if ((sqrt(sq(farbPunktFinder.gruenposition.y-farbPunktFinder.blauposition.y)+sq(farbPunktFinder.gruenposition.x-farbPunktFinder.blauposition.x)))<50) {
      sinnvoll = true;
    }
    else {
      sinnvoll = false;
    }
    //float massstab= 4/(sqrt(sq(farbPunktFinder.gruenposition.y-farbPunktFinder.blauposition.y)+sq(farbPunktFinder.gruenposition.x-farbPunktFinder.blauposition.x)));
    //der Winkel wird berechnet
    if (farbPunktFinder.gruenposition.qualitaet==true && farbPunktFinder.blauposition.qualitaet==true&&sinnvoll==true) {
      float beta = atan2((routenplaner.punktfolge.get(nummer).y-farbPunktFinder.blauposition.y), (routenplaner.punktfolge.get(nummer).x-farbPunktFinder.blauposition.x));
      float gamma = atan2((farbPunktFinder.gruenposition.y-farbPunktFinder.blauposition.y), (farbPunktFinder.gruenposition.x-farbPunktFinder.blauposition.x));
      float winkel=beta-gamma;
      if (winkel>PI) {
        winkel=winkel-2*PI;
      }
      if (winkel<-PI) {
        winkel= winkel+2*PI;
      }
      //der Abstand wird berechnet
      float abstand= (sqrt(sq(routenplaner.punktfolge.get(nummer).y-farbPunktFinder.blauposition.y)+sq(routenplaner.punktfolge.get(nummer).x-farbPunktFinder.blauposition.x)))/2.6;
      //Abstand, Winkel und Information über den Stift werden an den Arduino gesendet 
      OscMessage winkelMessage = new OscMessage("/someFloat");
      OscMessage abstandMessage = new OscMessage("/someInt");
      OscMessage stiftMessage = new OscMessage("stift");
 
      stiftMessage.add(stift);
      winkelMessage.add(winkel); // add a float to the osc message 
      abstandMessage.add(abstand); // add a float to the osc message
      println("send"+winkel); 
      println("send"+abstand);
      println("send stift "+stift);
      if (abstand>2) {
        // send the message 
        oscP5.send(winkelMessage, myRemoteLocation); 
        delay(100);
        oscP5.send(abstandMessage, myRemoteLocation); 
        delay(100);
        oscP5.send(stiftMessage, myRemoteLocation); 
        delay(100);
      }
    }
  }
  // die festgelegten Werte über Winkel, Abstand und Stift werden an den Arduino gesendet
  void sende(float winkel, float abstand) {
    println("sende winkel = "+winkel);
    println("sende abstand = "+abstand);
    OscMessage winkelMessage = new OscMessage("/someFloat");
    OscMessage abstandMessage = new OscMessage("/someInt");
    OscMessage stiftMessage = new OscMessage("stift");
 
    stiftMessage.add(stift);
    winkelMessage.add(winkel); // add a float to the osc message 
    abstandMessage.add(abstand);
 
    oscP5.send(winkelMessage, myRemoteLocation); 
    delay(100);
    oscP5.send(abstandMessage, myRemoteLocation); 
    delay(100);
  }
}