Dependencies: Farbsensor IRSensorLib PID_Control Servo mbed PixyLib
Diff: Fahren.cpp
- Revision:
- 12:472b26872a42
- Parent:
- 10:10bcb7fee9a6
- Child:
- 14:bee8101aad45
--- a/Fahren.cpp Sat May 20 12:02:51 2017 +0000
+++ b/Fahren.cpp Sat May 20 21:47:11 2017 +0000
@@ -4,197 +4,182 @@
#include "Fahren.h"
-
-Fahren::Fahren(DigitalOut* enable, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, AnalogIn* distance, DigitalOut* enableMotorDriver, PwmOut& _pwmLeft, PwmOut& _pwmRight, Pixy& _pixy, PID_Control& _pid, Button& _onoff) : pwmLeft(_pwmLeft), pwmRight(_pwmRight), pixy(_pixy), pid(_pid), onoff(_onoff)
+//Konstruktor
+Fahren::Fahren(DigitalOut& _enable, DigitalOut& _bit0, DigitalOut& _bit1, DigitalOut& _bit2, AnalogIn& _distance, DigitalOut& _enableMotorDriver, PwmOut& _pwmLeft, PwmOut& _pwmRight, Pixy& _pixy, PID_Control& _pid, Button& _onoff) : enable(_enable), bit0(_bit0), bit1(_bit1), bit2(_bit2), distance(_distance), enableMotorDriver(_enableMotorDriver), pwmLeft(_pwmLeft), pwmRight(_pwmRight), pixy(_pixy), pid(_pid), onoff(_onoff)
{
- init(enable, bit0, bit1, bit2, distance, enableMotorDriver);
-}
-
-//Fahren::Fahren() {}
-
-/**
- * Deletes the IRSensor object.
- */
-Fahren::~Fahren() {}
-
-void Fahren::init(DigitalOut* enable, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, AnalogIn* distance, DigitalOut* enableMotorDriver)
-{
- this->enable = enable;
- this->bit0 = bit0;
- this->bit1 = bit1;
- this->bit2 = bit2;
- this->distance = distance;
- this->enableMotorDriver = enableMotorDriver;
state = 0;
e = 0.0;
diff = 0.0;
-
-
}
+//Destruktor
+Fahren::~Fahren() {}
+
+//Methoden
void Fahren::setSerialOutput(Serial *pc)
{
this->pc = pc;
}
-int Fahren::getState()
+void Fahren::printState()
{
- return state;
+ if(pc) {
+ switch (state) {
+ case gerade:
+ pc->printf("Gerade\n\r");
+ break;
+ case cam:
+ pc->printf("Kamerafahrt\n\r");
+ break;
+ case rechts:
+ pc->printf("Rechts\n\r");
+ break;
+ case links:
+ pc->printf("Links\n\r");
+ break;
+ case drehen_ran:
+ pc->printf("Drehen Random\n\r");
+ break;
+ case drehen_l:
+ pc->printf("Drehen Links\n\r");
+ break;
+ case drehen_r:
+ pc->printf("Drehen Rechts\n\r");
+ break;
+ case zurueck:
+ pc->printf("Gerade Zurueck\n\r");
+ break;
+ case zurueck_l:
+ pc->printf("Links Zurueck\n\r");
+ break;
+ case zurueck_r:
+ pc->printf("Rechts Zurueck\n\r");
+ break;
+ case hart_zurueck_r:
+ pc->printf("Rechts Hart-Zurueck\n\r");
+ break;
+ case hart_zurueck_l:
+ pc->printf("Links Hart-Zurueck\n\r");
+ break;
+ }
+ }
}
+
void Fahren::fahrInit()
-{
+{
pwmLeft = 0.5;
pwmRight = 0.5;
for( int ii = 0; ii<6; ++ii) {
- sensors[ii].init(distance, bit0, bit1, bit2, ii);
-
- enable->write(1);
-
-
+ sensors[ii].init(&distance, &bit0, &bit1, &bit2, ii);
+ enable=1;
pwmLeft.period(0.00005);
pwmRight.period(0.00005);
-
- enableMotorDriver->write(1);
+ enableMotorDriver=1;
}
}
void Fahren::fahrRutine()
{
-
+ //Wenn Userbutton nicht Null ist soll er fahren
if (onoff.getState() != 0) {
//pc->printf("left:%f\n\r", sensors[5].read());
-
- if(sensors[0]<=0.1 && sensors[1]<=0.12 && sensors[5]<=0.12) {
- //wenn hinten rechts => leichte linkskurve
- if(sensors[2]<=0.25) {
- pwmLeft = 0.35;
- pwmRight = 0.55;
- if (pc) {
- pc->printf("zurueck-rechtskurve\n");
+
+ //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) {
+ if (pixy.getX()>=5 && pixy.getX()<=315) {
+ e = 160-pixy.getX();
+ diff = pid.calc( e, 0.005f );
+ //pwmLeft = 0.65 - diff;
+ //pwmRight = 0.35 - diff;
+ pwmLeft = 0.5f - diff;
+ pwmRight = 0.5f - diff;
+ state = cam;
+ } else {
+ //pwmLeft = 0.65;
+ //pwmRight = 0.35;
+ pwmLeft = 0.5f;
+ pwmRight = 0.5f;
+ state = gerade;
}
- state=zurueck_l;
- }
+ } else {
+
+ //Front Links und Recht prüfen ob viel zu nahe
+ if (sensors[1]<=wand || sensors[5]<=wand) {
+ if(sensors[1]<=wand) {
+ pwmLeft = 0.45;
+ pwmRight = 0.65;
+ state = hart_zurueck_r;
+ } else {
+ pwmLeft = 0.35;
+ pwmRight = 0.55;
+ state = hart_zurueck_l;
+ }
+ wait(0.8);
+ }
- if(sensors[4]<=0.25) {
- pwmLeft = 0.45;
- pwmRight = 0.65;
- if (pc) {
- pc->printf("zurueck-linkskurve\n");
+ //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) {
+ pwmLeft = 0.35;
+ pwmRight = 0.55;
+ state=zurueck_l;
+ } else if(sensors[4]<=0.25) {
+ pwmLeft = 0.45;
+ pwmRight = 0.65;
+ state=zurueck_r;
+ } else {
+ pwmLeft = 0.4;
+ pwmRight = 0.6;
+ state=zurueck;
+ }
}
- state=zurueck_r;
- }
- if(sensors[4]>0.25 && sensors[2]>0.25) {
- pwmLeft = 0.4;
- pwmRight = 0.6;
- state=zurueck;
- if (pc) {
- pc->printf("zurueck-gerade\n");
+
+ //Kontrolle ob gedreht werden muss
+ else if(sensors[0]<0.25) {
+ if(sensors[1]<=wenden) {
+ //Drehen Links
+ pwmLeft = 0.3;
+ pwmRight = 0.3;
+ state = drehen_l;
+ } else if(sensors[5]<=wenden) {
+ //Drehen Rechts
+ pwmLeft = 0.7;
+ pwmRight = 0.7;
+ state = drehen_r;
+ } else {
+ //Random Drehen
+ if (rand()%2==0 && state != drehen_ran) {
+ pwmLeft = 0.3;
+ pwmRight = 0.3;
+ } else if (rand()%2 != 0 && state != drehen_ran) {
+ pwmLeft = 0.7;
+ pwmRight = 0.7;
+ }
+ state=drehen_ran;
+ }
+ }
+
+ //Rechtsfahren
+ else if(sensors[5]<=wenden && sensors[5] >= 0.08) {
+ pwmLeft = 0.65;
+ pwmRight = 0.45;
+ state=rechts;
+ }
+
+ //Linksfahren
+ else if(sensors[1]<=wenden && sensors[1] >= 0.08) {
+ pwmLeft = 0.55;
+ pwmRight = 0.35;
+ state=links;
}
}
}
- if(sensors[5] <= wand) {
-
- pwmLeft = 0.35;
- pwmRight = 0.55;
- wait(0.8);
- }
- if (sensors[1] <= wand){
- pwmLeft = 0.45;
- pwmRight = 0.65;
- wait(0.8);
- }
-
- //plötzlicher Wand anschlag ***************************************
- /* else if(sensors[1] <= wand){
- notfall1=1;
- pwmLeft = 0.3;
- pwmRight = 0.3;
- if (pc) {
- pc->printf("Notfall links drehen");}
- }
-
- else if(sensors[5] <= wand){
- notfall1=2;
- pwmLeft = 0.7;
- pwmRight = 0.7;
- if (pc) {
- pc->printf("Notfall rechts drehen");}
- }
- */
-
-
- // Wenn Front etwas sehen => drehen***********************************
-
- else if(sensors[0]<0.25 && sensors [1]<=wenden) {
- pwmLeft = 0.3;
- pwmRight = 0.3;
- if (pc) {
- pc->printf("drehen links\n\n\n");
- }
- }
-
- else if (sensors[0]<0.25 && sensors [5]<=wenden) {
- pwmLeft = 0.7;
- pwmRight = 0.7;
- if (pc) {
- pc->printf("drehen rechts\n\n\n");
- }
- }
-
- else if(sensors[0]<0.25) {
- if (rand()%2==0 && state != drehen) {
- pwmLeft = 0.3;
- pwmRight = 0.3;
- if (pc) {
- pc->printf("random drehen\n\n\n");
- }
- } else if (rand()%2 != 0 && state != drehen) {
- pwmLeft = 0.7;
- pwmRight = 0.7;
- if (pc) {
- pc->printf("random drehen\n\n\n");
- }
- }
-
- state=drehen;
- }
- //Wenn Front-Left etwas sehen => nach Rechts**************************
- else if(sensors[1] >= 0.08 && (sensors[5]<=wenden && sensors[5] >= 0.08)) {
- if (pc) {
- pc->printf("rechts\n");
- }
- pwmLeft = 0.65;
- pwmRight = 0.45;
- state=rechts;
- }
-
- // Wenn Front-Right etwas sehen => Links*******************************
- else if(sensors[5] >= 0.08 && (sensors[1]<=wenden && sensors[1] >= 0.08)) {
- if(pc) {
- pc->printf("Links\n");
- }
- pwmLeft = 0.55;
- pwmRight = 0.35;
- state=links;
- }
-
- //Wenn kein Sensor anspricht => PixyCam wird abgerufen*****************************
- else if(sensors[0]>=0.26) {
- if (pixy.getX()>=5 && pixy.getX()<=215){
- e = 160-pixy.getX();
- diff = pid.calc( e, 0.005f );
- pwmLeft = 0.65 - diff;
- pwmRight = 0.35 - diff;
- } else {
- pwmLeft = 0.65;
- pwmRight = 0.35;
- }
- }
- } else {
+ //Userbutton ist Null also HALTEN!!
+ else {
pwmLeft = 0.5;
pwmRight = 0.5;
}
-}
\ No newline at end of file
+}