Gruppe 3
/
PES1
für holdor
Fork of PES by
main.cpp@22:ffd36f07c046, 2017-05-01 (annotated)
- Committer:
- Shukle
- Date:
- Mon May 01 10:18:19 2017 +0000
- Revision:
- 22:ffd36f07c046
- Parent:
- 20:c961dc550882
- Child:
- 24:be4fd3005611
?berarbeited;
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 | |
schneju2 | 20:c961dc550882 | 4 | |
itslinear | 0:306a2438de17 | 5 | DigitalOut led(LED1); |
itslinear | 0:306a2438de17 | 6 | |
itslinear | 0:306a2438de17 | 7 | //Periphery for distance sensors |
itslinear | 1:4e84271a70c6 | 8 | AnalogIn distance(PB_1); |
itslinear | 0:306a2438de17 | 9 | DigitalOut enable(PC_1); |
itslinear | 0:306a2438de17 | 10 | DigitalOut bit0(PH_1); |
itslinear | 0:306a2438de17 | 11 | DigitalOut bit1(PC_2); |
itslinear | 0:306a2438de17 | 12 | DigitalOut bit2(PC_3); |
itslinear | 0:306a2438de17 | 13 | IRSensor sensors[6]; |
itslinear | 0:306a2438de17 | 14 | |
schneju2 | 2:953263712480 | 15 | // Periphery for servos |
itslinear | 4:fdcf3b5009c6 | 16 | Servo servos1(PB_7); |
itslinear | 4:fdcf3b5009c6 | 17 | Servo servos2(PA_6); |
itslinear | 4:fdcf3b5009c6 | 18 | |
itslinear | 6:4af735d26b7a | 19 | //Periphery for Switch ON/OFF |
schneju2 | 5:f48b2609c328 | 20 | |
itslinear | 7:edb4e0cfc0d1 | 21 | DigitalIn switchOnOff(USER_BUTTON); |
schneju2 | 5:f48b2609c328 | 22 | |
itslinear | 0:306a2438de17 | 23 | //motor stuff |
itslinear | 0:306a2438de17 | 24 | DigitalOut enableMotorDriver(PB_2); |
itslinear | 3:24e098715b78 | 25 | PwmOut pwmL( PA_8 ); |
itslinear | 3:24e098715b78 | 26 | PwmOut pwmR( PA_9 ); |
itslinear | 0:306a2438de17 | 27 | |
itslinear | 0:306a2438de17 | 28 | //indicator leds arround robot |
itslinear | 0:306a2438de17 | 29 | DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 }; |
itslinear | 0:306a2438de17 | 30 | |
Shukle | 17:caf5ae550f2e | 31 | Roboter roboter1(&distance, &bit0, &bit1, &bit2, &pwmL, &pwmR, &servos1,&servos2); |
itslinear | 0:306a2438de17 | 32 | |
itslinear | 1:4e84271a70c6 | 33 | int main() |
itslinear | 1:4e84271a70c6 | 34 | { |
itslinear | 6:4af735d26b7a | 35 | enable = 1; // Sensoren einschalten |
itslinear | 6:4af735d26b7a | 36 | enableMotorDriver = 1; // Schaltet die Motoren ein |
itslinear | 7:edb4e0cfc0d1 | 37 | pwmL.period(0.00005f); // Setzt die Periode auf 50 μs |
schneju2 | 20:c961dc550882 | 38 | pwmR.period(0.00005f); // Setzt die Periode auf 50 μs |
schneju2 | 20:c961dc550882 | 39 | pwmL = 0.5f; // Geschwindikeit auf 0 |
schneju2 | 20:c961dc550882 | 40 | pwmR = 0.5f; // Geschwindikeit auf 0 |
itslinear | 9:d9e46f9c9e40 | 41 | |
itslinear | 6:4af735d26b7a | 42 | |
itslinear | 12:b9faf8637183 | 43 | int state = 0; // Diese Variable gibt an in welchem State man sich befindet |
schneju2 | 20:c961dc550882 | 44 | int tempState = 2; // |
schneju2 | 20:c961dc550882 | 45 | int time1 = 0; // Die Zeit wird auf 0 gesetzt |
schneju2 | 20:c961dc550882 | 46 | int time2 = 0; // Die Zeit wird auf 0 gesetzt |
schneju2 | 20:c961dc550882 | 47 | |
schneju2 | 20:c961dc550882 | 48 | |
schneju2 | 20:c961dc550882 | 49 | |
schneju2 | 20:c961dc550882 | 50 | |
schneju2 | 20:c961dc550882 | 51 | |
itslinear | 9:d9e46f9c9e40 | 52 | |
schneju2 | 20:c961dc550882 | 53 | while(1) { |
itslinear | 9:d9e46f9c9e40 | 54 | |
schneju2 | 20:c961dc550882 | 55 | double camValue = readCamera(); |
Shukle | 22:ffd36f07c046 | 56 | printf("\n\rcamValue: %f\n\r",camValue); |
itslinear | 6:4af735d26b7a | 57 | |
itslinear | 6:4af735d26b7a | 58 | switch(state) { |
itslinear | 6:4af735d26b7a | 59 | |
itslinear | 12:b9faf8637183 | 60 | case 0: // Roboter Anschalten mit Knopf |
Shukle | 16:0a1703438e8b | 61 | printf("0"); |
itslinear | 6:4af735d26b7a | 62 | |
itslinear | 12:b9faf8637183 | 63 | if (switchOnOff == 0) { |
itslinear | 12:b9faf8637183 | 64 | state = 1; |
itslinear | 9:d9e46f9c9e40 | 65 | } |
itslinear | 7:edb4e0cfc0d1 | 66 | |
itslinear | 9:d9e46f9c9e40 | 67 | break; |
itslinear | 6:4af735d26b7a | 68 | |
itslinear | 12:b9faf8637183 | 69 | case 1: |
Shukle | 16:0a1703438e8b | 70 | printf("1"); |
schneju2 | 20:c961dc550882 | 71 | if(camValue >= 100 && camValue < 400 ) { // Die Kamera erkennt ein grünen Klotz , Werte von 100 und 399 |
itslinear | 12:b9faf8637183 | 72 | state = 4; |
itslinear | 12:b9faf8637183 | 73 | } |
schneju2 | 20:c961dc550882 | 74 | if(camValue == 400) { // Roboter in Position |
itslinear | 12:b9faf8637183 | 75 | state = 5; |
itslinear | 12:b9faf8637183 | 76 | } |
itslinear | 14:7e330f65f26e | 77 | if(camValue == 0){ |
itslinear | 13:7eba9911e196 | 78 | state = tempState; |
Shukle | 16:0a1703438e8b | 79 | } |
Shukle | 16:0a1703438e8b | 80 | break; |
itslinear | 12:b9faf8637183 | 81 | |
itslinear | 9:d9e46f9c9e40 | 82 | case 2: |
Shukle | 16:0a1703438e8b | 83 | printf("2"); |
itslinear | 9:d9e46f9c9e40 | 84 | if(time1 < 20) { // Im Kreis drehen für 1s |
itslinear | 9:d9e46f9c9e40 | 85 | pwmL = 0.4f; |
itslinear | 9:d9e46f9c9e40 | 86 | pwmR = 0.4f; |
itslinear | 9:d9e46f9c9e40 | 87 | time1 ++; |
itslinear | 12:b9faf8637183 | 88 | state = 1; |
itslinear | 13:7eba9911e196 | 89 | tempState = 2; |
itslinear | 9:d9e46f9c9e40 | 90 | } else { |
itslinear | 9:d9e46f9c9e40 | 91 | time1 = 0; |
itslinear | 9:d9e46f9c9e40 | 92 | pwmL = 0.5f; |
itslinear | 9:d9e46f9c9e40 | 93 | pwmR = 0.5f; |
itslinear | 9:d9e46f9c9e40 | 94 | state = 3; |
itslinear | 9:d9e46f9c9e40 | 95 | } |
itslinear | 9:d9e46f9c9e40 | 96 | break; |
itslinear | 6:4af735d26b7a | 97 | |
itslinear | 3:24e098715b78 | 98 | |
itslinear | 9:d9e46f9c9e40 | 99 | case 3: |
Shukle | 16:0a1703438e8b | 100 | printf("3"); |
itslinear | 9:d9e46f9c9e40 | 101 | roboter1.bandeAusweichen(); |
itslinear | 9:d9e46f9c9e40 | 102 | if(time2 < 40) { // gerade aus fahren |
itslinear | 9:d9e46f9c9e40 | 103 | pwmL = 0.6f; |
itslinear | 9:d9e46f9c9e40 | 104 | pwmR = 0.4f; |
itslinear | 9:d9e46f9c9e40 | 105 | time2 ++; |
itslinear | 12:b9faf8637183 | 106 | state = 1; |
itslinear | 13:7eba9911e196 | 107 | tempState = 3; |
itslinear | 9:d9e46f9c9e40 | 108 | } else { |
itslinear | 9:d9e46f9c9e40 | 109 | time2 = 0; |
itslinear | 9:d9e46f9c9e40 | 110 | pwmL = 0.5f; |
itslinear | 9:d9e46f9c9e40 | 111 | pwmR = 0.5f; |
itslinear | 9:d9e46f9c9e40 | 112 | state = 2; |
itslinear | 9:d9e46f9c9e40 | 113 | } |
itslinear | 9:d9e46f9c9e40 | 114 | break; |
itslinear | 6:4af735d26b7a | 115 | |
itslinear | 6:4af735d26b7a | 116 | |
itslinear | 9:d9e46f9c9e40 | 117 | case 4: // Roboter erkennt gründer Klotz, Klotz wird angefahren |
Shukle | 16:0a1703438e8b | 118 | printf("4"); |
Shukle | 22:ffd36f07c046 | 119 | double maxWert = 0.2; // Maximale Werte für die pmL und pmR |
itslinear | 9:d9e46f9c9e40 | 120 | roboter1.bandeAusweichen(); |
schneju2 | 20:c961dc550882 | 121 | if(camValue >= 100 && camValue <200 ) { // links fahren, wenn wir Werte von 100 bis 199 haben |
schneju2 | 20:c961dc550882 | 122 | camValue = camValue -99; |
schneju2 | 20:c961dc550882 | 123 | double speedValue = camValue * (maxWert /100.0); |
itslinear | 9:d9e46f9c9e40 | 124 | pwmL = 0.45f; |
schneju2 | 20:c961dc550882 | 125 | pwmR = 0.45f; |
itslinear | 9:d9e46f9c9e40 | 126 | } |
schneju2 | 20:c961dc550882 | 127 | if(camValue >=200 && camValue < 300) { // rechts fahren, wenn wir Werte von 200 bis 299 haben |
schneju2 | 20:c961dc550882 | 128 | camValue = camValue -199; |
schneju2 | 20:c961dc550882 | 129 | double speedValue = camValue * (maxWert /100.0); |
schneju2 | 20:c961dc550882 | 130 | pwmL = 0.5f + speedValue; |
schneju2 | 20:c961dc550882 | 131 | pwmR = 0.5f + speedValue; |
itslinear | 9:d9e46f9c9e40 | 132 | } |
schneju2 | 20:c961dc550882 | 133 | if(camValue >= 300 && camValue <400) { // gerade fahren, wenn wir Werte von 300 bis 399 haben |
schneju2 | 20:c961dc550882 | 134 | camValue = camValue-299; // CamValue nimmt die Werte von 1 bis 100 an |
schneju2 | 20:c961dc550882 | 135 | double speedValue = camValue * (maxWert /100.0); // Berechnung des speedValue's |
schneju2 | 20:c961dc550882 | 136 | pwmL = 0.5f + speedValue; |
schneju2 | 20:c961dc550882 | 137 | pwmR = 0.5f - speedValue; |
itslinear | 9:d9e46f9c9e40 | 138 | } |
itslinear | 13:7eba9911e196 | 139 | state = 1; |
itslinear | 6:4af735d26b7a | 140 | |
itslinear | 9:d9e46f9c9e40 | 141 | break; |
itslinear | 9:d9e46f9c9e40 | 142 | |
itslinear | 9:d9e46f9c9e40 | 143 | case 5: // Aufnehmen des Klotzes |
Shukle | 16:0a1703438e8b | 144 | printf("5"); |
itslinear | 12:b9faf8637183 | 145 | pwmL = 0.5f; |
itslinear | 12:b9faf8637183 | 146 | pwmR = 0.5f; |
itslinear | 13:7eba9911e196 | 147 | state = 1; |
Shukle | 16:0a1703438e8b | 148 | time1 = 0; |
Shukle | 16:0a1703438e8b | 149 | time2 = 0; |
itslinear | 6:4af735d26b7a | 150 | |
itslinear | 9:d9e46f9c9e40 | 151 | break; |
itslinear | 9:d9e46f9c9e40 | 152 | |
itslinear | 6:4af735d26b7a | 153 | |
itslinear | 9:d9e46f9c9e40 | 154 | default: |
itslinear | 9:d9e46f9c9e40 | 155 | break; |
itslinear | 9:d9e46f9c9e40 | 156 | |
itslinear | 6:4af735d26b7a | 157 | |
itslinear | 6:4af735d26b7a | 158 | } |
Shukle | 16:0a1703438e8b | 159 | |
Shukle | 16:0a1703438e8b | 160 | //wait(0.1f); |
schneju2 | 5:f48b2609c328 | 161 | } |
itslinear | 1:4e84271a70c6 | 162 | |
schneju2 | 20:c961dc550882 | 163 | } |
itslinear | 9:d9e46f9c9e40 | 164 |