Projektlabor Robotik MINTgrün

Dies ist eine alte Version des Dokuments!

---



#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);

}