Totale Testversion
Dependencies: mbed
Fork of DrehungMitStopp by
Diff: main.cpp
- Revision:
- 5:24350e6fc9d7
- Parent:
- 4:2ebbd24aa610
diff -r 2ebbd24aa610 -r 24350e6fc9d7 main.cpp --- a/main.cpp Sun May 21 11:28:15 2017 +0000 +++ b/main.cpp Sun May 21 15:29:46 2017 +0000 @@ -176,8 +176,8 @@ counterRight.reset(); } if((counterLeft.read() - counterRight.read()) / 100.0f < distanceStatic) { - desiredSpeedLeft = 1.5 + 60; - desiredSpeedRight = 1.5 - 60; + desiredSpeedLeft = 1.5 + 90; + desiredSpeedRight = 1.5 - 90; } else { desiredSpeedLeft = 0.0; desiredSpeedRight = 0.0; @@ -303,14 +303,14 @@ wait(1.0f); desiredSpeedLeft = 0.0f; desiredSpeedRight = 00.0f; - for (int pos = 2300; pos > 440; pos -= 25) { //Arm heben + for (int pos = 2300; pos > 440; pos -= 30) { //Arm heben Arm.SetPosition(pos); wait_ms(5); } - wait(0.05f); + wait(0.1f); for (int pos = 800; pos < 1400; pos += 25) { //Greifer öffnen Greifer.SetPosition(pos); - wait_ms(2); + wait_ms(3); } } else if(Red>Green) { for (int pos = 1300; pos < 2250; pos += 25) { //Deckel schliessen @@ -324,18 +324,17 @@ desiredSpeedRight = 00.0f; for (int pos = 2300; pos > 440; pos -= 25) { //Arm heben Arm.SetPosition(pos); - wait_ms(7); + wait_ms(9); } - wait_ms(50); for (int pos = 800; pos < 1400; pos += 25) { //Greifer öffnen Greifer.SetPosition(pos); - wait_ms(2); + wait_ms(4); } 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 + for (int pos = 2250; pos > 1300; pos -= 30) { //Deckel öffnen Deckel.SetPosition(pos); wait_ms(5); } @@ -344,16 +343,20 @@ wait_ms(5); } } else { - desiredSpeedLeft = -35.0f; + for (int pos = 2300; pos > 1600; pos -= 25) { //Arm heben um alfällig eingeklemmte Klötze von den Sensoren zu entfernen + Arm.SetPosition(pos); + wait_ms(5); + } + desiredSpeedLeft = -40.0f; desiredSpeedRight = 30.0f; wait(1.0f); desiredSpeedLeft = 0.0f; - desiredSpeedRight = 00.0f; + desiredSpeedRight = 0.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 + for (int pos = 1600; pos > 440; pos -= 25) { //Arm heben Arm.SetPosition(pos); wait_ms(5); } @@ -364,8 +367,8 @@ //--------------------Status&Main----------------------------------------------- enum RobotState { - START = 0, - FAHREN, + // START = 0, + FAHREN = 0, AUSWEICHEN, VORSUCHEN, SUCHEN, @@ -423,7 +426,7 @@ } enableSensor = 1; //Aktiviert die IRSensoren - status = START; + status = FAHREN; int timer = 0; led = 0; @@ -436,13 +439,13 @@ switch(status) { - case START: //Start Sequenz - status = FAHREN; - break; - + /* case START: //Start Sequenz + status = FAHREN; + break; + */ case FAHREN: status = AUSWEICHEN; - if(geradeaus_schnell(80)) { + if(geradeaus_schnell(100)) { status = VORSUCHEN; } break; @@ -465,20 +468,22 @@ case SUCHEN: //for(int i = 0; i < 10; i++) - desiredSpeedLeft = 15.0f; //Drehung - desiredSpeedRight = 15.0f; + desiredSpeedLeft = 20.0f; //Drehung + desiredSpeedRight = 20.0f; if(z < 500) { - if(sensorZentimeter[0] > 60) sensorOben[z] = 60; + if(sensorZentimeter[0] > 60) sensorOben[z] = 60; //ACHTUUUUNG Z <500 und sensorOben = 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]) { - //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 = DREHUNGBACK; - z--; - printf("\n\r"); + // if(sensorLinks[z-7] + sensorLinks[z-6] + sensorLinks[z-5] > 3 * 33.5f) { + //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 = DREHUNGBACK; + z--; + printf("\n\r"); + // } } } z++; @@ -490,8 +495,8 @@ case DREHUNGBACK: if(y < 5) { - desiredSpeedLeft = -30.0f; //Drehung rückwärts - desiredSpeedRight = -30.0f; + desiredSpeedLeft = -25.0f; //Drehung rückwärts + desiredSpeedRight = -25.0f; y++; /* } else if {y < 30){ desiredSpeedLeft = 0.0f; //Stillstand für genauen Sensorwert