voidaufheben
Dependencies: Servo
Diff: main.cpp
- Revision:
- 0:be6af08fe19f
diff -r 000000000000 -r be6af08fe19f main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue May 16 13:47:29 2017 +0000 @@ -0,0 +1,78 @@ +#include "mbed.h" +#include "IRSensor.h" +#include "MotorEncoder.h" +#include "LowpassFilter.h" +#include "Servo.h" + + +Servo Arm(PA_10); +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 = -60f; + + 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; + } \ No newline at end of file