Gruppe 3
hallo
Diff: Roboter.cpp
- Revision:
- 18:3ee1b02ed3aa
- Parent:
- 17:caf5ae550f2e
- Child:
- 19:adae367247d4
--- a/Roboter.cpp Tue Apr 25 12:26:04 2017 +0000
+++ b/Roboter.cpp Tue May 09 15:25:54 2017 +0000
@@ -1,7 +1,7 @@
#include "Roboter.h"
-Roboter::Roboter(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, PwmOut* pwmL, PwmOut* pwmR, Servo* s1, Servo* s2)
+Roboter::Roboter(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, PwmOut* pwmL, PwmOut* pwmR, Servo* servo1, Servo* servo2, EncoderCounter* counterLeft, EncoderCounter* counterRight, DigitalOut* enableMotorDriver)
{
@@ -14,65 +14,230 @@
this->pwmR = pwmR;
this->pwmL = pwmL;
- this->s1 = s1;
- this->s2 = s2;
+ this->servo1 = servo1;
+ this->servo2 = servo2;
+ this->counterLeft = counterLeft;
+ this->counterRight = counterRight;
+ this->enableMotorDriver = enableMotorDriver;
+ initSpeedcontrol();
+
+ t1.attach(this, &Roboter::speedCtrl, PERIOD);
}
-float Roboter::readSensor1()
+float Roboter::readSensor1() // Sensoren auslesen
{
return sensors[0].read();
}
-void Roboter::bandeAusweichen()
+void Roboter::bandeAusweichen() // Hindernissen ausweichen
{
- float offsetDir;
- float offsetLin;
-
float x=0.13f; // Distanz ab welcher sensoren reagieren sollen
- offsetDir = 0;
- offsetLin = 0;
-
while(sensors[0] < x && sensors[1] < x && sensors[5] < x) { // alle sensoren aktiv, roboter fährt nach hinten
- offsetLin = 0.1f;
- *pwmL = 0.5f+offsetDir-offsetLin; // Setzt die Duty-Cycle auf 50%
- *pwmR = 0.5f+offsetDir+offsetLin;
+ desiredSpeedLeft = -v;
+ desiredSpeedRight = v;
}
- while(sensors[0].read() < x && sensors[5] > x) { // sensor vorne, roboter dreht nach links
- offsetDir = -0.05;
- *pwmL = 0.5f+offsetDir+offsetLin; // Setzt die Duty-Cycle auf 50%
- *pwmR = 0.5f+offsetDir-offsetLin;
+ while(sensors[0] < x && sensors[5] > x) { // sensor vorne, roboter dreht nach links
+ desiredSpeedLeft = -v;
+ desiredSpeedRight = -v;
}
while(sensors[1] < x) { // sensor rechts, roboter dreht nach links
- offsetDir = -0.05;
- *pwmL = 0.5f+offsetDir+offsetLin; // Setzt die Duty-Cycle auf 50%
- *pwmR = 0.5f+offsetDir-offsetLin;
+ desiredSpeedLeft = -v;
+ desiredSpeedRight = -v;
}
while(sensors[5] < x && sensors[1]>(x+0.02f)) { // sensor links, roboter dreht nach rechts
- offsetDir = 0.08;
- *pwmL = 0.5f+offsetDir+offsetLin; // Setzt die Duty-Cycle auf 50%
- *pwmR = 0.5f+offsetDir-offsetLin;
+ desiredSpeedLeft = v;
+ desiredSpeedRight = v;
}
}
+int Roboter::pickup() // Klotz aufnehmen
+{
+ static int ii = 0;
+ int r; // Rückegabewert
+ desiredSpeedLeft = 0;
+ desiredSpeedRight = 0;
+ if(ii < 70) { // Arm nach unten fahren
+ servo2->SetPosition(2250);
+ }
+ if(ii > 69 && ii < 110) { // gerade fahren
+ desiredSpeedLeft = v;
+ desiredSpeedRight = -v;
+ }
+ if(ii > 109 && ii < 160) { // gerade fahren + Greifer schliessen
+ desiredSpeedLeft = v;
+ desiredSpeedRight = -v;
+ servo1->SetPosition(500);
+ }
+ if(ii > 159 && ii < 230) { // Arm nach oben fahren
+ desiredSpeedLeft = 0;
+ desiredSpeedRight = 0;
+ servo2->SetPosition(700);
+ }
+ if(ii > 229 && ii < 280) { // Greifer öffnen
+ servo1->SetPosition(1500);
+ }
+ ii++;
+ r = 5; // Rückegabewert
+
+ if( ii > 279 ) {
+ r = 1;
+ ii = 0;
+ }
+ return r;
+}
-void Roboter::pickup ()
+void Roboter::anfahren(float cam)
+{
+ float speedValue;
+ float maxWert = 20;
+ if(cam == 100) {
+ desiredSpeedLeft = -v/2;
+ desiredSpeedRight = -v/2;
+ }
+ if(cam == 200) {
+ desiredSpeedLeft = v/2;
+ desiredSpeedRight = v/2;
+ }
+ if(cam > 100 && cam <200 ) { // links fahren, wenn wir Werte von 100 bis 199 haben
+ cam = cam -99.0f; // cam nimmt die Werte von 1 bis 100 an
+ speedValue = v + (cam * (maxWert /100.0f)); // Berechnung des speedValue's
+ desiredSpeedLeft = -speedValue;
+ desiredSpeedRight = -speedValue;
+ }
+ if(cam > 200 && cam < 300) { // rechts fahren, wenn wir Werte von 200 bis 299 haben
+ cam = cam -199.0f; // cam nimmt die Werte von 1 bis 100 an
+ speedValue = v + (cam * (maxWert /100.0f)); // Berechnung des speedValue's
+ desiredSpeedLeft = speedValue;
+ desiredSpeedRight = speedValue;
+ }
+ if(cam >= 300 && cam <400) { // gerade fahren, wenn wir Werte von 300 bis 399 haben
+ cam = cam-299.0f; // cam nimmt die Werte von 1 bis 100 an
+ speedValue = v*1.5f + (cam * (maxWert /100.0f)); // Berechnung des speedValue's
+ desiredSpeedLeft = speedValue;
+ desiredSpeedRight = -speedValue;
+ }
+}
+
+int Roboter::geradeFahren()
{
+ int r;
+ static int time2 = 0;
+ if(time2 < 200) { // 1s gerade aus fahren
+ desiredSpeedLeft = v*3;
+ desiredSpeedRight = -v*3;
+ time2 ++;
+ r = 1; // state
+ wait(0.01f);
+ } else {
+ time2 = 0;
+ desiredSpeedLeft = 0.0f;
+ desiredSpeedRight = 0.0f;
+ r = 2; //state
+ }
+ return r;
+}
+int Roboter::kreisFahren()
+{
+ int r;
+ static int time1 = 0;
+ if(time1 < 310) { // 1s im Kreis drehen
+ desiredSpeedLeft = v*1.7f;
+ desiredSpeedRight = v*1.7f;
+ time1 ++;
+ r = 1; // state
+ wait(0.01f);
+ } else {
+ time1 = 0;
+ desiredSpeedLeft = 0.0f;
+ desiredSpeedRight = 0.0f;
+ r = 3; // state
+ }
+ return r;
+}
+
+int Roboter::stateAuswahl(float cam, int temp)
+{
+ int s;
+ if(cam >= 100.0f && cam < 400.0f ) { // Die Kamera erkennt ein grünen Klotz , Werte von 100 und 399
+ s = 4;
+ }
+ if(cam == 400.0f) { // Roboter in Position
+ s = 5;
+ }
+ if(cam == 0.0f) {
+ s = temp;
+ }
+ return s;
}
+
+void Roboter::initSpeedcontrol()
+{
+ // Initialisieren der PWM Ausgaenge pwmLeft.period(0.00005f); // PWM Periode von 50 us
+ pwmL->period(0.00005f); // Setzt die Periode auf 50 μs
+ pwmR->period(0.00005f);
+ *pwmL = 0.5f; // Duty-Cycle von 50% pwmRight.period(0.00005f); // PWM Periode von 50 us
+ *pwmR = 0.5f; // Duty-Cycle von 50%
+
+ // Initialisieren von lokalen Variabeln
+ previousValueCounterLeft = counterLeft->read();
+ previousValueCounterRight = counterRight->read();
+ speedLeftFilter.setPeriod(PERIOD);
+ speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
+ speedRightFilter.setPeriod(PERIOD);
+ speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
+
+ desiredSpeedLeft = 0.0f;
+ desiredSpeedRight = 0.0f;
+ actualSpeedLeft = 0.0f;
+ actualSpeedRight = 0.0f;
+
+ *enableMotorDriver = 1;
+}
+
+void Roboter::speedCtrl()
+{
+ // Berechnen die effektiven Drehzahlen der Motoren in [rpm]
+ short valueCounterLeft = counterLeft->read();
+ short valueCounterRight = counterRight->read();
+ short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
+ short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
+
+ previousValueCounterLeft = valueCounterLeft;
+ previousValueCounterRight = valueCounterRight;
+ actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft /COUNTS_PER_TURN/PERIOD*60.0f);
+ actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight /COUNTS_PER_TURN/PERIOD*60.0f);
+
+ // Berechnen der Motorspannungen Uout
+ float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
+ float voltageRight = KP*(desiredSpeedRight*correction-actualSpeedRight)+desiredSpeedRight*correction/KN;
+
+ // Berechnen, Limitieren und Setzen der Duty-Cycle
+ float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
+ if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
+ else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
+
+ *pwmL = dutyCycleLeft;
+ float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
+ if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
+ else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
+
+ *pwmR = dutyCycleRight;
+}