Do NOT modify!
Dependencies: mbed Servo ServoArm
Fork of PES_Yanick by
Diff: Sources/main.cpp
- Revision:
- 11:292bdbd85a9c
- Parent:
- 10:f76476943a6c
- Child:
- 12:c0bcb95885dd
--- a/Sources/main.cpp Wed May 03 13:54:51 2017 +0000 +++ b/Sources/main.cpp Sat May 06 13:33:23 2017 +0000 @@ -33,67 +33,189 @@ ServoArm servoArm(PA_6); //Greifer: -Servo servoGreifer(PC_7); +Servo servoGreifer(PB_7); //Farbsensor: AnalogIn FarbVoltage(A0); Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer ); //Implement the Farbsensor into the Robot init function!! -void initializeDistanceSensors(){ +void initializeDistanceSensors() +{ for( int ii = 0; ii<9; ++ii) { sam.sensors[ii].init(&sensorVoltage, &frontS, &leftS, &rightS, &bit0, &bit1, &bit2, ii); - enable = 1; + enable = 1; } } /* */ -int main(){ +int main() +{ initializeDistanceSensors(); //Initialises IR-Sensors. - int counter = 0; //Counts how many times the robot has turned, before driving + //int counter = 0; //Counts how many times the robot has turned, before driving int timer = 0; //Is used, to reset the robot. Returns time in [0.1s] //int lastFun = -1; //Is used, to check if the same action in Robot::search() was made multiple times. - int found = 0; //0:= no block available, 1 := a lego is ready to be picked up - - while( 1 ){ + //int found = 0; //0:= no block available, 1 := a lego is ready to be picked up + //int done = 0; + + enum states { search = 0, forward, downward, down, upward, color, backwardDrop, readyDrop, backward }; + + int state = search; + + while( 1 ) { - if ( timer > TIMEOUT ){ + if ( timer > TIMEOUT ) { NVIC_SystemReset(); //Resets Sam. } - else if (found == 0){ - sam.search(&counter, &timer, &found); - } - else{ - //pick up lego - //found = 0; - sam.stop(); //Nur zum Testen + + switch( state ) { + case search: + if( sam.search(&timer) ){ + state = forward; + timer = 0; + } + + break; + + case forward: + if( sam.Arm.backToCollect() ){ + state = downward; + timer = 0; + } + + break; + + case downward: + if( sam.Arm.collectToDown() ){ + state = down; + timer = 0; + } + + break; + + case down: + if( sam.Greifer.take() ) { + state = upward; + timer = 0; + } + + break; + + case upward: + if( sam.Arm.downToCollect() ){ + state = color; + timer = 0; + } + + break; + + case color: { + int color = sam.FarbVoltage.read(); + + if( color == -1){ + //Do nothing + } + + else if( color == 0 || color == GREEN ){ + state = backwardDrop; + timer = 0; + } + + else if( color == RED ){ + state = readyDrop; + timer = 0; + } + + else{ + //Shit... + } + break; + } + + case backwardDrop: + if( sam.Arm.collectToBack() ){ + state = readyDrop; + timer = 0; + } + + break; + + case readyDrop: + if( sam.Greifer.leave() ){ + state = backward; + timer = 0; + } + + break; + + case backward: + if( sam.Arm.collectToBack() ){ + state = search; + timer = 0; + } + + break; } + timer++; + wait(0.1f); + } + + return 0; +} + + + + /* + if (sam.search(&counter, &timer)) { + + while(!done) { + sam.Arm.collectToDown(&done); + wait(0.1f); + } + done = 0; + + sam.Greifer.take(); + + while(!done) { + sam.Arm.downToCollect(&done); + sam.leds[1] = 1; + wait(0.1f); + } + done = 0; + + sam.Greifer.leave(); + + found = 0; + } + + wait( 0.1f ); } return 0; } +*/ /* * / int main(){ - + initializeDistanceSensors(); sam.stop(); - + while ( 1 ){ for (int i=0; i<6; i++){ sam.sensors[i] < NEAR ? sam.leds[i] = 1 : sam.leds[i] = 0; } - + sam.turnLeftS(); - + while (sam.sensors[FWD] < NEAR){ wait(0.05f); sam.stop(); } - + } return 0; } @@ -113,14 +235,14 @@ sam.stop(); int done = 0; //1:= finished process; 0:= not finished int fun = 0; //just to test. - int start = 0; - - + int start = 0; + + while(1){ if(fun == 0){ done = 0; sam.Arm.collecttoback(&done); - done == 0 ? fun = 0 : fun = 1; + done == 0 ? fun = 0 : fun = 1; } else if(fun == 1){ done = 0;