Gruppe 3
/
PES4
xx
Fork of PES1 by
main.cpp@12:b9faf8637183, 2017-04-19 (annotated)
- Committer:
- itslinear
- Date:
- Wed Apr 19 15:38:56 2017 +0000
- Revision:
- 12:b9faf8637183
- Parent:
- 9:d9e46f9c9e40
- Child:
- 13:7eba9911e196
new
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
itslinear | 0:306a2438de17 | 1 | #include <mbed.h> |
itslinear | 0:306a2438de17 | 2 | #include "Roboter.h" |
itslinear | 0:306a2438de17 | 3 | |
itslinear | 0:306a2438de17 | 4 | DigitalOut led(LED1); |
itslinear | 0:306a2438de17 | 5 | |
itslinear | 0:306a2438de17 | 6 | //Periphery for distance sensors |
itslinear | 1:4e84271a70c6 | 7 | AnalogIn distance(PB_1); |
itslinear | 0:306a2438de17 | 8 | DigitalOut enable(PC_1); |
itslinear | 0:306a2438de17 | 9 | DigitalOut bit0(PH_1); |
itslinear | 0:306a2438de17 | 10 | DigitalOut bit1(PC_2); |
itslinear | 0:306a2438de17 | 11 | DigitalOut bit2(PC_3); |
itslinear | 0:306a2438de17 | 12 | IRSensor sensors[6]; |
itslinear | 0:306a2438de17 | 13 | |
schneju2 | 2:953263712480 | 14 | // Periphery for servos |
itslinear | 4:fdcf3b5009c6 | 15 | Servo servos1(PB_7); |
itslinear | 4:fdcf3b5009c6 | 16 | Servo servos2(PA_6); |
itslinear | 4:fdcf3b5009c6 | 17 | |
itslinear | 6:4af735d26b7a | 18 | //Periphery for limit switch |
itslinear | 4:fdcf3b5009c6 | 19 | DigitalIn limitSwitch1(PA_10); |
itslinear | 6:4af735d26b7a | 20 | DigitalIn limitSwitch2(PB_13); |
schneju2 | 2:953263712480 | 21 | |
itslinear | 6:4af735d26b7a | 22 | //Periphery for Switch ON/OFF |
schneju2 | 5:f48b2609c328 | 23 | |
itslinear | 7:edb4e0cfc0d1 | 24 | DigitalIn switchOnOff(USER_BUTTON); |
schneju2 | 5:f48b2609c328 | 25 | |
itslinear | 0:306a2438de17 | 26 | //motor stuff |
itslinear | 0:306a2438de17 | 27 | DigitalOut enableMotorDriver(PB_2); |
itslinear | 3:24e098715b78 | 28 | PwmOut pwmL( PA_8 ); |
itslinear | 3:24e098715b78 | 29 | PwmOut pwmR( PA_9 ); |
itslinear | 0:306a2438de17 | 30 | |
itslinear | 0:306a2438de17 | 31 | //indicator leds arround robot |
itslinear | 0:306a2438de17 | 32 | DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 }; |
itslinear | 0:306a2438de17 | 33 | |
itslinear | 4:fdcf3b5009c6 | 34 | Roboter roboter1(&distance, &bit0, &bit1, &bit2, &pwmL, &pwmR, &servos1,&servos2, &limitSwitch1, &limitSwitch2); |
itslinear | 0:306a2438de17 | 35 | |
itslinear | 1:4e84271a70c6 | 36 | int main() |
itslinear | 1:4e84271a70c6 | 37 | { |
itslinear | 6:4af735d26b7a | 38 | enable = 1; // Sensoren einschalten |
itslinear | 6:4af735d26b7a | 39 | enableMotorDriver = 1; // Schaltet die Motoren ein |
itslinear | 7:edb4e0cfc0d1 | 40 | pwmL.period(0.00005f); // Setzt die Periode auf 50 μs |
itslinear | 7:edb4e0cfc0d1 | 41 | pwmR.period(0.00005f); |
schneju2 | 8:6d9cd5ad332d | 42 | pwmL = 0.5f; |
itslinear | 9:d9e46f9c9e40 | 43 | pwmR = 0.5f; |
itslinear | 9:d9e46f9c9e40 | 44 | |
itslinear | 6:4af735d26b7a | 45 | |
itslinear | 12:b9faf8637183 | 46 | int state = 0; // Diese Variable gibt an in welchem State man sich befindet |
itslinear | 3:24e098715b78 | 47 | |
itslinear | 9:d9e46f9c9e40 | 48 | |
itslinear | 9:d9e46f9c9e40 | 49 | while(1) { |
itslinear | 9:d9e46f9c9e40 | 50 | |
itslinear | 9:d9e46f9c9e40 | 51 | |
itslinear | 6:4af735d26b7a | 52 | |
itslinear | 6:4af735d26b7a | 53 | switch(state) { |
itslinear | 6:4af735d26b7a | 54 | |
itslinear | 12:b9faf8637183 | 55 | case 0: // Roboter Anschalten mit Knopf |
itslinear | 6:4af735d26b7a | 56 | |
itslinear | 12:b9faf8637183 | 57 | if (switchOnOff == 0) { |
itslinear | 12:b9faf8637183 | 58 | state = 1; |
itslinear | 9:d9e46f9c9e40 | 59 | } |
itslinear | 7:edb4e0cfc0d1 | 60 | |
itslinear | 9:d9e46f9c9e40 | 61 | break; |
itslinear | 6:4af735d26b7a | 62 | |
itslinear | 12:b9faf8637183 | 63 | case 1: |
itslinear | 12:b9faf8637183 | 64 | if(readCamera() == 1 || readCamera() == 2 || readCamera() == 3) { // Die Kamera erkennt ein grünen Klotz |
itslinear | 12:b9faf8637183 | 65 | state = 4; |
itslinear | 12:b9faf8637183 | 66 | } |
itslinear | 12:b9faf8637183 | 67 | if(readCamera() == 4) { // Roboter in Position |
itslinear | 12:b9faf8637183 | 68 | state = 5; |
itslinear | 12:b9faf8637183 | 69 | } |
itslinear | 12:b9faf8637183 | 70 | if(readCamera() == 0){ |
itslinear | 12:b9faf8637183 | 71 | state = 2; |
itslinear | 12:b9faf8637183 | 72 | } |
itslinear | 12:b9faf8637183 | 73 | |
itslinear | 12:b9faf8637183 | 74 | |
itslinear | 9:d9e46f9c9e40 | 75 | case 2: |
itslinear | 9:d9e46f9c9e40 | 76 | static int time1 = 0; |
itslinear | 9:d9e46f9c9e40 | 77 | if(time1 < 20) { // Im Kreis drehen für 1s |
itslinear | 9:d9e46f9c9e40 | 78 | pwmL = 0.4f; |
itslinear | 9:d9e46f9c9e40 | 79 | pwmR = 0.4f; |
itslinear | 9:d9e46f9c9e40 | 80 | time1 ++; |
itslinear | 12:b9faf8637183 | 81 | state = 1; |
itslinear | 9:d9e46f9c9e40 | 82 | } else { |
itslinear | 9:d9e46f9c9e40 | 83 | time1 = 0; |
itslinear | 9:d9e46f9c9e40 | 84 | pwmL = 0.5f; |
itslinear | 9:d9e46f9c9e40 | 85 | pwmR = 0.5f; |
itslinear | 9:d9e46f9c9e40 | 86 | state = 3; |
itslinear | 9:d9e46f9c9e40 | 87 | } |
itslinear | 9:d9e46f9c9e40 | 88 | break; |
itslinear | 6:4af735d26b7a | 89 | |
itslinear | 3:24e098715b78 | 90 | |
itslinear | 9:d9e46f9c9e40 | 91 | case 3: |
itslinear | 9:d9e46f9c9e40 | 92 | roboter1.bandeAusweichen(); |
itslinear | 9:d9e46f9c9e40 | 93 | static int time2 = 0; |
itslinear | 9:d9e46f9c9e40 | 94 | if(time2 < 40) { // gerade aus fahren |
itslinear | 9:d9e46f9c9e40 | 95 | pwmL = 0.6f; |
itslinear | 9:d9e46f9c9e40 | 96 | pwmR = 0.4f; |
itslinear | 9:d9e46f9c9e40 | 97 | time2 ++; |
itslinear | 12:b9faf8637183 | 98 | state = 1; |
itslinear | 9:d9e46f9c9e40 | 99 | } else { |
itslinear | 9:d9e46f9c9e40 | 100 | time2 = 0; |
itslinear | 9:d9e46f9c9e40 | 101 | pwmL = 0.5f; |
itslinear | 9:d9e46f9c9e40 | 102 | pwmR = 0.5f; |
itslinear | 9:d9e46f9c9e40 | 103 | state = 2; |
itslinear | 9:d9e46f9c9e40 | 104 | } |
itslinear | 9:d9e46f9c9e40 | 105 | break; |
itslinear | 6:4af735d26b7a | 106 | |
itslinear | 6:4af735d26b7a | 107 | |
itslinear | 9:d9e46f9c9e40 | 108 | case 4: // Roboter erkennt gründer Klotz, Klotz wird angefahren |
itslinear | 9:d9e46f9c9e40 | 109 | roboter1.bandeAusweichen(); |
itslinear | 12:b9faf8637183 | 110 | if(readCamera() == 1) { // links fahren |
itslinear | 9:d9e46f9c9e40 | 111 | pwmL = 0.45f; |
itslinear | 9:d9e46f9c9e40 | 112 | pwmR = 0.45f; |
itslinear | 9:d9e46f9c9e40 | 113 | } |
itslinear | 12:b9faf8637183 | 114 | if(readCamera() == 2) { // rechts fahren |
itslinear | 9:d9e46f9c9e40 | 115 | pwmL = 0.55f; |
itslinear | 9:d9e46f9c9e40 | 116 | pwmR = 0.55f; |
itslinear | 9:d9e46f9c9e40 | 117 | } |
itslinear | 12:b9faf8637183 | 118 | if(readCamera() == 3) { // gerade fahren |
itslinear | 9:d9e46f9c9e40 | 119 | pwmL = 0.55f; |
itslinear | 9:d9e46f9c9e40 | 120 | pwmR = 0.45f; |
itslinear | 9:d9e46f9c9e40 | 121 | } |
itslinear | 6:4af735d26b7a | 122 | |
itslinear | 9:d9e46f9c9e40 | 123 | break; |
itslinear | 9:d9e46f9c9e40 | 124 | |
itslinear | 9:d9e46f9c9e40 | 125 | case 5: // Aufnehmen des Klotzes |
itslinear | 12:b9faf8637183 | 126 | pwmL = 0.5f; |
itslinear | 12:b9faf8637183 | 127 | pwmR = 0.5f; |
itslinear | 9:d9e46f9c9e40 | 128 | |
itslinear | 6:4af735d26b7a | 129 | |
itslinear | 9:d9e46f9c9e40 | 130 | break; |
itslinear | 9:d9e46f9c9e40 | 131 | |
itslinear | 6:4af735d26b7a | 132 | |
itslinear | 9:d9e46f9c9e40 | 133 | default: |
itslinear | 9:d9e46f9c9e40 | 134 | break; |
itslinear | 9:d9e46f9c9e40 | 135 | |
itslinear | 6:4af735d26b7a | 136 | |
itslinear | 6:4af735d26b7a | 137 | } |
schneju2 | 5:f48b2609c328 | 138 | |
itslinear | 9:d9e46f9c9e40 | 139 | wait(0.1f); |
itslinear | 9:d9e46f9c9e40 | 140 | |
schneju2 | 5:f48b2609c328 | 141 | } |
itslinear | 1:4e84271a70c6 | 142 | |
itslinear | 9:d9e46f9c9e40 | 143 | } |
itslinear | 9:d9e46f9c9e40 | 144 |