x
Dependencies: Servo ServoArm mbed
Fork of PES_PIXY_Officiall by
main.cpp@3:63da1d1fae15, 2017-05-26 (annotated)
- Committer:
- EpicG10
- Date:
- Fri May 26 08:01:48 2017 +0000
- Revision:
- 3:63da1d1fae15
- Parent:
- 2:c8c965d48f8d
- Child:
- 4:d611df1ed42b
c
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
beacon | 0:15a8480061e8 | 1 | #include "mbed.h" |
beacon | 0:15a8480061e8 | 2 | #include "Robot.h" |
beacon | 0:15a8480061e8 | 3 | #include "Declarations.h" |
beacon | 0:15a8480061e8 | 4 | #include "Pixy.h" |
beacon | 0:15a8480061e8 | 5 | #include "PID_Control.h" |
beacon | 0:15a8480061e8 | 6 | #include <cstdlib> |
beacon | 0:15a8480061e8 | 7 | |
beacon | 0:15a8480061e8 | 8 | //Cam: |
beacon | 0:15a8480061e8 | 9 | Serial cam(PA_9, PA_10); |
beacon | 0:15a8480061e8 | 10 | Pixy pixy(cam); |
beacon | 0:15a8480061e8 | 11 | |
beacon | 0:15a8480061e8 | 12 | //DistanceSensors related bottom: |
EpicG10 | 2:c8c965d48f8d | 13 | Serial pc(SERIAL_TX, SERIAL_RX); |
beacon | 0:15a8480061e8 | 14 | |
beacon | 0:15a8480061e8 | 15 | AnalogIn sensorVoltage(PB_1); |
beacon | 0:15a8480061e8 | 16 | DigitalOut enable(PC_1); |
beacon | 0:15a8480061e8 | 17 | DigitalOut bit0(PH_1); |
beacon | 0:15a8480061e8 | 18 | DigitalOut bit1(PC_2); |
beacon | 0:15a8480061e8 | 19 | DigitalOut bit2(PC_3); |
beacon | 0:15a8480061e8 | 20 | |
beacon | 0:15a8480061e8 | 21 | //DistanceSensors top: |
beacon | 0:15a8480061e8 | 22 | AnalogIn frontS(A1); |
beacon | 0:15a8480061e8 | 23 | AnalogIn leftS(A2); |
beacon | 0:15a8480061e8 | 24 | AnalogIn rightS(A3); |
beacon | 0:15a8480061e8 | 25 | |
beacon | 0:15a8480061e8 | 26 | //Leds related: |
beacon | 0:15a8480061e8 | 27 | DigitalOut leds[6] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 }; |
beacon | 0:15a8480061e8 | 28 | |
beacon | 0:15a8480061e8 | 29 | //motor related: |
beacon | 0:15a8480061e8 | 30 | PwmOut left(PA_8); |
beacon | 0:15a8480061e8 | 31 | PwmOut right(PA_9); |
beacon | 0:15a8480061e8 | 32 | |
beacon | 0:15a8480061e8 | 33 | DigitalOut powerSignal(PB_2); |
beacon | 0:15a8480061e8 | 34 | DigitalIn errorMotor(PB_14); |
beacon | 0:15a8480061e8 | 35 | DigitalIn overtemperatur(PB_15); |
beacon | 0:15a8480061e8 | 36 | |
beacon | 0:15a8480061e8 | 37 | //Arm: |
beacon | 0:15a8480061e8 | 38 | ServoArm servoArm(PA_6); |
beacon | 0:15a8480061e8 | 39 | |
beacon | 0:15a8480061e8 | 40 | //Greifer: |
beacon | 0:15a8480061e8 | 41 | Servo servoGreifer(PC_7); |
beacon | 0:15a8480061e8 | 42 | |
beacon | 0:15a8480061e8 | 43 | //Button: |
beacon | 0:15a8480061e8 | 44 | DigitalIn mybutton(USER_BUTTON); |
beacon | 0:15a8480061e8 | 45 | |
beacon | 0:15a8480061e8 | 46 | //Farbsensor: |
beacon | 0:15a8480061e8 | 47 | AnalogIn FarbVoltage(A0); |
beacon | 0:15a8480061e8 | 48 | //DigitalOut led(D2); |
beacon | 0:15a8480061e8 | 49 | |
beacon | 0:15a8480061e8 | 50 | |
EpicG10 | 3:63da1d1fae15 | 51 | Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer, &pixy ); //Robot-Constructor |
beacon | 0:15a8480061e8 | 52 | |
EpicG10 | 3:63da1d1fae15 | 53 | void initializeDistanceSensors(){ |
EpicG10 | 3:63da1d1fae15 | 54 | for( int ii = 0; ii<9; ++ii){ |
beacon | 0:15a8480061e8 | 55 | sam.sensors[ii].init(&sensorVoltage, &frontS, &leftS, &rightS, &bit0, &bit1, &bit2, ii); |
beacon | 0:15a8480061e8 | 56 | enable = 1; |
beacon | 0:15a8480061e8 | 57 | } |
beacon | 0:15a8480061e8 | 58 | } |
EpicG10 | 3:63da1d1fae15 | 59 | |
EpicG10 | 3:63da1d1fae15 | 60 | void competition(){ |
EpicG10 | 3:63da1d1fae15 | 61 | initializeDistanceSensors(); //Initialises IR-Sensors |
EpicG10 | 3:63da1d1fae15 | 62 | enum states {search = 0, setPos, take}; //3-states machine |
EpicG10 | 3:63da1d1fae15 | 63 | int time = 0; //Time keeps track of time. [time] = ms |
EpicG10 | 2:c8c965d48f8d | 64 | |
beacon | 0:15a8480061e8 | 65 | PID_Control pid; |
EpicG10 | 2:c8c965d48f8d | 66 | pid.setPIDValues( 0.001f, 0.001f, 0.00015f, 0.15f, -0.15f, 1000); |
EpicG10 | 3:63da1d1fae15 | 67 | |
EpicG10 | 3:63da1d1fae15 | 68 | sam.Arm.init(&servoArm); //Arm initialisation |
EpicG10 | 3:63da1d1fae15 | 69 | sam.Greifer.init(&servoGreifer); //Greifer initialisation |
beacon | 0:15a8480061e8 | 70 | |
EpicG10 | 3:63da1d1fae15 | 71 | int state = search; |
EpicG10 | 3:63da1d1fae15 | 72 | while(!sam.Arm.collectToBack())wait(0.075); |
EpicG10 | 3:63da1d1fae15 | 73 | while(!sam.Greifer.leave())wait(0.001); |
EpicG10 | 3:63da1d1fae15 | 74 | |
EpicG10 | 3:63da1d1fae15 | 75 | //while(mybutton) wait(0.01); |
EpicG10 | 2:c8c965d48f8d | 76 | |
EpicG10 | 2:c8c965d48f8d | 77 | |
EpicG10 | 3:63da1d1fae15 | 78 | while(1){ |
EpicG10 | 3:63da1d1fae15 | 79 | |
EpicG10 | 3:63da1d1fae15 | 80 | switch(state){ |
EpicG10 | 3:63da1d1fae15 | 81 | |
EpicG10 | 3:63da1d1fae15 | 82 | case search:{ |
EpicG10 | 3:63da1d1fae15 | 83 | if(!((pixy.getX()>50 && pixy.getX()<225)&&(pixy.getY()>50 && pixy.getY()<300))){ |
EpicG10 | 3:63da1d1fae15 | 84 | sam.search(&time); |
beacon | 0:15a8480061e8 | 85 | } |
beacon | 0:15a8480061e8 | 86 | else{ |
EpicG10 | 3:63da1d1fae15 | 87 | state = setPos; |
beacon | 0:15a8480061e8 | 88 | } |
beacon | 0:15a8480061e8 | 89 | break; |
beacon | 0:15a8480061e8 | 90 | } |
EpicG10 | 3:63da1d1fae15 | 91 | |
EpicG10 | 3:63da1d1fae15 | 92 | case setPos: { |
EpicG10 | 3:63da1d1fae15 | 93 | |
EpicG10 | 3:63da1d1fae15 | 94 | sam.leds[5] = 1; |
EpicG10 | 3:63da1d1fae15 | 95 | static int i = 0; |
EpicG10 | 3:63da1d1fae15 | 96 | if(!((pixy.getX()>50 && pixy.getX()<225)&&(pixy.getY()>50 && pixy.getY()<300))){ |
EpicG10 | 3:63da1d1fae15 | 97 | state=search; |
EpicG10 | 3:63da1d1fae15 | 98 | i=0; |
EpicG10 | 3:63da1d1fae15 | 99 | } |
EpicG10 | 3:63da1d1fae15 | 100 | float eX = 133.0f - pixy.getX(); |
EpicG10 | 3:63da1d1fae15 | 101 | float diffX = pid.calc( eX, 0.006f ); |
EpicG10 | 3:63da1d1fae15 | 102 | |
EpicG10 | 3:63da1d1fae15 | 103 | float aX = 0.03; //minimum diff X |
EpicG10 | 3:63da1d1fae15 | 104 | float aY = 0.05; //minimum diff Y |
EpicG10 | 3:63da1d1fae15 | 105 | if(diffX>0) { |
EpicG10 | 3:63da1d1fae15 | 106 | if(diffX<aX)diffX=aX; |
EpicG10 | 3:63da1d1fae15 | 107 | } |
EpicG10 | 3:63da1d1fae15 | 108 | else if(diffX>-aX)diffX=-aX; |
EpicG10 | 3:63da1d1fae15 | 109 | |
EpicG10 | 3:63da1d1fae15 | 110 | //Set the X position |
EpicG10 | 3:63da1d1fae15 | 111 | if(!(pixy.getX()>128 && pixy.getX()<138)){ |
EpicG10 | 3:63da1d1fae15 | 112 | sam.setLeft(0.5f - diffX); |
EpicG10 | 3:63da1d1fae15 | 113 | sam.setRight(0.5f - diffX); |
EpicG10 | 3:63da1d1fae15 | 114 | } |
EpicG10 | 3:63da1d1fae15 | 115 | |
EpicG10 | 3:63da1d1fae15 | 116 | wait(0.001f); |
EpicG10 | 3:63da1d1fae15 | 117 | |
EpicG10 | 3:63da1d1fae15 | 118 | float eY = 121.0f - pixy.getY(); |
EpicG10 | 3:63da1d1fae15 | 119 | float diffY = pid.calc( eY, 0.006f ); |
EpicG10 | 3:63da1d1fae15 | 120 | if(diffY>0) { |
EpicG10 | 3:63da1d1fae15 | 121 | if(diffY<aY)diffY=aY; |
EpicG10 | 3:63da1d1fae15 | 122 | } |
EpicG10 | 3:63da1d1fae15 | 123 | else if(diffY>-aY)diffY=-aY; |
EpicG10 | 3:63da1d1fae15 | 124 | |
EpicG10 | 3:63da1d1fae15 | 125 | //Set the Y position |
EpicG10 | 3:63da1d1fae15 | 126 | if(!(pixy.getY()>115 && pixy.getY()<125)){ |
EpicG10 | 3:63da1d1fae15 | 127 | sam.setLeft(0.5f + diffY); |
EpicG10 | 3:63da1d1fae15 | 128 | sam.setRight(0.5f - diffY); |
EpicG10 | 3:63da1d1fae15 | 129 | } |
EpicG10 | 3:63da1d1fae15 | 130 | i++; |
EpicG10 | 3:63da1d1fae15 | 131 | if((pixy.getX()>132 && pixy.getX()<134)&&(pixy.getY()>119 && pixy.getY()<122)||!(i%1000)) { |
EpicG10 | 3:63da1d1fae15 | 132 | state = take; |
EpicG10 | 3:63da1d1fae15 | 133 | i = 0; |
EpicG10 | 3:63da1d1fae15 | 134 | sam.stop(); |
EpicG10 | 3:63da1d1fae15 | 135 | sam.leds[5] = 0; |
EpicG10 | 3:63da1d1fae15 | 136 | } |
EpicG10 | 3:63da1d1fae15 | 137 | break; |
EpicG10 | 3:63da1d1fae15 | 138 | } |
EpicG10 | 3:63da1d1fae15 | 139 | |
EpicG10 | 3:63da1d1fae15 | 140 | case take:{ |
EpicG10 | 3:63da1d1fae15 | 141 | enum takeStates {down=0, take, up, leave}; //4-states machine |
EpicG10 | 3:63da1d1fae15 | 142 | static int tState=down; |
EpicG10 | 3:63da1d1fae15 | 143 | |
EpicG10 | 3:63da1d1fae15 | 144 | sam.leds[1] = 1; |
EpicG10 | 3:63da1d1fae15 | 145 | sam.stop(); |
EpicG10 | 3:63da1d1fae15 | 146 | |
EpicG10 | 3:63da1d1fae15 | 147 | switch(tState){ |
EpicG10 | 3:63da1d1fae15 | 148 | case down: |
EpicG10 | 3:63da1d1fae15 | 149 | if(sam.Arm.backToDown()) tState=take; |
EpicG10 | 3:63da1d1fae15 | 150 | else tState=down; |
EpicG10 | 3:63da1d1fae15 | 151 | break; |
EpicG10 | 3:63da1d1fae15 | 152 | case take: |
EpicG10 | 3:63da1d1fae15 | 153 | if(sam.Greifer.take()) tState=up; |
EpicG10 | 3:63da1d1fae15 | 154 | else tState=take; |
EpicG10 | 3:63da1d1fae15 | 155 | break; |
EpicG10 | 3:63da1d1fae15 | 156 | case up: |
EpicG10 | 3:63da1d1fae15 | 157 | if(sam.Arm.downToBack()) {wait(0.1);tState=leave;} |
EpicG10 | 3:63da1d1fae15 | 158 | else tState=up; |
EpicG10 | 3:63da1d1fae15 | 159 | break; |
EpicG10 | 3:63da1d1fae15 | 160 | case leave: |
EpicG10 | 3:63da1d1fae15 | 161 | if(sam.Greifer.leave()){ |
EpicG10 | 3:63da1d1fae15 | 162 | state=search; |
EpicG10 | 3:63da1d1fae15 | 163 | tState=down; |
EpicG10 | 3:63da1d1fae15 | 164 | } |
EpicG10 | 3:63da1d1fae15 | 165 | else tState=leave; |
EpicG10 | 3:63da1d1fae15 | 166 | break; |
beacon | 0:15a8480061e8 | 167 | } |
EpicG10 | 3:63da1d1fae15 | 168 | sam.leds[1] = 0; |
beacon | 0:15a8480061e8 | 169 | break; |
beacon | 0:15a8480061e8 | 170 | } |
EpicG10 | 2:c8c965d48f8d | 171 | } |
EpicG10 | 3:63da1d1fae15 | 172 | time++; |
EpicG10 | 3:63da1d1fae15 | 173 | wait(0.005f); //Cycle-period |
EpicG10 | 3:63da1d1fae15 | 174 | } |
EpicG10 | 3:63da1d1fae15 | 175 | } |
EpicG10 | 3:63da1d1fae15 | 176 | int main(){ |
EpicG10 | 3:63da1d1fae15 | 177 | while( 1 ){ |
EpicG10 | 3:63da1d1fae15 | 178 | if( !mybutton ) competition(); |
beacon | 0:15a8480061e8 | 179 | wait(0.1f); |
beacon | 0:15a8480061e8 | 180 | } |
EpicG10 | 3:63da1d1fae15 | 181 | } |