Gruppe 3 / Mbed 2 deprecated PES3

hallo

Dependencies:   Servo mbed pixy

Fork of PES1 by Gruppe 3

main.cpp@16:0a1703438e8b, 2017-04-21 (annotated)

Committer:
Shukle
Date:
Fri Apr 21 11:53:23 2017 +0000
Revision:
16:0a1703438e8b
Parent:
14:7e330f65f26e
Child:
17:caf5ae550f2e
?berarbeited

Who changed what in which revision?

UserRevisionLine numberNew 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 13:7eba9911e196 47 int tempState = 2;
Shukle 16:0a1703438e8b 48 int time1 = 0;
Shukle 16:0a1703438e8b 49 int time2 = 0;
itslinear 9:d9e46f9c9e40 50
itslinear 9:d9e46f9c9e40 51 while(1) {
itslinear 9:d9e46f9c9e40 52
itslinear 14:7e330f65f26e 53 int camValue = readCamera();
itslinear 6:4af735d26b7a 54
itslinear 6:4af735d26b7a 55 switch(state) {
itslinear 6:4af735d26b7a 56
itslinear 12:b9faf8637183 57 case 0: // Roboter Anschalten mit Knopf
Shukle 16:0a1703438e8b 58 printf("0");
itslinear 6:4af735d26b7a 59
itslinear 12:b9faf8637183 60 if (switchOnOff == 0) {
itslinear 12:b9faf8637183 61 state = 1;
itslinear 9:d9e46f9c9e40 62 }
itslinear 7:edb4e0cfc0d1 63
itslinear 9:d9e46f9c9e40 64 break;
itslinear 6:4af735d26b7a 65
itslinear 12:b9faf8637183 66 case 1:
Shukle 16:0a1703438e8b 67 printf("1");
itslinear 14:7e330f65f26e 68 if(camValue == 1 || camValue == 2 || camValue == 3) { // Die Kamera erkennt ein grünen Klotz
itslinear 12:b9faf8637183 69 state = 4;
itslinear 12:b9faf8637183 70 }
itslinear 14:7e330f65f26e 71 if(camValue == 4) { // Roboter in Position
itslinear 12:b9faf8637183 72 state = 5;
itslinear 12:b9faf8637183 73 }
itslinear 14:7e330f65f26e 74 if(camValue == 0){
itslinear 13:7eba9911e196 75 state = tempState;
Shukle 16:0a1703438e8b 76 }
Shukle 16:0a1703438e8b 77 break;
itslinear 12:b9faf8637183 78
itslinear 9:d9e46f9c9e40 79 case 2:
Shukle 16:0a1703438e8b 80 printf("2");
itslinear 9:d9e46f9c9e40 81 if(time1 < 20) { // Im Kreis drehen für 1s
itslinear 9:d9e46f9c9e40 82 pwmL = 0.4f;
itslinear 9:d9e46f9c9e40 83 pwmR = 0.4f;
itslinear 9:d9e46f9c9e40 84 time1 ++;
itslinear 12:b9faf8637183 85 state = 1;
itslinear 13:7eba9911e196 86 tempState = 2;
itslinear 9:d9e46f9c9e40 87 } else {
itslinear 9:d9e46f9c9e40 88 time1 = 0;
itslinear 9:d9e46f9c9e40 89 pwmL = 0.5f;
itslinear 9:d9e46f9c9e40 90 pwmR = 0.5f;
itslinear 9:d9e46f9c9e40 91 state = 3;
itslinear 9:d9e46f9c9e40 92 }
itslinear 9:d9e46f9c9e40 93 break;
itslinear 6:4af735d26b7a 94
itslinear 3:24e098715b78 95
itslinear 9:d9e46f9c9e40 96 case 3:
Shukle 16:0a1703438e8b 97 printf("3");
itslinear 9:d9e46f9c9e40 98 roboter1.bandeAusweichen();
itslinear 9:d9e46f9c9e40 99 if(time2 < 40) { // gerade aus fahren
itslinear 9:d9e46f9c9e40 100 pwmL = 0.6f;
itslinear 9:d9e46f9c9e40 101 pwmR = 0.4f;
itslinear 9:d9e46f9c9e40 102 time2 ++;
itslinear 12:b9faf8637183 103 state = 1;
itslinear 13:7eba9911e196 104 tempState = 3;
itslinear 9:d9e46f9c9e40 105 } else {
itslinear 9:d9e46f9c9e40 106 time2 = 0;
itslinear 9:d9e46f9c9e40 107 pwmL = 0.5f;
itslinear 9:d9e46f9c9e40 108 pwmR = 0.5f;
itslinear 9:d9e46f9c9e40 109 state = 2;
itslinear 9:d9e46f9c9e40 110 }
itslinear 9:d9e46f9c9e40 111 break;
itslinear 6:4af735d26b7a 112
itslinear 6:4af735d26b7a 113
itslinear 9:d9e46f9c9e40 114 case 4: // Roboter erkennt gründer Klotz, Klotz wird angefahren
Shukle 16:0a1703438e8b 115 printf("4");
itslinear 9:d9e46f9c9e40 116 roboter1.bandeAusweichen();
itslinear 14:7e330f65f26e 117 if(camValue == 1) { // links fahren
itslinear 9:d9e46f9c9e40 118 pwmL = 0.45f;
itslinear 9:d9e46f9c9e40 119 pwmR = 0.45f;
itslinear 9:d9e46f9c9e40 120 }
itslinear 14:7e330f65f26e 121 if(camValue == 2) { // rechts fahren
itslinear 9:d9e46f9c9e40 122 pwmL = 0.55f;
itslinear 9:d9e46f9c9e40 123 pwmR = 0.55f;
itslinear 9:d9e46f9c9e40 124 }
itslinear 14:7e330f65f26e 125 if(camValue == 3) { // gerade fahren
itslinear 9:d9e46f9c9e40 126 pwmL = 0.55f;
itslinear 9:d9e46f9c9e40 127 pwmR = 0.45f;
itslinear 9:d9e46f9c9e40 128 }
itslinear 13:7eba9911e196 129 state = 1;
itslinear 6:4af735d26b7a 130
itslinear 9:d9e46f9c9e40 131 break;
itslinear 9:d9e46f9c9e40 132
itslinear 9:d9e46f9c9e40 133 case 5: // Aufnehmen des Klotzes
Shukle 16:0a1703438e8b 134 printf("5");
itslinear 12:b9faf8637183 135 pwmL = 0.5f;
itslinear 12:b9faf8637183 136 pwmR = 0.5f;
itslinear 13:7eba9911e196 137 state = 1;
Shukle 16:0a1703438e8b 138 time1 = 0;
Shukle 16:0a1703438e8b 139 time2 = 0;
itslinear 6:4af735d26b7a 140
itslinear 9:d9e46f9c9e40 141 break;
itslinear 9:d9e46f9c9e40 142
itslinear 6:4af735d26b7a 143
itslinear 9:d9e46f9c9e40 144 default:
itslinear 9:d9e46f9c9e40 145 break;
itslinear 9:d9e46f9c9e40 146
itslinear 6:4af735d26b7a 147
itslinear 6:4af735d26b7a 148 }
Shukle 16:0a1703438e8b 149
Shukle 16:0a1703438e8b 150 //wait(0.1f);
schneju2 5:f48b2609c328 151 }
itslinear 1:4e84271a70c6 152
itslinear 9:d9e46f9c9e40 153 }
itslinear 9:d9e46f9c9e40 154