Prometheus / Mbed 2 deprecated Prom_Roebi

Prom_Roebi_0.1

Dependencies:   Farbsensor IRSensorLib PID_Control Servo mbed PixyLib

Fahren.cpp@6:d611637e7cad, 2017-05-16 (annotated)

Committer:
schuema4
Date:
Tue May 16 14:02:23 2017 +0000
Revision:
6:d611637e7cad
Parent:
4:c1d1bcc96b14
Child:
7:5949f408b6da
version 16.05.17_16:02;

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