Prometheus / Mbed 2 deprecated Prom_Roebi

Dependencies:   Farbsensor IRSensorLib PID_Control Servo mbed PixyLib

Fahren.cpp@14:bee8101aad45, 2017-05-21 (annotated)

Committer:
ZHAW_Prometheus
Date:
Sun May 21 12:11:14 2017 +0000
Revision:
14:bee8101aad45
Parent:
12:472b26872a42
Child:
15:26dbcd6ff48d
Vers 21.05.2017 14:10

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ZHAW_Prometheus 0:422088ad7fc5 1 #include "mbed.h"
ZHAW_Prometheus 0:422088ad7fc5 2 #include "cstdlib"
ZHAW_Prometheus 0:422088ad7fc5 3 #include <cmath>
ZHAW_Prometheus 0:422088ad7fc5 4 #include "Fahren.h"
ZHAW_Prometheus 0:422088ad7fc5 5
ZHAW_Prometheus 0:422088ad7fc5 6
ZHAW_Prometheus 12:472b26872a42 7 //Konstruktor
ZHAW_Prometheus 12:472b26872a42 8 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)
ZHAW_Prometheus 0:422088ad7fc5 9 {
ZHAW_Prometheus 0:422088ad7fc5 10 state = 0;
ZHAW_Prometheus 9:b83994ef4b08 11 e = 0.0;
ZHAW_Prometheus 9:b83994ef4b08 12 diff = 0.0;
ZHAW_Prometheus 0:422088ad7fc5 13 }
ZHAW_Prometheus 0:422088ad7fc5 14
ZHAW_Prometheus 12:472b26872a42 15 //Destruktor
ZHAW_Prometheus 12:472b26872a42 16 Fahren::~Fahren() {}
ZHAW_Prometheus 12:472b26872a42 17
ZHAW_Prometheus 12:472b26872a42 18 //Methoden
ZHAW_Prometheus 0:422088ad7fc5 19 void Fahren::setSerialOutput(Serial *pc)
ZHAW_Prometheus 0:422088ad7fc5 20 {
ZHAW_Prometheus 0:422088ad7fc5 21 this->pc = pc;
ZHAW_Prometheus 0:422088ad7fc5 22 }
ZHAW_Prometheus 0:422088ad7fc5 23
ZHAW_Prometheus 12:472b26872a42 24 void Fahren::printState()
schuema4 4:c1d1bcc96b14 25 {
ZHAW_Prometheus 12:472b26872a42 26 if(pc) {
ZHAW_Prometheus 12:472b26872a42 27 switch (state) {
ZHAW_Prometheus 12:472b26872a42 28 case gerade:
ZHAW_Prometheus 12:472b26872a42 29 pc->printf("Gerade\n\r");
ZHAW_Prometheus 12:472b26872a42 30 break;
ZHAW_Prometheus 12:472b26872a42 31 case cam:
ZHAW_Prometheus 12:472b26872a42 32 pc->printf("Kamerafahrt\n\r");
ZHAW_Prometheus 12:472b26872a42 33 break;
ZHAW_Prometheus 12:472b26872a42 34 case rechts:
ZHAW_Prometheus 12:472b26872a42 35 pc->printf("Rechts\n\r");
ZHAW_Prometheus 12:472b26872a42 36 break;
ZHAW_Prometheus 12:472b26872a42 37 case links:
ZHAW_Prometheus 12:472b26872a42 38 pc->printf("Links\n\r");
ZHAW_Prometheus 12:472b26872a42 39 break;
ZHAW_Prometheus 12:472b26872a42 40 case drehen_ran:
ZHAW_Prometheus 12:472b26872a42 41 pc->printf("Drehen Random\n\r");
ZHAW_Prometheus 12:472b26872a42 42 break;
ZHAW_Prometheus 12:472b26872a42 43 case drehen_l:
ZHAW_Prometheus 12:472b26872a42 44 pc->printf("Drehen Links\n\r");
ZHAW_Prometheus 12:472b26872a42 45 break;
ZHAW_Prometheus 12:472b26872a42 46 case drehen_r:
ZHAW_Prometheus 12:472b26872a42 47 pc->printf("Drehen Rechts\n\r");
ZHAW_Prometheus 12:472b26872a42 48 break;
ZHAW_Prometheus 12:472b26872a42 49 case zurueck:
ZHAW_Prometheus 12:472b26872a42 50 pc->printf("Gerade Zurueck\n\r");
ZHAW_Prometheus 12:472b26872a42 51 break;
ZHAW_Prometheus 12:472b26872a42 52 case zurueck_l:
ZHAW_Prometheus 12:472b26872a42 53 pc->printf("Links Zurueck\n\r");
ZHAW_Prometheus 12:472b26872a42 54 break;
ZHAW_Prometheus 12:472b26872a42 55 case zurueck_r:
ZHAW_Prometheus 12:472b26872a42 56 pc->printf("Rechts Zurueck\n\r");
ZHAW_Prometheus 12:472b26872a42 57 break;
ZHAW_Prometheus 12:472b26872a42 58 case hart_zurueck_r:
ZHAW_Prometheus 12:472b26872a42 59 pc->printf("Rechts Hart-Zurueck\n\r");
ZHAW_Prometheus 12:472b26872a42 60 break;
ZHAW_Prometheus 12:472b26872a42 61 case hart_zurueck_l:
ZHAW_Prometheus 12:472b26872a42 62 pc->printf("Links Hart-Zurueck\n\r");
ZHAW_Prometheus 12:472b26872a42 63 break;
ZHAW_Prometheus 12:472b26872a42 64 }
ZHAW_Prometheus 12:472b26872a42 65 }
ZHAW_Prometheus 0:422088ad7fc5 66 }
ZHAW_Prometheus 0:422088ad7fc5 67
ZHAW_Prometheus 12:472b26872a42 68
schuema4 4:c1d1bcc96b14 69 void Fahren::fahrInit()
ZHAW_Prometheus 12:472b26872a42 70 {
ZHAW_Prometheus 9:b83994ef4b08 71 pwmLeft = 0.5;
ZHAW_Prometheus 9:b83994ef4b08 72 pwmRight = 0.5;
schuema4 4:c1d1bcc96b14 73
ZHAW_Prometheus 0:422088ad7fc5 74 for( int ii = 0; ii<6; ++ii) {
ZHAW_Prometheus 12:472b26872a42 75 sensors[ii].init(&distance, &bit0, &bit1, &bit2, ii);
ZHAW_Prometheus 12:472b26872a42 76 enable=1;
ZHAW_Prometheus 9:b83994ef4b08 77 pwmLeft.period(0.00005);
ZHAW_Prometheus 9:b83994ef4b08 78 pwmRight.period(0.00005);
ZHAW_Prometheus 12:472b26872a42 79 enableMotorDriver=1;
ZHAW_Prometheus 0:422088ad7fc5 80 }
ZHAW_Prometheus 0:422088ad7fc5 81 }
ZHAW_Prometheus 0:422088ad7fc5 82
schuema4 4:c1d1bcc96b14 83 void Fahren::fahrRutine()
schuema4 4:c1d1bcc96b14 84 {
ZHAW_Prometheus 12:472b26872a42 85 //Wenn Userbutton nicht Null ist soll er fahren
ZHAW_Prometheus 10:10bcb7fee9a6 86 if (onoff.getState() != 0) {
ZHAW_Prometheus 8:077d0bb213a2 87 //pc->printf("left:%f\n\r", sensors[5].read());
ZHAW_Prometheus 12:472b26872a42 88
ZHAW_Prometheus 12:472b26872a42 89 //Alle Sensoren genügend abstand -> Kamerafahrt
ZHAW_Prometheus 14:bee8101aad45 90 if ((sensors[0]>0.25 && sensors[1]>0.22 && sensors[2]>0.25 && sensors[4]>0.25 && sensors[5]>0.22) || pixy.getDetects() != 0) {
ZHAW_Prometheus 12:472b26872a42 91 if (pixy.getX()>=5 && pixy.getX()<=315) {
ZHAW_Prometheus 12:472b26872a42 92 e = 160-pixy.getX();
ZHAW_Prometheus 12:472b26872a42 93 diff = pid.calc( e, 0.005f );
ZHAW_Prometheus 14:bee8101aad45 94 pwmLeft = 0.65f - diff;
ZHAW_Prometheus 14:bee8101aad45 95 pwmRight = 0.35f - diff;
ZHAW_Prometheus 12:472b26872a42 96 state = cam;
ZHAW_Prometheus 12:472b26872a42 97 } else {
ZHAW_Prometheus 14:bee8101aad45 98 pwmLeft = 0.58;
ZHAW_Prometheus 14:bee8101aad45 99 pwmRight = 0.58;
ZHAW_Prometheus 12:472b26872a42 100 state = gerade;
schuema4 4:c1d1bcc96b14 101 }
ZHAW_Prometheus 12:472b26872a42 102 } else {
ZHAW_Prometheus 12:472b26872a42 103
ZHAW_Prometheus 12:472b26872a42 104 //Front Links und Recht prüfen ob viel zu nahe
ZHAW_Prometheus 12:472b26872a42 105 if (sensors[1]<=wand || sensors[5]<=wand) {
ZHAW_Prometheus 12:472b26872a42 106 if(sensors[1]<=wand) {
ZHAW_Prometheus 12:472b26872a42 107 pwmLeft = 0.45;
ZHAW_Prometheus 12:472b26872a42 108 pwmRight = 0.65;
ZHAW_Prometheus 12:472b26872a42 109 state = hart_zurueck_r;
ZHAW_Prometheus 12:472b26872a42 110 } else {
ZHAW_Prometheus 12:472b26872a42 111 pwmLeft = 0.35;
ZHAW_Prometheus 12:472b26872a42 112 pwmRight = 0.55;
ZHAW_Prometheus 12:472b26872a42 113 state = hart_zurueck_l;
ZHAW_Prometheus 12:472b26872a42 114 }
ZHAW_Prometheus 12:472b26872a42 115 wait(0.8);
ZHAW_Prometheus 12:472b26872a42 116 }
ZHAW_Prometheus 0:422088ad7fc5 117
ZHAW_Prometheus 12:472b26872a42 118 //Kontrolle ob zurückgefahren werden muss
ZHAW_Prometheus 12:472b26872a42 119 else if (sensors[0]<=0.1 && sensors[1]<=0.12 && sensors[5]<=0.12) {
ZHAW_Prometheus 12:472b26872a42 120 if(sensors[2]<=0.25) {
ZHAW_Prometheus 12:472b26872a42 121 pwmLeft = 0.35;
ZHAW_Prometheus 12:472b26872a42 122 pwmRight = 0.55;
ZHAW_Prometheus 12:472b26872a42 123 state=zurueck_l;
ZHAW_Prometheus 12:472b26872a42 124 } else if(sensors[4]<=0.25) {
ZHAW_Prometheus 12:472b26872a42 125 pwmLeft = 0.45;
ZHAW_Prometheus 12:472b26872a42 126 pwmRight = 0.65;
ZHAW_Prometheus 12:472b26872a42 127 state=zurueck_r;
ZHAW_Prometheus 12:472b26872a42 128 } else {
ZHAW_Prometheus 12:472b26872a42 129 pwmLeft = 0.4;
ZHAW_Prometheus 12:472b26872a42 130 pwmRight = 0.6;
ZHAW_Prometheus 12:472b26872a42 131 state=zurueck;
ZHAW_Prometheus 12:472b26872a42 132 }
schuema4 4:c1d1bcc96b14 133 }
ZHAW_Prometheus 12:472b26872a42 134
ZHAW_Prometheus 12:472b26872a42 135 //Kontrolle ob gedreht werden muss
ZHAW_Prometheus 12:472b26872a42 136 else if(sensors[0]<0.25) {
ZHAW_Prometheus 12:472b26872a42 137 if(sensors[1]<=wenden) {
ZHAW_Prometheus 12:472b26872a42 138 //Drehen Links
ZHAW_Prometheus 12:472b26872a42 139 pwmLeft = 0.3;
ZHAW_Prometheus 12:472b26872a42 140 pwmRight = 0.3;
ZHAW_Prometheus 12:472b26872a42 141 state = drehen_l;
ZHAW_Prometheus 12:472b26872a42 142 } else if(sensors[5]<=wenden) {
ZHAW_Prometheus 12:472b26872a42 143 //Drehen Rechts
ZHAW_Prometheus 12:472b26872a42 144 pwmLeft = 0.7;
ZHAW_Prometheus 12:472b26872a42 145 pwmRight = 0.7;
ZHAW_Prometheus 12:472b26872a42 146 state = drehen_r;
ZHAW_Prometheus 12:472b26872a42 147 } else {
ZHAW_Prometheus 12:472b26872a42 148 //Random Drehen
ZHAW_Prometheus 12:472b26872a42 149 if (rand()%2==0 && state != drehen_ran) {
ZHAW_Prometheus 12:472b26872a42 150 pwmLeft = 0.3;
ZHAW_Prometheus 12:472b26872a42 151 pwmRight = 0.3;
ZHAW_Prometheus 12:472b26872a42 152 } else if (rand()%2 != 0 && state != drehen_ran) {
ZHAW_Prometheus 12:472b26872a42 153 pwmLeft = 0.7;
ZHAW_Prometheus 12:472b26872a42 154 pwmRight = 0.7;
ZHAW_Prometheus 12:472b26872a42 155 }
ZHAW_Prometheus 12:472b26872a42 156 state=drehen_ran;
ZHAW_Prometheus 12:472b26872a42 157 }
ZHAW_Prometheus 12:472b26872a42 158 }
ZHAW_Prometheus 12:472b26872a42 159
ZHAW_Prometheus 12:472b26872a42 160 //Rechtsfahren
ZHAW_Prometheus 12:472b26872a42 161 else if(sensors[5]<=wenden && sensors[5] >= 0.08) {
ZHAW_Prometheus 12:472b26872a42 162 pwmLeft = 0.65;
ZHAW_Prometheus 12:472b26872a42 163 pwmRight = 0.45;
ZHAW_Prometheus 12:472b26872a42 164 state=rechts;
ZHAW_Prometheus 12:472b26872a42 165 }
ZHAW_Prometheus 12:472b26872a42 166
ZHAW_Prometheus 12:472b26872a42 167 //Linksfahren
ZHAW_Prometheus 12:472b26872a42 168 else if(sensors[1]<=wenden && sensors[1] >= 0.08) {
ZHAW_Prometheus 12:472b26872a42 169 pwmLeft = 0.55;
ZHAW_Prometheus 12:472b26872a42 170 pwmRight = 0.35;
ZHAW_Prometheus 12:472b26872a42 171 state=links;
schuema4 4:c1d1bcc96b14 172 }
ZHAW_Prometheus 0:422088ad7fc5 173 }
ZHAW_Prometheus 0:422088ad7fc5 174 }
schuema4 4:c1d1bcc96b14 175
ZHAW_Prometheus 12:472b26872a42 176 //Userbutton ist Null also HALTEN!!
ZHAW_Prometheus 12:472b26872a42 177 else {
ZHAW_Prometheus 10:10bcb7fee9a6 178 pwmLeft = 0.5;
ZHAW_Prometheus 10:10bcb7fee9a6 179 pwmRight = 0.5;
ZHAW_Prometheus 0:422088ad7fc5 180 }
ZHAW_Prometheus 12:472b26872a42 181 }