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@4:52685928a7c3, 2017-05-10 (annotated)

Committer:
schuema4
Date:
Wed May 10 11:37:42 2017 +0000
Revision:
4:52685928a7c3
Parent:
3:5388a3ec580a 
lift;

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