Gruppe 3 / Mbed 2 deprecated PES1

für holdor

Dependencies:   Servo mbed pixy

Fork of PES by Gruppe 3

main.cpp@18:ea4671f566d4, 2017-04-25 (annotated)

Committer:
Shukle
Date:
Tue Apr 25 13:44:12 2017 +0000
Revision:
18:ea4671f566d4
Parent:
17:caf5ae550f2e 
anfahren funktioniert

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 Switch ON/OFF
schneju2 5:f48b2609c328 19
itslinear 7:edb4e0cfc0d1 20 DigitalIn switchOnOff(USER_BUTTON);
schneju2 5:f48b2609c328 21
itslinear 0:306a2438de17 22 //motor stuff
itslinear 0:306a2438de17 23 DigitalOut enableMotorDriver(PB_2);
itslinear 3:24e098715b78 24 PwmOut pwmL( PA_8 );
itslinear 3:24e098715b78 25 PwmOut pwmR( PA_9 );
itslinear 0:306a2438de17 26
itslinear 0:306a2438de17 27 //indicator leds arround robot
itslinear 0:306a2438de17 28 DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };
itslinear 0:306a2438de17 29
Shukle 17:caf5ae550f2e 30 Roboter roboter1(&distance, &bit0, &bit1, &bit2, &pwmL, &pwmR, &servos1,&servos2);
itslinear 0:306a2438de17 31
itslinear 1:4e84271a70c6 32 int main()
itslinear 1:4e84271a70c6 33 {
itslinear 6:4af735d26b7a 34 enable = 1; // Sensoren einschalten
itslinear 6:4af735d26b7a 35 enableMotorDriver = 1; // Schaltet die Motoren ein
itslinear 7:edb4e0cfc0d1 36 pwmL.period(0.00005f); // Setzt die Periode auf 50 μs
itslinear 7:edb4e0cfc0d1 37 pwmR.period(0.00005f);
schneju2 8:6d9cd5ad332d 38 pwmL = 0.5f;
itslinear 9:d9e46f9c9e40 39 pwmR = 0.5f;
itslinear 9:d9e46f9c9e40 40
itslinear 6:4af735d26b7a 41
itslinear 12:b9faf8637183 42 int state = 0; // Diese Variable gibt an in welchem State man sich befindet
itslinear 13:7eba9911e196 43 int tempState = 2;
Shukle 16:0a1703438e8b 44 int time1 = 0;
Shukle 16:0a1703438e8b 45 int time2 = 0;
itslinear 9:d9e46f9c9e40 46
itslinear 9:d9e46f9c9e40 47 while(1) {
itslinear 9:d9e46f9c9e40 48
itslinear 14:7e330f65f26e 49 int camValue = readCamera();
itslinear 6:4af735d26b7a 50
itslinear 6:4af735d26b7a 51 switch(state) {
itslinear 6:4af735d26b7a 52
itslinear 12:b9faf8637183 53 case 0: // Roboter Anschalten mit Knopf
Shukle 16:0a1703438e8b 54 printf("0");
itslinear 6:4af735d26b7a 55
itslinear 12:b9faf8637183 56 if (switchOnOff == 0) {
itslinear 12:b9faf8637183 57 state = 1;
itslinear 9:d9e46f9c9e40 58 }
itslinear 7:edb4e0cfc0d1 59
itslinear 9:d9e46f9c9e40 60 break;
itslinear 6:4af735d26b7a 61
itslinear 12:b9faf8637183 62 case 1:
Shukle 16:0a1703438e8b 63 printf("1");
itslinear 14:7e330f65f26e 64 if(camValue == 1 || camValue == 2 || camValue == 3) { // Die Kamera erkennt ein grünen Klotz
itslinear 12:b9faf8637183 65 state = 4;
itslinear 12:b9faf8637183 66 }
itslinear 14:7e330f65f26e 67 if(camValue == 4) { // Roboter in Position
itslinear 12:b9faf8637183 68 state = 5;
itslinear 12:b9faf8637183 69 }
itslinear 14:7e330f65f26e 70 if(camValue == 0){
itslinear 13:7eba9911e196 71 state = tempState;
Shukle 16:0a1703438e8b 72 }
Shukle 16:0a1703438e8b 73 break;
itslinear 12:b9faf8637183 74
itslinear 9:d9e46f9c9e40 75 case 2:
Shukle 16:0a1703438e8b 76 printf("2");
Shukle 18:ea4671f566d4 77 if(time1 < 15) { // 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 13:7eba9911e196 82 tempState = 2;
itslinear 9:d9e46f9c9e40 83 } else {
itslinear 9:d9e46f9c9e40 84 time1 = 0;
itslinear 9:d9e46f9c9e40 85 pwmL = 0.5f;
itslinear 9:d9e46f9c9e40 86 pwmR = 0.5f;
itslinear 9:d9e46f9c9e40 87 state = 3;
itslinear 9:d9e46f9c9e40 88 }
itslinear 9:d9e46f9c9e40 89 break;
itslinear 6:4af735d26b7a 90
itslinear 3:24e098715b78 91
itslinear 9:d9e46f9c9e40 92 case 3:
Shukle 16:0a1703438e8b 93 printf("3");
itslinear 9:d9e46f9c9e40 94 roboter1.bandeAusweichen();
Shukle 18:ea4671f566d4 95 if(time2 < 100) { // gerade aus fahren
itslinear 9:d9e46f9c9e40 96 pwmL = 0.6f;
itslinear 9:d9e46f9c9e40 97 pwmR = 0.4f;
itslinear 9:d9e46f9c9e40 98 time2 ++;
itslinear 12:b9faf8637183 99 state = 1;
itslinear 13:7eba9911e196 100 tempState = 3;
itslinear 9:d9e46f9c9e40 101 } else {
itslinear 9:d9e46f9c9e40 102 time2 = 0;
itslinear 9:d9e46f9c9e40 103 pwmL = 0.5f;
itslinear 9:d9e46f9c9e40 104 pwmR = 0.5f;
itslinear 9:d9e46f9c9e40 105 state = 2;
itslinear 9:d9e46f9c9e40 106 }
itslinear 9:d9e46f9c9e40 107 break;
itslinear 6:4af735d26b7a 108
itslinear 6:4af735d26b7a 109
itslinear 9:d9e46f9c9e40 110 case 4: // Roboter erkennt gründer Klotz, Klotz wird angefahren
Shukle 16:0a1703438e8b 111 printf("4");
itslinear 9:d9e46f9c9e40 112 roboter1.bandeAusweichen();
itslinear 14:7e330f65f26e 113 if(camValue == 1) { // links fahren
Shukle 18:ea4671f566d4 114 printf(".1;");
Shukle 18:ea4671f566d4 115 pwmL = 0.425f;
Shukle 18:ea4671f566d4 116 pwmR = 0.425f;
itslinear 9:d9e46f9c9e40 117 }
itslinear 14:7e330f65f26e 118 if(camValue == 2) { // rechts fahren
Shukle 18:ea4671f566d4 119 printf(".2;");
Shukle 18:ea4671f566d4 120 pwmL = 0.575f;
Shukle 18:ea4671f566d4 121 pwmR = 0.575f;
itslinear 9:d9e46f9c9e40 122 }
itslinear 14:7e330f65f26e 123 if(camValue == 3) { // gerade fahren
Shukle 18:ea4671f566d4 124 printf(".3;");
Shukle 18:ea4671f566d4 125 pwmL = 0.6f;
Shukle 18:ea4671f566d4 126 pwmR = 0.4f;
itslinear 9:d9e46f9c9e40 127 }
itslinear 13:7eba9911e196 128 state = 1;
itslinear 6:4af735d26b7a 129
itslinear 9:d9e46f9c9e40 130 break;
itslinear 9:d9e46f9c9e40 131
itslinear 9:d9e46f9c9e40 132 case 5: // Aufnehmen des Klotzes
Shukle 16:0a1703438e8b 133 printf("5");
itslinear 12:b9faf8637183 134 pwmL = 0.5f;
itslinear 12:b9faf8637183 135 pwmR = 0.5f;
itslinear 13:7eba9911e196 136 state = 1;
Shukle 16:0a1703438e8b 137 time1 = 0;
Shukle 16:0a1703438e8b 138 time2 = 0;
itslinear 6:4af735d26b7a 139
itslinear 9:d9e46f9c9e40 140 break;
itslinear 9:d9e46f9c9e40 141
itslinear 6:4af735d26b7a 142
itslinear 9:d9e46f9c9e40 143 default:
itslinear 9:d9e46f9c9e40 144 break;
itslinear 9:d9e46f9c9e40 145
itslinear 6:4af735d26b7a 146
itslinear 6:4af735d26b7a 147 }
Shukle 16:0a1703438e8b 148
Shukle 16:0a1703438e8b 149 //wait(0.1f);
schneju2 5:f48b2609c328 150 }
itslinear 1:4e84271a70c6 151
itslinear 9:d9e46f9c9e40 152 }
itslinear 9:d9e46f9c9e40 153