![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
x
Dependencies: Servo ServoArm mbed
Fork of PES_PIXY_Officiall by
main.cpp@2:c8c965d48f8d, 2017-05-25 (annotated)
- Committer:
- EpicG10
- Date:
- Thu May 25 18:12:28 2017 +0000
- Revision:
- 2:c8c965d48f8d
- Parent:
- 1:fd3cef0f116d
- Child:
- 3:63da1d1fae15
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 "Ultraschall.h" |
beacon | 0:15a8480061e8 | 5 | #include "Pixy.h" |
beacon | 0:15a8480061e8 | 6 | #include "PID_Control.h" |
beacon | 0:15a8480061e8 | 7 | #include "LowpassFilter.h" |
beacon | 0:15a8480061e8 | 8 | #include "EncoderCounter.h" |
beacon | 0:15a8480061e8 | 9 | |
beacon | 0:15a8480061e8 | 10 | #include <cstdlib> |
beacon | 0:15a8480061e8 | 11 | |
beacon | 0:15a8480061e8 | 12 | //Cam: |
beacon | 0:15a8480061e8 | 13 | Serial cam(PA_9, PA_10); |
beacon | 0:15a8480061e8 | 14 | Pixy pixy(cam); |
beacon | 0:15a8480061e8 | 15 | |
beacon | 0:15a8480061e8 | 16 | //DistanceSensors related bottom: |
EpicG10 | 2:c8c965d48f8d | 17 | Serial pc(SERIAL_TX, SERIAL_RX); |
beacon | 0:15a8480061e8 | 18 | |
beacon | 0:15a8480061e8 | 19 | AnalogIn sensorVoltage(PB_1); |
beacon | 0:15a8480061e8 | 20 | DigitalOut enable(PC_1); |
beacon | 0:15a8480061e8 | 21 | DigitalOut bit0(PH_1); |
beacon | 0:15a8480061e8 | 22 | DigitalOut bit1(PC_2); |
beacon | 0:15a8480061e8 | 23 | DigitalOut bit2(PC_3); |
beacon | 0:15a8480061e8 | 24 | |
beacon | 0:15a8480061e8 | 25 | //DistanceSensors top: |
beacon | 0:15a8480061e8 | 26 | AnalogIn frontS(A1); |
beacon | 0:15a8480061e8 | 27 | AnalogIn leftS(A2); |
beacon | 0:15a8480061e8 | 28 | AnalogIn rightS(A3); |
beacon | 0:15a8480061e8 | 29 | |
beacon | 0:15a8480061e8 | 30 | |
beacon | 0:15a8480061e8 | 31 | //Ultraschall |
beacon | 0:15a8480061e8 | 32 | Ultraschall usensor(D6,D5); |
beacon | 0:15a8480061e8 | 33 | |
beacon | 0:15a8480061e8 | 34 | //Leds related: |
beacon | 0:15a8480061e8 | 35 | DigitalOut leds[6] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 }; |
beacon | 0:15a8480061e8 | 36 | |
beacon | 0:15a8480061e8 | 37 | //motor related: |
beacon | 0:15a8480061e8 | 38 | PwmOut left(PA_8); |
beacon | 0:15a8480061e8 | 39 | PwmOut right(PA_9); |
beacon | 0:15a8480061e8 | 40 | |
beacon | 0:15a8480061e8 | 41 | DigitalOut powerSignal(PB_2); |
beacon | 0:15a8480061e8 | 42 | DigitalIn errorMotor(PB_14); |
beacon | 0:15a8480061e8 | 43 | DigitalIn overtemperatur(PB_15); |
beacon | 0:15a8480061e8 | 44 | |
beacon | 0:15a8480061e8 | 45 | //Arm: |
beacon | 0:15a8480061e8 | 46 | ServoArm servoArm(PA_6); |
beacon | 0:15a8480061e8 | 47 | |
beacon | 0:15a8480061e8 | 48 | //Greifer: |
beacon | 0:15a8480061e8 | 49 | Servo servoGreifer(PC_7); |
beacon | 0:15a8480061e8 | 50 | |
beacon | 0:15a8480061e8 | 51 | //Leiste: |
beacon | 0:15a8480061e8 | 52 | Servo servoLeiste(PB_6); |
beacon | 0:15a8480061e8 | 53 | |
beacon | 0:15a8480061e8 | 54 | //Button: |
beacon | 0:15a8480061e8 | 55 | DigitalIn mybutton(USER_BUTTON); |
beacon | 0:15a8480061e8 | 56 | |
beacon | 0:15a8480061e8 | 57 | //Farbsensor: |
beacon | 0:15a8480061e8 | 58 | AnalogIn FarbVoltage(A0); |
beacon | 0:15a8480061e8 | 59 | //DigitalOut led(D2); |
beacon | 0:15a8480061e8 | 60 | |
beacon | 0:15a8480061e8 | 61 | |
beacon | 0:15a8480061e8 | 62 | Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer, &servoLeiste, &usensor, &pixy ); //Implement the Farbsensor into the Robot init function!! |
beacon | 0:15a8480061e8 | 63 | |
beacon | 0:15a8480061e8 | 64 | void initializeDistanceSensors() |
beacon | 0:15a8480061e8 | 65 | { |
beacon | 0:15a8480061e8 | 66 | for( int ii = 0; ii<9; ++ii) { |
beacon | 0:15a8480061e8 | 67 | sam.sensors[ii].init(&sensorVoltage, &frontS, &leftS, &rightS, &bit0, &bit1, &bit2, ii); |
beacon | 0:15a8480061e8 | 68 | |
beacon | 0:15a8480061e8 | 69 | enable = 1; |
beacon | 0:15a8480061e8 | 70 | } |
beacon | 0:15a8480061e8 | 71 | } |
beacon | 0:15a8480061e8 | 72 | /* */ |
EpicG10 | 2:c8c965d48f8d | 73 | int main() |
EpicG10 | 2:c8c965d48f8d | 74 | { |
beacon | 0:15a8480061e8 | 75 | initializeDistanceSensors(); //Initialises IR-Sensors. |
EpicG10 | 2:c8c965d48f8d | 76 | enum states {search = 0, setPos, take}; |
beacon | 0:15a8480061e8 | 77 | int time = 0; //Time keeps track of time. [time] = ms |
EpicG10 | 2:c8c965d48f8d | 78 | |
beacon | 0:15a8480061e8 | 79 | PID_Control pid; |
EpicG10 | 2:c8c965d48f8d | 80 | pid.setPIDValues( 0.001f, 0.001f, 0.00015f, 0.15f, -0.15f, 1000); |
EpicG10 | 2:c8c965d48f8d | 81 | // pc.baud( 115200 ); |
EpicG10 | 2:c8c965d48f8d | 82 | |
EpicG10 | 2:c8c965d48f8d | 83 | int state = search; |
EpicG10 | 2:c8c965d48f8d | 84 | while(!sam.Arm.collectToBack())wait(0.075); |
EpicG10 | 2:c8c965d48f8d | 85 | while(!sam.Greifer.leave())wait(0.001); |
EpicG10 | 2:c8c965d48f8d | 86 | |
EpicG10 | 2:c8c965d48f8d | 87 | |
EpicG10 | 2:c8c965d48f8d | 88 | while( 1 ) { |
EpicG10 | 2:c8c965d48f8d | 89 | |
EpicG10 | 2:c8c965d48f8d | 90 | switch(state){ |
beacon | 0:15a8480061e8 | 91 | |
EpicG10 | 2:c8c965d48f8d | 92 | case search: { |
EpicG10 | 2:c8c965d48f8d | 93 | if(!((pixy.getX()>50 && pixy.getX()<300)&&(pixy.getY()>50 && pixy.getY()<300))){ |
EpicG10 | 2:c8c965d48f8d | 94 | sam.search(&time); |
beacon | 0:15a8480061e8 | 95 | } |
EpicG10 | 2:c8c965d48f8d | 96 | else { |
EpicG10 | 2:c8c965d48f8d | 97 | state = setPos; |
EpicG10 | 2:c8c965d48f8d | 98 | } |
EpicG10 | 2:c8c965d48f8d | 99 | |
EpicG10 | 2:c8c965d48f8d | 100 | break; |
EpicG10 | 2:c8c965d48f8d | 101 | } |
EpicG10 | 2:c8c965d48f8d | 102 | |
EpicG10 | 2:c8c965d48f8d | 103 | case setPos: { |
EpicG10 | 2:c8c965d48f8d | 104 | sam.leds[5] = 1; |
EpicG10 | 2:c8c965d48f8d | 105 | static int i = 0; |
EpicG10 | 2:c8c965d48f8d | 106 | float eX = 133.0f - pixy.getX(); |
EpicG10 | 2:c8c965d48f8d | 107 | float diffX = pid.calc( eX, 0.006f ); |
EpicG10 | 2:c8c965d48f8d | 108 | |
EpicG10 | 2:c8c965d48f8d | 109 | float aX = 0.03; //minimum diff |
EpicG10 | 2:c8c965d48f8d | 110 | float aY = 0.05; |
EpicG10 | 2:c8c965d48f8d | 111 | if(diffX>0) { |
EpicG10 | 2:c8c965d48f8d | 112 | if(diffX<aX)diffX=aX; |
EpicG10 | 2:c8c965d48f8d | 113 | } |
EpicG10 | 2:c8c965d48f8d | 114 | else if(diffX>-aX)diffX=-aX; |
EpicG10 | 2:c8c965d48f8d | 115 | |
EpicG10 | 2:c8c965d48f8d | 116 | if(!(pixy.getX()>128 && pixy.getX()<138)){ |
EpicG10 | 2:c8c965d48f8d | 117 | sam.setLeft(0.5f - diffX); |
EpicG10 | 2:c8c965d48f8d | 118 | sam.setRight(0.5f - diffX); |
EpicG10 | 2:c8c965d48f8d | 119 | } |
EpicG10 | 2:c8c965d48f8d | 120 | |
EpicG10 | 2:c8c965d48f8d | 121 | wait(0.001f); |
EpicG10 | 2:c8c965d48f8d | 122 | |
EpicG10 | 2:c8c965d48f8d | 123 | float eY = 121.0f - pixy.getY(); |
EpicG10 | 2:c8c965d48f8d | 124 | float diffY = pid.calc( eY, 0.006f ); |
EpicG10 | 2:c8c965d48f8d | 125 | if(diffY>0) { |
EpicG10 | 2:c8c965d48f8d | 126 | if(diffY<aY)diffY=aY; |
EpicG10 | 2:c8c965d48f8d | 127 | } |
EpicG10 | 2:c8c965d48f8d | 128 | else if(diffY>-aY) diffY=-aY; |
EpicG10 | 2:c8c965d48f8d | 129 | if(!(pixy.getY()>115 && pixy.getY()<125)){ |
EpicG10 | 2:c8c965d48f8d | 130 | sam.setLeft(0.5f + diffY); |
EpicG10 | 2:c8c965d48f8d | 131 | sam.setRight(0.5f - diffY); |
EpicG10 | 2:c8c965d48f8d | 132 | } |
EpicG10 | 2:c8c965d48f8d | 133 | i++; |
EpicG10 | 2:c8c965d48f8d | 134 | if((pixy.getX()>132 && pixy.getX()<134)&&(pixy.getY()>119 && pixy.getY()<122)||!(i%1000)) { |
EpicG10 | 2:c8c965d48f8d | 135 | state = take; |
EpicG10 | 2:c8c965d48f8d | 136 | i = 0; |
EpicG10 | 2:c8c965d48f8d | 137 | sam.stop(); |
EpicG10 | 2:c8c965d48f8d | 138 | sam.leds[5] = 0; |
EpicG10 | 2:c8c965d48f8d | 139 | } |
EpicG10 | 2:c8c965d48f8d | 140 | break; |
EpicG10 | 2:c8c965d48f8d | 141 | } |
EpicG10 | 2:c8c965d48f8d | 142 | |
EpicG10 | 2:c8c965d48f8d | 143 | case take: { |
EpicG10 | 2:c8c965d48f8d | 144 | enum takeStates {down=0, take, up, leave}; |
EpicG10 | 2:c8c965d48f8d | 145 | static int tState=down; |
EpicG10 | 2:c8c965d48f8d | 146 | |
EpicG10 | 2:c8c965d48f8d | 147 | sam.leds[1] = 1; |
EpicG10 | 2:c8c965d48f8d | 148 | sam.stop(); |
EpicG10 | 2:c8c965d48f8d | 149 | //tState=leave; |
EpicG10 | 2:c8c965d48f8d | 150 | |
EpicG10 | 2:c8c965d48f8d | 151 | switch(tState) { |
EpicG10 | 2:c8c965d48f8d | 152 | case down: |
EpicG10 | 2:c8c965d48f8d | 153 | if(sam.Arm.backToDown()) tState=take; |
EpicG10 | 2:c8c965d48f8d | 154 | else tState=down; |
EpicG10 | 2:c8c965d48f8d | 155 | break; |
EpicG10 | 2:c8c965d48f8d | 156 | case take: |
EpicG10 | 2:c8c965d48f8d | 157 | if(sam.Greifer.take()) tState=up; |
EpicG10 | 2:c8c965d48f8d | 158 | else tState=take; |
EpicG10 | 2:c8c965d48f8d | 159 | break; |
EpicG10 | 2:c8c965d48f8d | 160 | case up: |
EpicG10 | 2:c8c965d48f8d | 161 | if(sam.Arm.downToBack()) {wait(0.05);tState=leave;} |
EpicG10 | 2:c8c965d48f8d | 162 | else tState=up; |
EpicG10 | 2:c8c965d48f8d | 163 | break; |
EpicG10 | 2:c8c965d48f8d | 164 | case leave: |
EpicG10 | 2:c8c965d48f8d | 165 | if(sam.Greifer.leave()) {state=search; tState=down;} |
EpicG10 | 2:c8c965d48f8d | 166 | else tState=leave; |
EpicG10 | 2:c8c965d48f8d | 167 | break; |
beacon | 0:15a8480061e8 | 168 | } |
EpicG10 | 2:c8c965d48f8d | 169 | |
EpicG10 | 2:c8c965d48f8d | 170 | |
EpicG10 | 2:c8c965d48f8d | 171 | sam.leds[1] = 0; |
EpicG10 | 2:c8c965d48f8d | 172 | break; |
beacon | 0:15a8480061e8 | 173 | } |
beacon | 0:15a8480061e8 | 174 | } |
EpicG10 | 2:c8c965d48f8d | 175 | time++; |
EpicG10 | 2:c8c965d48f8d | 176 | wait(0.005f); |
EpicG10 | 2:c8c965d48f8d | 177 | } |
EpicG10 | 2:c8c965d48f8d | 178 | |
EpicG10 | 2:c8c965d48f8d | 179 | return 0; |
beacon | 0:15a8480061e8 | 180 | } |
beacon | 0:15a8480061e8 | 181 | |
beacon | 0:15a8480061e8 | 182 | /* * / |
beacon | 0:15a8480061e8 | 183 | int main() |
beacon | 0:15a8480061e8 | 184 | { |
beacon | 0:15a8480061e8 | 185 | initializeDistanceSensors(); //Initialises IR-Sensors. |
beacon | 0:15a8480061e8 | 186 | //int counter = 0; //Counts how many times the robot has turned, before driving |
beacon | 0:15a8480061e8 | 187 | int timer = 0; //Is used, to reset the robot. Returns time in [0.1s] |
beacon | 0:15a8480061e8 | 188 | //int lastFun = -1; //Is used, to check if the same action in Robot::search() was made multiple times. |
beacon | 0:15a8480061e8 | 189 | //int found = 0; //0:= no block available, 1 := a lego is ready to be picked up |
beacon | 0:15a8480061e8 | 190 | //int done = 0; |
beacon | 0:15a8480061e8 | 191 | int color; |
EpicG10 | 2:c8c965d48f8d | 192 | |
beacon | 0:15a8480061e8 | 193 | enum states { search = 0, LeisteDown, turn, push, backOff, forward, downward, down, upward, colorS, readyDrop, backward, LeisteUp }; |
beacon | 0:15a8480061e8 | 194 | |
beacon | 0:15a8480061e8 | 195 | int state = search; |
EpicG10 | 2:c8c965d48f8d | 196 | |
beacon | 0:15a8480061e8 | 197 | static float messung = 0; |
EpicG10 | 2:c8c965d48f8d | 198 | |
beacon | 0:15a8480061e8 | 199 | while( 1 ){ |
beacon | 0:15a8480061e8 | 200 | printf("\n\r%f", sam.see(FWD_L)); |
EpicG10 | 2:c8c965d48f8d | 201 | |
beacon | 0:15a8480061e8 | 202 | wait(1.0f); |
EpicG10 | 2:c8c965d48f8d | 203 | |
beacon | 0:15a8480061e8 | 204 | } |
EpicG10 | 2:c8c965d48f8d | 205 | |
beacon | 0:15a8480061e8 | 206 | while( 1 ) { |
EpicG10 | 2:c8c965d48f8d | 207 | |
beacon | 0:15a8480061e8 | 208 | if ( timer > TIMEOUT ) { |
beacon | 0:15a8480061e8 | 209 | NVIC_SystemReset(); //Resets Sam. |
beacon | 0:15a8480061e8 | 210 | } |
EpicG10 | 2:c8c965d48f8d | 211 | |
beacon | 0:15a8480061e8 | 212 | //if( timer == 0 ) |
beacon | 0:15a8480061e8 | 213 | //printf("\n\rLEFT: %.3f,\tFWD: %.3f,\tRIGHT: %.3f", sam.sensors[LEFT].read(), sam.sensors[FWD].read(), sam.sensors[RIGHT].read()); |
EpicG10 | 2:c8c965d48f8d | 214 | |
EpicG10 | 2:c8c965d48f8d | 215 | |
beacon | 0:15a8480061e8 | 216 | //printf("\n\rcurrent main state: %d", state); |
EpicG10 | 2:c8c965d48f8d | 217 | |
beacon | 0:15a8480061e8 | 218 | sam.sensors[FWD_L].read() < NEAR ? sam.leds[1] = 1 : sam.leds[1] = 0; |
beacon | 0:15a8480061e8 | 219 | switch( state ) { |
beacon | 0:15a8480061e8 | 220 | case search: |
beacon | 0:15a8480061e8 | 221 | if( sam.search(&timer) ){ |
beacon | 0:15a8480061e8 | 222 | //sam.Greifer.nullPos(); |
beacon | 0:15a8480061e8 | 223 | state = LeisteDown; |
beacon | 0:15a8480061e8 | 224 | timer = 0; |
beacon | 0:15a8480061e8 | 225 | } |
beacon | 0:15a8480061e8 | 226 | break; |
EpicG10 | 2:c8c965d48f8d | 227 | |
beacon | 0:15a8480061e8 | 228 | case LeisteDown: |
beacon | 0:15a8480061e8 | 229 | int count = 0; |
beacon | 0:15a8480061e8 | 230 | if( sam.Leiste.upToDown() ){ |
beacon | 0:15a8480061e8 | 231 | //sam.Greifer.leave(); |
beacon | 0:15a8480061e8 | 232 | state = turn; |
beacon | 0:15a8480061e8 | 233 | timer = 0; |
beacon | 0:15a8480061e8 | 234 | } |
beacon | 0:15a8480061e8 | 235 | break; |
EpicG10 | 2:c8c965d48f8d | 236 | |
beacon | 0:15a8480061e8 | 237 | case turn: |
beacon | 0:15a8480061e8 | 238 | static int i = 0; |
beacon | 0:15a8480061e8 | 239 | if( i > 7 ){ |
beacon | 0:15a8480061e8 | 240 | sam.stop(); |
beacon | 0:15a8480061e8 | 241 | state = push; |
beacon | 0:15a8480061e8 | 242 | i = 0; |
beacon | 0:15a8480061e8 | 243 | } |
beacon | 0:15a8480061e8 | 244 | else{ |
beacon | 0:15a8480061e8 | 245 | i++; |
beacon | 0:15a8480061e8 | 246 | sam.turnRight(); |
beacon | 0:15a8480061e8 | 247 | } |
beacon | 0:15a8480061e8 | 248 | break; |
EpicG10 | 2:c8c965d48f8d | 249 | |
beacon | 0:15a8480061e8 | 250 | case push:{ |
beacon | 0:15a8480061e8 | 251 | static int i = 0; |
beacon | 0:15a8480061e8 | 252 | if( i > 5 ){ |
beacon | 0:15a8480061e8 | 253 | sam.stop(); |
beacon | 0:15a8480061e8 | 254 | i = 0; |
beacon | 0:15a8480061e8 | 255 | state = backOff; |
beacon | 0:15a8480061e8 | 256 | timer = 0; |
beacon | 0:15a8480061e8 | 257 | } |
beacon | 0:15a8480061e8 | 258 | else{ |
beacon | 0:15a8480061e8 | 259 | sam.driveSlowly(); |
beacon | 0:15a8480061e8 | 260 | i++; |
beacon | 0:15a8480061e8 | 261 | } |
beacon | 0:15a8480061e8 | 262 | break; |
beacon | 0:15a8480061e8 | 263 | } |
EpicG10 | 2:c8c965d48f8d | 264 | |
beacon | 0:15a8480061e8 | 265 | case backOff:{ |
beacon | 0:15a8480061e8 | 266 | static int i = 0; |
beacon | 0:15a8480061e8 | 267 | if( i > 1 ){ |
beacon | 0:15a8480061e8 | 268 | sam.stop(); |
beacon | 0:15a8480061e8 | 269 | i = 0; |
beacon | 0:15a8480061e8 | 270 | state = forward; |
beacon | 0:15a8480061e8 | 271 | timer = 0; |
beacon | 0:15a8480061e8 | 272 | } |
beacon | 0:15a8480061e8 | 273 | else{ |
beacon | 0:15a8480061e8 | 274 | sam.driveBackSlowly(); |
beacon | 0:15a8480061e8 | 275 | i++; |
beacon | 0:15a8480061e8 | 276 | } |
beacon | 0:15a8480061e8 | 277 | break; |
beacon | 0:15a8480061e8 | 278 | } |
EpicG10 | 2:c8c965d48f8d | 279 | |
EpicG10 | 2:c8c965d48f8d | 280 | |
beacon | 0:15a8480061e8 | 281 | case forward: |
beacon | 0:15a8480061e8 | 282 | if( sam.Arm.backToCollect() ){ |
beacon | 0:15a8480061e8 | 283 | state = downward; |
beacon | 0:15a8480061e8 | 284 | timer = 0; |
beacon | 0:15a8480061e8 | 285 | } |
beacon | 0:15a8480061e8 | 286 | break; |
beacon | 0:15a8480061e8 | 287 | |
beacon | 0:15a8480061e8 | 288 | case downward: |
beacon | 0:15a8480061e8 | 289 | if( sam.Arm.collectToDown() ){ |
beacon | 0:15a8480061e8 | 290 | state = down; |
beacon | 0:15a8480061e8 | 291 | timer = 0; |
beacon | 0:15a8480061e8 | 292 | } |
beacon | 0:15a8480061e8 | 293 | break; |
beacon | 0:15a8480061e8 | 294 | |
beacon | 0:15a8480061e8 | 295 | case down: |
beacon | 0:15a8480061e8 | 296 | if( sam.Greifer.take() ) { |
beacon | 0:15a8480061e8 | 297 | state = upward; |
beacon | 0:15a8480061e8 | 298 | timer = 0; |
beacon | 0:15a8480061e8 | 299 | } |
beacon | 0:15a8480061e8 | 300 | break; |
EpicG10 | 2:c8c965d48f8d | 301 | |
beacon | 0:15a8480061e8 | 302 | case upward: |
beacon | 0:15a8480061e8 | 303 | if( sam.Arm.downToCollect() ){ |
beacon | 0:15a8480061e8 | 304 | state = colorS; |
beacon | 0:15a8480061e8 | 305 | timer = 0; |
beacon | 0:15a8480061e8 | 306 | } |
beacon | 0:15a8480061e8 | 307 | break; |
EpicG10 | 2:c8c965d48f8d | 308 | |
beacon | 0:15a8480061e8 | 309 | case colorS: |
beacon | 0:15a8480061e8 | 310 | led = 1; |
beacon | 0:15a8480061e8 | 311 | color = sam.FarbVoltage.read(); |
EpicG10 | 2:c8c965d48f8d | 312 | |
beacon | 0:15a8480061e8 | 313 | if( color == -1 ){ |
beacon | 0:15a8480061e8 | 314 | //Do nothing |
beacon | 0:15a8480061e8 | 315 | } |
EpicG10 | 2:c8c965d48f8d | 316 | |
EpicG10 | 2:c8c965d48f8d | 317 | |
beacon | 0:15a8480061e8 | 318 | else if( color == 0 || color == GREEN ){ |
beacon | 0:15a8480061e8 | 319 | state = backward; |
beacon | 0:15a8480061e8 | 320 | led = 0; |
beacon | 0:15a8480061e8 | 321 | timer = 0; |
beacon | 0:15a8480061e8 | 322 | } |
EpicG10 | 2:c8c965d48f8d | 323 | |
beacon | 0:15a8480061e8 | 324 | else if( color == RED ){ |
beacon | 0:15a8480061e8 | 325 | state = readyDrop; |
beacon | 0:15a8480061e8 | 326 | led = 0; |
beacon | 0:15a8480061e8 | 327 | timer = 0; |
beacon | 0:15a8480061e8 | 328 | } |
EpicG10 | 2:c8c965d48f8d | 329 | |
beacon | 0:15a8480061e8 | 330 | else{ |
beacon | 0:15a8480061e8 | 331 | //Shit... |
beacon | 0:15a8480061e8 | 332 | } |
beacon | 0:15a8480061e8 | 333 | break; |
EpicG10 | 2:c8c965d48f8d | 334 | |
beacon | 0:15a8480061e8 | 335 | case readyDrop: |
beacon | 0:15a8480061e8 | 336 | if( sam.Greifer.leave() ){ |
beacon | 0:15a8480061e8 | 337 | if( color == GREEN || color == 0 ){ |
beacon | 0:15a8480061e8 | 338 | state = LeisteUp; |
beacon | 0:15a8480061e8 | 339 | } |
beacon | 0:15a8480061e8 | 340 | else{ |
beacon | 0:15a8480061e8 | 341 | state = backward; |
beacon | 0:15a8480061e8 | 342 | } |
beacon | 0:15a8480061e8 | 343 | timer = 0; |
beacon | 0:15a8480061e8 | 344 | } |
EpicG10 | 2:c8c965d48f8d | 345 | |
beacon | 0:15a8480061e8 | 346 | break; |
EpicG10 | 2:c8c965d48f8d | 347 | |
beacon | 0:15a8480061e8 | 348 | case backward: |
beacon | 0:15a8480061e8 | 349 | if( sam.Arm.collectToBack() ){ |
beacon | 0:15a8480061e8 | 350 | //sam.Greifer.nullPos(); |
beacon | 0:15a8480061e8 | 351 | state = LeisteUp; |
beacon | 0:15a8480061e8 | 352 | timer = 0; |
beacon | 0:15a8480061e8 | 353 | if( color == GREEN || color == 0 ){ |
beacon | 0:15a8480061e8 | 354 | state = readyDrop; |
beacon | 0:15a8480061e8 | 355 | sam.Greifer.leave(); |
beacon | 0:15a8480061e8 | 356 | } |
beacon | 0:15a8480061e8 | 357 | else{ |
beacon | 0:15a8480061e8 | 358 | state = LeisteUp; |
beacon | 0:15a8480061e8 | 359 | } |
beacon | 0:15a8480061e8 | 360 | } |
beacon | 0:15a8480061e8 | 361 | break; |
EpicG10 | 2:c8c965d48f8d | 362 | |
beacon | 0:15a8480061e8 | 363 | case LeisteUp: |
beacon | 0:15a8480061e8 | 364 | if( sam.Leiste.downToUp() ){ |
beacon | 0:15a8480061e8 | 365 | state = search; |
beacon | 0:15a8480061e8 | 366 | timer = 0; |
beacon | 0:15a8480061e8 | 367 | } |
beacon | 0:15a8480061e8 | 368 | break; |
EpicG10 | 2:c8c965d48f8d | 369 | } |
beacon | 0:15a8480061e8 | 370 | timer++; |
beacon | 0:15a8480061e8 | 371 | wait(0.1f); |
beacon | 0:15a8480061e8 | 372 | } |
EpicG10 | 2:c8c965d48f8d | 373 | |
beacon | 0:15a8480061e8 | 374 | return 0; |
beacon | 0:15a8480061e8 | 375 | } |
beacon | 0:15a8480061e8 | 376 | /* */ |