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