Prometheus / Mbed 2 deprecated Prom_Roebi

Dependencies:   Farbsensor IRSensorLib PID_Control Servo mbed PixyLib

Fahren.cpp@0:422088ad7fc5, 2017-05-10 (annotated)

Committer:
ZHAW_Prometheus
Date:
Wed May 10 08:59:22 2017 +0000
Revision:
0:422088ad7fc5
Child:
1:5c44e2462a8b
Prom_Roebi_0.1

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 0:422088ad7fc5 62 pwmLeft=0.35;
ZHAW_Prometheus 0:422088ad7fc5 63 pwmRight=0.55;
ZHAW_Prometheus 0:422088ad7fc5 64 printf("zurueck-linkskurve\n");
ZHAW_Prometheus 0:422088ad7fc5 65 state=zurueck_l;
ZHAW_Prometheus 0:422088ad7fc5 66 }
ZHAW_Prometheus 0:422088ad7fc5 67
ZHAW_Prometheus 0:422088ad7fc5 68 if(sensors[4]<=0.25) {
ZHAW_Prometheus 0:422088ad7fc5 69 pwmLeft=0.35;
ZHAW_Prometheus 0:422088ad7fc5 70 pwmRight=0.55;
ZHAW_Prometheus 0:422088ad7fc5 71 printf("zurueck-rechtskurve\n");
ZHAW_Prometheus 0:422088ad7fc5 72 state=zurueck_r;
ZHAW_Prometheus 0:422088ad7fc5 73 }
ZHAW_Prometheus 0:422088ad7fc5 74 if(sensors[4]>=0.25 && sensors[2]>=0.25) {
ZHAW_Prometheus 0:422088ad7fc5 75 pwmLeft=0.4;
ZHAW_Prometheus 0:422088ad7fc5 76 pwmRight=0.6;
ZHAW_Prometheus 0:422088ad7fc5 77 state=zurueck;
ZHAW_Prometheus 0:422088ad7fc5 78 printf("zurueck-gerade\n");
ZHAW_Prometheus 0:422088ad7fc5 79 }
ZHAW_Prometheus 0:422088ad7fc5 80 }
ZHAW_Prometheus 0:422088ad7fc5 81
ZHAW_Prometheus 0:422088ad7fc5 82 // Wenn Front etwas sehen => drehen***********************************
ZHAW_Prometheus 0:422088ad7fc5 83
ZHAW_Prometheus 0:422088ad7fc5 84 else if(sensors[1] <= wand || sensors[5]<=wand){
ZHAW_Prometheus 0:422088ad7fc5 85 pwmLeft=0.35;
ZHAW_Prometheus 0:422088ad7fc5 86 pwmRight=0.65;
ZHAW_Prometheus 0:422088ad7fc5 87 }
ZHAW_Prometheus 0:422088ad7fc5 88
ZHAW_Prometheus 0:422088ad7fc5 89 else if(sensors[0]<0.25 && sensors [1]<=wenden) {
ZHAW_Prometheus 0:422088ad7fc5 90 pwmLeft=0.4;
ZHAW_Prometheus 0:422088ad7fc5 91 pwmRight=0.4;
ZHAW_Prometheus 0:422088ad7fc5 92 printf("drehen links\n\n\n");
ZHAW_Prometheus 0:422088ad7fc5 93 }
ZHAW_Prometheus 0:422088ad7fc5 94
ZHAW_Prometheus 0:422088ad7fc5 95 else if (sensors[0]<0.25 && sensors [5]<=wenden){
ZHAW_Prometheus 0:422088ad7fc5 96 pwmLeft=0.6;
ZHAW_Prometheus 0:422088ad7fc5 97 pwmRight=0.6;
ZHAW_Prometheus 0:422088ad7fc5 98 printf("drehen\n\n\n");
ZHAW_Prometheus 0:422088ad7fc5 99 }
ZHAW_Prometheus 0:422088ad7fc5 100 else if(sensors[0]<0.25) {
ZHAW_Prometheus 0:422088ad7fc5 101 if (rand()%2==0 && state != drehen) {
ZHAW_Prometheus 0:422088ad7fc5 102 pwmLeft=0.4;
ZHAW_Prometheus 0:422088ad7fc5 103 pwmRight=0.4;
ZHAW_Prometheus 0:422088ad7fc5 104 } else if (rand()%2 != 0 && state != drehen) {
ZHAW_Prometheus 0:422088ad7fc5 105 pwmLeft=0.6;
ZHAW_Prometheus 0:422088ad7fc5 106 pwmRight=0.6;
ZHAW_Prometheus 0:422088ad7fc5 107 }
ZHAW_Prometheus 0:422088ad7fc5 108 state=drehen;
ZHAW_Prometheus 0:422088ad7fc5 109 }
ZHAW_Prometheus 0:422088ad7fc5 110 //Wenn Front-Left etwas sehen => nach Rechts**************************
ZHAW_Prometheus 0:422088ad7fc5 111 else if(sensors[5]<=wenden) {
ZHAW_Prometheus 0:422088ad7fc5 112 printf("rechts\n");
ZHAW_Prometheus 0:422088ad7fc5 113 pwmLeft=0.65;
ZHAW_Prometheus 0:422088ad7fc5 114 pwmRight=0.45;
ZHAW_Prometheus 0:422088ad7fc5 115 state=rechts;
ZHAW_Prometheus 0:422088ad7fc5 116 }
ZHAW_Prometheus 0:422088ad7fc5 117
ZHAW_Prometheus 0:422088ad7fc5 118 // Wenn Front-Right etwas sehen => Links*******************************
ZHAW_Prometheus 0:422088ad7fc5 119 else if(sensors[1]<=wenden) {
ZHAW_Prometheus 0:422088ad7fc5 120 printf("Links\n");
ZHAW_Prometheus 0:422088ad7fc5 121 pwmLeft=0.55;
ZHAW_Prometheus 0:422088ad7fc5 122 pwmRight=0.35;
ZHAW_Prometheus 0:422088ad7fc5 123 state=links;
ZHAW_Prometheus 0:422088ad7fc5 124 }
ZHAW_Prometheus 0:422088ad7fc5 125
ZHAW_Prometheus 0:422088ad7fc5 126 //Wenn kein Sensor anspricht => gerade aus*****************************
ZHAW_Prometheus 0:422088ad7fc5 127 else if(sensors[0]>=0.26) {
ZHAW_Prometheus 0:422088ad7fc5 128 printf("gerade\n");
ZHAW_Prometheus 0:422088ad7fc5 129 pwmLeft=0.65;
ZHAW_Prometheus 0:422088ad7fc5 130 pwmRight=0.35;
ZHAW_Prometheus 0:422088ad7fc5 131 state=gerade;
ZHAW_Prometheus 0:422088ad7fc5 132 }
ZHAW_Prometheus 0:422088ad7fc5 133 }