Prometheus / Mbed 2 deprecated Prom_Roebi

Dependencies:   Farbsensor IRSensorLib PID_Control Servo mbed PixyLib

Fahren.cpp@8:077d0bb213a2, 2017-05-18 (annotated)

Committer:
ZHAW_Prometheus
Date:
Thu May 18 09:12:46 2017 +0000
Revision:
8:077d0bb213a2
Parent:
7:5949f408b6da
Child:
9:b83994ef4b08
Version 18.05.2017 11:12

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 7:5949f408b6da 7
ZHAW_Prometheus 8:077d0bb213a2 8 Fahren::Fahren(DigitalOut* enable, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, AnalogIn* distance, DigitalOut* enableMotorDriver, PwmOut* pwmLeft, PwmOut* pwmRight, Pixy* pixy, PID_Control* pid)
ZHAW_Prometheus 0:422088ad7fc5 9 {
ZHAW_Prometheus 8:077d0bb213a2 10 init(enable, bit0, bit1, bit2, distance, enableMotorDriver, pwmLeft, pwmRight, pixy, pid);
ZHAW_Prometheus 0:422088ad7fc5 11 }
ZHAW_Prometheus 0:422088ad7fc5 12
ZHAW_Prometheus 0:422088ad7fc5 13 Fahren::Fahren() {}
ZHAW_Prometheus 0:422088ad7fc5 14
ZHAW_Prometheus 0:422088ad7fc5 15 /**
ZHAW_Prometheus 0:422088ad7fc5 16 * Deletes the IRSensor object.
ZHAW_Prometheus 0:422088ad7fc5 17 */
ZHAW_Prometheus 0:422088ad7fc5 18 Fahren::~Fahren() {}
ZHAW_Prometheus 0:422088ad7fc5 19
ZHAW_Prometheus 8:077d0bb213a2 20 void Fahren::init(DigitalOut* enable, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, AnalogIn* distance, DigitalOut* enableMotorDriver, PwmOut* pwmLeft, PwmOut* pwmRight, Pixy* pixy, PID_Control* pid)
ZHAW_Prometheus 0:422088ad7fc5 21 {
ZHAW_Prometheus 0:422088ad7fc5 22 this->enable = enable;
ZHAW_Prometheus 0:422088ad7fc5 23 this->bit0 = bit0;
ZHAW_Prometheus 0:422088ad7fc5 24 this->bit1 = bit1;
ZHAW_Prometheus 0:422088ad7fc5 25 this->bit2 = bit2;
ZHAW_Prometheus 0:422088ad7fc5 26 this->distance = distance;
ZHAW_Prometheus 0:422088ad7fc5 27 this->enableMotorDriver = enableMotorDriver;
ZHAW_Prometheus 0:422088ad7fc5 28 this->pwmLeft = pwmLeft;
ZHAW_Prometheus 0:422088ad7fc5 29 this->pwmRight = pwmRight;
ZHAW_Prometheus 8:077d0bb213a2 30 this->pixy = pixy;
ZHAW_Prometheus 0:422088ad7fc5 31 state = 0;
ZHAW_Prometheus 7:5949f408b6da 32
schuema4 4:c1d1bcc96b14 33
ZHAW_Prometheus 0:422088ad7fc5 34 }
ZHAW_Prometheus 0:422088ad7fc5 35
ZHAW_Prometheus 0:422088ad7fc5 36 void Fahren::setSerialOutput(Serial *pc)
ZHAW_Prometheus 0:422088ad7fc5 37 {
ZHAW_Prometheus 0:422088ad7fc5 38 this->pc = pc;
ZHAW_Prometheus 0:422088ad7fc5 39 }
ZHAW_Prometheus 0:422088ad7fc5 40
schuema4 4:c1d1bcc96b14 41 int Fahren::getState()
schuema4 4:c1d1bcc96b14 42 {
ZHAW_Prometheus 0:422088ad7fc5 43 return state;
ZHAW_Prometheus 0:422088ad7fc5 44 }
ZHAW_Prometheus 0:422088ad7fc5 45
schuema4 4:c1d1bcc96b14 46 void Fahren::fahrInit()
schuema4 6:d611637e7cad 47 { pwmLeft->write(0.5);
ZHAW_Prometheus 0:422088ad7fc5 48 pwmRight->write(0.5);
schuema4 4:c1d1bcc96b14 49
ZHAW_Prometheus 0:422088ad7fc5 50 for( int ii = 0; ii<6; ++ii) {
ZHAW_Prometheus 0:422088ad7fc5 51 sensors[ii].init(distance, bit0, bit1, bit2, ii);
ZHAW_Prometheus 0:422088ad7fc5 52
ZHAW_Prometheus 0:422088ad7fc5 53 enable->write(1);
ZHAW_Prometheus 0:422088ad7fc5 54
ZHAW_Prometheus 0:422088ad7fc5 55
ZHAW_Prometheus 0:422088ad7fc5 56 pwmLeft->period(0.00005);
ZHAW_Prometheus 0:422088ad7fc5 57 pwmRight->period(0.00005);
ZHAW_Prometheus 0:422088ad7fc5 58
ZHAW_Prometheus 0:422088ad7fc5 59 enableMotorDriver->write(1);
ZHAW_Prometheus 0:422088ad7fc5 60 }
ZHAW_Prometheus 0:422088ad7fc5 61 }
ZHAW_Prometheus 0:422088ad7fc5 62
schuema4 4:c1d1bcc96b14 63 void Fahren::fahrRutine()
schuema4 4:c1d1bcc96b14 64 {
ZHAW_Prometheus 7:5949f408b6da 65 if (pc) {
ZHAW_Prometheus 8:077d0bb213a2 66 //pc->printf("left:%f\n\r", sensors[5].read());
ZHAW_Prometheus 7:5949f408b6da 67 }
ZHAW_Prometheus 0:422088ad7fc5 68 if(sensors[0]<=0.1 && sensors[1]<=0.12 && sensors[5]<=0.12) {
ZHAW_Prometheus 0:422088ad7fc5 69 //wenn hinten rechts => leichte linkskurve
ZHAW_Prometheus 0:422088ad7fc5 70 if(sensors[2]<=0.25) {
ZHAW_Prometheus 1:5c44e2462a8b 71 pwmLeft->write(0.35);
ZHAW_Prometheus 1:5c44e2462a8b 72 pwmRight->write(0.55);
ZHAW_Prometheus 2:dea0bab5e45c 73 if (pc) {
ZHAW_Prometheus 7:5949f408b6da 74 pc->printf("zurueck-rechtskurve\n");
schuema4 4:c1d1bcc96b14 75 }
ZHAW_Prometheus 0:422088ad7fc5 76 state=zurueck_l;
ZHAW_Prometheus 0:422088ad7fc5 77 }
ZHAW_Prometheus 0:422088ad7fc5 78
ZHAW_Prometheus 0:422088ad7fc5 79 if(sensors[4]<=0.25) {
schuema4 4:c1d1bcc96b14 80 pwmLeft->write(0.45);
schuema4 4:c1d1bcc96b14 81 pwmRight->write(0.65);
ZHAW_Prometheus 2:dea0bab5e45c 82 if (pc) {
ZHAW_Prometheus 7:5949f408b6da 83 pc->printf("zurueck-linkskurve\n");
schuema4 4:c1d1bcc96b14 84 }
ZHAW_Prometheus 0:422088ad7fc5 85 state=zurueck_r;
ZHAW_Prometheus 0:422088ad7fc5 86 }
ZHAW_Prometheus 0:422088ad7fc5 87 if(sensors[4]>=0.25 && sensors[2]>=0.25) {
ZHAW_Prometheus 1:5c44e2462a8b 88 pwmLeft->write(0.4);
ZHAW_Prometheus 1:5c44e2462a8b 89 pwmRight->write(0.6);
ZHAW_Prometheus 0:422088ad7fc5 90 state=zurueck;
ZHAW_Prometheus 2:dea0bab5e45c 91 if (pc) {
ZHAW_Prometheus 7:5949f408b6da 92 pc->printf("zurueck-gerade\n");
schuema4 4:c1d1bcc96b14 93 }
ZHAW_Prometheus 0:422088ad7fc5 94 }
ZHAW_Prometheus 0:422088ad7fc5 95 }
schuema4 4:c1d1bcc96b14 96 if(sensors[5] <= wand) {
schuema4 4:c1d1bcc96b14 97
schuema4 4:c1d1bcc96b14 98 pwmLeft->write(0.35);
schuema4 4:c1d1bcc96b14 99 pwmRight->write(0.55);
schuema4 4:c1d1bcc96b14 100 wait(0.8);
schuema4 4:c1d1bcc96b14 101 }
schuema4 4:c1d1bcc96b14 102 if (sensors[1] <= wand){
schuema4 4:c1d1bcc96b14 103 pwmLeft->write(0.45);
schuema4 4:c1d1bcc96b14 104 pwmRight->write(0.65);
schuema4 4:c1d1bcc96b14 105 wait(0.8);
schuema4 4:c1d1bcc96b14 106 }
schuema4 4:c1d1bcc96b14 107
schuema4 4:c1d1bcc96b14 108
schuema4 4:c1d1bcc96b14 109 //plötzlicher Wand anschlag ***************************************
schuema4 4:c1d1bcc96b14 110 /* else if(sensors[1] <= wand){
schuema4 4:c1d1bcc96b14 111 notfall1=1;
schuema4 4:c1d1bcc96b14 112 pwmLeft->write(0.3);
schuema4 4:c1d1bcc96b14 113 pwmRight->write(0.3);
schuema4 4:c1d1bcc96b14 114 if (pc) {
schuema4 4:c1d1bcc96b14 115 pc->printf("Notfall links drehen");}
schuema4 4:c1d1bcc96b14 116 }
schuema4 4:c1d1bcc96b14 117
schuema4 4:c1d1bcc96b14 118 else if(sensors[5] <= wand){
schuema4 4:c1d1bcc96b14 119 notfall1=2;
schuema4 4:c1d1bcc96b14 120 pwmLeft->write(0.7);
schuema4 4:c1d1bcc96b14 121 pwmRight->write(0.7);
schuema4 4:c1d1bcc96b14 122 if (pc) {
schuema4 4:c1d1bcc96b14 123 pc->printf("Notfall rechts drehen");}
schuema4 4:c1d1bcc96b14 124 }
schuema4 4:c1d1bcc96b14 125 */
schuema4 4:c1d1bcc96b14 126
schuema4 4:c1d1bcc96b14 127
schuema4 4:c1d1bcc96b14 128 // Wenn Front etwas sehen => drehen***********************************
schuema4 4:c1d1bcc96b14 129
schuema4 4:c1d1bcc96b14 130 else if(sensors[0]<0.25 && sensors [1]<=wenden) {
schuema4 3:017c85c4b14b 131 pwmLeft->write(0.3);
schuema4 3:017c85c4b14b 132 pwmRight->write(0.3);
schuema4 4:c1d1bcc96b14 133 if (pc) {
ZHAW_Prometheus 7:5949f408b6da 134 pc->printf("drehen links\n\n\n");
schuema4 4:c1d1bcc96b14 135 }
schuema4 4:c1d1bcc96b14 136 }
schuema4 4:c1d1bcc96b14 137
schuema4 4:c1d1bcc96b14 138 else if (sensors[0]<0.25 && sensors [5]<=wenden) {
schuema4 3:017c85c4b14b 139 pwmLeft->write(0.7);
schuema4 3:017c85c4b14b 140 pwmRight->write(0.7);
schuema4 4:c1d1bcc96b14 141 if (pc) {
ZHAW_Prometheus 7:5949f408b6da 142 pc->printf("drehen rechts\n\n\n");
schuema4 4:c1d1bcc96b14 143 }
ZHAW_Prometheus 0:422088ad7fc5 144 }
schuema4 4:c1d1bcc96b14 145
ZHAW_Prometheus 0:422088ad7fc5 146 else if(sensors[0]<0.25) {
schuema4 4:c1d1bcc96b14 147 if (rand()%2==0 && state != drehen) {
schuema4 3:017c85c4b14b 148 pwmLeft->write(0.3);
schuema4 3:017c85c4b14b 149 pwmRight->write(0.3);
ZHAW_Prometheus 7:5949f408b6da 150 if (pc) {
ZHAW_Prometheus 7:5949f408b6da 151 pc->printf("random drehen\n\n\n");
ZHAW_Prometheus 7:5949f408b6da 152 }
ZHAW_Prometheus 0:422088ad7fc5 153 } else if (rand()%2 != 0 && state != drehen) {
schuema4 3:017c85c4b14b 154 pwmLeft->write(0.7);
schuema4 3:017c85c4b14b 155 pwmRight->write(0.7);
ZHAW_Prometheus 7:5949f408b6da 156 if (pc) {
ZHAW_Prometheus 7:5949f408b6da 157 pc->printf("random drehen\n\n\n");
ZHAW_Prometheus 7:5949f408b6da 158 }
ZHAW_Prometheus 0:422088ad7fc5 159 }
schuema4 4:c1d1bcc96b14 160
schuema4 4:c1d1bcc96b14 161 state=drehen;
ZHAW_Prometheus 0:422088ad7fc5 162 }
ZHAW_Prometheus 0:422088ad7fc5 163 //Wenn Front-Left etwas sehen => nach Rechts**************************
schuema4 4:c1d1bcc96b14 164 else if(sensors[1] >= 0.08 && (sensors[5]<=wenden && sensors[5] >= 0.08)) {
ZHAW_Prometheus 2:dea0bab5e45c 165 if (pc) {
ZHAW_Prometheus 7:5949f408b6da 166 pc->printf("rechts\n");
schuema4 4:c1d1bcc96b14 167 }
ZHAW_Prometheus 1:5c44e2462a8b 168 pwmLeft->write(0.65);
ZHAW_Prometheus 1:5c44e2462a8b 169 pwmRight->write(0.45);
ZHAW_Prometheus 0:422088ad7fc5 170 state=rechts;
ZHAW_Prometheus 0:422088ad7fc5 171 }
ZHAW_Prometheus 0:422088ad7fc5 172
ZHAW_Prometheus 0:422088ad7fc5 173 // Wenn Front-Right etwas sehen => Links*******************************
schuema4 4:c1d1bcc96b14 174 else if(sensors[5] >= 0.08 && (sensors[1]<=wenden && sensors[1] >= 0.08)) {
ZHAW_Prometheus 2:dea0bab5e45c 175 if(pc) {
ZHAW_Prometheus 7:5949f408b6da 176 pc->printf("Links\n");
schuema4 4:c1d1bcc96b14 177 }
ZHAW_Prometheus 1:5c44e2462a8b 178 pwmLeft->write(0.55);
ZHAW_Prometheus 1:5c44e2462a8b 179 pwmRight->write(0.35);
ZHAW_Prometheus 0:422088ad7fc5 180 state=links;
ZHAW_Prometheus 0:422088ad7fc5 181 }
ZHAW_Prometheus 0:422088ad7fc5 182
ZHAW_Prometheus 8:077d0bb213a2 183 //Wenn kein Sensor anspricht => PixyCam wird abgerufen*****************************
ZHAW_Prometheus 0:422088ad7fc5 184 else if(sensors[0]>=0.26) {
ZHAW_Prometheus 8:077d0bb213a2 185 if (pixy->getX()){
ZHAW_Prometheus 8:077d0bb213a2 186 if (pc) {
ZHAW_Prometheus 8:077d0bb213a2 187 pc->printf("Cam Fahren\tX:\t%d\n\r", pixy->getX());
ZHAW_Prometheus 8:077d0bb213a2 188 }
ZHAW_Prometheus 8:077d0bb213a2 189 e = 160-pixy->getX();
ZHAW_Prometheus 8:077d0bb213a2 190 diff = pid->calc( e, 0.005f );
ZHAW_Prometheus 8:077d0bb213a2 191 pwmLeft->write(0.65-diff);
ZHAW_Prometheus 8:077d0bb213a2 192 pwmRight->write(0.35-diff);
ZHAW_Prometheus 8:077d0bb213a2 193 } else {
ZHAW_Prometheus 8:077d0bb213a2 194 if(pc) {
ZHAW_Prometheus 8:077d0bb213a2 195 pc->printf("gerade\n\r");
ZHAW_Prometheus 8:077d0bb213a2 196 }
ZHAW_Prometheus 8:077d0bb213a2 197 pwmLeft->write(0.65);
ZHAW_Prometheus 8:077d0bb213a2 198 pwmRight->write(0.35);
ZHAW_Prometheus 8:077d0bb213a2 199 state=gerade;
schuema4 4:c1d1bcc96b14 200 }
schuema4 4:c1d1bcc96b14 201 } else {
schuema4 4:c1d1bcc96b14 202 if(pc) {
ZHAW_Prometheus 7:5949f408b6da 203 pc->printf("gerade2\n\r");
schuema4 4:c1d1bcc96b14 204 }
ZHAW_Prometheus 0:422088ad7fc5 205 }
ZHAW_Prometheus 0:422088ad7fc5 206 }