voidaufheben
Dependencies: Servo
main.cpp
- Committer:
- freunjor
- Date:
- 2017-05-16
- Revision:
- 0:be6af08fe19f
File content as of revision 0:be6af08fe19f:
#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; }