Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: Farbsensor IRSensorLib PID_Control Servo mbed PixyLib
Revision 16:ad45ef4fee04, committed 2017-05-26
- Comitter:
- ZHAW_Prometheus
- Date:
- Fri May 26 09:11:49 2017 +0000
- Parent:
- 15:26dbcd6ff48d
- Commit message:
- Vers. 26.05.2017 11:10
Changed in this revision
--- a/Fahren.cpp Wed May 24 13:54:02 2017 +0000 +++ b/Fahren.cpp Fri May 26 09:11:49 2017 +0000 @@ -11,6 +11,7 @@ e = 0.0; diff = 0.0; searchTimer = 0; + zurTimer = 40; } //Destruktor @@ -32,6 +33,12 @@ case cam: pc->printf("Kamerafahrt\n\r"); break; + case camSearchRot: + pc->printf("Kamerafahrt Rotation\n\r"); + break; + case camSearchDrive: + pc->printf("Kamerafahrt Fahrsuche\n\r"); + break; case rechts: pc->printf("Rechts\n\r"); break; @@ -83,13 +90,20 @@ void Fahren::fahrRutine() { + //IR-Sensoren auslesen s0 = sensors[0]; s1 = sensors[1]; s2 = sensors[2]; s4 = sensors[4]; s5 = sensors[5]; + + //Erst prüfen ob aus einem vorhergegangenen Schritt noch eine Aktion ausgeführt werden muss + if ((state == hart_zurueck_r || state == hart_zurueck_l) && zurTimer<40) { + zurTimer++; + } + //Alle Sensoren genügend abstand -> Kamerafahrt - if (sensors[0]>0.25 && sensors[1]>0.22 && sensors[2]>0.25 && sensors[4]>0.25 && sensors[5]>0.22) { + else if (s0>0.25 && s1>0.22 && s2>0.25 && s4>0.25 && s5>0.22) { if ((pixy.getX()>=5 && pixy.getX()<=315) && pixy.getDetects() != 0 ) { e = 160-pixy.getX(); diff = pid.calc( e, 0.005f ); @@ -110,28 +124,28 @@ } } } else { - //Front Links und Recht prüfen ob viel zu nahe - if (sensors[1]<=wand || sensors[5]<=wand) { - if(sensors[1]<=wand) { + if (s1<=wand || s5<=wand) { //Abfrage noch ändern + if(s1<=wand) { pwmLeft = 0.45; pwmRight = 0.65; state = hart_zurueck_r; + zurTimer = 0; } else { pwmLeft = 0.35; pwmRight = 0.55; state = hart_zurueck_l; + zurTimer = 0; } - wait(0.8); } //Kontrolle ob zurückgefahren werden muss - else if (sensors[0]<=0.1 && sensors[1]<=0.12 && sensors[5]<=0.12) { - if(sensors[2]<=0.25) { + else if (s0<=0.1 && s1<=0.12 && s5<=0.12) { + if(s2<=0.25) { pwmLeft = 0.65; pwmRight = 0.65; state=zurueck_l; - } else if(sensors[4]<=0.25) { + } else if(s4<=0.25) { pwmLeft = 0.35; pwmRight = 0.35; state=zurueck_r; @@ -143,13 +157,13 @@ } //Kontrolle ob gedreht werden muss - else if(sensors[0]<0.25) { + else if(s0<0.25) { if(sensors[1]<=wenden) { //Drehen Links pwmLeft = 0.3; pwmRight = 0.3; state = drehen_l; - } else if(sensors[5]<=wenden) { + } else if(s5<=wenden) { //Drehen Rechts pwmLeft = 0.7; pwmRight = 0.7; @@ -168,14 +182,14 @@ } //Rechtsfahren - else if(sensors[5]<=wenden && sensors[5] >= 0.08) { + else if(s5<=wenden && s5 >= 0.08) { pwmLeft = 0.65; pwmRight = 0.45; state=rechts; } //Linksfahren - else if(sensors[1]<=wenden && sensors[1] >= 0.08) { + else if(s1<=wenden && s1 >= 0.08) { pwmLeft = 0.55; pwmRight = 0.35; state=links;
--- a/Fahren.h Wed May 24 13:54:02 2017 +0000 +++ b/Fahren.h Fri May 26 09:11:49 2017 +0000 @@ -45,6 +45,7 @@ //Variabeln uint8_t state; uint8_t searchTimer; + uint8_t zurTimer; enum state {gerade=0, cam, rechts, links, drehen_ran, drehen_r, drehen_l, zurueck, zurueck_l, zurueck_r, hart_zurueck_r, hart_zurueck_l, camSearchRot, camSearchDrive}; Serial *pc; float e;
--- a/Farbauswertung.cpp Wed May 24 13:54:02 2017 +0000 +++ b/Farbauswertung.cpp Fri May 26 09:11:49 2017 +0000 @@ -4,7 +4,7 @@ #include "Farbauswertung.h" //Konstruktor -Farbauswertung::Farbauswertung(AnalogIn* SensorG, AnalogIn* SensorR, Servo& _servoAusw, Servo& _servoFoerder, int verzEin, int verzAus) : servoAusw(_servoAusw), servoFoerder(_servoFoerder) +Farbauswertung::Farbauswertung(AnalogIn* SensorG, AnalogIn* SensorR, Servo& _servoAusw, Servo& _servoFoerder, LiftAnsteuerung& _lift, int verzEin, int verzAus) : servoAusw(_servoAusw), servoFoerder(_servoFoerder), lift(_lift) { this->SensorG = SensorG; this->SensorR = SensorR; @@ -46,29 +46,40 @@ zustand = gruen; } merker[p1] = zustand; - if (p1 < (merkerSize-1)) { - p1++; - } else { - p1 = 0; - } //Zustände an der Klappe abrufen und Klappe öffnen und schliessen sum = 0; - for (uint16_t i = 0; i<m_verzAus; i++) { + uint8_t i = 0; + while (i<m_verzAus) { if ((p2+i) < merkerSize) { sum += merker[p2+i]; } else { sum += merker[p2+i-merkerSize]; } + i++; + } + + //Klappe öffnen oder schliessen + if (sum > 0) { + servoAusw = 1.0f; + } else { + servoAusw = 0.01f; + } + + //Liftmodul alle vier Durchgänge aufrufen + if (p1%4 == 0) { + lift.steuerung(); + } + + //Zähler aufsummieren oder zurücksetzen. + if (p1 < (merkerSize-1)) { + p1++; + } else { + p1 = 0; } if (p2 < (merkerSize-1)) { p2++; } else { p2 = 0; } - if (sum > 0) { - servoAusw = 1.0f; - } else { - servoAusw = 0.01f; - } }
--- a/Farbauswertung.h Wed May 24 13:54:02 2017 +0000 +++ b/Farbauswertung.h Fri May 26 09:11:49 2017 +0000 @@ -5,12 +5,13 @@ #include <cstdlib> #include "Farbsensor.h" #include "Servo.h" +#include "liftAnsteuerung.h" class Farbauswertung { public: //Konstruktoren und Destruktoren - Farbauswertung(AnalogIn* SensorG, AnalogIn* SensorR, Servo& servoAusw, Servo& servoFoerder, int verzEin, int verzAus); + Farbauswertung(AnalogIn* SensorG, AnalogIn* SensorR, Servo& servoAusw, Servo& servoFoerder, LiftAnsteuerung& lift, int verzEin, int verzAus); Farbauswertung(); virtual ~Farbauswertung(); @@ -40,6 +41,7 @@ uint8_t zustand; enum zustand {gruen=0, rot}; Farbsensor farbsensor; + LiftAnsteuerung& lift; Serial *pc; };
--- a/main.cpp Wed May 24 13:54:02 2017 +0000 +++ b/main.cpp Fri May 26 09:11:49 2017 +0000 @@ -41,9 +41,9 @@ Pixy pixy(cam); //Robotersteuerung -Farbauswertung farbauswertung(&SensorG, &SensorR, ServoAusw, ServoFoerder, 4, 8); +LiftAnsteuerung lift(7, 1, ServoLift); +Farbauswertung farbauswertung(&SensorG, &SensorR, ServoAusw, ServoFoerder, lift, 4, 8); Fahren fahren(enable, bit0, bit1, bit2, distance, enableMotorDriver, pwmLeft, pwmRight, pixy, pid); -LiftAnsteuerung lift(7, 1, ServoLift); int main() @@ -54,9 +54,8 @@ //farbauswertung.setSerialOutput(&pc); //fahren.setSerialOutput(&pc); - Ticker farbe; + Ticker auswertung; Ticker drive; - Ticker elevator; pid.setPIDValues( 0.001f, 0.001f, 0.00015f, 0.3f, -0.3f, 1000); fahren.fahrInit(); @@ -65,12 +64,10 @@ wait(1.0); } - farbe.attach(&farbauswertung, &Farbauswertung::auswertung, 0.5); + auswertung.attach(&farbauswertung, &Farbauswertung::auswertung, 0.5); drive.attach(&fahren, &Fahren::fahrRutine, 0.02); - elevator.attach(&lift, &LiftAnsteuerung::steuerung, 2); - while (1) { wait(10.0); //farbauswertung.printState();