Do NOT modify!
Dependencies: mbed Servo ServoArm
Fork of PES_Yanick by
Sources/main.cpp
- Committer:
- beacon
- Date:
- 2017-05-06
- Revision:
- 11:292bdbd85a9c
- Parent:
- 10:f76476943a6c
- Child:
- 12:c0bcb95885dd
File content as of revision 11:292bdbd85a9c:
#include "mbed.h" #include "Robot.h" #include "Declarations.h" #include <cstdlib> //DistanceSensors related bottom: Serial pc(SERIAL_TX, SERIAL_RX); AnalogIn sensorVoltage(PB_1); DigitalOut enable(PC_1); DigitalOut bit0(PH_1); DigitalOut bit1(PC_2); DigitalOut bit2(PC_3); //DistanceSensors top: AnalogIn frontS(A1); AnalogIn leftS(A2); AnalogIn rightS(A3); //Leds related: DigitalOut leds[6] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 }; //motor related: PwmOut left(PA_8); PwmOut right(PA_9); DigitalOut powerSignal(PB_2); DigitalIn errorMotor(PB_14); DigitalIn overtemperatur(PB_15); //Arm: ServoArm servoArm(PA_6); //Greifer: 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() { for( int ii = 0; ii<9; ++ii) { sam.sensors[ii].init(&sensorVoltage, &frontS, &leftS, &rightS, &bit0, &bit1, &bit2, ii); enable = 1; } } /* */ int main() { initializeDistanceSensors(); //Initialises IR-Sensors. //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 //int done = 0; enum states { search = 0, forward, downward, down, upward, color, backwardDrop, readyDrop, backward }; int state = search; while( 1 ) { if ( timer > TIMEOUT ) { NVIC_SystemReset(); //Resets Sam. } 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; } /* */ /* * / int main(){ for(float p=0; p<1.0; p += 0.1) { // arm = p; wait(0.2); } } /* */ /* * / int main(){ sam.stop(); int done = 0; //1:= finished process; 0:= not finished int fun = 0; //just to test. int start = 0; while(1){ if(fun == 0){ done = 0; sam.Arm.collecttoback(&done); done == 0 ? fun = 0 : fun = 1; } else if(fun == 1){ done = 0; sam.Arm.backtocollect(&done); done == 0 ? fun = 1 : fun = 2; } else if(fun == 2){ done = 0; sam.Arm.collecttodown(&done); done == 0 ? fun = 2 : fun = 3; } else if(fun == 3){ done = 0; sam.Arm.downtocollect(&done); done == 0 ? fun = 3 : fun = 0; } wait(0.1); } } */