x
Dependencies: Servo ServoArm mbed
Fork of PES_PIXY_Officiall by
Diff: main.cpp
- Revision:
- 3:63da1d1fae15
- Parent:
- 2:c8c965d48f8d
- Child:
- 4:d611df1ed42b
--- a/main.cpp Thu May 25 18:12:28 2017 +0000 +++ b/main.cpp Fri May 26 08:01:48 2017 +0000 @@ -1,12 +1,8 @@ #include "mbed.h" #include "Robot.h" #include "Declarations.h" -#include "Ultraschall.h" #include "Pixy.h" #include "PID_Control.h" -#include "LowpassFilter.h" -#include "EncoderCounter.h" - #include <cstdlib> //Cam: @@ -27,10 +23,6 @@ AnalogIn leftS(A2); AnalogIn rightS(A3); - -//Ultraschall -Ultraschall usensor(D6,D5); - //Leds related: DigitalOut leds[6] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 }; @@ -48,9 +40,6 @@ //Greifer: Servo servoGreifer(PC_7); -//Leiste: -Servo servoLeiste(PB_6); - //Button: DigitalIn mybutton(USER_BUTTON); @@ -59,318 +48,134 @@ //DigitalOut led(D2); -Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer, &servoLeiste, &usensor, &pixy ); //Implement the Farbsensor into the Robot init function!! +Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer, &pixy ); //Robot-Constructor -void initializeDistanceSensors() -{ - for( int ii = 0; ii<9; ++ii) { +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. - enum states {search = 0, setPos, take}; - int time = 0; //Time keeps track of time. [time] = ms + +void competition(){ + initializeDistanceSensors(); //Initialises IR-Sensors + enum states {search = 0, setPos, take}; //3-states machine + int time = 0; //Time keeps track of time. [time] = ms PID_Control pid; pid.setPIDValues( 0.001f, 0.001f, 0.00015f, 0.15f, -0.15f, 1000); - // pc.baud( 115200 ); - - int state = search; - while(!sam.Arm.collectToBack())wait(0.075); - while(!sam.Greifer.leave())wait(0.001); - - - while( 1 ) { - - switch(state){ + + sam.Arm.init(&servoArm); //Arm initialisation + sam.Greifer.init(&servoGreifer); //Greifer initialisation - case search: { - if(!((pixy.getX()>50 && pixy.getX()<300)&&(pixy.getY()>50 && pixy.getY()<300))){ - sam.search(&time); - } - else { - state = setPos; - } - - break; - } - - case setPos: { - sam.leds[5] = 1; - static int i = 0; - float eX = 133.0f - pixy.getX(); - float diffX = pid.calc( eX, 0.006f ); - - float aX = 0.03; //minimum diff - float aY = 0.05; - if(diffX>0) { - if(diffX<aX)diffX=aX; - } - else if(diffX>-aX)diffX=-aX; - - if(!(pixy.getX()>128 && pixy.getX()<138)){ - sam.setLeft(0.5f - diffX); - sam.setRight(0.5f - diffX); - } - - wait(0.001f); - - float eY = 121.0f - pixy.getY(); - float diffY = pid.calc( eY, 0.006f ); - if(diffY>0) { - if(diffY<aY)diffY=aY; - } - else if(diffY>-aY) diffY=-aY; - if(!(pixy.getY()>115 && pixy.getY()<125)){ - sam.setLeft(0.5f + diffY); - sam.setRight(0.5f - diffY); - } - i++; - if((pixy.getX()>132 && pixy.getX()<134)&&(pixy.getY()>119 && pixy.getY()<122)||!(i%1000)) { - state = take; - i = 0; - sam.stop(); - sam.leds[5] = 0; - } - break; - } - - case take: { - enum takeStates {down=0, take, up, leave}; - static int tState=down; - - sam.leds[1] = 1; - sam.stop(); - //tState=leave; - - switch(tState) { - case down: - if(sam.Arm.backToDown()) tState=take; - else tState=down; - break; - case take: - if(sam.Greifer.take()) tState=up; - else tState=take; - break; - case up: - if(sam.Arm.downToBack()) {wait(0.05);tState=leave;} - else tState=up; - break; - case leave: - if(sam.Greifer.leave()) {state=search; tState=down;} - else tState=leave; - break; - } + int state = search; + while(!sam.Arm.collectToBack())wait(0.075); + while(!sam.Greifer.leave())wait(0.001); + + //while(mybutton) wait(0.01); - sam.leds[1] = 0; - break; - } - } - time++; - wait(0.005f); -} - -return 0; -} - -/* * / -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; - int color; - - enum states { search = 0, LeisteDown, turn, push, backOff, forward, downward, down, upward, colorS, readyDrop, backward, LeisteUp }; - - int state = search; - - static float messung = 0; - - while( 1 ){ - printf("\n\r%f", sam.see(FWD_L)); - - wait(1.0f); - - } - - while( 1 ) { - - if ( timer > TIMEOUT ) { - NVIC_SystemReset(); //Resets Sam. - } - - //if( timer == 0 ) - //printf("\n\rLEFT: %.3f,\tFWD: %.3f,\tRIGHT: %.3f", sam.sensors[LEFT].read(), sam.sensors[FWD].read(), sam.sensors[RIGHT].read()); - - - //printf("\n\rcurrent main state: %d", state); - - sam.sensors[FWD_L].read() < NEAR ? sam.leds[1] = 1 : sam.leds[1] = 0; - switch( state ) { - case search: - if( sam.search(&timer) ){ - //sam.Greifer.nullPos(); - state = LeisteDown; - timer = 0; - } - break; - - case LeisteDown: - int count = 0; - if( sam.Leiste.upToDown() ){ - //sam.Greifer.leave(); - state = turn; - timer = 0; - } - break; - - case turn: - static int i = 0; - if( i > 7 ){ - sam.stop(); - state = push; - i = 0; + while(1){ + + switch(state){ + + case search:{ + if(!((pixy.getX()>50 && pixy.getX()<225)&&(pixy.getY()>50 && pixy.getY()<300))){ + sam.search(&time); } else{ - i++; - sam.turnRight(); - } - break; - - case push:{ - static int i = 0; - if( i > 5 ){ - sam.stop(); - i = 0; - state = backOff; - timer = 0; - } - else{ - sam.driveSlowly(); - i++; + state = setPos; } break; } - - case backOff:{ - static int i = 0; - if( i > 1 ){ - sam.stop(); - i = 0; - state = forward; - timer = 0; + + case setPos: { + + sam.leds[5] = 1; + static int i = 0; + if(!((pixy.getX()>50 && pixy.getX()<225)&&(pixy.getY()>50 && pixy.getY()<300))){ + state=search; + i=0; + } + float eX = 133.0f - pixy.getX(); + float diffX = pid.calc( eX, 0.006f ); + + float aX = 0.03; //minimum diff X + float aY = 0.05; //minimum diff Y + if(diffX>0) { + if(diffX<aX)diffX=aX; + } + else if(diffX>-aX)diffX=-aX; + + //Set the X position + if(!(pixy.getX()>128 && pixy.getX()<138)){ + sam.setLeft(0.5f - diffX); + sam.setRight(0.5f - diffX); + } + + wait(0.001f); + + float eY = 121.0f - pixy.getY(); + float diffY = pid.calc( eY, 0.006f ); + if(diffY>0) { + if(diffY<aY)diffY=aY; + } + else if(diffY>-aY)diffY=-aY; + + //Set the Y position + if(!(pixy.getY()>115 && pixy.getY()<125)){ + sam.setLeft(0.5f + diffY); + sam.setRight(0.5f - diffY); + } + i++; + if((pixy.getX()>132 && pixy.getX()<134)&&(pixy.getY()>119 && pixy.getY()<122)||!(i%1000)) { + state = take; + i = 0; + sam.stop(); + sam.leds[5] = 0; + } + break; + } + + case take:{ + enum takeStates {down=0, take, up, leave}; //4-states machine + static int tState=down; + + sam.leds[1] = 1; + sam.stop(); + + switch(tState){ + case down: + if(sam.Arm.backToDown()) tState=take; + else tState=down; + break; + case take: + if(sam.Greifer.take()) tState=up; + else tState=take; + break; + case up: + if(sam.Arm.downToBack()) {wait(0.1);tState=leave;} + else tState=up; + break; + case leave: + if(sam.Greifer.leave()){ + state=search; + tState=down; + } + else tState=leave; + break; } - else{ - sam.driveBackSlowly(); - i++; - } + sam.leds[1] = 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 = colorS; - timer = 0; - } - break; - - case colorS: - led = 1; - color = sam.FarbVoltage.read(); - - if( color == -1 ){ - //Do nothing - } - - - else if( color == 0 || color == GREEN ){ - state = backward; - led = 0; - timer = 0; - } - - else if( color == RED ){ - state = readyDrop; - led = 0; - timer = 0; - } - - else{ - //Shit... - } - break; - - case readyDrop: - if( sam.Greifer.leave() ){ - if( color == GREEN || color == 0 ){ - state = LeisteUp; - } - else{ - state = backward; - } - timer = 0; - } - - break; - - case backward: - if( sam.Arm.collectToBack() ){ - //sam.Greifer.nullPos(); - state = LeisteUp; - timer = 0; - if( color == GREEN || color == 0 ){ - state = readyDrop; - sam.Greifer.leave(); - } - else{ - state = LeisteUp; - } - } - break; - - case LeisteUp: - if( sam.Leiste.downToUp() ){ - state = search; - timer = 0; - } - break; } - timer++; + time++; + wait(0.005f); //Cycle-period + } +} +int main(){ + while( 1 ){ + if( !mybutton ) competition(); wait(0.1f); } - - return 0; -} -/* */ \ No newline at end of file +} \ No newline at end of file