x
Dependencies: Servo ServoArm mbed
Fork of PES_PIXY_Officiall by
main.cpp
- Committer:
- EpicG10
- Date:
- 2017-05-26
- Revision:
- 3:63da1d1fae15
- Parent:
- 2:c8c965d48f8d
- Child:
- 4:d611df1ed42b
File content as of revision 3:63da1d1fae15:
#include "mbed.h" #include "Robot.h" #include "Declarations.h" #include "Pixy.h" #include "PID_Control.h" #include <cstdlib> //Cam: Serial cam(PA_9, PA_10); Pixy pixy(cam); //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(PC_7); //Button: DigitalIn mybutton(USER_BUTTON); //Farbsensor: AnalogIn FarbVoltage(A0); //DigitalOut led(D2); Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer, &pixy ); //Robot-Constructor void initializeDistanceSensors(){ for( int ii = 0; ii<9; ++ii){ sam.sensors[ii].init(&sensorVoltage, &frontS, &leftS, &rightS, &bit0, &bit1, &bit2, ii); enable = 1; } } 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); sam.Arm.init(&servoArm); //Arm initialisation sam.Greifer.init(&servoGreifer); //Greifer initialisation int state = search; while(!sam.Arm.collectToBack())wait(0.075); while(!sam.Greifer.leave())wait(0.001); //while(mybutton) wait(0.01); while(1){ switch(state){ case search:{ if(!((pixy.getX()>50 && pixy.getX()<225)&&(pixy.getY()>50 && pixy.getY()<300))){ sam.search(&time); } else{ state = setPos; } break; } 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; } sam.leds[1] = 0; break; } } time++; wait(0.005f); //Cycle-period } } int main(){ while( 1 ){ if( !mybutton ) competition(); wait(0.1f); } }