Projektlabor Robotik MINTgrün

#include <Servo.h>
Servo myservo1;
int servoPin = 6;
 
int stepcounter = 0;
float CMdist;                //Distanz in CM
int stepsstraight;           //Distanz in Gesammtschritten
int stepdist;                //Distanz verringert in Abhängigkeit zum Winkel
float stepturn;              //Schrittanzahl für die Drehung
int stepsL;                  //Anzahl an Linksschritten
int stepsR;                  //Anzahl an Rechtschritten
int gesSteps;                //Links- und Rechsschritte zusammen
float alpha;                 //Winkel
float stepmode = 4.0*200.0;  //Schritteinstellung (hier MS2 [1/4 Steps])
float rad = 4.5;             //Durchmesser des Rades
float robo = 13.5;           //Breite des Roboters
float stepAngle = (2.0*PI / stepmode)*(rad/robo);       //Winkeländerung eines Steps
int delayL;                  //Delay für Linksschritte
int delayR;                  
int delayMAX = 1000;         //Maximaler delay der Schritte (Geschwindigkeit über diese Variable einstellbar)
unsigned long gesZ;          //Gesammtzeit um den angesteuerten Kurs komplett abzufahren
unsigned long timeL;         //interne Zeit des Arduinos nach dem letzen Linksschrit
unsigned long timeR;
 
int pen;                     //Stiftposition (true = oben; false = unten)
int penOld = 0;              //momentane Stiftposition
 
int Dir1 = A1;               //Drehrichtung des ersten Motors
int Step1 = A2;              //erster Steppermotor
int Dir2 = A4;  
int Step2 = A3;
 
///Datenempangung///
 
 
#define USE_OSC
#include <WiFlyHQ.h>        //Libraries werden hinzugefügt
#include <ArdOSCForWiFlyHQ.h>
#include <ArduPar.h>
 
#include <SoftwareSerial.h>
 
const int softSerialRxPin =2;      //Eingabe Pins
const int softSerialTxPin =3;
SoftwareSerial softSerial(softSerialRxPin,softSerialTxPin);
 
 
WiFly wifly;       //Objekt Wifly wird erstellt
OSCServer oscServer(&wifly);  
 
FloatArduPar someIntSetting;
FloatArduPar someFloatSetting;
FloatArduPar stift;
 
void setup() {
  myservo1.attach(servoPin);
 
  Serial.begin(115200);    //nur notwendig zur Fehlerfindung
 
  pinMode(Dir1, OUTPUT);    //Pin werden für den Steppermotor auf output gestellt
  pinMode(Dir2, OUTPUT);
  pinMode(Step1, OUTPUT);
  pinMode(Step2, OUTPUT);
 
  wifly.setupForUDP<SoftwareSerial>(
  &softSerial,             //wird für Arduiuno Nano benötigt
  19200,                   //Boutrate
  true,	
  "wally",                 //Name des Wifi
  "robomint",              //Passwort
  "WiFly",                 // Name im Wifi
  0,                       // IP Adresse des Wifly (hier 0 da nicht notwendig)
  8000,                    // WiFly Eingangsport
  "255.255.255.255",       // wird an alle gesendet
  8001,                    // Ausgangsport
  true	
  );
 
  wifly.printStatusInfo(); //Debug 
 
  globalArduParOscServer=&oscServer;
 
  someIntSetting.setup(
  F("/someInt"),                // die vom Processig gesendete Datei trägt hier den Namen /someInt
  0,                            // sie kann einen Wert zwischen 0 und 250 annehmen 
  255                           
  );
  someFloatSetting.setup(
  F("/someFloat"),              
  -255,                            
  255                           
  );
  stift.setup(
  F("stift"),                
  0,                           
  1                           
  );
}
 
void loop() {
 
  updateParametersFromStream(&Serial,10); // es werden 10 ms auf neue Daten gewartet
 
  oscServer.availableCheck();
 
  if(stift.valueReceived){               // wenn ein Signal kommt wird überprüft, ob der Stift eine andere Position annehmen soll als er sich bereits befindet 
    pen = stift.value;
    if(pen == 1 && penOld == 0) {
      lowerpen();                        //Stift wird gesenkt
      penOld = 1;                        //es wird gemerkt, dass der Stift nun unten ist
    }
    if(pen == 0 && penOld == 1) {
      liftpen();                         //Stift wird gehoben
      penOld = 0;
    }
    stift.valueReceived = false;         //die Variable wird auf flase gesetzt bis wieder ein Befehl eingegangen ist
  }
  if(someIntSetting.valueReceived && someFloatSetting.valueReceived){ //nur wenn Daten empfangen wurden, werden Steps ausgehführt
 
    calculateData();                         //empfangene Werte werden verwertet
 
    someIntSetting.valueReceived = false;    //die Variable wird auf flase gesetzt bis wieder ein Befehl eingegangen ist
    someFloatSetting.valueReceived = false;
 
    stepcounter = 0;                         //der letzte Schrittzähler wird auf 0 gesetzt
 
  }
  if(stepcounter < gesSteps){                //solang das Ziel nicht erreicht ist, wird weitergefahren :^)
    if((timeL + delayL) <= micros()){        //wenn sobald der letzte Linksschritt genau "delayL" her ist, kann der nächste Linksschritt gemacht werden
      if(stepsL > 0) {                       //wenn die Anzahl an Linksschritten positiv ist vollführt das Linke Rad eine Vorwärtsdrehung
        stepleft();
        stepcounter++;                       //der Schritt wird gezählt
        //Serial.println("<^");              //Debug-Anzeige für Fahrtest ohne den Roboter
      } 
      if(stepsL < 0) {                       //wenn negativ dann rückwärts
        backleft();
        stepcounter++;
        //Serial.println("<v");
      }
      timeL = micros();                      //Zeitpunkt des Linksschittes
    }
    if((timeR + delayR) <= micros()){
      if(stepsR > 0) {
        stepright();
        stepcounter++;
        //Serial.println("^>");
      } 
      if(stepsR < 0) {
        backright();
        stepcounter++;
        //Serial.println("v>");
      }
      timeR = micros();
    }
  }
}
 
void calculateData() {
  alpha = someFloatSetting.value;              //Speicherung der Variablen
  CMdist = someIntSetting.value;
 
  stepsstraight = (int) (CMdist / ((9.0*PI)/stepmode));    //Umrechnung von Zentimetern in Schrittzahl
  if(fabs(alpha) <= (PI/2.0)) {                            //ist der Winkel kleiner/gleich 90° wird die Distanz verringert
    stepdist = (int) (stepsstraight * (((PI/2.0) - fabs(alpha)) / (PI/2.0))); // desto größer der Winkel, desto geringer die Distanz, desto schörfer die Kurve
  }
  else{                      //ist der Kinkel größer als 90° soll zuerst nur eine Drehung ausgeführt werden, da alles andere uns vom Ziel entfernen würde
    stepdist = 0;
  }
 
  stepturn = (int) (alpha / stepAngle);    
 
  stepsL = stepdist + stepturn;
  stepsR = stepdist - stepturn;
 
  gesZ = (unsigned long)(stepdist) * (unsigned long) (delayMAX) + (unsigned long)(abs(stepturn)) * (unsigned long)(delayMAX); //die Gesammtzeit setzt sich aus der Zeit für den geraden weg als auch für die Drehung zusammen
  delayL = (unsigned long) (gesZ / abs(stepsL));        //der zeitliche Abstand zweier Linksschritte ist die Gesammtzeit durch die Anzahl an Linksschitten
  delayR = (unsigned long) (gesZ / abs(stepsR));
 
  gesSteps = abs(stepsL) +  abs(stepsR);                //die Anzahl an Schritten die gemacht werden ist lediglich die Anzahl an Links- und Rechtschritten
 
  timeL = micros();                                     //da noch kein Schritt gemacht wurde, werden hier die Timer auf die Aktuelle zeit des Arduinos gesetzt
  timeR = micros(); 
}
 
void stepleft() {    // Da die beiden Motoren sich gegenüberlegen, müssen sie in umgekehrter Drehrichtung laufen um den Roboter geradeausfahren zu lassen
 
  digitalWrite(Dir1, HIGH);    //Vorwärts (für den ersten Motor)
  digitalWrite(Step1, HIGH);
  delayMicroseconds(5);            
  digitalWrite(Step1, LOW);
}
 
void stepright() {
 
  digitalWrite(Dir2, LOW);    //Vorwärts (für den zweiten Motor)
  digitalWrite(Step2, HIGH);
  delayMicroseconds(5);
  digitalWrite(Step2, LOW);
}
 
void backleft() {
 
  digitalWrite(Dir1, LOW);    //Rückwärts (für den ersten Motor)
  digitalWrite(Step1, HIGH);
  delayMicroseconds(5);            
  digitalWrite(Step1, LOW);
}
 
void backright() {
 
  digitalWrite(Dir2, HIGH);    //Rückwärts (für den zweiten Motor)
  digitalWrite(Step2, HIGH);
  delayMicroseconds(5);
  digitalWrite(Step2, LOW); 
 
}
 
void liftpen() {
  myservo1.write(10);          //Servomotor wird auf 10° gestellt, was den Stift möglichst hoch hebt
  delay(1000);
}
 
void lowerpen() {
  myservo1.write(110);        //110° des Servomotors lässt den Stift den Boden berühren
  delay(1000);
}