Prometheus / Mbed 2 deprecated Prom_Roebi

Dependencies:   Farbsensor IRSensorLib PID_Control Servo mbed PixyLib

Fahren.cpp@3:017c85c4b14b, 2017-05-13 (annotated)

Committer:
schuema4
Date:
Sat May 13 12:52:51 2017 +0000
Revision:
3:017c85c4b14b
Parent:
2:dea0bab5e45c
Child:
4:c1d1bcc96b14
stand 13.05.17_14:52

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