vf
Dependencies: Servo ServoArm mbed
Fork of PES_Official by
Sources/main.cpp@15:915f8839fe48, 2017-05-11 (annotated)
- Committer:
- EpicG10
- Date:
- Thu May 11 18:57:45 2017 +0000
- Revision:
- 15:915f8839fe48
- Parent:
- 14:b61fbd13a7c9
- Child:
- 16:f157e5ccd7d3
- Child:
- 17:4e1be70bdedb
c
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
beacon | 0:d267b248eff4 | 1 | #include "mbed.h" |
beacon | 0:d267b248eff4 | 2 | #include "Robot.h" |
beacon | 4:67d7177c213f | 3 | #include "Declarations.h" |
beacon | 0:d267b248eff4 | 4 | |
beacon | 0:d267b248eff4 | 5 | #include <cstdlib> |
beacon | 0:d267b248eff4 | 6 | |
beacon | 4:67d7177c213f | 7 | //DistanceSensors related bottom: |
beacon | 0:d267b248eff4 | 8 | Serial pc(SERIAL_TX, SERIAL_RX); |
beacon | 0:d267b248eff4 | 9 | |
beacon | 0:d267b248eff4 | 10 | AnalogIn sensorVoltage(PB_1); |
beacon | 0:d267b248eff4 | 11 | DigitalOut enable(PC_1); |
beacon | 0:d267b248eff4 | 12 | DigitalOut bit0(PH_1); |
beacon | 0:d267b248eff4 | 13 | DigitalOut bit1(PC_2); |
beacon | 0:d267b248eff4 | 14 | DigitalOut bit2(PC_3); |
beacon | 1:388c915756f5 | 15 | |
beacon | 4:67d7177c213f | 16 | //DistanceSensors top: |
beacon | 4:67d7177c213f | 17 | AnalogIn frontS(A1); |
beacon | 4:67d7177c213f | 18 | AnalogIn leftS(A2); |
beacon | 4:67d7177c213f | 19 | AnalogIn rightS(A3); |
beacon | 4:67d7177c213f | 20 | |
beacon | 1:388c915756f5 | 21 | //Leds related: |
beacon | 1:388c915756f5 | 22 | DigitalOut leds[6] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 }; |
beacon | 0:d267b248eff4 | 23 | |
beacon | 1:388c915756f5 | 24 | //motor related: |
beacon | 1:388c915756f5 | 25 | PwmOut left(PA_8); |
beacon | 1:388c915756f5 | 26 | PwmOut right(PA_9); |
beacon | 0:d267b248eff4 | 27 | |
beacon | 1:388c915756f5 | 28 | DigitalOut powerSignal(PB_2); |
beacon | 10:f76476943a6c | 29 | DigitalIn errorMotor(PB_14); |
beacon | 1:388c915756f5 | 30 | DigitalIn overtemperatur(PB_15); |
beacon | 1:388c915756f5 | 31 | |
beacon | 4:67d7177c213f | 32 | //Arm: |
beacon | 10:f76476943a6c | 33 | ServoArm servoArm(PA_6); |
beacon | 10:f76476943a6c | 34 | |
beacon | 10:f76476943a6c | 35 | //Greifer: |
beacon | 12:c0bcb95885dd | 36 | Servo servoGreifer(PC_7); |
beacon | 4:67d7177c213f | 37 | |
EpicG10 | 14:b61fbd13a7c9 | 38 | //Leiste: |
EpicG10 | 15:915f8839fe48 | 39 | Servo servoLeiste(PB_6); |
EpicG10 | 14:b61fbd13a7c9 | 40 | |
beacon | 1:388c915756f5 | 41 | //Farbsensor: |
beacon | 1:388c915756f5 | 42 | AnalogIn FarbVoltage(A0); |
beacon | 12:c0bcb95885dd | 43 | DigitalOut led(D2); |
beacon | 1:388c915756f5 | 44 | |
EpicG10 | 15:915f8839fe48 | 45 | Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer, &servoLeiste ); //Implement the Farbsensor into the Robot init function!! |
beacon | 10:f76476943a6c | 46 | |
beacon | 11:292bdbd85a9c | 47 | void initializeDistanceSensors() |
beacon | 11:292bdbd85a9c | 48 | { |
beacon | 10:f76476943a6c | 49 | for( int ii = 0; ii<9; ++ii) { |
beacon | 10:f76476943a6c | 50 | sam.sensors[ii].init(&sensorVoltage, &frontS, &leftS, &rightS, &bit0, &bit1, &bit2, ii); |
beacon | 10:f76476943a6c | 51 | |
beacon | 11:292bdbd85a9c | 52 | enable = 1; |
beacon | 10:f76476943a6c | 53 | } |
beacon | 10:f76476943a6c | 54 | } |
beacon | 1:388c915756f5 | 55 | |
beacon | 0:d267b248eff4 | 56 | |
beacon | 10:f76476943a6c | 57 | /* */ |
beacon | 11:292bdbd85a9c | 58 | int main() |
beacon | 11:292bdbd85a9c | 59 | { |
beacon | 4:67d7177c213f | 60 | initializeDistanceSensors(); //Initialises IR-Sensors. |
beacon | 11:292bdbd85a9c | 61 | //int counter = 0; //Counts how many times the robot has turned, before driving |
beacon | 4:67d7177c213f | 62 | int timer = 0; //Is used, to reset the robot. Returns time in [0.1s] |
beacon | 4:67d7177c213f | 63 | //int lastFun = -1; //Is used, to check if the same action in Robot::search() was made multiple times. |
beacon | 11:292bdbd85a9c | 64 | //int found = 0; //0:= no block available, 1 := a lego is ready to be picked up |
beacon | 11:292bdbd85a9c | 65 | //int done = 0; |
beacon | 11:292bdbd85a9c | 66 | |
EpicG10 | 15:915f8839fe48 | 67 | enum states { search = 0, LeisteDown, forward, downward, down, upward, color, backwardDrop, readyDrop, backward, LeisteUp }; |
beacon | 11:292bdbd85a9c | 68 | |
EpicG10 | 13:6c1f8e35b242 | 69 | int state = search; |
EpicG10 | 15:915f8839fe48 | 70 | wait(5); |
beacon | 11:292bdbd85a9c | 71 | while( 1 ) { |
beacon | 10:f76476943a6c | 72 | |
beacon | 11:292bdbd85a9c | 73 | if ( timer > TIMEOUT ) { |
beacon | 4:67d7177c213f | 74 | NVIC_SystemReset(); //Resets Sam. |
beacon | 4:67d7177c213f | 75 | } |
beacon | 11:292bdbd85a9c | 76 | |
beacon | 11:292bdbd85a9c | 77 | switch( state ) { |
beacon | 11:292bdbd85a9c | 78 | case search: |
EpicG10 | 13:6c1f8e35b242 | 79 | if( 1 || sam.search(&timer) ){ |
EpicG10 | 15:915f8839fe48 | 80 | state = LeisteDown; |
EpicG10 | 15:915f8839fe48 | 81 | timer = 0; |
EpicG10 | 15:915f8839fe48 | 82 | } |
EpicG10 | 15:915f8839fe48 | 83 | |
EpicG10 | 15:915f8839fe48 | 84 | break; |
EpicG10 | 15:915f8839fe48 | 85 | |
EpicG10 | 15:915f8839fe48 | 86 | case LeisteDown: |
EpicG10 | 15:915f8839fe48 | 87 | if( sam.Leiste.UpToDown() ){ |
EpicG10 | 15:915f8839fe48 | 88 | sam.Greifer.leave(); |
beacon | 11:292bdbd85a9c | 89 | state = forward; |
beacon | 11:292bdbd85a9c | 90 | timer = 0; |
beacon | 11:292bdbd85a9c | 91 | } |
beacon | 11:292bdbd85a9c | 92 | |
beacon | 11:292bdbd85a9c | 93 | break; |
beacon | 11:292bdbd85a9c | 94 | |
beacon | 11:292bdbd85a9c | 95 | case forward: |
beacon | 11:292bdbd85a9c | 96 | if( sam.Arm.backToCollect() ){ |
beacon | 11:292bdbd85a9c | 97 | state = downward; |
beacon | 11:292bdbd85a9c | 98 | timer = 0; |
beacon | 11:292bdbd85a9c | 99 | } |
beacon | 11:292bdbd85a9c | 100 | |
beacon | 11:292bdbd85a9c | 101 | break; |
beacon | 11:292bdbd85a9c | 102 | |
beacon | 11:292bdbd85a9c | 103 | case downward: |
beacon | 11:292bdbd85a9c | 104 | if( sam.Arm.collectToDown() ){ |
beacon | 11:292bdbd85a9c | 105 | state = down; |
beacon | 11:292bdbd85a9c | 106 | timer = 0; |
beacon | 11:292bdbd85a9c | 107 | } |
beacon | 11:292bdbd85a9c | 108 | |
beacon | 11:292bdbd85a9c | 109 | break; |
beacon | 11:292bdbd85a9c | 110 | |
beacon | 11:292bdbd85a9c | 111 | case down: |
beacon | 11:292bdbd85a9c | 112 | if( sam.Greifer.take() ) { |
beacon | 11:292bdbd85a9c | 113 | state = upward; |
beacon | 11:292bdbd85a9c | 114 | timer = 0; |
beacon | 11:292bdbd85a9c | 115 | } |
beacon | 11:292bdbd85a9c | 116 | |
beacon | 11:292bdbd85a9c | 117 | break; |
beacon | 11:292bdbd85a9c | 118 | |
beacon | 11:292bdbd85a9c | 119 | case upward: |
beacon | 11:292bdbd85a9c | 120 | if( sam.Arm.downToCollect() ){ |
beacon | 11:292bdbd85a9c | 121 | state = color; |
beacon | 11:292bdbd85a9c | 122 | timer = 0; |
beacon | 11:292bdbd85a9c | 123 | } |
beacon | 11:292bdbd85a9c | 124 | |
beacon | 11:292bdbd85a9c | 125 | break; |
beacon | 11:292bdbd85a9c | 126 | |
beacon | 11:292bdbd85a9c | 127 | case color: { |
beacon | 12:c0bcb95885dd | 128 | led = 1; |
beacon | 11:292bdbd85a9c | 129 | int color = sam.FarbVoltage.read(); |
beacon | 11:292bdbd85a9c | 130 | |
beacon | 12:c0bcb95885dd | 131 | if( color == -1 ){ |
beacon | 11:292bdbd85a9c | 132 | //Do nothing |
beacon | 11:292bdbd85a9c | 133 | } |
beacon | 11:292bdbd85a9c | 134 | |
beacon | 12:c0bcb95885dd | 135 | |
beacon | 11:292bdbd85a9c | 136 | else if( color == 0 || color == GREEN ){ |
beacon | 11:292bdbd85a9c | 137 | state = backwardDrop; |
beacon | 12:c0bcb95885dd | 138 | led = 0; |
beacon | 11:292bdbd85a9c | 139 | timer = 0; |
beacon | 11:292bdbd85a9c | 140 | } |
beacon | 11:292bdbd85a9c | 141 | |
beacon | 11:292bdbd85a9c | 142 | else if( color == RED ){ |
beacon | 11:292bdbd85a9c | 143 | state = readyDrop; |
beacon | 12:c0bcb95885dd | 144 | led = 0; |
beacon | 11:292bdbd85a9c | 145 | timer = 0; |
beacon | 11:292bdbd85a9c | 146 | } |
beacon | 11:292bdbd85a9c | 147 | |
beacon | 11:292bdbd85a9c | 148 | else{ |
beacon | 11:292bdbd85a9c | 149 | //Shit... |
beacon | 11:292bdbd85a9c | 150 | } |
beacon | 11:292bdbd85a9c | 151 | break; |
beacon | 11:292bdbd85a9c | 152 | } |
beacon | 11:292bdbd85a9c | 153 | |
beacon | 11:292bdbd85a9c | 154 | case backwardDrop: |
beacon | 11:292bdbd85a9c | 155 | if( sam.Arm.collectToBack() ){ |
beacon | 11:292bdbd85a9c | 156 | state = readyDrop; |
beacon | 11:292bdbd85a9c | 157 | timer = 0; |
beacon | 11:292bdbd85a9c | 158 | } |
beacon | 11:292bdbd85a9c | 159 | |
beacon | 11:292bdbd85a9c | 160 | break; |
beacon | 11:292bdbd85a9c | 161 | |
beacon | 11:292bdbd85a9c | 162 | case readyDrop: |
beacon | 11:292bdbd85a9c | 163 | if( sam.Greifer.leave() ){ |
beacon | 11:292bdbd85a9c | 164 | state = backward; |
beacon | 11:292bdbd85a9c | 165 | timer = 0; |
beacon | 11:292bdbd85a9c | 166 | } |
beacon | 11:292bdbd85a9c | 167 | |
beacon | 11:292bdbd85a9c | 168 | break; |
beacon | 11:292bdbd85a9c | 169 | |
beacon | 11:292bdbd85a9c | 170 | case backward: |
beacon | 11:292bdbd85a9c | 171 | if( sam.Arm.collectToBack() ){ |
EpicG10 | 15:915f8839fe48 | 172 | sam.Greifer.nullPos(); |
EpicG10 | 15:915f8839fe48 | 173 | state = LeisteUp; |
beacon | 11:292bdbd85a9c | 174 | timer = 0; |
beacon | 11:292bdbd85a9c | 175 | } |
beacon | 11:292bdbd85a9c | 176 | |
beacon | 11:292bdbd85a9c | 177 | break; |
EpicG10 | 15:915f8839fe48 | 178 | |
EpicG10 | 15:915f8839fe48 | 179 | case LeisteUp: |
EpicG10 | 15:915f8839fe48 | 180 | if( sam.Leiste.DownToUp() ){ |
EpicG10 | 15:915f8839fe48 | 181 | state = search; |
EpicG10 | 15:915f8839fe48 | 182 | timer = 0; |
EpicG10 | 15:915f8839fe48 | 183 | wait(2); |
EpicG10 | 15:915f8839fe48 | 184 | } |
EpicG10 | 15:915f8839fe48 | 185 | break; |
beacon | 4:67d7177c213f | 186 | } |
beacon | 1:388c915756f5 | 187 | |
beacon | 11:292bdbd85a9c | 188 | timer++; |
beacon | 11:292bdbd85a9c | 189 | wait(0.1f); |
beacon | 11:292bdbd85a9c | 190 | } |
beacon | 11:292bdbd85a9c | 191 | |
beacon | 11:292bdbd85a9c | 192 | return 0; |
beacon | 11:292bdbd85a9c | 193 | } |
beacon | 11:292bdbd85a9c | 194 | |
beacon | 11:292bdbd85a9c | 195 | |
beacon | 11:292bdbd85a9c | 196 | |
beacon | 11:292bdbd85a9c | 197 | /* |
beacon | 11:292bdbd85a9c | 198 | if (sam.search(&counter, &timer)) { |
beacon | 11:292bdbd85a9c | 199 | |
beacon | 11:292bdbd85a9c | 200 | while(!done) { |
beacon | 11:292bdbd85a9c | 201 | sam.Arm.collectToDown(&done); |
beacon | 11:292bdbd85a9c | 202 | wait(0.1f); |
beacon | 11:292bdbd85a9c | 203 | } |
beacon | 11:292bdbd85a9c | 204 | done = 0; |
beacon | 11:292bdbd85a9c | 205 | |
beacon | 11:292bdbd85a9c | 206 | sam.Greifer.take(); |
beacon | 11:292bdbd85a9c | 207 | |
beacon | 11:292bdbd85a9c | 208 | while(!done) { |
beacon | 11:292bdbd85a9c | 209 | sam.Arm.downToCollect(&done); |
beacon | 11:292bdbd85a9c | 210 | sam.leds[1] = 1; |
beacon | 11:292bdbd85a9c | 211 | wait(0.1f); |
beacon | 11:292bdbd85a9c | 212 | } |
beacon | 11:292bdbd85a9c | 213 | done = 0; |
beacon | 11:292bdbd85a9c | 214 | |
beacon | 11:292bdbd85a9c | 215 | sam.Greifer.leave(); |
beacon | 11:292bdbd85a9c | 216 | |
beacon | 11:292bdbd85a9c | 217 | found = 0; |
beacon | 11:292bdbd85a9c | 218 | } |
beacon | 11:292bdbd85a9c | 219 | |
beacon | 11:292bdbd85a9c | 220 | |
beacon | 1:388c915756f5 | 221 | wait( 0.1f ); |
beacon | 0:d267b248eff4 | 222 | } |
beacon | 0:d267b248eff4 | 223 | return 0; |
beacon | 4:67d7177c213f | 224 | } |
beacon | 11:292bdbd85a9c | 225 | */ |
beacon | 4:67d7177c213f | 226 | |
beacon | 4:67d7177c213f | 227 | /* * / |
beacon | 4:67d7177c213f | 228 | int main(){ |
beacon | 11:292bdbd85a9c | 229 | |
beacon | 4:67d7177c213f | 230 | initializeDistanceSensors(); |
beacon | 4:67d7177c213f | 231 | sam.stop(); |
beacon | 11:292bdbd85a9c | 232 | |
beacon | 4:67d7177c213f | 233 | while ( 1 ){ |
beacon | 4:67d7177c213f | 234 | for (int i=0; i<6; i++){ |
beacon | 4:67d7177c213f | 235 | sam.sensors[i] < NEAR ? sam.leds[i] = 1 : sam.leds[i] = 0; |
beacon | 4:67d7177c213f | 236 | } |
beacon | 11:292bdbd85a9c | 237 | |
beacon | 4:67d7177c213f | 238 | sam.turnLeftS(); |
beacon | 11:292bdbd85a9c | 239 | |
beacon | 4:67d7177c213f | 240 | while (sam.sensors[FWD] < NEAR){ |
beacon | 4:67d7177c213f | 241 | wait(0.05f); |
beacon | 4:67d7177c213f | 242 | sam.stop(); |
beacon | 4:67d7177c213f | 243 | } |
beacon | 11:292bdbd85a9c | 244 | |
beacon | 4:67d7177c213f | 245 | } |
beacon | 4:67d7177c213f | 246 | return 0; |
beacon | 4:67d7177c213f | 247 | } |
beacon | 4:67d7177c213f | 248 | /* */ |
beacon | 4:67d7177c213f | 249 | |
beacon | 4:67d7177c213f | 250 | /* * / |
beacon | 4:67d7177c213f | 251 | int main(){ |
beacon | 4:67d7177c213f | 252 | for(float p=0; p<1.0; p += 0.1) { |
beacon | 4:67d7177c213f | 253 | // arm = p; |
beacon | 4:67d7177c213f | 254 | wait(0.2); |
beacon | 4:67d7177c213f | 255 | } |
beacon | 4:67d7177c213f | 256 | } |
beacon | 6:ba26dd3251b3 | 257 | /* */ |
beacon | 6:ba26dd3251b3 | 258 | |
beacon | 10:f76476943a6c | 259 | /* * / |
beacon | 6:ba26dd3251b3 | 260 | int main(){ |
beacon | 6:ba26dd3251b3 | 261 | sam.stop(); |
beacon | 6:ba26dd3251b3 | 262 | int done = 0; //1:= finished process; 0:= not finished |
beacon | 6:ba26dd3251b3 | 263 | int fun = 0; //just to test. |
beacon | 11:292bdbd85a9c | 264 | int start = 0; |
beacon | 11:292bdbd85a9c | 265 | |
beacon | 11:292bdbd85a9c | 266 | |
beacon | 6:ba26dd3251b3 | 267 | while(1){ |
beacon | 6:ba26dd3251b3 | 268 | if(fun == 0){ |
beacon | 6:ba26dd3251b3 | 269 | done = 0; |
beacon | 6:ba26dd3251b3 | 270 | sam.Arm.collecttoback(&done); |
beacon | 11:292bdbd85a9c | 271 | done == 0 ? fun = 0 : fun = 1; |
beacon | 6:ba26dd3251b3 | 272 | } |
beacon | 6:ba26dd3251b3 | 273 | else if(fun == 1){ |
beacon | 6:ba26dd3251b3 | 274 | done = 0; |
beacon | 6:ba26dd3251b3 | 275 | sam.Arm.backtocollect(&done); |
beacon | 6:ba26dd3251b3 | 276 | done == 0 ? fun = 1 : fun = 2; |
beacon | 6:ba26dd3251b3 | 277 | } |
beacon | 6:ba26dd3251b3 | 278 | else if(fun == 2){ |
beacon | 6:ba26dd3251b3 | 279 | done = 0; |
beacon | 6:ba26dd3251b3 | 280 | sam.Arm.collecttodown(&done); |
beacon | 6:ba26dd3251b3 | 281 | done == 0 ? fun = 2 : fun = 3; |
beacon | 6:ba26dd3251b3 | 282 | } |
beacon | 6:ba26dd3251b3 | 283 | else if(fun == 3){ |
beacon | 6:ba26dd3251b3 | 284 | done = 0; |
beacon | 6:ba26dd3251b3 | 285 | sam.Arm.downtocollect(&done); |
beacon | 6:ba26dd3251b3 | 286 | done == 0 ? fun = 3 : fun = 0; |
beacon | 6:ba26dd3251b3 | 287 | } |
beacon | 6:ba26dd3251b3 | 288 | wait(0.1); |
beacon | 6:ba26dd3251b3 | 289 | } |
beacon | 10:f76476943a6c | 290 | } |
beacon | 10:f76476943a6c | 291 | |
beacon | 10:f76476943a6c | 292 | */ |