kings
Totale Testversion
Dependencies: mbed
Fork of
DrehungMitStopp
by 
kings
Diff: main.cpp
- Revision:
- 4:2ebbd24aa610
- Parent:
- 3:cfb0b6bcf568
- Child:
- 5:24350e6fc9d7
--- a/main.cpp Thu May 18 13:22:47 2017 +0000
+++ b/main.cpp Sun May 21 11:28:15 2017 +0000
@@ -62,7 +62,7 @@
break;
}
}
- printf("%f\t%f\t%f\n\r", sensorZentimeter[5], sensorZentimeter[0], sensorZentimeter[1]); //Plottet die unteren Sensoren + Mitte-Oben
+ printf("%f\t%f\n\r", sensorZentimeter[5], sensorZentimeter[0]); //Plottet die unteren Sensoren + Mitte-Oben
}
//--------------------Motor&Encoder---------------------------------------------
@@ -130,13 +130,34 @@
int geradeaus_langsam(float distance) // Rückgabewert 1 -> distance erreicht
{
if(active == 0) {
- active =1;
+ active = 1;
distanceStatic = distance;
counterLeft.reset();
+ counterRight.reset();
}
if((counterLeft.read() - counterRight.read()) / 100.0f < distanceStatic) {
- desiredSpeedLeft = 1.5 + 15;
- desiredSpeedRight = 1.5 - 15;
+ desiredSpeedLeft = 1.5 + 30;
+ desiredSpeedRight = 1.5 - 30;
+ } else {
+ desiredSpeedLeft = 0.0;
+ desiredSpeedRight = 0.0;
+ active = 0;
+ return 1;
+ }
+ return 0;
+}
+
+int zurueck_langsam(float distance) // Rückgabewert 1 -> distance erreicht
+{
+ if(active == 0) {
+ active = 1;
+ distanceStatic = distance;
+ counterLeft.reset();
+ counterRight.reset();
+ }
+ if((counterRight.read() - counterLeft.read()) / 100.0f < distanceStatic) {
+ desiredSpeedLeft = 1.5 - 50;
+ desiredSpeedRight = 1.5 + 50;
} else {
desiredSpeedLeft = 0.0;
desiredSpeedRight = 0.0;
@@ -152,6 +173,7 @@
active = 1;
distanceStatic = distance;
counterLeft.reset();
+ counterRight.reset();
}
if((counterLeft.read() - counterRight.read()) / 100.0f < distanceStatic) {
desiredSpeedLeft = 1.5 + 60;
@@ -170,25 +192,26 @@
int a = 0;
int statusAusweichen = 0;
int zaehlerAusweichen = 0;
-int ausweichen(){
- if(statusAusweichen == 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){
+ 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){
+ } else if(sensorZentimeter[4] < 10) {
if(a > 0) statusAusweichen = 9; //Spezial
else statusAusweichen = 4; //statusAusweichen 4 -> Wand rechts -> 30° links
}
}
- switch(statusAusweichen){
+ switch(statusAusweichen) {
case 0: //kein Hindernis
a = 0;
return 1; //Ausgewichen
case 1: //Hindernis vorne
case 2: //Ecke
- if(zaehlerAusweichen > 30){
+ if(zaehlerAusweichen > 12) {
statusAusweichen = 0;
zaehlerAusweichen = 0;
desiredSpeedLeft = 0;
@@ -196,10 +219,11 @@
} else {
desiredSpeedLeft = 60;
desiredSpeedRight = 60;
+ zaehlerAusweichen++;
}
break;
case 3: //Hindernis links
- if(zaehlerAusweichen > 10){
+ if(zaehlerAusweichen > 4) {
a++;
statusAusweichen = 0;
zaehlerAusweichen = 0;
@@ -208,10 +232,11 @@
} else {
desiredSpeedLeft = 60;
desiredSpeedRight = 60;
+ zaehlerAusweichen++;
}
break;
case 4: //Hindernis rechts
- if(zaehlerAusweichen > 10){
+ if(zaehlerAusweichen > 4) {
a++;
statusAusweichen = 0;
zaehlerAusweichen = 0;
@@ -220,10 +245,11 @@
} else {
desiredSpeedLeft = -60;
desiredSpeedRight = -60;
+ zaehlerAusweichen++;
}
break;
case 9: //Spezial 120°
- if(zaehlerAusweichen > 40){
+ if(zaehlerAusweichen > 16) {
a = 0;
statusAusweichen = 0;
zaehlerAusweichen = 0;
@@ -232,6 +258,7 @@
} else {
desiredSpeedLeft = 60;
desiredSpeedRight = 60;
+ zaehlerAusweichen++;
}
break;
}
@@ -243,11 +270,15 @@
Servo Arm(PA_10);
Servo Greifer(PC_4);
Servo Deckel(PB_8);
-AnalogIn Red (PB_0); //Farbauswertung Rot
+AnalogIn Red (PB_0); //Farbauswertung Rot
AnalogIn Green (PA_4); //Farbauswertung Grün
void aufheben()
{
+ enableSensor = 0;
+ desiredSpeedLeft = 0.0f;
+ desiredSpeedRight = 0.0f;
+
for (int pos = 800; pos < 1400; pos += 25) { //Greifer öffnen
Greifer.SetPosition(pos);
wait_ms(7);
@@ -256,22 +287,27 @@
Arm.SetPosition(pos);
wait_ms(7);
}
- desiredSpeedLeft = 18.0f;
- desiredSpeedRight = -15.0f;
+ desiredSpeedLeft = 65.0f;
+ desiredSpeedRight = -60.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;
+ desiredSpeedLeft = 0.0f;
+ desiredSpeedRight = 0.0f;
if (Green>Red) {
+ desiredSpeedLeft = -35.0f;
+ desiredSpeedRight = 30.0f;
+ wait(1.0f);
+ desiredSpeedLeft = 0.0f;
+ desiredSpeedRight = 00.0f;
for (int pos = 2300; pos > 440; pos -= 25) { //Arm heben
Arm.SetPosition(pos);
wait_ms(5);
}
+ wait(0.05f);
for (int pos = 800; pos < 1400; pos += 25) { //Greifer öffnen
Greifer.SetPosition(pos);
wait_ms(2);
@@ -281,6 +317,11 @@
Deckel.SetPosition(pos);
wait_ms(2);
}
+ desiredSpeedLeft = -35.0f;
+ desiredSpeedRight = 30.0f;
+ wait(1.0f);
+ desiredSpeedLeft = 0.0f;
+ desiredSpeedRight = 00.0f;
for (int pos = 2300; pos > 440; pos -= 25) { //Arm heben
Arm.SetPosition(pos);
wait_ms(7);
@@ -302,8 +343,22 @@
Arm.SetPosition(pos);
wait_ms(5);
}
+ } else {
+ desiredSpeedLeft = -35.0f;
+ desiredSpeedRight = 30.0f;
+ wait(1.0f);
+ desiredSpeedLeft = 0.0f;
+ desiredSpeedRight = 00.0f;
+ for (int pos = 800; pos < 1400; pos += 25) { //Greifer öffnen
+ Greifer.SetPosition(pos);
+ wait_ms(2);
+ }
+ for (int pos = 2300; pos > 440; pos -= 25) { //Arm heben
+ Arm.SetPosition(pos);
+ wait_ms(5);
+ }
}
- enableSensor=1;
+ enableSensor = 1;
}
//--------------------Status&Main-----------------------------------------------
@@ -347,11 +402,16 @@
int status = 0; //status definiert aktuellen Programmcode (0: Startprogramm, 1: Suchen, 2: Ausrichten, 3: Anfahren, 4: Aufnehmen)
+ //Servos
+ Greifer.Enable(1500,20000);
+ Arm.Enable (440,20000);
+ Deckel.Enable(1500,20000);
+
//SensorArrays
int z = 0; //Zähler
int y = 0;
- float sensorLinks[100];
- float sensorOben[100];
+ float sensorLinks[500];
+ float sensorOben[500];
enableMotorDriver = 1;
@@ -372,22 +432,21 @@
//Lese Sensorwerte, Tiefpass Initialisierung
sensorWerte();
timer++;
- wait(0.025f);
+ wait(0.0125f);
switch(status) {
case START: //Start Sequenz
- if( timer > 40 )
- status = FAHREN;
+ status = FAHREN;
break;
case FAHREN:
- if(geradeaus_schnell(40)){
+ status = AUSWEICHEN;
+ if(geradeaus_schnell(80)) {
status = VORSUCHEN;
}
- status = AUSWEICHEN;
break;
-
+
case AUSWEICHEN:
if(ausweichen()) status = FAHREN; //Wenn fertig ausgewichen -> FAHREN
break;
@@ -397,21 +456,21 @@
desiredSpeedLeft = 15.0f; //Drehung
desiredSpeedRight = 15.0f;
if(z<12) { //Schreibt die Sensorwerte in ein Array
- sensorOben[z] = sensorZentimeter[0];
+ if(sensorZentimeter[0] > 60) sensorOben[z] = 60;
+ else sensorOben[z] = sensorZentimeter[0];
sensorLinks[z] = sensorZentimeter[5];
z++;
} else status = SUCHEN;
break;
case SUCHEN:
- led = 1;
-
//for(int i = 0; i < 10; i++)
desiredSpeedLeft = 15.0f; //Drehung
desiredSpeedRight = 15.0f;
- if(z < 100) {
- sensorOben[z] = sensorZentimeter[0];
+ if(z < 500) {
+ if(sensorZentimeter[0] > 60) sensorOben[z] = 60;
+ else sensorOben[z] = sensorZentimeter[0];
sensorLinks[z] = sensorZentimeter[5];
//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]) {
@@ -419,6 +478,7 @@
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--;
+ printf("\n\r");
}
}
z++;
@@ -429,9 +489,9 @@
break;
case DREHUNGBACK:
- if(y < 15) {
- desiredSpeedLeft = -15.0f; //Drehung rückwärts
- desiredSpeedRight = -15.0f;
+ if(y < 5) {
+ desiredSpeedLeft = -30.0f; //Drehung rückwärts
+ desiredSpeedRight = -30.0f;
y++;
/* } else if {y < 30){
desiredSpeedLeft = 0.0f; //Stillstand für genauen Sensorwert
@@ -445,7 +505,12 @@
break;
case ANFAHREN:
- if(geradeaus_langsam(sensorLinks[z-6]-10-15)) { //-10cm Sensortoleranz, -15cm Roboterarm-Offset
+ if(sensorLinks[z-6] < 15.0f) {
+ if(zurueck_langsam(15.0f - sensorLinks[z-6])) {
+ z = 0;
+ status = LADEN;
+ }
+ } else if(geradeaus_langsam(sensorLinks[z-6] - 15.0f)) { //-15cm Roboterarm-Offset
z = 0;
status = LADEN;
}
@@ -453,6 +518,12 @@
case LADEN:
aufheben();
+ desiredSpeedLeft = -50.0f; //Drehung rückwärts
+ desiredSpeedRight = -50.0f;
+ wait(0.1f);
+ desiredSpeedLeft = 0.0f; //Stopp
+ desiredSpeedRight = 0.0f;
+ status = VORSUCHEN;
break;
}