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@3:5388a3ec580a, 2017-05-09 (annotated)

Committer:
schuema4
Date:
Tue May 09 12:51:23 2017 +0000
Revision:
3:5388a3ec580a
Parent:
2:035fc65548f6
Child:
4:52685928a7c3
version 09.05.17;

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 1:4f0d5546aa02 4 #include "Servo.h"
schuema4 0:7107985673dc 5
schuema4 0:7107985673dc 6 DigitalOut enable(PC_1);
schuema4 0:7107985673dc 7 DigitalOut bit0(PH_1);
schuema4 0:7107985673dc 8 DigitalOut bit1(PC_2);
schuema4 0:7107985673dc 9 DigitalOut bit2(PC_3);
schuema4 0:7107985673dc 10
schuema4 0:7107985673dc 11 DigitalOut enableMotorDriver(PB_2);
schuema4 0:7107985673dc 12 PwmOut pwmLeft(PA_8);
schuema4 0:7107985673dc 13 PwmOut pwmRight(PA_9);
schuema4 0:7107985673dc 14 IRSensor sensors[6];
schuema4 1:4f0d5546aa02 15 Servo Servo1(PB_7);
schuema4 0:7107985673dc 16
schuema4 0:7107985673dc 17 //LED's
schuema4 0:7107985673dc 18 DigitalOut led0(PC_8);
schuema4 0:7107985673dc 19 DigitalOut led1(PC_6);
schuema4 0:7107985673dc 20 DigitalOut led5(PC_9);
schuema4 0:7107985673dc 21 DigitalOut led3(PC_7);
schuema4 0:7107985673dc 22
schuema4 0:7107985673dc 23 // Analog In
schuema4 0:7107985673dc 24 AnalogIn distance(PB_1);
schuema4 0:7107985673dc 25 AnalogIn Sensor1(PA_0);
schuema4 0:7107985673dc 26 AnalogIn Sensor2(PA_1);
schuema4 0:7107985673dc 27
schuema4 0:7107985673dc 28 //Variablen definieren*********************************************************
schuema4 0:7107985673dc 29 int robot_state;
schuema4 2:035fc65548f6 30 enum robot_state {gerade=0,rechts,links,drehen,zurueck,zurueck_l,zurueck_r,Lego_R,Lego_G};
schuema4 2:035fc65548f6 31 int zustand =0; // variable to store the value Green
schuema4 2:035fc65548f6 32 enum zustand {rot=0, gruen};
schuema4 2:035fc65548f6 33 int merker_gruen = 0;
schuema4 2:035fc65548f6 34 int merker_gruen1 = 0;
schuema4 0:7107985673dc 35 double wenden =0.26;
schuema4 1:4f0d5546aa02 36 double wand =0.07;
schuema4 0:7107985673dc 37
schuema4 0:7107985673dc 38
schuema4 0:7107985673dc 39 int main()
schuema4 0:7107985673dc 40 {
schuema4 1:4f0d5546aa02 41 //Motoren Bremsen
schuema4 1:4f0d5546aa02 42
schuema4 1:4f0d5546aa02 43 pwmLeft=0.5;
schuema4 1:4f0d5546aa02 44 pwmRight=0.5;
schuema4 0:7107985673dc 45
schuema4 0:7107985673dc 46
schuema4 0:7107985673dc 47 //Sensoren auslesen*********************************************************
schuema4 0:7107985673dc 48 for( int ii = 0; ii<6; ++ii) {
schuema4 0:7107985673dc 49 sensors[ii].init(&distance, &bit0, &bit1, &bit2, ii);
schuema4 0:7107985673dc 50
schuema4 0:7107985673dc 51 enable = 1;
schuema4 0:7107985673dc 52
schuema4 0:7107985673dc 53
schuema4 0:7107985673dc 54 pwmLeft.period(0.00005);
schuema4 0:7107985673dc 55 pwmRight.period(0.00005);
schuema4 0:7107985673dc 56
schuema4 0:7107985673dc 57 enableMotorDriver =1;
schuema4 0:7107985673dc 58 }
schuema4 0:7107985673dc 59
schuema4 2:035fc65548f6 60
schuema4 0:7107985673dc 61 float valInG = 0;
schuema4 0:7107985673dc 62 float valInR = 0;
schuema4 0:7107985673dc 63
schuema4 1:4f0d5546aa02 64
schuema4 0:7107985673dc 65 while( 1 ) {
schuema4 1:4f0d5546aa02 66
schuema4 0:7107985673dc 67
schuema4 0:7107985673dc 68
schuema4 0:7107985673dc 69
schuema4 2:035fc65548f6 70 //Farbsensor auslesen + Auswertung***************************************************
schuema4 0:7107985673dc 71
schuema4 0:7107985673dc 72 valInG = Sensor1*100.0;
schuema4 0:7107985673dc 73 valInR = Sensor2*100.0;
schuema4 2:035fc65548f6 74
schuema4 3:5388a3ec580a 75
schuema4 2:035fc65548f6 76
schuema4 2:035fc65548f6 77
schuema4 2:035fc65548f6 78
schuema4 2:035fc65548f6 79
schuema4 0:7107985673dc 80
schuema4 0:7107985673dc 81
schuema4 0:7107985673dc 82 //State setzen****************************************************************
schuema4 0:7107985673dc 83 switch (robot_state) {
schuema4 0:7107985673dc 84
schuema4 2:035fc65548f6 85 /* case gerade:
schuema4 0:7107985673dc 86 led0=1;
schuema4 0:7107985673dc 87 led1=0;
schuema4 0:7107985673dc 88 led3=0;
schuema4 0:7107985673dc 89 led5=0;
schuema4 0:7107985673dc 90 break;
schuema4 0:7107985673dc 91
schuema4 0:7107985673dc 92 case rechts:
schuema4 0:7107985673dc 93 led0=0;
schuema4 0:7107985673dc 94 led1=1;
schuema4 0:7107985673dc 95 led3=0;
schuema4 0:7107985673dc 96 led5=0;
schuema4 0:7107985673dc 97 break;
schuema4 0:7107985673dc 98
schuema4 0:7107985673dc 99 case links:
schuema4 0:7107985673dc 100 led0=0;
schuema4 0:7107985673dc 101 led1=0;
schuema4 0:7107985673dc 102 led3=0;
schuema4 0:7107985673dc 103 led5=1;
schuema4 0:7107985673dc 104 break;
schuema4 0:7107985673dc 105
schuema4 0:7107985673dc 106 case drehen:
schuema4 0:7107985673dc 107 led0=1;
schuema4 0:7107985673dc 108 led1=1;
schuema4 0:7107985673dc 109 led3=0;
schuema4 0:7107985673dc 110 led5=0;
schuema4 0:7107985673dc 111 break;
schuema4 0:7107985673dc 112
schuema4 0:7107985673dc 113 case zurueck:
schuema4 0:7107985673dc 114 led0=0;
schuema4 0:7107985673dc 115 led1=0;
schuema4 0:7107985673dc 116 led3=1;
schuema4 0:7107985673dc 117 led5=0;
schuema4 2:035fc65548f6 118 */
schuema4 2:035fc65548f6 119 case rot:
schuema4 2:035fc65548f6 120 led0=1;
schuema4 2:035fc65548f6 121 led1=1;
schuema4 2:035fc65548f6 122 led3=1;
schuema4 2:035fc65548f6 123 led5=1;
schuema4 2:035fc65548f6 124 break;
schuema4 2:035fc65548f6 125
schuema4 2:035fc65548f6 126 case gruen:
schuema4 2:035fc65548f6 127 led0=1;
schuema4 2:035fc65548f6 128 led1=0;
schuema4 2:035fc65548f6 129 led3=1;
schuema4 2:035fc65548f6 130 led5=0;
schuema4 2:035fc65548f6 131 break;
schuema4 2:035fc65548f6 132
schuema4 0:7107985673dc 133 }
schuema4 2:035fc65548f6 134 //Sensoren abfrgagen und Motoren stellen***************************************
schuema4 0:7107985673dc 135
schuema4 1:4f0d5546aa02 136
schuema4 0:7107985673dc 137 // Wenn Front + Front-Right + Front-Left etwas sehen => gerade zurück bis nur noch einer etwas sieht
schuema4 0:7107985673dc 138 if(sensors[0]<=0.1 && sensors[1]<=0.12 && sensors[5]<=0.12) {
schuema4 1:4f0d5546aa02 139
schuema4 0:7107985673dc 140
schuema4 0:7107985673dc 141 //wenn hinten rechts => leichte linkskurve
schuema4 0:7107985673dc 142 if(sensors[2]<=0.25) {
schuema4 0:7107985673dc 143 pwmLeft=0.35;
schuema4 0:7107985673dc 144 pwmRight=0.55;
schuema4 0:7107985673dc 145 printf("zurueck-linkskurve\n");
schuema4 0:7107985673dc 146 robot_state=zurueck_l;
schuema4 0:7107985673dc 147 }
schuema4 0:7107985673dc 148
schuema4 0:7107985673dc 149 if(sensors[4]<=0.25) {
schuema4 0:7107985673dc 150 pwmLeft=0.35;
schuema4 0:7107985673dc 151 pwmRight=0.55;
schuema4 0:7107985673dc 152 printf("zurueck-rechtskurve\n");
schuema4 0:7107985673dc 153 robot_state=zurueck_r;
schuema4 0:7107985673dc 154 }
schuema4 0:7107985673dc 155 if(sensors[4]>=0.25 && sensors[2]>=0.25) {
schuema4 0:7107985673dc 156 pwmLeft=0.4;
schuema4 0:7107985673dc 157 pwmRight=0.6;
schuema4 0:7107985673dc 158 robot_state=zurueck;
schuema4 0:7107985673dc 159 printf("zurueck-gerade\n");
schuema4 0:7107985673dc 160 }
schuema4 0:7107985673dc 161 }
schuema4 1:4f0d5546aa02 162
schuema4 2:035fc65548f6 163 // Wenn Front etwas sehen => drehen***********************************
schuema4 1:4f0d5546aa02 164
schuema4 1:4f0d5546aa02 165 else if(sensors[1] <= wand || sensors[5]<=wand){
schuema4 1:4f0d5546aa02 166 pwmLeft=0.35;
schuema4 1:4f0d5546aa02 167 pwmRight=0.65;
schuema4 1:4f0d5546aa02 168 }
schuema4 1:4f0d5546aa02 169
schuema4 1:4f0d5546aa02 170
schuema4 1:4f0d5546aa02 171
schuema4 1:4f0d5546aa02 172 else if(sensors[0]<0.25 && sensors [1]<=wenden) {
schuema4 1:4f0d5546aa02 173 pwmLeft=0.4;
schuema4 1:4f0d5546aa02 174 pwmRight=0.4;
schuema4 1:4f0d5546aa02 175 printf("drehen links\n\n\n");
schuema4 1:4f0d5546aa02 176 }
schuema4 1:4f0d5546aa02 177
schuema4 1:4f0d5546aa02 178 else if (sensors[0]<0.25 && sensors [5]<=wenden){
schuema4 1:4f0d5546aa02 179 pwmLeft=0.6;
schuema4 1:4f0d5546aa02 180 pwmRight=0.6;
schuema4 1:4f0d5546aa02 181 printf("drehen\n\n\n");
schuema4 1:4f0d5546aa02 182 }
schuema4 0:7107985673dc 183 else if(sensors[0]<0.25) {
schuema4 1:4f0d5546aa02 184 if (rand()%2==0 && robot_state != drehen) {
schuema4 0:7107985673dc 185 pwmLeft=0.4;
schuema4 0:7107985673dc 186 pwmRight=0.4;
schuema4 1:4f0d5546aa02 187 } else if (rand()%2 != 0 && robot_state != drehen) {
schuema4 0:7107985673dc 188 pwmLeft=0.6;
schuema4 0:7107985673dc 189 pwmRight=0.6;
schuema4 0:7107985673dc 190 }
schuema4 1:4f0d5546aa02 191 robot_state=drehen;
schuema4 0:7107985673dc 192 }
schuema4 0:7107985673dc 193
schuema4 0:7107985673dc 194
schuema4 2:035fc65548f6 195 //Wenn Front-Left etwas sehen => nach Rechts**************************
schuema4 0:7107985673dc 196 else if(sensors[5]<=wenden) {
schuema4 0:7107985673dc 197 printf("rechts\n");
schuema4 0:7107985673dc 198 pwmLeft=0.65;
schuema4 0:7107985673dc 199 pwmRight=0.45;
schuema4 0:7107985673dc 200 robot_state=rechts;
schuema4 0:7107985673dc 201 }
schuema4 0:7107985673dc 202
schuema4 2:035fc65548f6 203 // Wenn Front-Right etwas sehen => Links*******************************
schuema4 0:7107985673dc 204 else if(sensors[1]<=wenden) {
schuema4 0:7107985673dc 205 printf("Links\n");
schuema4 0:7107985673dc 206 pwmLeft=0.55;
schuema4 0:7107985673dc 207 pwmRight=0.35;
schuema4 0:7107985673dc 208 robot_state=links;
schuema4 0:7107985673dc 209 }
schuema4 0:7107985673dc 210
schuema4 2:035fc65548f6 211 //Wenn kein Sensor anspricht => gerade aus*****************************
schuema4 0:7107985673dc 212 else if(sensors[0]>=0.26) {
schuema4 0:7107985673dc 213 printf("gerade\n");
schuema4 0:7107985673dc 214 pwmLeft=0.65;
schuema4 0:7107985673dc 215 pwmRight=0.35;
schuema4 0:7107985673dc 216 robot_state=gerade;
schuema4 0:7107985673dc 217 }
schuema4 2:035fc65548f6 218
schuema4 2:035fc65548f6 219 // Auswurf ansteuern*******************************************************
schuema4 3:5388a3ec580a 220 if (valInG>=70.5 && valInG<=77.0 && valInR>=52.5 && valInR<=54.5){ //Lego Normal
schuema4 3:5388a3ec580a 221 zustand = gruen;
schuema4 3:5388a3ec580a 222 }
schuema4 3:5388a3ec580a 223 else{
schuema4 3:5388a3ec580a 224 zustand = rot;
schuema4 3:5388a3ec580a 225 }
schuema4 3:5388a3ec580a 226 /*
schuema4 3:5388a3ec580a 227 else if (valInG>=77.8 && valInG<=79 && valInR>=72 && valInR<=74){ // Lego Aufrecht Boden
schuema4 3:5388a3ec580a 228 zustand = gruen;
schuema4 3:5388a3ec580a 229 }
schuema4 3:5388a3ec580a 230 else {
schuema4 3:5388a3ec580a 231 zustand = rot;
schuema4 3:5388a3ec580a 232 }
schuema4 3:5388a3ec580a 233 else if(valInG>=76 && valInG<=77 && valInR>=64 && valInR<=68){ // Lego Aufrecht Deckel
schuema4 3:5388a3ec580a 234 zustand = gruen;
schuema4 3:5388a3ec580a 235 }
schuema4 3:5388a3ec580a 236 else{
schuema4 3:5388a3ec580a 237 zustand = rot;
schuema4 3:5388a3ec580a 238 }
schuema4 3:5388a3ec580a 239 */
schuema4 3:5388a3ec580a 240 /*
schuema4 2:035fc65548f6 241 if(zustand = gruen || (merker_gruen >=1 && merker_gruen < 300)){ //Einschaltverzögerung
schuema4 2:035fc65548f6 242 merker_gruen ++;
schuema4 2:035fc65548f6 243 robot_state=Lego_G;
schuema4 2:035fc65548f6 244 }
schuema4 2:035fc65548f6 245
schuema4 2:035fc65548f6 246 else if(merker_gruen == 300 || (merker_gruen1 >=1 && merker_gruen1 < 300)){ // Ausschaltverzögerung
schuema4 2:035fc65548f6 247 merker_gruen =0;
schuema4 2:035fc65548f6 248 merker_gruen1 ++;
schuema4 2:035fc65548f6 249 Servo1 = 1.1f;
schuema4 2:035fc65548f6 250 robot_state=Lego_G;
schuema4 2:035fc65548f6 251 }
schuema4 2:035fc65548f6 252
schuema4 2:035fc65548f6 253 else if(zustand = rot && merker_gruen == 0 && merker_gruen1 ==0){
schuema4 2:035fc65548f6 254 Servo1 =0.1f; //Grenze 0..7.4
schuema4 2:035fc65548f6 255 robot_state=Lego_R;
schuema4 2:035fc65548f6 256 }
schuema4 3:5388a3ec580a 257 */
schuema4 3:5388a3ec580a 258 if(zustand == gruen){
schuema4 3:5388a3ec580a 259 Servo1 = 1.1f;
schuema4 3:5388a3ec580a 260 robot_state=Lego_G;
schuema4 3:5388a3ec580a 261 }
schuema4 3:5388a3ec580a 262 else{
schuema4 3:5388a3ec580a 263 Servo1 = 0.1f;
schuema4 3:5388a3ec580a 264 robot_state=Lego_R;
schuema4 3:5388a3ec580a 265 }
schuema4 2:035fc65548f6 266
schuema4 2:035fc65548f6 267 //Ausgaben an Konsole******************************************************
schuema4 0:7107985673dc 268
schuema4 1:4f0d5546aa02 269 printf("\n\r");
schuema4 1:4f0d5546aa02 270 // printf("front:%f\n\r", sensors[0].read());
schuema4 1:4f0d5546aa02 271 //printf("right:%f\n\r", sensors[1].read());
schuema4 1:4f0d5546aa02 272 //printf("left:%f\n\r", sensors[5].read());
schuema4 1:4f0d5546aa02 273 printf("Gruen:%f\n\r", valInG);
schuema4 1:4f0d5546aa02 274 printf("Rot:%f\n\r", valInR);
schuema4 3:5388a3ec580a 275 printf("Status:%d",zustand);
schuema4 0:7107985673dc 276
schuema4 2:035fc65548f6 277 wait(0.001);
schuema4 0:7107985673dc 278
schuema4 0:7107985673dc 279 }
schuema4 0:7107985673dc 280
schuema4 0:7107985673dc 281 }