Do NOT modify!
Dependencies: mbed Servo ServoArm
Fork of PES_Yanick by
Diff: Sources/main.cpp
- Revision:
- 12:c0bcb95885dd
- Parent:
- 11:292bdbd85a9c
diff -r 292bdbd85a9c -r c0bcb95885dd Sources/main.cpp --- a/Sources/main.cpp Sat May 06 13:33:23 2017 +0000 +++ b/Sources/main.cpp Wed May 10 13:30:13 2017 +0000 @@ -33,10 +33,11 @@ ServoArm servoArm(PA_6); //Greifer: -Servo servoGreifer(PB_7); +Servo servoGreifer(PC_7); //Farbsensor: AnalogIn FarbVoltage(A0); +DigitalOut led(D2); Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer ); //Implement the Farbsensor into the Robot init function!! @@ -62,7 +63,7 @@ enum states { search = 0, forward, downward, down, upward, color, backwardDrop, readyDrop, backward }; - int state = search; + int state = downward; while( 1 ) { @@ -112,19 +113,23 @@ break; case color: { + led = 1; int color = sam.FarbVoltage.read(); - if( color == -1){ + if( color == -1 ){ //Do nothing } + else if( color == 0 || color == GREEN ){ state = backwardDrop; + led = 0; timer = 0; } else if( color == RED ){ state = readyDrop; + led = 0; timer = 0; } @@ -152,7 +157,7 @@ case backward: if( sam.Arm.collectToBack() ){ - state = search; + state = downward; timer = 0; }