zhaw_st16b_pes2_10 / Mbed 2 deprecated PES_Official-US

a

Dependencies:   Servo ServoArm mbed

Fork of PES_Official-TestF by zhaw_st16b_pes2_10

Sources/main.cpp@16:f157e5ccd7d3, 2017-05-15 (annotated)

Committer:
beacon
Date:
Mon May 15 17:35:24 2017 +0000
Revision:
16:f157e5ccd7d3
Parent:
15:915f8839fe48 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
beacon 0:d267b248eff4 1 #include "mbed.h"
beacon 0:d267b248eff4 2 #include "Robot.h"
beacon 4:67d7177c213f 3 #include "Declarations.h"
beacon 0:d267b248eff4 4
beacon 0:d267b248eff4 5 #include <cstdlib>
beacon 0:d267b248eff4 6
beacon 4:67d7177c213f 7 //DistanceSensors related bottom:
beacon 0:d267b248eff4 8 Serial pc(SERIAL_TX, SERIAL_RX);
beacon 0:d267b248eff4 9
beacon 0:d267b248eff4 10 AnalogIn sensorVoltage(PB_1);
beacon 0:d267b248eff4 11 DigitalOut enable(PC_1);
beacon 0:d267b248eff4 12 DigitalOut bit0(PH_1);
beacon 0:d267b248eff4 13 DigitalOut bit1(PC_2);
beacon 0:d267b248eff4 14 DigitalOut bit2(PC_3);
beacon 1:388c915756f5 15
beacon 4:67d7177c213f 16 //DistanceSensors top:
beacon 4:67d7177c213f 17 AnalogIn frontS(A1);
beacon 4:67d7177c213f 18 AnalogIn leftS(A2);
beacon 4:67d7177c213f 19 AnalogIn rightS(A3);
beacon 4:67d7177c213f 20
beacon 1:388c915756f5 21 //Leds related:
beacon 1:388c915756f5 22 DigitalOut leds[6] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };
beacon 0:d267b248eff4 23
beacon 1:388c915756f5 24 //motor related:
beacon 1:388c915756f5 25 PwmOut left(PA_8);
beacon 1:388c915756f5 26 PwmOut right(PA_9);
beacon 0:d267b248eff4 27
beacon 1:388c915756f5 28 DigitalOut powerSignal(PB_2);
beacon 10:f76476943a6c 29 DigitalIn errorMotor(PB_14);
beacon 1:388c915756f5 30 DigitalIn overtemperatur(PB_15);
beacon 1:388c915756f5 31
beacon 4:67d7177c213f 32 //Arm:
beacon 10:f76476943a6c 33 ServoArm servoArm(PA_6);
beacon 10:f76476943a6c 34
beacon 10:f76476943a6c 35 //Greifer:
beacon 12:c0bcb95885dd 36 Servo servoGreifer(PC_7);
beacon 4:67d7177c213f 37
EpicG10 14:b61fbd13a7c9 38 //Leiste:
EpicG10 15:915f8839fe48 39 Servo servoLeiste(PB_6);
EpicG10 14:b61fbd13a7c9 40
beacon 1:388c915756f5 41 //Farbsensor:
beacon 1:388c915756f5 42 AnalogIn FarbVoltage(A0);
beacon 12:c0bcb95885dd 43 DigitalOut led(D2);
beacon 1:388c915756f5 44
EpicG10 15:915f8839fe48 45 Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer, &servoLeiste ); //Implement the Farbsensor into the Robot init function!!
beacon 10:f76476943a6c 46
beacon 11:292bdbd85a9c 47 void initializeDistanceSensors()
beacon 11:292bdbd85a9c 48 {
beacon 10:f76476943a6c 49 for( int ii = 0; ii<9; ++ii) {
beacon 10:f76476943a6c 50 sam.sensors[ii].init(&sensorVoltage, &frontS, &leftS, &rightS, &bit0, &bit1, &bit2, ii);
beacon 10:f76476943a6c 51
beacon 11:292bdbd85a9c 52 enable = 1;
beacon 10:f76476943a6c 53 }
beacon 10:f76476943a6c 54 }
beacon 1:388c915756f5 55
beacon 0:d267b248eff4 56
beacon 10:f76476943a6c 57 /* */
beacon 11:292bdbd85a9c 58 int main()
beacon 11:292bdbd85a9c 59 {
beacon 4:67d7177c213f 60 initializeDistanceSensors(); //Initialises IR-Sensors.
beacon 11:292bdbd85a9c 61 //int counter = 0; //Counts how many times the robot has turned, before driving
beacon 4:67d7177c213f 62 int timer = 0; //Is used, to reset the robot. Returns time in [0.1s]
beacon 4:67d7177c213f 63 //int lastFun = -1; //Is used, to check if the same action in Robot::search() was made multiple times.
beacon 11:292bdbd85a9c 64 //int found = 0; //0:= no block available, 1 := a lego is ready to be picked up
beacon 11:292bdbd85a9c 65 //int done = 0;
beacon 16:f157e5ccd7d3 66 int color;
beacon 16:f157e5ccd7d3 67
beacon 16:f157e5ccd7d3 68 enum states { search = 0, LeisteDown, push, backOff, forward, downward, down, upward, colorS, readyDrop, backward, LeisteUp };
beacon 11:292bdbd85a9c 69
EpicG10 13:6c1f8e35b242 70 int state = search;
beacon 16:f157e5ccd7d3 71
beacon 11:292bdbd85a9c 72 while( 1 ) {
beacon 10:f76476943a6c 73
beacon 11:292bdbd85a9c 74 if ( timer > TIMEOUT ) {
beacon 4:67d7177c213f 75 NVIC_SystemReset(); //Resets Sam.
beacon 4:67d7177c213f 76 }
beacon 11:292bdbd85a9c 77
beacon 11:292bdbd85a9c 78 switch( state ) {
beacon 11:292bdbd85a9c 79 case search:
beacon 16:f157e5ccd7d3 80 if( sam.search(&timer) ){
beacon 16:f157e5ccd7d3 81 //sam.Greifer.nullPos();
beacon 16:f157e5ccd7d3 82 state = LeisteDown;
beacon 16:f157e5ccd7d3 83 timer = 0;
EpicG10 15:915f8839fe48 84 }
EpicG10 15:915f8839fe48 85 break;
EpicG10 15:915f8839fe48 86
EpicG10 15:915f8839fe48 87 case LeisteDown:
beacon 16:f157e5ccd7d3 88 if( sam.Leiste.upToDown() ){
beacon 16:f157e5ccd7d3 89 //sam.Greifer.leave();
beacon 16:f157e5ccd7d3 90 state = push;
beacon 16:f157e5ccd7d3 91 timer = 0;
beacon 16:f157e5ccd7d3 92 }
beacon 16:f157e5ccd7d3 93 break;
beacon 16:f157e5ccd7d3 94
beacon 16:f157e5ccd7d3 95 case push:{
beacon 16:f157e5ccd7d3 96 static int i = 0;
beacon 16:f157e5ccd7d3 97 if( i > 5 ){
beacon 16:f157e5ccd7d3 98 sam.stop();
beacon 16:f157e5ccd7d3 99 i = 0;
beacon 16:f157e5ccd7d3 100 state = backOff;
beacon 16:f157e5ccd7d3 101 timer = 0;
beacon 16:f157e5ccd7d3 102 }
beacon 16:f157e5ccd7d3 103 else{
beacon 16:f157e5ccd7d3 104 sam.driveSlowly();
beacon 16:f157e5ccd7d3 105 i++;
beacon 11:292bdbd85a9c 106 }
beacon 16:f157e5ccd7d3 107 break;
beacon 16:f157e5ccd7d3 108 }
beacon 16:f157e5ccd7d3 109
beacon 16:f157e5ccd7d3 110 case backOff:{
beacon 16:f157e5ccd7d3 111 static int i = 0;
beacon 16:f157e5ccd7d3 112 if( i > 5 ){
beacon 16:f157e5ccd7d3 113 sam.stop();
beacon 16:f157e5ccd7d3 114 i = 0;
beacon 16:f157e5ccd7d3 115 state = forward;
beacon 16:f157e5ccd7d3 116 timer = 0;
beacon 16:f157e5ccd7d3 117 }
beacon 16:f157e5ccd7d3 118 else{
beacon 16:f157e5ccd7d3 119 sam.driveBackSlowly();
beacon 16:f157e5ccd7d3 120 i++;
beacon 16:f157e5ccd7d3 121 }
beacon 16:f157e5ccd7d3 122 break;
beacon 16:f157e5ccd7d3 123 }
beacon 11:292bdbd85a9c 124
beacon 11:292bdbd85a9c 125
beacon 11:292bdbd85a9c 126 case forward:
beacon 11:292bdbd85a9c 127 if( sam.Arm.backToCollect() ){
beacon 11:292bdbd85a9c 128 state = downward;
beacon 11:292bdbd85a9c 129 timer = 0;
beacon 11:292bdbd85a9c 130 }
beacon 11:292bdbd85a9c 131 break;
beacon 11:292bdbd85a9c 132
beacon 11:292bdbd85a9c 133 case downward:
beacon 11:292bdbd85a9c 134 if( sam.Arm.collectToDown() ){
beacon 11:292bdbd85a9c 135 state = down;
beacon 11:292bdbd85a9c 136 timer = 0;
beacon 11:292bdbd85a9c 137 }
beacon 11:292bdbd85a9c 138 break;
beacon 11:292bdbd85a9c 139
beacon 11:292bdbd85a9c 140 case down:
beacon 11:292bdbd85a9c 141 if( sam.Greifer.take() ) {
beacon 11:292bdbd85a9c 142 state = upward;
beacon 11:292bdbd85a9c 143 timer = 0;
beacon 11:292bdbd85a9c 144 }
beacon 11:292bdbd85a9c 145 break;
beacon 11:292bdbd85a9c 146
beacon 11:292bdbd85a9c 147 case upward:
beacon 11:292bdbd85a9c 148 if( sam.Arm.downToCollect() ){
beacon 16:f157e5ccd7d3 149 state = colorS;
beacon 11:292bdbd85a9c 150 timer = 0;
beacon 11:292bdbd85a9c 151 }
beacon 11:292bdbd85a9c 152 break;
beacon 11:292bdbd85a9c 153
beacon 16:f157e5ccd7d3 154 case colorS:
beacon 12:c0bcb95885dd 155 led = 1;
beacon 16:f157e5ccd7d3 156 color = sam.FarbVoltage.read();
beacon 11:292bdbd85a9c 157
beacon 12:c0bcb95885dd 158 if( color == -1 ){
beacon 11:292bdbd85a9c 159 //Do nothing
beacon 11:292bdbd85a9c 160 }
beacon 11:292bdbd85a9c 161
beacon 12:c0bcb95885dd 162
beacon 11:292bdbd85a9c 163 else if( color == 0 || color == GREEN ){
beacon 16:f157e5ccd7d3 164 state = backward;
beacon 12:c0bcb95885dd 165 led = 0;
beacon 11:292bdbd85a9c 166 timer = 0;
beacon 11:292bdbd85a9c 167 }
beacon 11:292bdbd85a9c 168
beacon 11:292bdbd85a9c 169 else if( color == RED ){
beacon 11:292bdbd85a9c 170 state = readyDrop;
beacon 12:c0bcb95885dd 171 led = 0;
beacon 11:292bdbd85a9c 172 timer = 0;
beacon 11:292bdbd85a9c 173 }
beacon 11:292bdbd85a9c 174
beacon 11:292bdbd85a9c 175 else{
beacon 11:292bdbd85a9c 176 //Shit...
beacon 11:292bdbd85a9c 177 }
beacon 11:292bdbd85a9c 178 break;
beacon 16:f157e5ccd7d3 179
beacon 11:292bdbd85a9c 180 case readyDrop:
beacon 11:292bdbd85a9c 181 if( sam.Greifer.leave() ){
beacon 16:f157e5ccd7d3 182 if( color == GREEN || color == 0 ){
beacon 16:f157e5ccd7d3 183 state = LeisteUp;
beacon 16:f157e5ccd7d3 184 }
beacon 16:f157e5ccd7d3 185 else{
beacon 16:f157e5ccd7d3 186 state = backward;
beacon 16:f157e5ccd7d3 187 }
beacon 11:292bdbd85a9c 188 timer = 0;
beacon 11:292bdbd85a9c 189 }
beacon 11:292bdbd85a9c 190
beacon 11:292bdbd85a9c 191 break;
beacon 11:292bdbd85a9c 192
beacon 11:292bdbd85a9c 193 case backward:
beacon 11:292bdbd85a9c 194 if( sam.Arm.collectToBack() ){
beacon 16:f157e5ccd7d3 195 //sam.Greifer.nullPos();
EpicG10 15:915f8839fe48 196 state = LeisteUp;
beacon 11:292bdbd85a9c 197 timer = 0;
beacon 16:f157e5ccd7d3 198 if( color == GREEN || color == 0 ){
beacon 16:f157e5ccd7d3 199 state = readyDrop;
beacon 16:f157e5ccd7d3 200 sam.Greifer.leave();
beacon 16:f157e5ccd7d3 201 }
beacon 16:f157e5ccd7d3 202 else{
beacon 16:f157e5ccd7d3 203 state = LeisteUp;
beacon 16:f157e5ccd7d3 204 }
beacon 11:292bdbd85a9c 205 }
beacon 11:292bdbd85a9c 206 break;
EpicG10 15:915f8839fe48 207
EpicG10 15:915f8839fe48 208 case LeisteUp:
beacon 16:f157e5ccd7d3 209 if( sam.Leiste.downToUp() ){
EpicG10 15:915f8839fe48 210 state = search;
EpicG10 15:915f8839fe48 211 timer = 0;
EpicG10 15:915f8839fe48 212 }
EpicG10 15:915f8839fe48 213 break;
beacon 4:67d7177c213f 214 }
beacon 1:388c915756f5 215
beacon 16:f157e5ccd7d3 216 /*
beacon 16:f157e5ccd7d3 217 sam.leds[1] = sam.sensors[RIGHT] < NEAR;
beacon 16:f157e5ccd7d3 218 sam.leds[5] = sam.sensors[LEFT] < NEAR;
beacon 16:f157e5ccd7d3 219 sam.leds[4] = sam.sensors[FWD] < NEAR;
beacon 16:f157e5ccd7d3 220 */
beacon 11:292bdbd85a9c 221 timer++;
beacon 11:292bdbd85a9c 222 wait(0.1f);
beacon 11:292bdbd85a9c 223 }
beacon 11:292bdbd85a9c 224
beacon 11:292bdbd85a9c 225 return 0;
beacon 11:292bdbd85a9c 226 }
beacon 11:292bdbd85a9c 227
beacon 11:292bdbd85a9c 228
beacon 11:292bdbd85a9c 229 /*
beacon 11:292bdbd85a9c 230 if (sam.search(&counter, &timer)) {
beacon 11:292bdbd85a9c 231
beacon 11:292bdbd85a9c 232 while(!done) {
beacon 11:292bdbd85a9c 233 sam.Arm.collectToDown(&done);
beacon 11:292bdbd85a9c 234 wait(0.1f);
beacon 11:292bdbd85a9c 235 }
beacon 11:292bdbd85a9c 236 done = 0;
beacon 11:292bdbd85a9c 237
beacon 11:292bdbd85a9c 238 sam.Greifer.take();
beacon 11:292bdbd85a9c 239
beacon 11:292bdbd85a9c 240 while(!done) {
beacon 11:292bdbd85a9c 241 sam.Arm.downToCollect(&done);
beacon 11:292bdbd85a9c 242 sam.leds[1] = 1;
beacon 11:292bdbd85a9c 243 wait(0.1f);
beacon 11:292bdbd85a9c 244 }
beacon 11:292bdbd85a9c 245 done = 0;
beacon 11:292bdbd85a9c 246
beacon 11:292bdbd85a9c 247 sam.Greifer.leave();
beacon 11:292bdbd85a9c 248
beacon 11:292bdbd85a9c 249 found = 0;
beacon 11:292bdbd85a9c 250 }
beacon 11:292bdbd85a9c 251
beacon 11:292bdbd85a9c 252
beacon 1:388c915756f5 253 wait( 0.1f );
beacon 0:d267b248eff4 254 }
beacon 0:d267b248eff4 255 return 0;
beacon 4:67d7177c213f 256 }
beacon 11:292bdbd85a9c 257 */
beacon 4:67d7177c213f 258
beacon 4:67d7177c213f 259 /* * /
beacon 4:67d7177c213f 260 int main(){
beacon 11:292bdbd85a9c 261
beacon 4:67d7177c213f 262 initializeDistanceSensors();
beacon 4:67d7177c213f 263 sam.stop();
beacon 16:f157e5ccd7d3 264
beacon 16:f157e5ccd7d3 265 //DigitalOut userLed(LD2);
beacon 11:292bdbd85a9c 266
beacon 4:67d7177c213f 267 while ( 1 ){
beacon 16:f157e5ccd7d3 268 sam.sensors[FWD_L] < NEAR ? sam.leds[4] = 1 : sam.leds[4] = 0;
beacon 16:f157e5ccd7d3 269 sam.sensors[RIGHT_L] < NEAR ? sam.leds[RIGHT_L] = 1 : sam.leds[RIGHT_L] = 0;
beacon 16:f157e5ccd7d3 270 sam.sensors[LEFT_L] < NEAR ? sam.leds[LEFT_L] = 1 : sam.leds[LEFT_L] = 0;
beacon 16:f157e5ccd7d3 271 //sam.sensors[FWD_L] < NEAR ? sam.leds[2] = 1 : sam.leds[4] = 0;
beacon 16:f157e5ccd7d3 272 //sam.sensors[RIGHT_L] < NEAR ? sam.leds[3] = 1 : sam.leds[4] = 0;
beacon 16:f157e5ccd7d3 273 //sam.sensors[LEFT_L] < NEAR ? userLED = 1 : sam.leds[4] = 0;
beacon 16:f157e5ccd7d3 274
beacon 16:f157e5ccd7d3 275 wait (0.2f);
beacon 16:f157e5ccd7d3 276 }
beacon 11:292bdbd85a9c 277
beacon 4:67d7177c213f 278 return 0;
beacon 4:67d7177c213f 279 }
beacon 4:67d7177c213f 280 /* */
beacon 4:67d7177c213f 281
beacon 4:67d7177c213f 282 /* * /
beacon 4:67d7177c213f 283 int main(){
beacon 4:67d7177c213f 284 for(float p=0; p<1.0; p += 0.1) {
beacon 4:67d7177c213f 285 // arm = p;
beacon 4:67d7177c213f 286 wait(0.2);
beacon 4:67d7177c213f 287 }
beacon 4:67d7177c213f 288 }
beacon 6:ba26dd3251b3 289 /* */
beacon 6:ba26dd3251b3 290
beacon 10:f76476943a6c 291 /* * /
beacon 6:ba26dd3251b3 292 int main(){
beacon 6:ba26dd3251b3 293 sam.stop();
beacon 6:ba26dd3251b3 294 int done = 0; //1:= finished process; 0:= not finished
beacon 6:ba26dd3251b3 295 int fun = 0; //just to test.
beacon 11:292bdbd85a9c 296 int start = 0;
beacon 11:292bdbd85a9c 297
beacon 11:292bdbd85a9c 298
beacon 6:ba26dd3251b3 299 while(1){
beacon 6:ba26dd3251b3 300 if(fun == 0){
beacon 6:ba26dd3251b3 301 done = 0;
beacon 6:ba26dd3251b3 302 sam.Arm.collecttoback(&done);
beacon 11:292bdbd85a9c 303 done == 0 ? fun = 0 : fun = 1;
beacon 6:ba26dd3251b3 304 }
beacon 6:ba26dd3251b3 305 else if(fun == 1){
beacon 6:ba26dd3251b3 306 done = 0;
beacon 6:ba26dd3251b3 307 sam.Arm.backtocollect(&done);
beacon 6:ba26dd3251b3 308 done == 0 ? fun = 1 : fun = 2;
beacon 6:ba26dd3251b3 309 }
beacon 6:ba26dd3251b3 310 else if(fun == 2){
beacon 6:ba26dd3251b3 311 done = 0;
beacon 6:ba26dd3251b3 312 sam.Arm.collecttodown(&done);
beacon 6:ba26dd3251b3 313 done == 0 ? fun = 2 : fun = 3;
beacon 6:ba26dd3251b3 314 }
beacon 6:ba26dd3251b3 315 else if(fun == 3){
beacon 6:ba26dd3251b3 316 done = 0;
beacon 6:ba26dd3251b3 317 sam.Arm.downtocollect(&done);
beacon 6:ba26dd3251b3 318 done == 0 ? fun = 3 : fun = 0;
beacon 6:ba26dd3251b3 319 }
beacon 6:ba26dd3251b3 320 wait(0.1);
beacon 6:ba26dd3251b3 321 }
beacon 10:f76476943a6c 322 }
beacon 10:f76476943a6c 323
beacon 10:f76476943a6c 324 */