Prometheus / Mbed 2 deprecated Prom_Roebi

Dependencies:   Farbsensor IRSensorLib PID_Control Servo mbed PixyLib

Fahren.cpp@4:c1d1bcc96b14, 2017-05-13 (annotated)

Committer:
schuema4
Date:
Sat May 13 13:53:16 2017 +0000
Revision:
4:c1d1bcc96b14
Parent:
3:017c85c4b14b
Child:
6:d611637e7cad
stand 13.05.17_15:53;

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