Prometheus / Mbed 2 deprecated Prom_Roebi

Dependencies:   Farbsensor IRSensorLib PID_Control Servo mbed PixyLib

Fahren.cpp@16:ad45ef4fee04, 2017-05-26 (annotated)

Committer:
ZHAW_Prometheus
Date:
Fri May 26 09:11:49 2017 +0000
Revision:
16:ad45ef4fee04
Parent:
15:26dbcd6ff48d 
Vers. 26.05.2017 11: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 15:26dbcd6ff48d 8 Fahren::Fahren(DigitalOut& _enable, DigitalOut& _bit0, DigitalOut& _bit1, DigitalOut& _bit2, AnalogIn& _distance, DigitalOut& _enableMotorDriver, PwmOut& _pwmLeft, PwmOut& _pwmRight, Pixy& _pixy, PID_Control& _pid) : enable(_enable), bit0(_bit0), bit1(_bit1), bit2(_bit2), distance(_distance), enableMotorDriver(_enableMotorDriver), pwmLeft(_pwmLeft), pwmRight(_pwmRight), pixy(_pixy), pid(_pid)
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 15:26dbcd6ff48d 13 searchTimer = 0;
ZHAW_Prometheus 16:ad45ef4fee04 14 zurTimer = 40;
ZHAW_Prometheus 0:422088ad7fc5 15 }
ZHAW_Prometheus 0:422088ad7fc5 16
ZHAW_Prometheus 12:472b26872a42 17 //Destruktor
ZHAW_Prometheus 12:472b26872a42 18 Fahren::~Fahren() {}
ZHAW_Prometheus 12:472b26872a42 19
ZHAW_Prometheus 12:472b26872a42 20 //Methoden
ZHAW_Prometheus 0:422088ad7fc5 21 void Fahren::setSerialOutput(Serial *pc)
ZHAW_Prometheus 0:422088ad7fc5 22 {
ZHAW_Prometheus 0:422088ad7fc5 23 this->pc = pc;
ZHAW_Prometheus 0:422088ad7fc5 24 }
ZHAW_Prometheus 0:422088ad7fc5 25
ZHAW_Prometheus 12:472b26872a42 26 void Fahren::printState()
schuema4 4:c1d1bcc96b14 27 {
ZHAW_Prometheus 12:472b26872a42 28 if(pc) {
ZHAW_Prometheus 12:472b26872a42 29 switch (state) {
ZHAW_Prometheus 12:472b26872a42 30 case gerade:
ZHAW_Prometheus 12:472b26872a42 31 pc->printf("Gerade\n\r");
ZHAW_Prometheus 12:472b26872a42 32 break;
ZHAW_Prometheus 12:472b26872a42 33 case cam:
ZHAW_Prometheus 12:472b26872a42 34 pc->printf("Kamerafahrt\n\r");
ZHAW_Prometheus 12:472b26872a42 35 break;
ZHAW_Prometheus 16:ad45ef4fee04 36 case camSearchRot:
ZHAW_Prometheus 16:ad45ef4fee04 37 pc->printf("Kamerafahrt Rotation\n\r");
ZHAW_Prometheus 16:ad45ef4fee04 38 break;
ZHAW_Prometheus 16:ad45ef4fee04 39 case camSearchDrive:
ZHAW_Prometheus 16:ad45ef4fee04 40 pc->printf("Kamerafahrt Fahrsuche\n\r");
ZHAW_Prometheus 16:ad45ef4fee04 41 break;
ZHAW_Prometheus 12:472b26872a42 42 case rechts:
ZHAW_Prometheus 12:472b26872a42 43 pc->printf("Rechts\n\r");
ZHAW_Prometheus 12:472b26872a42 44 break;
ZHAW_Prometheus 12:472b26872a42 45 case links:
ZHAW_Prometheus 12:472b26872a42 46 pc->printf("Links\n\r");
ZHAW_Prometheus 12:472b26872a42 47 break;
ZHAW_Prometheus 12:472b26872a42 48 case drehen_ran:
ZHAW_Prometheus 12:472b26872a42 49 pc->printf("Drehen Random\n\r");
ZHAW_Prometheus 12:472b26872a42 50 break;
ZHAW_Prometheus 12:472b26872a42 51 case drehen_l:
ZHAW_Prometheus 12:472b26872a42 52 pc->printf("Drehen Links\n\r");
ZHAW_Prometheus 12:472b26872a42 53 break;
ZHAW_Prometheus 12:472b26872a42 54 case drehen_r:
ZHAW_Prometheus 12:472b26872a42 55 pc->printf("Drehen Rechts\n\r");
ZHAW_Prometheus 12:472b26872a42 56 break;
ZHAW_Prometheus 12:472b26872a42 57 case zurueck:
ZHAW_Prometheus 12:472b26872a42 58 pc->printf("Gerade Zurueck\n\r");
ZHAW_Prometheus 12:472b26872a42 59 break;
ZHAW_Prometheus 12:472b26872a42 60 case zurueck_l:
ZHAW_Prometheus 12:472b26872a42 61 pc->printf("Links Zurueck\n\r");
ZHAW_Prometheus 12:472b26872a42 62 break;
ZHAW_Prometheus 12:472b26872a42 63 case zurueck_r:
ZHAW_Prometheus 12:472b26872a42 64 pc->printf("Rechts Zurueck\n\r");
ZHAW_Prometheus 12:472b26872a42 65 break;
ZHAW_Prometheus 12:472b26872a42 66 case hart_zurueck_r:
ZHAW_Prometheus 12:472b26872a42 67 pc->printf("Rechts Hart-Zurueck\n\r");
ZHAW_Prometheus 12:472b26872a42 68 break;
ZHAW_Prometheus 12:472b26872a42 69 case hart_zurueck_l:
ZHAW_Prometheus 12:472b26872a42 70 pc->printf("Links Hart-Zurueck\n\r");
ZHAW_Prometheus 12:472b26872a42 71 break;
ZHAW_Prometheus 12:472b26872a42 72 }
ZHAW_Prometheus 12:472b26872a42 73 }
ZHAW_Prometheus 0:422088ad7fc5 74 }
ZHAW_Prometheus 0:422088ad7fc5 75
ZHAW_Prometheus 12:472b26872a42 76
schuema4 4:c1d1bcc96b14 77 void Fahren::fahrInit()
ZHAW_Prometheus 12:472b26872a42 78 {
ZHAW_Prometheus 9:b83994ef4b08 79 pwmLeft = 0.5;
ZHAW_Prometheus 9:b83994ef4b08 80 pwmRight = 0.5;
schuema4 4:c1d1bcc96b14 81
ZHAW_Prometheus 0:422088ad7fc5 82 for( int ii = 0; ii<6; ++ii) {
ZHAW_Prometheus 12:472b26872a42 83 sensors[ii].init(&distance, &bit0, &bit1, &bit2, ii);
ZHAW_Prometheus 12:472b26872a42 84 enable=1;
ZHAW_Prometheus 9:b83994ef4b08 85 pwmLeft.period(0.00005);
ZHAW_Prometheus 9:b83994ef4b08 86 pwmRight.period(0.00005);
ZHAW_Prometheus 12:472b26872a42 87 enableMotorDriver=1;
ZHAW_Prometheus 0:422088ad7fc5 88 }
ZHAW_Prometheus 0:422088ad7fc5 89 }
ZHAW_Prometheus 0:422088ad7fc5 90
schuema4 4:c1d1bcc96b14 91 void Fahren::fahrRutine()
schuema4 4:c1d1bcc96b14 92 {
ZHAW_Prometheus 16:ad45ef4fee04 93 //IR-Sensoren auslesen
ZHAW_Prometheus 15:26dbcd6ff48d 94 s0 = sensors[0];
ZHAW_Prometheus 15:26dbcd6ff48d 95 s1 = sensors[1];
ZHAW_Prometheus 15:26dbcd6ff48d 96 s2 = sensors[2];
ZHAW_Prometheus 15:26dbcd6ff48d 97 s4 = sensors[4];
ZHAW_Prometheus 15:26dbcd6ff48d 98 s5 = sensors[5];
ZHAW_Prometheus 16:ad45ef4fee04 99
ZHAW_Prometheus 16:ad45ef4fee04 100 //Erst prüfen ob aus einem vorhergegangenen Schritt noch eine Aktion ausgeführt werden muss
ZHAW_Prometheus 16:ad45ef4fee04 101 if ((state == hart_zurueck_r || state == hart_zurueck_l) && zurTimer<40) {
ZHAW_Prometheus 16:ad45ef4fee04 102 zurTimer++;
ZHAW_Prometheus 16:ad45ef4fee04 103 }
ZHAW_Prometheus 16:ad45ef4fee04 104
ZHAW_Prometheus 15:26dbcd6ff48d 105 //Alle Sensoren genügend abstand -> Kamerafahrt
ZHAW_Prometheus 16:ad45ef4fee04 106 else if (s0>0.25 && s1>0.22 && s2>0.25 && s4>0.25 && s5>0.22) {
ZHAW_Prometheus 15:26dbcd6ff48d 107 if ((pixy.getX()>=5 && pixy.getX()<=315) && pixy.getDetects() != 0 ) {
ZHAW_Prometheus 15:26dbcd6ff48d 108 e = 160-pixy.getX();
ZHAW_Prometheus 15:26dbcd6ff48d 109 diff = pid.calc( e, 0.005f );
ZHAW_Prometheus 15:26dbcd6ff48d 110 pwmLeft = 0.65f - diff;
ZHAW_Prometheus 15:26dbcd6ff48d 111 pwmRight = 0.35f - diff;
ZHAW_Prometheus 15:26dbcd6ff48d 112 state = cam;
ZHAW_Prometheus 15:26dbcd6ff48d 113 } else {
ZHAW_Prometheus 15:26dbcd6ff48d 114 if (searchTimer < 150) {
ZHAW_Prometheus 15:26dbcd6ff48d 115 pwmLeft = 0.65;
ZHAW_Prometheus 15:26dbcd6ff48d 116 pwmRight = 0.65;
ZHAW_Prometheus 15:26dbcd6ff48d 117 searchTimer++;
ZHAW_Prometheus 15:26dbcd6ff48d 118 state = camSearchRot;
ZHAW_Prometheus 12:472b26872a42 119 } else {
ZHAW_Prometheus 15:26dbcd6ff48d 120 pwmLeft = 0.65f;
ZHAW_Prometheus 15:26dbcd6ff48d 121 pwmRight = 0.35f;
ZHAW_Prometheus 15:26dbcd6ff48d 122 searchTimer -= 2;
ZHAW_Prometheus 15:26dbcd6ff48d 123 state = camSearchDrive;
schuema4 4:c1d1bcc96b14 124 }
ZHAW_Prometheus 15:26dbcd6ff48d 125 }
ZHAW_Prometheus 15:26dbcd6ff48d 126 } else {
ZHAW_Prometheus 15:26dbcd6ff48d 127 //Front Links und Recht prüfen ob viel zu nahe
ZHAW_Prometheus 16:ad45ef4fee04 128 if (s1<=wand || s5<=wand) { //Abfrage noch ändern
ZHAW_Prometheus 16:ad45ef4fee04 129 if(s1<=wand) {
ZHAW_Prometheus 15:26dbcd6ff48d 130 pwmLeft = 0.45;
ZHAW_Prometheus 15:26dbcd6ff48d 131 pwmRight = 0.65;
ZHAW_Prometheus 15:26dbcd6ff48d 132 state = hart_zurueck_r;
ZHAW_Prometheus 16:ad45ef4fee04 133 zurTimer = 0;
ZHAW_Prometheus 15:26dbcd6ff48d 134 } else {
ZHAW_Prometheus 15:26dbcd6ff48d 135 pwmLeft = 0.35;
ZHAW_Prometheus 15:26dbcd6ff48d 136 pwmRight = 0.55;
ZHAW_Prometheus 15:26dbcd6ff48d 137 state = hart_zurueck_l;
ZHAW_Prometheus 16:ad45ef4fee04 138 zurTimer = 0;
ZHAW_Prometheus 12:472b26872a42 139 }
ZHAW_Prometheus 15:26dbcd6ff48d 140 }
ZHAW_Prometheus 0:422088ad7fc5 141
ZHAW_Prometheus 15:26dbcd6ff48d 142 //Kontrolle ob zurückgefahren werden muss
ZHAW_Prometheus 16:ad45ef4fee04 143 else if (s0<=0.1 && s1<=0.12 && s5<=0.12) {
ZHAW_Prometheus 16:ad45ef4fee04 144 if(s2<=0.25) {
ZHAW_Prometheus 15:26dbcd6ff48d 145 pwmLeft = 0.65;
ZHAW_Prometheus 15:26dbcd6ff48d 146 pwmRight = 0.65;
ZHAW_Prometheus 15:26dbcd6ff48d 147 state=zurueck_l;
ZHAW_Prometheus 16:ad45ef4fee04 148 } else if(s4<=0.25) {
ZHAW_Prometheus 15:26dbcd6ff48d 149 pwmLeft = 0.35;
ZHAW_Prometheus 15:26dbcd6ff48d 150 pwmRight = 0.35;
ZHAW_Prometheus 15:26dbcd6ff48d 151 state=zurueck_r;
ZHAW_Prometheus 15:26dbcd6ff48d 152 } else {
ZHAW_Prometheus 15:26dbcd6ff48d 153 pwmLeft = 0.4;
ZHAW_Prometheus 15:26dbcd6ff48d 154 pwmRight = 0.6;
ZHAW_Prometheus 15:26dbcd6ff48d 155 state=zurueck;
schuema4 4:c1d1bcc96b14 156 }
ZHAW_Prometheus 15:26dbcd6ff48d 157 }
ZHAW_Prometheus 12:472b26872a42 158
ZHAW_Prometheus 15:26dbcd6ff48d 159 //Kontrolle ob gedreht werden muss
ZHAW_Prometheus 16:ad45ef4fee04 160 else if(s0<0.25) {
ZHAW_Prometheus 15:26dbcd6ff48d 161 if(sensors[1]<=wenden) {
ZHAW_Prometheus 15:26dbcd6ff48d 162 //Drehen Links
ZHAW_Prometheus 15:26dbcd6ff48d 163 pwmLeft = 0.3;
ZHAW_Prometheus 15:26dbcd6ff48d 164 pwmRight = 0.3;
ZHAW_Prometheus 15:26dbcd6ff48d 165 state = drehen_l;
ZHAW_Prometheus 16:ad45ef4fee04 166 } else if(s5<=wenden) {
ZHAW_Prometheus 15:26dbcd6ff48d 167 //Drehen Rechts
ZHAW_Prometheus 15:26dbcd6ff48d 168 pwmLeft = 0.7;
ZHAW_Prometheus 15:26dbcd6ff48d 169 pwmRight = 0.7;
ZHAW_Prometheus 15:26dbcd6ff48d 170 state = drehen_r;
ZHAW_Prometheus 15:26dbcd6ff48d 171 } else {
ZHAW_Prometheus 15:26dbcd6ff48d 172 //Random Drehen
ZHAW_Prometheus 15:26dbcd6ff48d 173 if (rand()%2==0 && state != drehen_ran) {
ZHAW_Prometheus 12:472b26872a42 174 pwmLeft = 0.3;
ZHAW_Prometheus 12:472b26872a42 175 pwmRight = 0.3;
ZHAW_Prometheus 15:26dbcd6ff48d 176 } else if (rand()%2 != 0 && state != drehen_ran) {
ZHAW_Prometheus 12:472b26872a42 177 pwmLeft = 0.7;
ZHAW_Prometheus 12:472b26872a42 178 pwmRight = 0.7;
ZHAW_Prometheus 12:472b26872a42 179 }
ZHAW_Prometheus 15:26dbcd6ff48d 180 state=drehen_ran;
schuema4 4:c1d1bcc96b14 181 }
ZHAW_Prometheus 0:422088ad7fc5 182 }
schuema4 4:c1d1bcc96b14 183
ZHAW_Prometheus 15:26dbcd6ff48d 184 //Rechtsfahren
ZHAW_Prometheus 16:ad45ef4fee04 185 else if(s5<=wenden && s5 >= 0.08) {
ZHAW_Prometheus 15:26dbcd6ff48d 186 pwmLeft = 0.65;
ZHAW_Prometheus 15:26dbcd6ff48d 187 pwmRight = 0.45;
ZHAW_Prometheus 15:26dbcd6ff48d 188 state=rechts;
ZHAW_Prometheus 15:26dbcd6ff48d 189 }
ZHAW_Prometheus 15:26dbcd6ff48d 190
ZHAW_Prometheus 15:26dbcd6ff48d 191 //Linksfahren
ZHAW_Prometheus 16:ad45ef4fee04 192 else if(s1<=wenden && s1 >= 0.08) {
ZHAW_Prometheus 15:26dbcd6ff48d 193 pwmLeft = 0.55;
ZHAW_Prometheus 15:26dbcd6ff48d 194 pwmRight = 0.35;
ZHAW_Prometheus 15:26dbcd6ff48d 195 state=links;
ZHAW_Prometheus 15:26dbcd6ff48d 196 }
ZHAW_Prometheus 0:422088ad7fc5 197 }
ZHAW_Prometheus 12:472b26872a42 198 }