kings / Mbed 2 deprecated MainMix_20170517

Totale Testversion

Dependencies:   mbed

Fork of DrehungMitStopp by kings

main.cpp@5:24350e6fc9d7, 2017-05-21 (annotated)

Committer:
freunjor
Date:
Sun May 21 15:29:46 2017 +0000
Revision:
5:24350e6fc9d7
Parent:
4:2ebbd24aa610 
mainmix_1 pro min;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
EHess 0:96f88638114b 1 #include "mbed.h"
EHess 0:96f88638114b 2 #include "IRSensor.h"
EHess 1:d40ff07e2fe0 3 #include "MotorEncoder.h"
EHess 1:d40ff07e2fe0 4 #include "LowpassFilter.h"
EHess 3:cfb0b6bcf568 5 #include "Servo.h"
EHess 2:365bf16abbf6 6 #include <cmath>
EHess 0:96f88638114b 7
EHess 1:d40ff07e2fe0 8 DigitalOut led(LED1); //Zustands-LED: Grüne LED für Benutzer
EHess 0:96f88638114b 9
EHess 3:cfb0b6bcf568 10 //--------------------IRSensoren------------------------------------------------
EHess 3:cfb0b6bcf568 11
EHess 1:d40ff07e2fe0 12 AnalogIn distance(PB_1); //Input der Distanzsensoren
EHess 1:d40ff07e2fe0 13 DigitalOut enableSensor(PC_1); //Aktivierung der IRSensoren
EHess 1:d40ff07e2fe0 14 DigitalOut bit0(PH_1); //Ansteuerung der Sensoren 0-5 mit 3 Bits
EHess 0:96f88638114b 15 DigitalOut bit1(PC_2);
EHess 0:96f88638114b 16 DigitalOut bit2(PC_3);
EHess 1:d40ff07e2fe0 17 IRSensor sensors[6]; //Erstellt 6 IRSensor-Objekte als Array
EHess 0:96f88638114b 18
EHess 1:d40ff07e2fe0 19 DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 }; //LED-Outputs der Sensoren
EHess 0:96f88638114b 20
EHess 2:365bf16abbf6 21 float sensorMittelwert[6]; //Array der 6 Sensorenwerte
EHess 2:365bf16abbf6 22 float sensorTiefbass[6];
EHess 2:365bf16abbf6 23 float sensorZentimeter[6];
EHess 2:365bf16abbf6 24 int printfZaehler = 0;
EHess 0:96f88638114b 25
EHess 1:d40ff07e2fe0 26 //Titel printf()
EHess 2:365bf16abbf6 27 void title()
EHess 2:365bf16abbf6 28 {
EHess 2:365bf16abbf6 29 printf("\f"); //"\f" Setzt den Cursor an den Anfang der nächsten Seite
EHess 2:365bf16abbf6 30 }
EHess 2:365bf16abbf6 31
EHess 2:365bf16abbf6 32 void sensorWerte() //Rechnet Sensorwerte aus und speichert sie unter sensorZentimeter[]
EHess 2:365bf16abbf6 33 {
EHess 2:365bf16abbf6 34 for(int j = 0; j < 25; j++) { //Zählt 25 Sensorwerten pro Sensor zusammen
EHess 2:365bf16abbf6 35 for(int i = 0; i < 6; i++) {
EHess 2:365bf16abbf6 36 sensorMittelwert[i] += sensors[i].read();
EHess 2:365bf16abbf6 37 }
EHess 2:365bf16abbf6 38 }
EHess 2:365bf16abbf6 39 for(int i = 0; i < 6; i++) {
EHess 2:365bf16abbf6 40 sensorTiefbass[i] = sensorTiefbass[i]*0.75f + sensorMittelwert[i]*0.25f; //Verrechnet den neuen Wert mit dem alten
EHess 2:365bf16abbf6 41 sensorMittelwert[i] = 0.0f; //Setzt die Sensorwerte auf NULL
EHess 2:365bf16abbf6 42 switch(i) { //Rechnet die Werte in Zentimeter
EHess 2:365bf16abbf6 43 case 0: //Mitte
EHess 2:365bf16abbf6 44 if(sensorTiefbass[i] < 2.97f) sensorZentimeter[i] = 80.0f; //Grenzwert
EHess 2:365bf16abbf6 45 else sensorZentimeter[i] = 110.0f/pow((sensorTiefbass[i]-1.9f),0.41f)-27.0f;
EHess 2:365bf16abbf6 46 break;
EHess 2:365bf16abbf6 47 case 1: //UntenRechts
EHess 2:365bf16abbf6 48 if(sensorTiefbass[i] < 3.2f) sensorZentimeter[i] = 69.0f; //Grenzwert
EHess 2:365bf16abbf6 49 else sensorZentimeter[i] = 87.0f/pow((sensorTiefbass[i]-2.2f),0.41f)-18.0f;
EHess 2:365bf16abbf6 50 break;
EHess 2:365bf16abbf6 51 case 3: //Links
EHess 2:365bf16abbf6 52 if(sensorTiefbass[i] < 3.67f) sensorZentimeter[i] = 80.0f; //Grenzwert
EHess 2:365bf16abbf6 53 sensorZentimeter[i] = 80.0f/pow((sensorTiefbass[i]-3.1f),0.38f)-19.0f;
EHess 2:365bf16abbf6 54 break;
EHess 2:365bf16abbf6 55 case 4: //Rechts
EHess 2:365bf16abbf6 56 if(sensorTiefbass[i] < 3.53f) sensorZentimeter[i] = 80.0f; //Grenzwert
EHess 2:365bf16abbf6 57 else sensorZentimeter[i] = 90.0f/pow((sensorTiefbass[i]-2.8f),0.4f)-22.0f;
EHess 2:365bf16abbf6 58 break;
EHess 2:365bf16abbf6 59 case 5: //UntenLinks
EHess 2:365bf16abbf6 60 if(sensorTiefbass[i] < 4.4f) sensorZentimeter[i] = 59.0f; //Grenzwert
EHess 2:365bf16abbf6 61 else sensorZentimeter[i] = 115.0f/pow((sensorTiefbass[i]-2.5f),0.4f)-30.0f;
EHess 2:365bf16abbf6 62 break;
EHess 2:365bf16abbf6 63 }
EHess 2:365bf16abbf6 64 }
EHess 4:2ebbd24aa610 65 printf("%f\t%f\n\r", sensorZentimeter[5], sensorZentimeter[0]); //Plottet die unteren Sensoren + Mitte-Oben
EHess 0:96f88638114b 66 }
EHess 0:96f88638114b 67
EHess 3:cfb0b6bcf568 68 //--------------------Motor&Encoder---------------------------------------------
EHess 1:d40ff07e2fe0 69
EHess 1:d40ff07e2fe0 70 const float PERIOD = 0.002f; // period of control task, given in [s]
EHess 1:d40ff07e2fe0 71 const float COUNTS_PER_TURN = 1200.0f; // resolution of encoder counter
EHess 1:d40ff07e2fe0 72 const float LOWPASS_FILTER_FREQUENCY = 300.0f; // frequency of lowpass filter for actual speed values, given in [rad/s]
EHess 1:d40ff07e2fe0 73 const float KN = 40.0f; // speed constant of motor, given in [rpm/V]
EHess 1:d40ff07e2fe0 74 const float KP = 0.2f; // speed controller gain, given in [V/rpm]
EHess 1:d40ff07e2fe0 75 const float MAX_VOLTAGE = 12.0f; // supply voltage for power stage in [V]
EHess 1:d40ff07e2fe0 76 const float MIN_DUTY_CYCLE = 0.02f; // minimum allowed value for duty cycle (2%)
EHess 1:d40ff07e2fe0 77 const float MAX_DUTY_CYCLE = 0.98f; // maximum allowed value for duty cycle (98%)
EHess 1:d40ff07e2fe0 78
EHess 1:d40ff07e2fe0 79 MotorEncoder counterLeft(PB_6, PB_7);
EHess 1:d40ff07e2fe0 80 MotorEncoder counterRight(PA_6, PC_7);
EHess 1:d40ff07e2fe0 81
EHess 1:d40ff07e2fe0 82 LowpassFilter speedLeftFilter;
EHess 1:d40ff07e2fe0 83 LowpassFilter speedRightFilter;
EHess 1:d40ff07e2fe0 84
EHess 1:d40ff07e2fe0 85 DigitalOut enableMotorDriver(PB_2);
EHess 1:d40ff07e2fe0 86 PwmOut pwmLeft(PA_8);
EHess 1:d40ff07e2fe0 87 PwmOut pwmRight(PA_9);
EHess 1:d40ff07e2fe0 88
EHess 1:d40ff07e2fe0 89 short previousValueCounterRight = 0;
EHess 1:d40ff07e2fe0 90 short previousValueCounterLeft = 0;
EHess 1:d40ff07e2fe0 91
EHess 3:cfb0b6bcf568 92 float desiredSpeedLeft; //Ansteuerung des linken Motors
EHess 3:cfb0b6bcf568 93 float desiredSpeedRight; //Ansteuerung des rechten Motors
EHess 0:96f88638114b 94
EHess 1:d40ff07e2fe0 95 float actualSpeedLeft;
EHess 1:d40ff07e2fe0 96 float actualSpeedRight;
EHess 1:d40ff07e2fe0 97
EHess 1:d40ff07e2fe0 98 void speedCtrl()
EHess 1:d40ff07e2fe0 99 {
EHess 1:d40ff07e2fe0 100 // Berechnen die effektiven Drehzahlen der Motoren in [rpm]
EHess 1:d40ff07e2fe0 101 short valueCounterLeft = counterLeft.read();
EHess 1:d40ff07e2fe0 102 short valueCounterRight = counterRight.read();
EHess 1:d40ff07e2fe0 103 short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
EHess 1:d40ff07e2fe0 104 short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
EHess 1:d40ff07e2fe0 105
EHess 1:d40ff07e2fe0 106 previousValueCounterLeft = valueCounterLeft;
EHess 1:d40ff07e2fe0 107 previousValueCounterRight = valueCounterRight;
EHess 1:d40ff07e2fe0 108 actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft /COUNTS_PER_TURN/PERIOD*60.0f);
EHess 1:d40ff07e2fe0 109 actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight /COUNTS_PER_TURN/PERIOD*60.0f);
EHess 1:d40ff07e2fe0 110
EHess 1:d40ff07e2fe0 111 // Berechnen der Motorspannungen Uout
EHess 1:d40ff07e2fe0 112 float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
EHess 1:d40ff07e2fe0 113 float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;
EHess 1:d40ff07e2fe0 114
EHess 1:d40ff07e2fe0 115 // Berechnen, Limitieren und Setzen der Duty-Cycle
EHess 1:d40ff07e2fe0 116 float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
EHess 1:d40ff07e2fe0 117 if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
EHess 1:d40ff07e2fe0 118 else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
EHess 1:d40ff07e2fe0 119
EHess 1:d40ff07e2fe0 120 pwmLeft = dutyCycleLeft;
EHess 1:d40ff07e2fe0 121 float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
EHess 1:d40ff07e2fe0 122 if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
EHess 1:d40ff07e2fe0 123 else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
EHess 1:d40ff07e2fe0 124
EHess 1:d40ff07e2fe0 125 pwmRight = dutyCycleRight;
EHess 0:96f88638114b 126 }
EHess 0:96f88638114b 127
EHess 3:cfb0b6bcf568 128 int active = 0;
EHess 3:cfb0b6bcf568 129 float distanceStatic;
EHess 3:cfb0b6bcf568 130 int geradeaus_langsam(float distance) // Rückgabewert 1 -> distance erreicht
EHess 3:cfb0b6bcf568 131 {
EHess 3:cfb0b6bcf568 132 if(active == 0) {
EHess 4:2ebbd24aa610 133 active = 1;
EHess 3:cfb0b6bcf568 134 distanceStatic = distance;
EHess 3:cfb0b6bcf568 135 counterLeft.reset();
EHess 4:2ebbd24aa610 136 counterRight.reset();
EHess 3:cfb0b6bcf568 137 }
EHess 3:cfb0b6bcf568 138 if((counterLeft.read() - counterRight.read()) / 100.0f < distanceStatic) {
EHess 4:2ebbd24aa610 139 desiredSpeedLeft = 1.5 + 30;
EHess 4:2ebbd24aa610 140 desiredSpeedRight = 1.5 - 30;
EHess 4:2ebbd24aa610 141 } else {
EHess 4:2ebbd24aa610 142 desiredSpeedLeft = 0.0;
EHess 4:2ebbd24aa610 143 desiredSpeedRight = 0.0;
EHess 4:2ebbd24aa610 144 active = 0;
EHess 4:2ebbd24aa610 145 return 1;
EHess 4:2ebbd24aa610 146 }
EHess 4:2ebbd24aa610 147 return 0;
EHess 4:2ebbd24aa610 148 }
EHess 4:2ebbd24aa610 149
EHess 4:2ebbd24aa610 150 int zurueck_langsam(float distance) // Rückgabewert 1 -> distance erreicht
EHess 4:2ebbd24aa610 151 {
EHess 4:2ebbd24aa610 152 if(active == 0) {
EHess 4:2ebbd24aa610 153 active = 1;
EHess 4:2ebbd24aa610 154 distanceStatic = distance;
EHess 4:2ebbd24aa610 155 counterLeft.reset();
EHess 4:2ebbd24aa610 156 counterRight.reset();
EHess 4:2ebbd24aa610 157 }
EHess 4:2ebbd24aa610 158 if((counterRight.read() - counterLeft.read()) / 100.0f < distanceStatic) {
EHess 4:2ebbd24aa610 159 desiredSpeedLeft = 1.5 - 50;
EHess 4:2ebbd24aa610 160 desiredSpeedRight = 1.5 + 50;
EHess 3:cfb0b6bcf568 161 } else {
EHess 3:cfb0b6bcf568 162 desiredSpeedLeft = 0.0;
EHess 3:cfb0b6bcf568 163 desiredSpeedRight = 0.0;
EHess 3:cfb0b6bcf568 164 active = 0;
EHess 3:cfb0b6bcf568 165 return 1;
EHess 3:cfb0b6bcf568 166 }
EHess 3:cfb0b6bcf568 167 return 0;
EHess 3:cfb0b6bcf568 168 }
EHess 3:cfb0b6bcf568 169
EHess 3:cfb0b6bcf568 170 int geradeaus_schnell(float distance) // RRückgabewert 1 -> distance erreicht
EHess 3:cfb0b6bcf568 171 {
EHess 3:cfb0b6bcf568 172 if(active == 0) {
EHess 3:cfb0b6bcf568 173 active = 1;
EHess 3:cfb0b6bcf568 174 distanceStatic = distance;
EHess 3:cfb0b6bcf568 175 counterLeft.reset();
EHess 4:2ebbd24aa610 176 counterRight.reset();
EHess 3:cfb0b6bcf568 177 }
EHess 3:cfb0b6bcf568 178 if((counterLeft.read() - counterRight.read()) / 100.0f < distanceStatic) {
freunjor 5:24350e6fc9d7 179 desiredSpeedLeft = 1.5 + 90;
freunjor 5:24350e6fc9d7 180 desiredSpeedRight = 1.5 - 90;
EHess 3:cfb0b6bcf568 181 } else {
EHess 3:cfb0b6bcf568 182 desiredSpeedLeft = 0.0;
EHess 3:cfb0b6bcf568 183 desiredSpeedRight = 0.0;
EHess 3:cfb0b6bcf568 184 active = 0;
EHess 3:cfb0b6bcf568 185 return 1;
EHess 3:cfb0b6bcf568 186 }
EHess 3:cfb0b6bcf568 187 return 0;
EHess 3:cfb0b6bcf568 188 }
EHess 3:cfb0b6bcf568 189
EHess 3:cfb0b6bcf568 190 //--------------------Ausweichen------------------------------------------------
EHess 3:cfb0b6bcf568 191
EHess 3:cfb0b6bcf568 192 int a = 0;
EHess 3:cfb0b6bcf568 193 int statusAusweichen = 0;
EHess 3:cfb0b6bcf568 194 int zaehlerAusweichen = 0;
EHess 4:2ebbd24aa610 195 int ausweichen()
EHess 4:2ebbd24aa610 196 {
EHess 4:2ebbd24aa610 197 if(statusAusweichen == 0) {
EHess 3:cfb0b6bcf568 198 if(sensorZentimeter[0] < 10) statusAusweichen = 1; //statusAusweichen 1 -> Wand vorne -> 90° rechts
EHess 3:cfb0b6bcf568 199 else if(sensorZentimeter[0] + sensorZentimeter[3] + sensorZentimeter[4] < 70) statusAusweichen = 2; //statusAusweichen 2 -> Ecke -> 90° rechts
EHess 4:2ebbd24aa610 200 else if(sensorZentimeter[3] < 10) {
EHess 3:cfb0b6bcf568 201 if(a > 0) statusAusweichen = 9; //Spezial
EHess 3:cfb0b6bcf568 202 else statusAusweichen = 3; //statusAusweichen 3 -> Wand links -> 30° rechts
EHess 4:2ebbd24aa610 203 } else if(sensorZentimeter[4] < 10) {
EHess 3:cfb0b6bcf568 204 if(a > 0) statusAusweichen = 9; //Spezial
EHess 3:cfb0b6bcf568 205 else statusAusweichen = 4; //statusAusweichen 4 -> Wand rechts -> 30° links
EHess 3:cfb0b6bcf568 206 }
EHess 3:cfb0b6bcf568 207 }
EHess 4:2ebbd24aa610 208 switch(statusAusweichen) {
EHess 3:cfb0b6bcf568 209 case 0: //kein Hindernis
EHess 3:cfb0b6bcf568 210 a = 0;
EHess 3:cfb0b6bcf568 211 return 1; //Ausgewichen
EHess 3:cfb0b6bcf568 212 case 1: //Hindernis vorne
EHess 3:cfb0b6bcf568 213 case 2: //Ecke
EHess 4:2ebbd24aa610 214 if(zaehlerAusweichen > 12) {
EHess 3:cfb0b6bcf568 215 statusAusweichen = 0;
EHess 3:cfb0b6bcf568 216 zaehlerAusweichen = 0;
EHess 3:cfb0b6bcf568 217 desiredSpeedLeft = 0;
EHess 3:cfb0b6bcf568 218 desiredSpeedRight = 0;
EHess 3:cfb0b6bcf568 219 } else {
EHess 3:cfb0b6bcf568 220 desiredSpeedLeft = 60;
EHess 3:cfb0b6bcf568 221 desiredSpeedRight = 60;
EHess 4:2ebbd24aa610 222 zaehlerAusweichen++;
EHess 3:cfb0b6bcf568 223 }
EHess 3:cfb0b6bcf568 224 break;
EHess 3:cfb0b6bcf568 225 case 3: //Hindernis links
EHess 4:2ebbd24aa610 226 if(zaehlerAusweichen > 4) {
EHess 3:cfb0b6bcf568 227 a++;
EHess 3:cfb0b6bcf568 228 statusAusweichen = 0;
EHess 3:cfb0b6bcf568 229 zaehlerAusweichen = 0;
EHess 3:cfb0b6bcf568 230 desiredSpeedLeft = 0;
EHess 3:cfb0b6bcf568 231 desiredSpeedRight = 0;
EHess 3:cfb0b6bcf568 232 } else {
EHess 3:cfb0b6bcf568 233 desiredSpeedLeft = 60;
EHess 3:cfb0b6bcf568 234 desiredSpeedRight = 60;
EHess 4:2ebbd24aa610 235 zaehlerAusweichen++;
EHess 3:cfb0b6bcf568 236 }
EHess 3:cfb0b6bcf568 237 break;
EHess 3:cfb0b6bcf568 238 case 4: //Hindernis rechts
EHess 4:2ebbd24aa610 239 if(zaehlerAusweichen > 4) {
EHess 3:cfb0b6bcf568 240 a++;
EHess 3:cfb0b6bcf568 241 statusAusweichen = 0;
EHess 3:cfb0b6bcf568 242 zaehlerAusweichen = 0;
EHess 3:cfb0b6bcf568 243 desiredSpeedLeft = 0;
EHess 3:cfb0b6bcf568 244 desiredSpeedRight = 0;
EHess 3:cfb0b6bcf568 245 } else {
EHess 3:cfb0b6bcf568 246 desiredSpeedLeft = -60;
EHess 3:cfb0b6bcf568 247 desiredSpeedRight = -60;
EHess 4:2ebbd24aa610 248 zaehlerAusweichen++;
EHess 3:cfb0b6bcf568 249 }
EHess 3:cfb0b6bcf568 250 break;
EHess 3:cfb0b6bcf568 251 case 9: //Spezial 120°
EHess 4:2ebbd24aa610 252 if(zaehlerAusweichen > 16) {
EHess 3:cfb0b6bcf568 253 a = 0;
EHess 3:cfb0b6bcf568 254 statusAusweichen = 0;
EHess 3:cfb0b6bcf568 255 zaehlerAusweichen = 0;
EHess 3:cfb0b6bcf568 256 desiredSpeedLeft = 0;
EHess 3:cfb0b6bcf568 257 desiredSpeedRight = 0;
EHess 3:cfb0b6bcf568 258 } else {
EHess 3:cfb0b6bcf568 259 desiredSpeedLeft = 60;
EHess 3:cfb0b6bcf568 260 desiredSpeedRight = 60;
EHess 4:2ebbd24aa610 261 zaehlerAusweichen++;
EHess 3:cfb0b6bcf568 262 }
EHess 3:cfb0b6bcf568 263 break;
EHess 3:cfb0b6bcf568 264 }
EHess 3:cfb0b6bcf568 265 return 0; //Am ausweichen...
EHess 3:cfb0b6bcf568 266 }
EHess 3:cfb0b6bcf568 267
EHess 3:cfb0b6bcf568 268 //--------------------Servos----------------------------------------------------
EHess 3:cfb0b6bcf568 269
EHess 3:cfb0b6bcf568 270 Servo Arm(PA_10);
EHess 3:cfb0b6bcf568 271 Servo Greifer(PC_4);
EHess 3:cfb0b6bcf568 272 Servo Deckel(PB_8);
EHess 4:2ebbd24aa610 273 AnalogIn Red (PB_0); //Farbauswertung Rot
EHess 3:cfb0b6bcf568 274 AnalogIn Green (PA_4); //Farbauswertung Grün
EHess 3:cfb0b6bcf568 275
EHess 3:cfb0b6bcf568 276 void aufheben()
EHess 3:cfb0b6bcf568 277 {
EHess 4:2ebbd24aa610 278 enableSensor = 0;
EHess 4:2ebbd24aa610 279 desiredSpeedLeft = 0.0f;
EHess 4:2ebbd24aa610 280 desiredSpeedRight = 0.0f;
EHess 4:2ebbd24aa610 281
EHess 3:cfb0b6bcf568 282 for (int pos = 800; pos < 1400; pos += 25) { //Greifer öffnen
EHess 3:cfb0b6bcf568 283 Greifer.SetPosition(pos);
EHess 3:cfb0b6bcf568 284 wait_ms(7);
EHess 3:cfb0b6bcf568 285 }
EHess 3:cfb0b6bcf568 286 for (int pos = 440; pos < 2300; pos += 25) { //Arm Runter
EHess 3:cfb0b6bcf568 287 Arm.SetPosition(pos);
EHess 3:cfb0b6bcf568 288 wait_ms(7);
EHess 3:cfb0b6bcf568 289 }
EHess 4:2ebbd24aa610 290 desiredSpeedLeft = 65.0f;
EHess 4:2ebbd24aa610 291 desiredSpeedRight = -60.0f;
EHess 3:cfb0b6bcf568 292 wait_ms(400);
EHess 3:cfb0b6bcf568 293 for (int pos = 1400; pos > 800; pos -= 25) { //Greifer Schliessen
EHess 3:cfb0b6bcf568 294 Greifer.SetPosition(pos);
EHess 3:cfb0b6bcf568 295 wait_ms(80);
EHess 3:cfb0b6bcf568 296 }
EHess 4:2ebbd24aa610 297 desiredSpeedLeft = 0.0f;
EHess 4:2ebbd24aa610 298 desiredSpeedRight = 0.0f;
EHess 3:cfb0b6bcf568 299
EHess 3:cfb0b6bcf568 300 if (Green>Red) {
EHess 4:2ebbd24aa610 301 desiredSpeedLeft = -35.0f;
EHess 4:2ebbd24aa610 302 desiredSpeedRight = 30.0f;
EHess 4:2ebbd24aa610 303 wait(1.0f);
EHess 4:2ebbd24aa610 304 desiredSpeedLeft = 0.0f;
EHess 4:2ebbd24aa610 305 desiredSpeedRight = 00.0f;
freunjor 5:24350e6fc9d7 306 for (int pos = 2300; pos > 440; pos -= 30) { //Arm heben
EHess 3:cfb0b6bcf568 307 Arm.SetPosition(pos);
EHess 3:cfb0b6bcf568 308 wait_ms(5);
EHess 3:cfb0b6bcf568 309 }
freunjor 5:24350e6fc9d7 310 wait(0.1f);
EHess 3:cfb0b6bcf568 311 for (int pos = 800; pos < 1400; pos += 25) { //Greifer öffnen
EHess 3:cfb0b6bcf568 312 Greifer.SetPosition(pos);
freunjor 5:24350e6fc9d7 313 wait_ms(3);
EHess 3:cfb0b6bcf568 314 }
EHess 3:cfb0b6bcf568 315 } else if(Red>Green) {
EHess 3:cfb0b6bcf568 316 for (int pos = 1300; pos < 2250; pos += 25) { //Deckel schliessen
EHess 3:cfb0b6bcf568 317 Deckel.SetPosition(pos);
EHess 3:cfb0b6bcf568 318 wait_ms(2);
EHess 3:cfb0b6bcf568 319 }
EHess 4:2ebbd24aa610 320 desiredSpeedLeft = -35.0f;
EHess 4:2ebbd24aa610 321 desiredSpeedRight = 30.0f;
EHess 4:2ebbd24aa610 322 wait(1.0f);
EHess 4:2ebbd24aa610 323 desiredSpeedLeft = 0.0f;
EHess 4:2ebbd24aa610 324 desiredSpeedRight = 00.0f;
EHess 3:cfb0b6bcf568 325 for (int pos = 2300; pos > 440; pos -= 25) { //Arm heben
EHess 3:cfb0b6bcf568 326 Arm.SetPosition(pos);
freunjor 5:24350e6fc9d7 327 wait_ms(9);
EHess 3:cfb0b6bcf568 328 }
EHess 3:cfb0b6bcf568 329 for (int pos = 800; pos < 1400; pos += 25) { //Greifer öffnen
EHess 3:cfb0b6bcf568 330 Greifer.SetPosition(pos);
freunjor 5:24350e6fc9d7 331 wait_ms(4);
EHess 3:cfb0b6bcf568 332 }
EHess 3:cfb0b6bcf568 333 for (int pos = 440; pos < 700; pos += 25) { //Arm Runter
EHess 3:cfb0b6bcf568 334 Arm.SetPosition(pos);
EHess 3:cfb0b6bcf568 335 wait_ms(5);
EHess 3:cfb0b6bcf568 336 }
freunjor 5:24350e6fc9d7 337 for (int pos = 2250; pos > 1300; pos -= 30) { //Deckel öffnen
EHess 3:cfb0b6bcf568 338 Deckel.SetPosition(pos);
EHess 3:cfb0b6bcf568 339 wait_ms(5);
EHess 3:cfb0b6bcf568 340 }
EHess 3:cfb0b6bcf568 341 for (int pos = 700; pos > 440; pos -= 25) { //Arm heben
EHess 3:cfb0b6bcf568 342 Arm.SetPosition(pos);
EHess 3:cfb0b6bcf568 343 wait_ms(5);
EHess 3:cfb0b6bcf568 344 }
EHess 4:2ebbd24aa610 345 } else {
freunjor 5:24350e6fc9d7 346 for (int pos = 2300; pos > 1600; pos -= 25) { //Arm heben um alfällig eingeklemmte Klötze von den Sensoren zu entfernen
freunjor 5:24350e6fc9d7 347 Arm.SetPosition(pos);
freunjor 5:24350e6fc9d7 348 wait_ms(5);
freunjor 5:24350e6fc9d7 349 }
freunjor 5:24350e6fc9d7 350 desiredSpeedLeft = -40.0f;
EHess 4:2ebbd24aa610 351 desiredSpeedRight = 30.0f;
EHess 4:2ebbd24aa610 352 wait(1.0f);
EHess 4:2ebbd24aa610 353 desiredSpeedLeft = 0.0f;
freunjor 5:24350e6fc9d7 354 desiredSpeedRight = 0.0f;
EHess 4:2ebbd24aa610 355 for (int pos = 800; pos < 1400; pos += 25) { //Greifer öffnen
EHess 4:2ebbd24aa610 356 Greifer.SetPosition(pos);
EHess 4:2ebbd24aa610 357 wait_ms(2);
EHess 4:2ebbd24aa610 358 }
freunjor 5:24350e6fc9d7 359 for (int pos = 1600; pos > 440; pos -= 25) { //Arm heben
EHess 4:2ebbd24aa610 360 Arm.SetPosition(pos);
EHess 4:2ebbd24aa610 361 wait_ms(5);
EHess 4:2ebbd24aa610 362 }
EHess 3:cfb0b6bcf568 363 }
EHess 4:2ebbd24aa610 364 enableSensor = 1;
EHess 3:cfb0b6bcf568 365 }
EHess 3:cfb0b6bcf568 366
EHess 3:cfb0b6bcf568 367 //--------------------Status&Main-----------------------------------------------
EHess 3:cfb0b6bcf568 368
EHess 2:365bf16abbf6 369 enum RobotState {
freunjor 5:24350e6fc9d7 370 // START = 0,
freunjor 5:24350e6fc9d7 371 FAHREN = 0,
EHess 3:cfb0b6bcf568 372 AUSWEICHEN,
EHess 2:365bf16abbf6 373 VORSUCHEN,
EHess 2:365bf16abbf6 374 SUCHEN,
EHess 3:cfb0b6bcf568 375 DREHUNGBACK,
EHess 3:cfb0b6bcf568 376 ANFAHREN,
EHess 3:cfb0b6bcf568 377 LADEN
EHess 2:365bf16abbf6 378 };
EHess 2:365bf16abbf6 379
EHess 2:365bf16abbf6 380
EHess 2:365bf16abbf6 381 int main()
EHess 2:365bf16abbf6 382 {
EHess 2:365bf16abbf6 383
EHess 1:d40ff07e2fe0 384 // Initialisieren der PWM Ausgaenge pwmLeft.period(0.00005f); // PWM Periode von 50 us
EHess 1:d40ff07e2fe0 385 pwmLeft.period(0.00005f); // Setzt die Periode auf 50 μs
EHess 1:d40ff07e2fe0 386 pwmRight.period(0.00005f);
EHess 1:d40ff07e2fe0 387 pwmLeft = 0.5f; // Duty-Cycle von 50% pwmRight.period(0.00005f); // PWM Periode von 50 us
EHess 1:d40ff07e2fe0 388 pwmRight = 0.5f; // Duty-Cycle von 50%
EHess 1:d40ff07e2fe0 389
EHess 1:d40ff07e2fe0 390 // Initialisieren von lokalen Variabeln
EHess 1:d40ff07e2fe0 391 previousValueCounterLeft = counterLeft.read();
EHess 1:d40ff07e2fe0 392 previousValueCounterRight = counterRight.read();
EHess 1:d40ff07e2fe0 393 speedLeftFilter.setPeriod(PERIOD);
EHess 1:d40ff07e2fe0 394 speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
EHess 1:d40ff07e2fe0 395 speedRightFilter.setPeriod(PERIOD);
EHess 1:d40ff07e2fe0 396 speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
EHess 0:96f88638114b 397
EHess 3:cfb0b6bcf568 398 desiredSpeedLeft = 0.0f; //Initialisiere Geschwindigkeit auf 0
EHess 3:cfb0b6bcf568 399 desiredSpeedRight = 0.0f; //Initialisiere Geschwindigkeit auf 0
EHess 1:d40ff07e2fe0 400 actualSpeedLeft = 0.0f;
EHess 1:d40ff07e2fe0 401 actualSpeedRight = 0.0f;
EHess 1:d40ff07e2fe0 402
EHess 1:d40ff07e2fe0 403 Ticker t1;
EHess 1:d40ff07e2fe0 404 t1.attach( &speedCtrl, PERIOD);
EHess 0:96f88638114b 405
EHess 2:365bf16abbf6 406 int status = 0; //status definiert aktuellen Programmcode (0: Startprogramm, 1: Suchen, 2: Ausrichten, 3: Anfahren, 4: Aufnehmen)
EHess 2:365bf16abbf6 407
EHess 4:2ebbd24aa610 408 //Servos
EHess 4:2ebbd24aa610 409 Greifer.Enable(1500,20000);
EHess 4:2ebbd24aa610 410 Arm.Enable (440,20000);
EHess 4:2ebbd24aa610 411 Deckel.Enable(1500,20000);
EHess 4:2ebbd24aa610 412
EHess 2:365bf16abbf6 413 //SensorArrays
EHess 2:365bf16abbf6 414 int z = 0; //Zähler
EHess 3:cfb0b6bcf568 415 int y = 0;
EHess 4:2ebbd24aa610 416 float sensorLinks[500];
EHess 4:2ebbd24aa610 417 float sensorOben[500];
EHess 2:365bf16abbf6 418
EHess 1:d40ff07e2fe0 419 enableMotorDriver = 1;
EHess 0:96f88638114b 420
EHess 1:d40ff07e2fe0 421 //Initialisiert Distanzsensoren und setzt sensorValue und sensorTiefbass auf NULL
EHess 1:d40ff07e2fe0 422 for( int i = 0; i < 6; i++) {
EHess 1:d40ff07e2fe0 423 sensors[i].init(&distance, &bit0, &bit1, &bit2, i);
EHess 1:d40ff07e2fe0 424 sensorMittelwert[i] = 0.0f;
EHess 1:d40ff07e2fe0 425 sensorTiefbass[i] = 0.0f;
EHess 1:d40ff07e2fe0 426 }
EHess 1:d40ff07e2fe0 427 enableSensor = 1; //Aktiviert die IRSensoren
EHess 0:96f88638114b 428
freunjor 5:24350e6fc9d7 429 status = FAHREN;
EHess 2:365bf16abbf6 430
EHess 2:365bf16abbf6 431 int timer = 0;
EHess 3:cfb0b6bcf568 432 led = 0;
EHess 2:365bf16abbf6 433
EHess 0:96f88638114b 434 while(1) {
EHess 3:cfb0b6bcf568 435 //Lese Sensorwerte, Tiefpass Initialisierung
EHess 2:365bf16abbf6 436 sensorWerte();
EHess 3:cfb0b6bcf568 437 timer++;
EHess 4:2ebbd24aa610 438 wait(0.0125f);
EHess 2:365bf16abbf6 439
EHess 2:365bf16abbf6 440 switch(status) {
EHess 2:365bf16abbf6 441
freunjor 5:24350e6fc9d7 442 /* case START: //Start Sequenz
freunjor 5:24350e6fc9d7 443 status = FAHREN;
freunjor 5:24350e6fc9d7 444 break;
freunjor 5:24350e6fc9d7 445 */
EHess 3:cfb0b6bcf568 446 case FAHREN:
EHess 4:2ebbd24aa610 447 status = AUSWEICHEN;
freunjor 5:24350e6fc9d7 448 if(geradeaus_schnell(100)) {
EHess 3:cfb0b6bcf568 449 status = VORSUCHEN;
EHess 3:cfb0b6bcf568 450 }
EHess 3:cfb0b6bcf568 451 break;
EHess 4:2ebbd24aa610 452
EHess 3:cfb0b6bcf568 453 case AUSWEICHEN:
EHess 3:cfb0b6bcf568 454 if(ausweichen()) status = FAHREN; //Wenn fertig ausgewichen -> FAHREN
EHess 3:cfb0b6bcf568 455 break;
EHess 3:cfb0b6bcf568 456
EHess 3:cfb0b6bcf568 457 case VORSUCHEN:
EHess 3:cfb0b6bcf568 458 led = 1;
EHess 2:365bf16abbf6 459 desiredSpeedLeft = 15.0f; //Drehung
EHess 2:365bf16abbf6 460 desiredSpeedRight = 15.0f;
EHess 2:365bf16abbf6 461 if(z<12) { //Schreibt die Sensorwerte in ein Array
EHess 4:2ebbd24aa610 462 if(sensorZentimeter[0] > 60) sensorOben[z] = 60;
EHess 4:2ebbd24aa610 463 else sensorOben[z] = sensorZentimeter[0];
EHess 2:365bf16abbf6 464 sensorLinks[z] = sensorZentimeter[5];
EHess 2:365bf16abbf6 465 z++;
EHess 2:365bf16abbf6 466 } else status = SUCHEN;
EHess 2:365bf16abbf6 467 break;
EHess 3:cfb0b6bcf568 468
EHess 3:cfb0b6bcf568 469 case SUCHEN:
EHess 2:365bf16abbf6 470 //for(int i = 0; i < 10; i++)
freunjor 5:24350e6fc9d7 471 desiredSpeedLeft = 20.0f; //Drehung
freunjor 5:24350e6fc9d7 472 desiredSpeedRight = 20.0f;
EHess 2:365bf16abbf6 473
EHess 4:2ebbd24aa610 474 if(z < 500) {
freunjor 5:24350e6fc9d7 475 if(sensorZentimeter[0] > 60) sensorOben[z] = 60; //ACHTUUUUNG Z <500 und sensorOben = 60
EHess 4:2ebbd24aa610 476 else sensorOben[z] = sensorZentimeter[0];
EHess 2:365bf16abbf6 477 sensorLinks[z] = sensorZentimeter[5];
EHess 2:365bf16abbf6 478 //Prüft Ausschlag && vergleicht ersten mit letztem Wert && prüft Zwischenwerte
EHess 2:365bf16abbf6 479 if(3.0f * sensorLinks[z] > sensorLinks[z-7] + sensorLinks[z-6] + sensorLinks[z-5] + 13.5f && sensorLinks[z] + 1 > sensorLinks[z-12] && sensorLinks[z] >= sensorLinks[z-2] && sensorLinks[z] >= sensorLinks[z-3] && sensorLinks[z] >= sensorLinks[z-4] && sensorLinks[z-12] >= sensorLinks[z-10] && sensorLinks[z-12] >= sensorLinks[z-9] && sensorLinks[z-12] >= sensorLinks[z-8]) {
freunjor 5:24350e6fc9d7 480 // if(sensorLinks[z-7] + sensorLinks[z-6] + sensorLinks[z-5] > 3 * 33.5f) {
freunjor 5:24350e6fc9d7 481 //Prüft Hindernis
freunjor 5:24350e6fc9d7 482 if(sensorLinks[z-5] + 3.5f < sensorOben[z-5] && sensorLinks[z-6] + 3.5f < sensorOben[z-6] && sensorLinks[z-7] + 3.5f < sensorOben[z-7]) {
freunjor 5:24350e6fc9d7 483 status = DREHUNGBACK;
freunjor 5:24350e6fc9d7 484 z--;
freunjor 5:24350e6fc9d7 485 printf("\n\r");
freunjor 5:24350e6fc9d7 486 // }
EHess 2:365bf16abbf6 487 }
EHess 2:365bf16abbf6 488 }
EHess 2:365bf16abbf6 489 z++;
EHess 3:cfb0b6bcf568 490 } else {
EHess 2:365bf16abbf6 491 z = 0;
EHess 3:cfb0b6bcf568 492 status = FAHREN;
EHess 2:365bf16abbf6 493 }
EHess 2:365bf16abbf6 494 break;
EHess 2:365bf16abbf6 495
EHess 3:cfb0b6bcf568 496 case DREHUNGBACK:
EHess 4:2ebbd24aa610 497 if(y < 5) {
freunjor 5:24350e6fc9d7 498 desiredSpeedLeft = -25.0f; //Drehung rückwärts
freunjor 5:24350e6fc9d7 499 desiredSpeedRight = -25.0f;
EHess 3:cfb0b6bcf568 500 y++;
EHess 3:cfb0b6bcf568 501 /* } else if {y < 30){
EHess 3:cfb0b6bcf568 502 desiredSpeedLeft = 0.0f; //Stillstand für genauen Sensorwert
EHess 3:cfb0b6bcf568 503 desiredSpeedRight = 0.0f;
EHess 3:cfb0b6bcf568 504 y++;
EHess 3:cfb0b6bcf568 505 */
EHess 3:cfb0b6bcf568 506 } else {
EHess 3:cfb0b6bcf568 507 y = 0;
EHess 3:cfb0b6bcf568 508 status = ANFAHREN;
EHess 3:cfb0b6bcf568 509 }
EHess 2:365bf16abbf6 510 break;
EHess 2:365bf16abbf6 511
EHess 3:cfb0b6bcf568 512 case ANFAHREN:
EHess 4:2ebbd24aa610 513 if(sensorLinks[z-6] < 15.0f) {
EHess 4:2ebbd24aa610 514 if(zurueck_langsam(15.0f - sensorLinks[z-6])) {
EHess 4:2ebbd24aa610 515 z = 0;
EHess 4:2ebbd24aa610 516 status = LADEN;
EHess 4:2ebbd24aa610 517 }
EHess 4:2ebbd24aa610 518 } else if(geradeaus_langsam(sensorLinks[z-6] - 15.0f)) { //-15cm Roboterarm-Offset
EHess 3:cfb0b6bcf568 519 z = 0;
EHess 3:cfb0b6bcf568 520 status = LADEN;
EHess 3:cfb0b6bcf568 521 }
EHess 3:cfb0b6bcf568 522 break;
EHess 3:cfb0b6bcf568 523
EHess 3:cfb0b6bcf568 524 case LADEN:
EHess 3:cfb0b6bcf568 525 aufheben();
EHess 4:2ebbd24aa610 526 desiredSpeedLeft = -50.0f; //Drehung rückwärts
EHess 4:2ebbd24aa610 527 desiredSpeedRight = -50.0f;
EHess 4:2ebbd24aa610 528 wait(0.1f);
EHess 4:2ebbd24aa610 529 desiredSpeedLeft = 0.0f; //Stopp
EHess 4:2ebbd24aa610 530 desiredSpeedRight = 0.0f;
EHess 4:2ebbd24aa610 531 status = VORSUCHEN;
EHess 2:365bf16abbf6 532 break;
EHess 2:365bf16abbf6 533
EHess 1:d40ff07e2fe0 534 }
EHess 0:96f88638114b 535 }
EHess 1:d40ff07e2fe0 536 }