Prometheus / Mbed 2 deprecated Roebi

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed Servo

main.cpp@0:7107985673dc, 2017-04-25 (annotated)

Committer:
schuema4
Date:
Tue Apr 25 11:48:00 2017 +0000
Revision:
0:7107985673dc
Child:
1:4f0d5546aa02
test;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
schuema4 0:7107985673dc 1 #include "mbed.h"
schuema4 0:7107985673dc 2 #include "Sensor_auslesen.h"
schuema4 0:7107985673dc 3 #include "cstdlib"
schuema4 0:7107985673dc 4
schuema4 0:7107985673dc 5 DigitalOut enable(PC_1);
schuema4 0:7107985673dc 6 DigitalOut bit0(PH_1);
schuema4 0:7107985673dc 7 DigitalOut bit1(PC_2);
schuema4 0:7107985673dc 8 DigitalOut bit2(PC_3);
schuema4 0:7107985673dc 9
schuema4 0:7107985673dc 10 DigitalOut enableMotorDriver(PB_2);
schuema4 0:7107985673dc 11 PwmOut pwmLeft(PA_8);
schuema4 0:7107985673dc 12 PwmOut pwmRight(PA_9);
schuema4 0:7107985673dc 13 IRSensor sensors[6];
schuema4 0:7107985673dc 14
schuema4 0:7107985673dc 15 //LED's
schuema4 0:7107985673dc 16 DigitalOut led0(PC_8);
schuema4 0:7107985673dc 17 DigitalOut led1(PC_6);
schuema4 0:7107985673dc 18 DigitalOut led5(PC_9);
schuema4 0:7107985673dc 19 DigitalOut led3(PC_7);
schuema4 0:7107985673dc 20
schuema4 0:7107985673dc 21 // Analog In
schuema4 0:7107985673dc 22 AnalogIn distance(PB_1);
schuema4 0:7107985673dc 23 AnalogIn Sensor1(PA_0);
schuema4 0:7107985673dc 24 AnalogIn Sensor2(PA_1);
schuema4 0:7107985673dc 25
schuema4 0:7107985673dc 26 //Variablen definieren*********************************************************
schuema4 0:7107985673dc 27 int robot_state;
schuema4 0:7107985673dc 28 enum robot_state {gerade=0,rechts,links,drehen,zurueck,zurueck_l,zurueck_r};
schuema4 0:7107985673dc 29 double wenden =0.26;
schuema4 0:7107985673dc 30
schuema4 0:7107985673dc 31
schuema4 0:7107985673dc 32 int main()
schuema4 0:7107985673dc 33 {
schuema4 0:7107985673dc 34
schuema4 0:7107985673dc 35
schuema4 0:7107985673dc 36 //Sensoren auslesen*********************************************************
schuema4 0:7107985673dc 37 for( int ii = 0; ii<6; ++ii) {
schuema4 0:7107985673dc 38 sensors[ii].init(&distance, &bit0, &bit1, &bit2, ii);
schuema4 0:7107985673dc 39
schuema4 0:7107985673dc 40 enable = 1;
schuema4 0:7107985673dc 41
schuema4 0:7107985673dc 42
schuema4 0:7107985673dc 43 pwmLeft.period(0.00005);
schuema4 0:7107985673dc 44 pwmRight.period(0.00005);
schuema4 0:7107985673dc 45
schuema4 0:7107985673dc 46 enableMotorDriver =1;
schuema4 0:7107985673dc 47 }
schuema4 0:7107985673dc 48
schuema4 0:7107985673dc 49 int zustand=0; // variable to store the value read
schuema4 0:7107985673dc 50 float valInG = 0;
schuema4 0:7107985673dc 51 float valInR = 0;
schuema4 0:7107985673dc 52
schuema4 0:7107985673dc 53 while( 1 ) {
schuema4 0:7107985673dc 54
schuema4 0:7107985673dc 55
schuema4 0:7107985673dc 56
schuema4 0:7107985673dc 57 //Farbsensor auslesen***************************************************
schuema4 0:7107985673dc 58
schuema4 0:7107985673dc 59
schuema4 0:7107985673dc 60 // int range = 10000;
schuema4 0:7107985673dc 61
schuema4 0:7107985673dc 62 valInG = Sensor1*100.0;
schuema4 0:7107985673dc 63 valInR = Sensor2*100.0;
schuema4 0:7107985673dc 64
schuema4 0:7107985673dc 65 if ((valInG >= 596 && valInG <= 665) && (valInR >=370 && valInR <= 470)) {
schuema4 0:7107985673dc 66 //roter Klotz
schuema4 0:7107985673dc 67 zustand = 1;
schuema4 0:7107985673dc 68 } else {
schuema4 0:7107985673dc 69 if ((valInG >= 370 && valInG <= 380) && (valInR >=400 && valInR <= 610)) {
schuema4 0:7107985673dc 70 //gruener Klotz
schuema4 0:7107985673dc 71 zustand = 2;
schuema4 0:7107985673dc 72 } else {
schuema4 0:7107985673dc 73 //kein Klotz oder falscher Klotz
schuema4 0:7107985673dc 74 zustand = 0;
schuema4 0:7107985673dc 75 }
schuema4 0:7107985673dc 76 }
schuema4 0:7107985673dc 77
schuema4 0:7107985673dc 78
schuema4 0:7107985673dc 79
schuema4 0:7107985673dc 80
schuema4 0:7107985673dc 81 //State setzen****************************************************************
schuema4 0:7107985673dc 82 switch (robot_state) {
schuema4 0:7107985673dc 83
schuema4 0:7107985673dc 84 case gerade:
schuema4 0:7107985673dc 85 led0=1;
schuema4 0:7107985673dc 86 led1=0;
schuema4 0:7107985673dc 87 led3=0;
schuema4 0:7107985673dc 88 led5=0;
schuema4 0:7107985673dc 89 break;
schuema4 0:7107985673dc 90
schuema4 0:7107985673dc 91 case rechts:
schuema4 0:7107985673dc 92 led0=0;
schuema4 0:7107985673dc 93 led1=1;
schuema4 0:7107985673dc 94 led3=0;
schuema4 0:7107985673dc 95 led5=0;
schuema4 0:7107985673dc 96 break;
schuema4 0:7107985673dc 97
schuema4 0:7107985673dc 98 case links:
schuema4 0:7107985673dc 99 led0=0;
schuema4 0:7107985673dc 100 led1=0;
schuema4 0:7107985673dc 101 led3=0;
schuema4 0:7107985673dc 102 led5=1;
schuema4 0:7107985673dc 103 break;
schuema4 0:7107985673dc 104
schuema4 0:7107985673dc 105 case drehen:
schuema4 0:7107985673dc 106 led0=1;
schuema4 0:7107985673dc 107 led1=1;
schuema4 0:7107985673dc 108 led3=0;
schuema4 0:7107985673dc 109 led5=0;
schuema4 0:7107985673dc 110 break;
schuema4 0:7107985673dc 111
schuema4 0:7107985673dc 112 case zurueck:
schuema4 0:7107985673dc 113 led0=0;
schuema4 0:7107985673dc 114 led1=0;
schuema4 0:7107985673dc 115 led3=1;
schuema4 0:7107985673dc 116 led5=0;
schuema4 0:7107985673dc 117 }
schuema4 0:7107985673dc 118 //Sensorden abfrgagen und Motoren stellen***************************************
schuema4 0:7107985673dc 119
schuema4 0:7107985673dc 120 // Wenn Front + Front-Right + Front-Left etwas sehen => gerade zurück bis nur noch einer etwas sieht
schuema4 0:7107985673dc 121 if(sensors[0]<=0.1 && sensors[1]<=0.12 && sensors[5]<=0.12) {
schuema4 0:7107985673dc 122
schuema4 0:7107985673dc 123 //wenn hinten rechts => leichte linkskurve
schuema4 0:7107985673dc 124 if(sensors[2]<=0.25) {
schuema4 0:7107985673dc 125 pwmLeft=0.35;
schuema4 0:7107985673dc 126 pwmRight=0.55;
schuema4 0:7107985673dc 127 printf("zurueck-linkskurve\n");
schuema4 0:7107985673dc 128 robot_state=zurueck_l;
schuema4 0:7107985673dc 129 }
schuema4 0:7107985673dc 130
schuema4 0:7107985673dc 131 if(sensors[4]<=0.25) {
schuema4 0:7107985673dc 132 pwmLeft=0.35;
schuema4 0:7107985673dc 133 pwmRight=0.55;
schuema4 0:7107985673dc 134 printf("zurueck-rechtskurve\n");
schuema4 0:7107985673dc 135 robot_state=zurueck_r;
schuema4 0:7107985673dc 136 }
schuema4 0:7107985673dc 137 if(sensors[4]>=0.25 && sensors[2]>=0.25) {
schuema4 0:7107985673dc 138 pwmLeft=0.4;
schuema4 0:7107985673dc 139 pwmRight=0.6;
schuema4 0:7107985673dc 140 robot_state=zurueck;
schuema4 0:7107985673dc 141 printf("zurueck-gerade\n");
schuema4 0:7107985673dc 142 }
schuema4 0:7107985673dc 143 }
schuema4 0:7107985673dc 144
schuema4 0:7107985673dc 145 // Wenn Front etwas sehen => drehen
schuema4 0:7107985673dc 146 else if(sensors[0]<0.25) {
schuema4 0:7107985673dc 147
schuema4 0:7107985673dc 148 printf("drehen\n\n\n");
schuema4 0:7107985673dc 149 if (rand()%2==0 && robot_state != drehen && sensors[1]>=wenden) {
schuema4 0:7107985673dc 150 pwmLeft=0.4;
schuema4 0:7107985673dc 151 pwmRight=0.4;
schuema4 0:7107985673dc 152 } else if (rand()%2 != 0 && robot_state != drehen && sensors[5]>=wenden) {
schuema4 0:7107985673dc 153 pwmLeft=0.6;
schuema4 0:7107985673dc 154 pwmRight=0.6;
schuema4 0:7107985673dc 155 }
schuema4 0:7107985673dc 156 robot_state=drehen;
schuema4 0:7107985673dc 157 }
schuema4 0:7107985673dc 158
schuema4 0:7107985673dc 159
schuema4 0:7107985673dc 160 //Wenn Front-Left etwas sehen => nach Rechts
schuema4 0:7107985673dc 161 else if(sensors[5]<=wenden) {
schuema4 0:7107985673dc 162 printf("rechts\n");
schuema4 0:7107985673dc 163 pwmLeft=0.65;
schuema4 0:7107985673dc 164 pwmRight=0.45;
schuema4 0:7107985673dc 165 robot_state=rechts;
schuema4 0:7107985673dc 166 }
schuema4 0:7107985673dc 167
schuema4 0:7107985673dc 168 // Wenn Front-Right etwas sehen => Links
schuema4 0:7107985673dc 169 else if(sensors[1]<=wenden) {
schuema4 0:7107985673dc 170 printf("Links\n");
schuema4 0:7107985673dc 171 pwmLeft=0.55;
schuema4 0:7107985673dc 172 pwmRight=0.35;
schuema4 0:7107985673dc 173 robot_state=links;
schuema4 0:7107985673dc 174 }
schuema4 0:7107985673dc 175
schuema4 0:7107985673dc 176 //Wenn kein Sensor anspricht => gerade aus
schuema4 0:7107985673dc 177 else if(sensors[0]>=0.26) {
schuema4 0:7107985673dc 178 printf("gerade\n");
schuema4 0:7107985673dc 179 pwmLeft=0.65;
schuema4 0:7107985673dc 180 pwmRight=0.35;
schuema4 0:7107985673dc 181 robot_state=gerade;
schuema4 0:7107985673dc 182 }
schuema4 0:7107985673dc 183
schuema4 0:7107985673dc 184 //printf("sensorG: %f\t", valInG);
schuema4 0:7107985673dc 185 //printf("sensorR: %f\n\r", valInR);
schuema4 0:7107985673dc 186 printf("front:%f\n\r", sensors[0].read());
schuema4 0:7107985673dc 187 printf("right:%f\n\r", sensors[1].read());
schuema4 0:7107985673dc 188 printf("left:%f\n\r", sensors[5].read());
schuema4 0:7107985673dc 189
schuema4 0:7107985673dc 190 wait( 0.001);
schuema4 0:7107985673dc 191
schuema4 0:7107985673dc 192 }
schuema4 0:7107985673dc 193
schuema4 0:7107985673dc 194 }