x
Dependencies: Servo ServoArm mbed
Fork of PES_PIXY_Officiall by
Diff: main.cpp
- Revision:
- 0:15a8480061e8
- Child:
- 1:fd3cef0f116d
diff -r 000000000000 -r 15a8480061e8 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon May 22 11:24:46 2017 +0000 @@ -0,0 +1,341 @@ +#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: +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); + + +//Ultraschall +Ultraschall usensor(D6,D5); + +//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); + +//Leiste: +Servo servoLeiste(PB_6); + +//Button: +DigitalIn mybutton(USER_BUTTON); + +//Farbsensor: +AnalogIn FarbVoltage(A0); +//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!! + +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, setX, setY, fine, take, test}; + int time = 0; //Time keeps track of time. [time] = ms + + PID_Control pid; + pid.setPIDValues( 0.001f, 0.001f, 0.00015f, 0.3f, -0.3f, 1000); + pc.baud( 115200 ); + + int state = search; + + sam.stop(); + + /* + while( 1 ){ + printf("\n\rX: %d,\t Y: %d", pixy.getX(), pixy.getY()); + wait(1.0f); + } + */ + + while( 1 ){ + switch( state ){ + case test: + int x = pixy.getX(), y = pixy.getY(); + if( !mybutton ) printf("\n\rX:%d\tY:%d", x, y); + break; + + case search: + if( sam.search(&time) ) state = found; + break; + + case setX:{ + static int messungen[20], i = 0; + float e = 132.5f - pixy.getX(); + float diff = pid.calc( e, 0.001f ); + + sam.setLeft(0.5f - diff); + sam.setRight(0.5f - diff); + break; + } + case setY:{ + static int messungen[20], i = 0; + float e = 121.5f - pixy.getY(); + float diff = pid.calc( e, 0.001f ); + + sam.setLeft(0.5f + diff); + sam.setRight(0.5f - diff); + break; + } + + case take:{ + //sam.leds[1] = 1; + sam.stop(); + while (1) wait(0.1f); + static int down = 0, closed = 0, up = 0; + if( down || sam.Arm.backToDown() ) down = 1; + else if( closed || sam.Greifer.take() ) closed = 1; + else if( up || sam.Arm.downToBack() ) up = 1; + else if( sam.Greifer.leave() ){ + state = search; + down = 0; + closed = 0; + up = 0; + } + break; + } + } + time++; + wait(0.001f); + } + + 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; + } + 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++; + } + break; + } + + case backOff:{ + static int i = 0; + if( i > 1 ){ + sam.stop(); + i = 0; + state = forward; + timer = 0; + } + else{ + sam.driveBackSlowly(); + i++; + } + 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++; + wait(0.1f); + } + + return 0; +} +/* */ \ No newline at end of file