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@1:4f0d5546aa02, 2017-05-05 (annotated)

Committer:
schuema4
Date:
Fri May 05 11:48:16 2017 +0000
Revision:
1:4f0d5546aa02
Parent:
0:7107985673dc
Child:
2:035fc65548f6
Version 05.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 0:7107985673dc 30 enum robot_state {gerade=0,rechts,links,drehen,zurueck,zurueck_l,zurueck_r};
schuema4 0:7107985673dc 31 double wenden =0.26;
schuema4 1:4f0d5546aa02 32 double wand =0.07;
schuema4 0:7107985673dc 33
schuema4 0:7107985673dc 34
schuema4 0:7107985673dc 35 int main()
schuema4 0:7107985673dc 36 {
schuema4 1:4f0d5546aa02 37 //Motoren Bremsen
schuema4 1:4f0d5546aa02 38
schuema4 1:4f0d5546aa02 39 pwmLeft=0.5;
schuema4 1:4f0d5546aa02 40 pwmRight=0.5;
schuema4 0:7107985673dc 41
schuema4 0:7107985673dc 42
schuema4 0:7107985673dc 43 //Sensoren auslesen*********************************************************
schuema4 0:7107985673dc 44 for( int ii = 0; ii<6; ++ii) {
schuema4 0:7107985673dc 45 sensors[ii].init(&distance, &bit0, &bit1, &bit2, ii);
schuema4 0:7107985673dc 46
schuema4 0:7107985673dc 47 enable = 1;
schuema4 0:7107985673dc 48
schuema4 0:7107985673dc 49
schuema4 0:7107985673dc 50 pwmLeft.period(0.00005);
schuema4 0:7107985673dc 51 pwmRight.period(0.00005);
schuema4 0:7107985673dc 52
schuema4 0:7107985673dc 53 enableMotorDriver =1;
schuema4 0:7107985673dc 54 }
schuema4 0:7107985673dc 55
schuema4 0:7107985673dc 56 int zustand=0; // variable to store the value read
schuema4 0:7107985673dc 57 float valInG = 0;
schuema4 0:7107985673dc 58 float valInR = 0;
schuema4 0:7107985673dc 59
schuema4 1:4f0d5546aa02 60
schuema4 0:7107985673dc 61 while( 1 ) {
schuema4 1:4f0d5546aa02 62
schuema4 0:7107985673dc 63
schuema4 0:7107985673dc 64
schuema4 0:7107985673dc 65
schuema4 0:7107985673dc 66 //Farbsensor auslesen***************************************************
schuema4 0:7107985673dc 67
schuema4 0:7107985673dc 68
schuema4 1:4f0d5546aa02 69
schuema4 0:7107985673dc 70
schuema4 0:7107985673dc 71 valInG = Sensor1*100.0;
schuema4 0:7107985673dc 72 valInR = Sensor2*100.0;
schuema4 0:7107985673dc 73
schuema4 0:7107985673dc 74 if ((valInG >= 596 && valInG <= 665) && (valInR >=370 && valInR <= 470)) {
schuema4 0:7107985673dc 75 //roter Klotz
schuema4 0:7107985673dc 76 zustand = 1;
schuema4 1:4f0d5546aa02 77 Servo1 =0.8f; //Grenze 0..7.4
schuema4 0:7107985673dc 78 } else {
schuema4 0:7107985673dc 79 if ((valInG >= 370 && valInG <= 380) && (valInR >=400 && valInR <= 610)) {
schuema4 0:7107985673dc 80 //gruener Klotz
schuema4 0:7107985673dc 81 zustand = 2;
schuema4 1:4f0d5546aa02 82 Servo1 =0.3f; //Grenze 0..7.4
schuema4 0:7107985673dc 83 } else {
schuema4 0:7107985673dc 84 //kein Klotz oder falscher Klotz
schuema4 1:4f0d5546aa02 85 Servo1 =0.8f; //Grenze 0..7.4
schuema4 0:7107985673dc 86 zustand = 0;
schuema4 0:7107985673dc 87 }
schuema4 0:7107985673dc 88 }
schuema4 0:7107985673dc 89
schuema4 0:7107985673dc 90 //State setzen****************************************************************
schuema4 0:7107985673dc 91 switch (robot_state) {
schuema4 0:7107985673dc 92
schuema4 0:7107985673dc 93 case gerade:
schuema4 0:7107985673dc 94 led0=1;
schuema4 0:7107985673dc 95 led1=0;
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 rechts:
schuema4 0:7107985673dc 101 led0=0;
schuema4 0:7107985673dc 102 led1=1;
schuema4 0:7107985673dc 103 led3=0;
schuema4 0:7107985673dc 104 led5=0;
schuema4 0:7107985673dc 105 break;
schuema4 0:7107985673dc 106
schuema4 0:7107985673dc 107 case links:
schuema4 0:7107985673dc 108 led0=0;
schuema4 0:7107985673dc 109 led1=0;
schuema4 0:7107985673dc 110 led3=0;
schuema4 0:7107985673dc 111 led5=1;
schuema4 0:7107985673dc 112 break;
schuema4 0:7107985673dc 113
schuema4 0:7107985673dc 114 case drehen:
schuema4 0:7107985673dc 115 led0=1;
schuema4 0:7107985673dc 116 led1=1;
schuema4 0:7107985673dc 117 led3=0;
schuema4 0:7107985673dc 118 led5=0;
schuema4 0:7107985673dc 119 break;
schuema4 0:7107985673dc 120
schuema4 0:7107985673dc 121 case zurueck:
schuema4 0:7107985673dc 122 led0=0;
schuema4 0:7107985673dc 123 led1=0;
schuema4 0:7107985673dc 124 led3=1;
schuema4 0:7107985673dc 125 led5=0;
schuema4 0:7107985673dc 126 }
schuema4 0:7107985673dc 127 //Sensorden abfrgagen und Motoren stellen***************************************
schuema4 0:7107985673dc 128
schuema4 1:4f0d5546aa02 129
schuema4 0:7107985673dc 130 // Wenn Front + Front-Right + Front-Left etwas sehen => gerade zurück bis nur noch einer etwas sieht
schuema4 0:7107985673dc 131 if(sensors[0]<=0.1 && sensors[1]<=0.12 && sensors[5]<=0.12) {
schuema4 1:4f0d5546aa02 132
schuema4 0:7107985673dc 133
schuema4 0:7107985673dc 134 //wenn hinten rechts => leichte linkskurve
schuema4 0:7107985673dc 135 if(sensors[2]<=0.25) {
schuema4 0:7107985673dc 136 pwmLeft=0.35;
schuema4 0:7107985673dc 137 pwmRight=0.55;
schuema4 0:7107985673dc 138 printf("zurueck-linkskurve\n");
schuema4 0:7107985673dc 139 robot_state=zurueck_l;
schuema4 0:7107985673dc 140 }
schuema4 0:7107985673dc 141
schuema4 0:7107985673dc 142 if(sensors[4]<=0.25) {
schuema4 0:7107985673dc 143 pwmLeft=0.35;
schuema4 0:7107985673dc 144 pwmRight=0.55;
schuema4 0:7107985673dc 145 printf("zurueck-rechtskurve\n");
schuema4 0:7107985673dc 146 robot_state=zurueck_r;
schuema4 0:7107985673dc 147 }
schuema4 0:7107985673dc 148 if(sensors[4]>=0.25 && sensors[2]>=0.25) {
schuema4 0:7107985673dc 149 pwmLeft=0.4;
schuema4 0:7107985673dc 150 pwmRight=0.6;
schuema4 0:7107985673dc 151 robot_state=zurueck;
schuema4 0:7107985673dc 152 printf("zurueck-gerade\n");
schuema4 0:7107985673dc 153 }
schuema4 0:7107985673dc 154 }
schuema4 1:4f0d5546aa02 155
schuema4 0:7107985673dc 156 // Wenn Front etwas sehen => drehen
schuema4 1:4f0d5546aa02 157
schuema4 1:4f0d5546aa02 158 else if(sensors[1] <= wand || sensors[5]<=wand){
schuema4 1:4f0d5546aa02 159 pwmLeft=0.35;
schuema4 1:4f0d5546aa02 160 pwmRight=0.65;
schuema4 1:4f0d5546aa02 161 }
schuema4 1:4f0d5546aa02 162
schuema4 1:4f0d5546aa02 163
schuema4 1:4f0d5546aa02 164
schuema4 1:4f0d5546aa02 165 else if(sensors[0]<0.25 && sensors [1]<=wenden) {
schuema4 1:4f0d5546aa02 166 pwmLeft=0.4;
schuema4 1:4f0d5546aa02 167 pwmRight=0.4;
schuema4 1:4f0d5546aa02 168 printf("drehen links\n\n\n");
schuema4 1:4f0d5546aa02 169 }
schuema4 1:4f0d5546aa02 170
schuema4 1:4f0d5546aa02 171 else if (sensors[0]<0.25 && sensors [5]<=wenden){
schuema4 1:4f0d5546aa02 172 pwmLeft=0.6;
schuema4 1:4f0d5546aa02 173 pwmRight=0.6;
schuema4 1:4f0d5546aa02 174 printf("drehen\n\n\n");
schuema4 1:4f0d5546aa02 175 }
schuema4 0:7107985673dc 176 else if(sensors[0]<0.25) {
schuema4 1:4f0d5546aa02 177 if (rand()%2==0 && robot_state != drehen) {
schuema4 0:7107985673dc 178 pwmLeft=0.4;
schuema4 0:7107985673dc 179 pwmRight=0.4;
schuema4 1:4f0d5546aa02 180 } else if (rand()%2 != 0 && robot_state != drehen) {
schuema4 0:7107985673dc 181 pwmLeft=0.6;
schuema4 0:7107985673dc 182 pwmRight=0.6;
schuema4 0:7107985673dc 183 }
schuema4 1:4f0d5546aa02 184 robot_state=drehen;
schuema4 0:7107985673dc 185 }
schuema4 0:7107985673dc 186
schuema4 0:7107985673dc 187
schuema4 0:7107985673dc 188 //Wenn Front-Left etwas sehen => nach Rechts
schuema4 0:7107985673dc 189 else if(sensors[5]<=wenden) {
schuema4 0:7107985673dc 190 printf("rechts\n");
schuema4 0:7107985673dc 191 pwmLeft=0.65;
schuema4 0:7107985673dc 192 pwmRight=0.45;
schuema4 0:7107985673dc 193 robot_state=rechts;
schuema4 0:7107985673dc 194 }
schuema4 0:7107985673dc 195
schuema4 0:7107985673dc 196 // Wenn Front-Right etwas sehen => Links
schuema4 0:7107985673dc 197 else if(sensors[1]<=wenden) {
schuema4 0:7107985673dc 198 printf("Links\n");
schuema4 0:7107985673dc 199 pwmLeft=0.55;
schuema4 0:7107985673dc 200 pwmRight=0.35;
schuema4 0:7107985673dc 201 robot_state=links;
schuema4 0:7107985673dc 202 }
schuema4 0:7107985673dc 203
schuema4 0:7107985673dc 204 //Wenn kein Sensor anspricht => gerade aus
schuema4 0:7107985673dc 205 else if(sensors[0]>=0.26) {
schuema4 0:7107985673dc 206 printf("gerade\n");
schuema4 0:7107985673dc 207 pwmLeft=0.65;
schuema4 0:7107985673dc 208 pwmRight=0.35;
schuema4 0:7107985673dc 209 robot_state=gerade;
schuema4 0:7107985673dc 210 }
schuema4 0:7107985673dc 211
schuema4 0:7107985673dc 212 //printf("sensorG: %f\t", valInG);
schuema4 0:7107985673dc 213 //printf("sensorR: %f\n\r", valInR);
schuema4 1:4f0d5546aa02 214 printf("\n\r");
schuema4 1:4f0d5546aa02 215 // printf("front:%f\n\r", sensors[0].read());
schuema4 1:4f0d5546aa02 216 //printf("right:%f\n\r", sensors[1].read());
schuema4 1:4f0d5546aa02 217 //printf("left:%f\n\r", sensors[5].read());
schuema4 1:4f0d5546aa02 218 printf("Gruen:%f\n\r", valInG);
schuema4 1:4f0d5546aa02 219 printf("Rot:%f\n\r", valInR);
schuema4 0:7107985673dc 220
schuema4 1:4f0d5546aa02 221 wait( 0.01);
schuema4 0:7107985673dc 222
schuema4 0:7107985673dc 223 }
schuema4 0:7107985673dc 224
schuema4 0:7107985673dc 225 }