Gruppe 3 / Mbed 2 deprecated PES1

für holdor

Dependencies:   Servo mbed pixy

Fork of PES by Gruppe 3

main.cpp@25:a50ca6f2eca4, 2017-05-01 (annotated)

Committer:
schneju2
Date:
Mon May 01 14:02:35 2017 +0000
Revision:
25:a50ca6f2eca4
Parent:
24:be4fd3005611
Child:
26:c2f7c6cdeece
Neu

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
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();
Shukle 24:be4fd3005611 102 if(time2 < 80) { // 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");
schneju2 25:a50ca6f2eca4 119 double maxWert = 0.1; // 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 25:a50ca6f2eca4 122 camValue = camValue -99.0;
schneju2 20:c961dc550882 123 double speedValue = camValue * (maxWert /100.0);
schneju2 25:a50ca6f2eca4 124 pwmL = 0.45f - speedValue;
schneju2 25:a50ca6f2eca4 125 pwmR = 0.45f - speedValue;
itslinear 9:d9e46f9c9e40 126 }
schneju2 20:c961dc550882 127 if(camValue >=200 && camValue < 300) { // rechts fahren, wenn wir Werte von 200 bis 299 haben
schneju2 25:a50ca6f2eca4 128 camValue = camValue -199.0;
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 25:a50ca6f2eca4 134 camValue = camValue-299.0; // 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