Prometheus / Mbed 2 deprecated Prom_Roebi

Dependencies:   Farbsensor IRSensorLib PID_Control Servo mbed PixyLib

Fahren.cpp@2:dea0bab5e45c, 2017-05-10 (annotated)

Committer:
ZHAW_Prometheus
Date:
Wed May 10 18:12:34 2017 +0000
Revision:
2:dea0bab5e45c
Parent:
1:5c44e2462a8b
Child:
3:017c85c4b14b
Prom_Roebi_V-0.3

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
ZHAW_Prometheus 0:422088ad7fc5 85 // Wenn Front etwas sehen => drehen***********************************
ZHAW_Prometheus 0:422088ad7fc5 86
ZHAW_Prometheus 0:422088ad7fc5 87 else if(sensors[1] <= wand || sensors[5]<=wand){
ZHAW_Prometheus 1:5c44e2462a8b 88 pwmLeft->write(0.35);
ZHAW_Prometheus 1:5c44e2462a8b 89 pwmRight->write(0.65);
ZHAW_Prometheus 0:422088ad7fc5 90 }
ZHAW_Prometheus 0:422088ad7fc5 91
ZHAW_Prometheus 0:422088ad7fc5 92 else if(sensors[0]<0.25 && sensors [1]<=wenden) {
ZHAW_Prometheus 1:5c44e2462a8b 93 pwmLeft->write(0.4);
ZHAW_Prometheus 2:dea0bab5e45c 94 pwmRight->write(0.4);
ZHAW_Prometheus 2:dea0bab5e45c 95 if (pc) {
ZHAW_Prometheus 2:dea0bab5e45c 96 pc->printf("drehen links\n\n\n");}
ZHAW_Prometheus 0:422088ad7fc5 97 }
ZHAW_Prometheus 0:422088ad7fc5 98
ZHAW_Prometheus 0:422088ad7fc5 99 else if (sensors[0]<0.25 && sensors [5]<=wenden){
ZHAW_Prometheus 1:5c44e2462a8b 100 pwmLeft->write(0.6);
ZHAW_Prometheus 2:dea0bab5e45c 101 pwmRight->write(0.6);
ZHAW_Prometheus 2:dea0bab5e45c 102 if (pc) {
ZHAW_Prometheus 2:dea0bab5e45c 103 pc->printf("drehen\n\n\n");}
ZHAW_Prometheus 0:422088ad7fc5 104 }
ZHAW_Prometheus 0:422088ad7fc5 105 else if(sensors[0]<0.25) {
ZHAW_Prometheus 0:422088ad7fc5 106 if (rand()%2==0 && state != drehen) {
ZHAW_Prometheus 1:5c44e2462a8b 107 pwmLeft->write(0.4);
ZHAW_Prometheus 1:5c44e2462a8b 108 pwmRight->write(0.4);
ZHAW_Prometheus 0:422088ad7fc5 109 } else if (rand()%2 != 0 && state != drehen) {
ZHAW_Prometheus 1:5c44e2462a8b 110 pwmLeft->write(0.6);
ZHAW_Prometheus 1:5c44e2462a8b 111 pwmRight->write(0.6);
ZHAW_Prometheus 0:422088ad7fc5 112 }
ZHAW_Prometheus 0:422088ad7fc5 113 state=drehen;
ZHAW_Prometheus 0:422088ad7fc5 114 }
ZHAW_Prometheus 0:422088ad7fc5 115 //Wenn Front-Left etwas sehen => nach Rechts**************************
ZHAW_Prometheus 0:422088ad7fc5 116 else if(sensors[5]<=wenden) {
ZHAW_Prometheus 2:dea0bab5e45c 117 if (pc) {
ZHAW_Prometheus 2:dea0bab5e45c 118 pc->printf("rechts\n");}
ZHAW_Prometheus 1:5c44e2462a8b 119 pwmLeft->write(0.65);
ZHAW_Prometheus 1:5c44e2462a8b 120 pwmRight->write(0.45);
ZHAW_Prometheus 0:422088ad7fc5 121 state=rechts;
ZHAW_Prometheus 0:422088ad7fc5 122 }
ZHAW_Prometheus 0:422088ad7fc5 123
ZHAW_Prometheus 0:422088ad7fc5 124 // Wenn Front-Right etwas sehen => Links*******************************
ZHAW_Prometheus 0:422088ad7fc5 125 else if(sensors[1]<=wenden) {
ZHAW_Prometheus 2:dea0bab5e45c 126 if(pc) {
ZHAW_Prometheus 2:dea0bab5e45c 127 pc->printf("Links\n");}
ZHAW_Prometheus 1:5c44e2462a8b 128 pwmLeft->write(0.55);
ZHAW_Prometheus 1:5c44e2462a8b 129 pwmRight->write(0.35);
ZHAW_Prometheus 0:422088ad7fc5 130 state=links;
ZHAW_Prometheus 0:422088ad7fc5 131 }
ZHAW_Prometheus 0:422088ad7fc5 132
ZHAW_Prometheus 0:422088ad7fc5 133 //Wenn kein Sensor anspricht => gerade aus*****************************
ZHAW_Prometheus 0:422088ad7fc5 134 else if(sensors[0]>=0.26) {
ZHAW_Prometheus 2:dea0bab5e45c 135 if(pc) {
ZHAW_Prometheus 2:dea0bab5e45c 136 pc->printf("gerade\n");}
ZHAW_Prometheus 1:5c44e2462a8b 137 pwmLeft->write(0.65);
ZHAW_Prometheus 1:5c44e2462a8b 138 pwmRight->write(0.35);
ZHAW_Prometheus 0:422088ad7fc5 139 state=gerade;
ZHAW_Prometheus 0:422088ad7fc5 140 }
ZHAW_Prometheus 1:5c44e2462a8b 141 else {
ZHAW_Prometheus 2:dea0bab5e45c 142 if(pc) {
ZHAW_Prometheus 2:dea0bab5e45c 143 pc->printf("gerade2\n\r");}
ZHAW_Prometheus 1:5c44e2462a8b 144 }
ZHAW_Prometheus 0:422088ad7fc5 145 }