kings
Totale Testversion
Dependencies: mbed
Fork of
DrehungMitStopp
by 
kings
Diff: main.cpp
- Revision:
- 3:cfb0b6bcf568
- Parent:
- 2:365bf16abbf6
- Child:
- 4:2ebbd24aa610
--- a/main.cpp Tue May 16 14:14:08 2017 +0000
+++ b/main.cpp Thu May 18 13:22:47 2017 +0000
@@ -2,10 +2,13 @@
#include "IRSensor.h"
#include "MotorEncoder.h"
#include "LowpassFilter.h"
+#include "Servo.h"
#include <cmath>
DigitalOut led(LED1); //Zustands-LED: Grüne LED für Benutzer
+//--------------------IRSensoren------------------------------------------------
+
AnalogIn distance(PB_1); //Input der Distanzsensoren
DigitalOut enableSensor(PC_1); //Aktivierung der IRSensoren
DigitalOut bit0(PH_1); //Ansteuerung der Sensoren 0-5 mit 3 Bits
@@ -32,7 +35,6 @@
for(int i = 0; i < 6; i++) {
sensorMittelwert[i] += sensors[i].read();
}
- //wait( 0.001f );
}
for(int i = 0; i < 6; i++) {
sensorTiefbass[i] = sensorTiefbass[i]*0.75f + sensorMittelwert[i]*0.25f; //Verrechnet den neuen Wert mit dem alten
@@ -60,16 +62,10 @@
break;
}
}
-
printf("%f\t%f\t%f\n\r", sensorZentimeter[5], sensorZentimeter[0], sensorZentimeter[1]); //Plottet die unteren Sensoren + Mitte-Oben
-
- printfZaehler++;
-// if(printfZaehler % 120 == 0){
-// wait(3.5f);
-// }
- if(printfZaehler % 40 == 0) title(); //Erstellt nach 40 Zeilen eine neue Seite
}
+//--------------------Motor&Encoder---------------------------------------------
const float PERIOD = 0.002f; // period of control task, given in [s]
const float COUNTS_PER_TURN = 1200.0f; // resolution of encoder counter
@@ -90,13 +86,11 @@
PwmOut pwmLeft(PA_8);
PwmOut pwmRight(PA_9);
-DigitalOut my_led(LED1);
-
short previousValueCounterRight = 0;
short previousValueCounterLeft = 0;
-float desiredSpeedLeft;
-float desiredSpeedRight;
+float desiredSpeedLeft; //Ansteuerung des linken Motors
+float desiredSpeedRight; //Ansteuerung des rechten Motors
float actualSpeedLeft;
float actualSpeedRight;
@@ -131,13 +125,198 @@
pwmRight = dutyCycleRight;
}
+int active = 0;
+float distanceStatic;
+int geradeaus_langsam(float distance) // Rückgabewert 1 -> distance erreicht
+{
+ if(active == 0) {
+ active =1;
+ distanceStatic = distance;
+ counterLeft.reset();
+ }
+ if((counterLeft.read() - counterRight.read()) / 100.0f < distanceStatic) {
+ desiredSpeedLeft = 1.5 + 15;
+ desiredSpeedRight = 1.5 - 15;
+ } else {
+ desiredSpeedLeft = 0.0;
+ desiredSpeedRight = 0.0;
+ active = 0;
+ return 1;
+ }
+ return 0;
+}
+
+int geradeaus_schnell(float distance) // RRückgabewert 1 -> distance erreicht
+{
+ if(active == 0) {
+ active = 1;
+ distanceStatic = distance;
+ counterLeft.reset();
+ }
+ if((counterLeft.read() - counterRight.read()) / 100.0f < distanceStatic) {
+ desiredSpeedLeft = 1.5 + 60;
+ desiredSpeedRight = 1.5 - 60;
+ } else {
+ desiredSpeedLeft = 0.0;
+ desiredSpeedRight = 0.0;
+ active = 0;
+ return 1;
+ }
+ return 0;
+}
+
+//--------------------Ausweichen------------------------------------------------
+
+int a = 0;
+int statusAusweichen = 0;
+int zaehlerAusweichen = 0;
+int ausweichen(){
+ if(statusAusweichen == 0){
+ if(sensorZentimeter[0] < 10) statusAusweichen = 1; //statusAusweichen 1 -> Wand vorne -> 90° rechts
+ else if(sensorZentimeter[0] + sensorZentimeter[3] + sensorZentimeter[4] < 70) statusAusweichen = 2; //statusAusweichen 2 -> Ecke -> 90° rechts
+ else if(sensorZentimeter[3] < 10){
+ if(a > 0) statusAusweichen = 9; //Spezial
+ else statusAusweichen = 3; //statusAusweichen 3 -> Wand links -> 30° rechts
+ } else if(sensorZentimeter[4] < 10){
+ if(a > 0) statusAusweichen = 9; //Spezial
+ else statusAusweichen = 4; //statusAusweichen 4 -> Wand rechts -> 30° links
+ }
+ }
+ switch(statusAusweichen){
+ case 0: //kein Hindernis
+ a = 0;
+ return 1; //Ausgewichen
+ case 1: //Hindernis vorne
+ case 2: //Ecke
+ if(zaehlerAusweichen > 30){
+ statusAusweichen = 0;
+ zaehlerAusweichen = 0;
+ desiredSpeedLeft = 0;
+ desiredSpeedRight = 0;
+ } else {
+ desiredSpeedLeft = 60;
+ desiredSpeedRight = 60;
+ }
+ break;
+ case 3: //Hindernis links
+ if(zaehlerAusweichen > 10){
+ a++;
+ statusAusweichen = 0;
+ zaehlerAusweichen = 0;
+ desiredSpeedLeft = 0;
+ desiredSpeedRight = 0;
+ } else {
+ desiredSpeedLeft = 60;
+ desiredSpeedRight = 60;
+ }
+ break;
+ case 4: //Hindernis rechts
+ if(zaehlerAusweichen > 10){
+ a++;
+ statusAusweichen = 0;
+ zaehlerAusweichen = 0;
+ desiredSpeedLeft = 0;
+ desiredSpeedRight = 0;
+ } else {
+ desiredSpeedLeft = -60;
+ desiredSpeedRight = -60;
+ }
+ break;
+ case 9: //Spezial 120°
+ if(zaehlerAusweichen > 40){
+ a = 0;
+ statusAusweichen = 0;
+ zaehlerAusweichen = 0;
+ desiredSpeedLeft = 0;
+ desiredSpeedRight = 0;
+ } else {
+ desiredSpeedLeft = 60;
+ desiredSpeedRight = 60;
+ }
+ break;
+ }
+ return 0; //Am ausweichen...
+}
+
+//--------------------Servos----------------------------------------------------
+
+Servo Arm(PA_10);
+Servo Greifer(PC_4);
+Servo Deckel(PB_8);
+AnalogIn Red (PB_0); //Farbauswertung Rot
+AnalogIn Green (PA_4); //Farbauswertung Grün
+
+void aufheben()
+{
+ for (int pos = 800; pos < 1400; pos += 25) { //Greifer öffnen
+ Greifer.SetPosition(pos);
+ wait_ms(7);
+ }
+ for (int pos = 440; pos < 2300; pos += 25) { //Arm Runter
+ Arm.SetPosition(pos);
+ wait_ms(7);
+ }
+ desiredSpeedLeft = 18.0f;
+ desiredSpeedRight = -15.0f;
+ wait_ms(400);
+ for (int pos = 1400; pos > 800; pos -= 25) { //Greifer Schliessen
+ Greifer.SetPosition(pos);
+ wait_ms(80);
+ }
+ wait_ms(700);
+ desiredSpeedLeft =65.0f; //50 RPM
+ desiredSpeedRight = -60.0f;
+
+ if (Green>Red) {
+ for (int pos = 2300; pos > 440; pos -= 25) { //Arm heben
+ Arm.SetPosition(pos);
+ wait_ms(5);
+ }
+ for (int pos = 800; pos < 1400; pos += 25) { //Greifer öffnen
+ Greifer.SetPosition(pos);
+ wait_ms(2);
+ }
+ } else if(Red>Green) {
+ for (int pos = 1300; pos < 2250; pos += 25) { //Deckel schliessen
+ Deckel.SetPosition(pos);
+ wait_ms(2);
+ }
+ for (int pos = 2300; pos > 440; pos -= 25) { //Arm heben
+ Arm.SetPosition(pos);
+ wait_ms(7);
+ }
+ wait_ms(50);
+ for (int pos = 800; pos < 1400; pos += 25) { //Greifer öffnen
+ Greifer.SetPosition(pos);
+ wait_ms(2);
+ }
+ for (int pos = 440; pos < 700; pos += 25) { //Arm Runter
+ Arm.SetPosition(pos);
+ wait_ms(5);
+ }
+ for (int pos = 2250; pos > 1300; pos -= 25) { //Deckel öffnen
+ Deckel.SetPosition(pos);
+ wait_ms(5);
+ }
+ for (int pos = 700; pos > 440; pos -= 25) { //Arm heben
+ Arm.SetPosition(pos);
+ wait_ms(5);
+ }
+ }
+ enableSensor=1;
+}
+
+//--------------------Status&Main-----------------------------------------------
+
enum RobotState {
START = 0,
+ FAHREN,
+ AUSWEICHEN,
VORSUCHEN,
SUCHEN,
- LADEN,
- SONST
-
+ DREHUNGBACK,
+ ANFAHREN,
+ LADEN
};
@@ -158,8 +337,8 @@
speedRightFilter.setPeriod(PERIOD);
speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
- desiredSpeedLeft = 0.0f;
- desiredSpeedRight = 0.0f;
+ desiredSpeedLeft = 0.0f; //Initialisiere Geschwindigkeit auf 0
+ desiredSpeedRight = 0.0f; //Initialisiere Geschwindigkeit auf 0
actualSpeedLeft = 0.0f;
actualSpeedRight = 0.0f;
@@ -170,9 +349,9 @@
//SensorArrays
int z = 0; //Zähler
+ int y = 0;
float sensorLinks[100];
float sensorOben[100];
- float sensorRechts[100];
enableMotorDriver = 1;
@@ -187,43 +366,45 @@
status = START;
int timer = 0;
- my_led = 0;
+ led = 0;
while(1) {
- //lese sensorwerte, tiefpass initialisierung
+ //Lese Sensorwerte, Tiefpass Initialisierung
sensorWerte();
- ++timer;
+ timer++;
wait(0.025f);
-//DISPLAY STATE
-
- //while(status == 1){
switch(status) {
- //start sequenz
- case START:
- // desiredSpeedLeft = 15.0f; //Drehung
- // desiredSpeedRight = 15.0f;
-
- //roboter dreh links
+ case START: //Start Sequenz
if( timer > 40 )
- status = 1;
+ status = FAHREN;
break;
- case VORSUCHEN: {
- my_led = 1;
+ case FAHREN:
+ if(geradeaus_schnell(40)){
+ status = VORSUCHEN;
+ }
+ status = AUSWEICHEN;
+ break;
+
+ case AUSWEICHEN:
+ if(ausweichen()) status = FAHREN; //Wenn fertig ausgewichen -> FAHREN
+ break;
+
+ case VORSUCHEN:
+ led = 1;
desiredSpeedLeft = 15.0f; //Drehung
desiredSpeedRight = 15.0f;
if(z<12) { //Schreibt die Sensorwerte in ein Array
sensorOben[z] = sensorZentimeter[0];
sensorLinks[z] = sensorZentimeter[5];
- sensorRechts[z] = sensorZentimeter[1];
z++;
} else status = SUCHEN;
break;
- }
- case SUCHEN: {
- my_led = 1;
+
+ case SUCHEN:
+ led = 1;
//for(int i = 0; i < 10; i++)
desiredSpeedLeft = 15.0f; //Drehung
@@ -232,37 +413,46 @@
if(z < 100) {
sensorOben[z] = sensorZentimeter[0];
sensorLinks[z] = sensorZentimeter[5];
- sensorRechts[z] = sensorZentimeter[1];
//Prüft Ausschlag && vergleicht ersten mit letztem Wert && prüft Zwischenwerte
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]) {
//Prüft Hindernis
- 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]){
- status = 3;
- z=0;
+ 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]) {
+ status = DREHUNGBACK;
+ z--;
}
}
z++;
- }
- else{
+ } else {
z = 0;
- status = 4;
+ status = FAHREN;
}
-
break;
- }
- case 3:
- desiredSpeedLeft = -20.0f; //Stillstand
- desiredSpeedRight = 15.0f;
- wait(2.0f);
- status = VORSUCHEN;
+ case DREHUNGBACK:
+ if(y < 15) {
+ desiredSpeedLeft = -15.0f; //Drehung rückwärts
+ desiredSpeedRight = -15.0f;
+ y++;
+ /* } else if {y < 30){
+ desiredSpeedLeft = 0.0f; //Stillstand für genauen Sensorwert
+ desiredSpeedRight = 0.0f;
+ y++;
+ */
+ } else {
+ y = 0;
+ status = ANFAHREN;
+ }
break;
- case 4:
- desiredSpeedLeft = 15.0f; //Stillstand
- desiredSpeedRight = -20.0f;
- wait(2.0f);
- status = VORSUCHEN;
+ case ANFAHREN:
+ if(geradeaus_langsam(sensorLinks[z-6]-10-15)) { //-10cm Sensortoleranz, -15cm Roboterarm-Offset
+ z = 0;
+ status = LADEN;
+ }
+ break;
+
+ case LADEN:
+ aufheben();
break;
}