voidaufheben
Dependencies: Servo
main.cpp@0:be6af08fe19f, 2017-05-16 (annotated)
- Committer:
- freunjor
- Date:
- Tue May 16 13:47:29 2017 +0000
- Revision:
- 0:be6af08fe19f
funktion aufheben und farberkennung
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
freunjor | 0:be6af08fe19f | 1 | #include "mbed.h" |
freunjor | 0:be6af08fe19f | 2 | #include "IRSensor.h" |
freunjor | 0:be6af08fe19f | 3 | #include "MotorEncoder.h" |
freunjor | 0:be6af08fe19f | 4 | #include "LowpassFilter.h" |
freunjor | 0:be6af08fe19f | 5 | #include "Servo.h" |
freunjor | 0:be6af08fe19f | 6 | |
freunjor | 0:be6af08fe19f | 7 | |
freunjor | 0:be6af08fe19f | 8 | Servo Arm(PA_10); |
freunjor | 0:be6af08fe19f | 9 | Servo Deckel(PB_8); |
freunjor | 0:be6af08fe19f | 10 | AnalogIn Red (PB_0); //Farbauswertung Rot |
freunjor | 0:be6af08fe19f | 11 | AnalogIn Green (PA_4); //Farbauswertung Grün |
freunjor | 0:be6af08fe19f | 12 | |
freunjor | 0:be6af08fe19f | 13 | |
freunjor | 0:be6af08fe19f | 14 | void aufheben (){ |
freunjor | 0:be6af08fe19f | 15 | |
freunjor | 0:be6af08fe19f | 16 | for (int pos = 800; pos < 1400; pos += 25) { //Greifer öffnen |
freunjor | 0:be6af08fe19f | 17 | Greifer.SetPosition(pos); |
freunjor | 0:be6af08fe19f | 18 | wait_ms(7); |
freunjor | 0:be6af08fe19f | 19 | } |
freunjor | 0:be6af08fe19f | 20 | |
freunjor | 0:be6af08fe19f | 21 | for (int pos = 440; pos < 2300; pos += 25) { //Arm Runter |
freunjor | 0:be6af08fe19f | 22 | Arm.SetPosition(pos); |
freunjor | 0:be6af08fe19f | 23 | wait_ms(7); |
freunjor | 0:be6af08fe19f | 24 | } |
freunjor | 0:be6af08fe19f | 25 | desiredSpeedLeft = 18.0f; |
freunjor | 0:be6af08fe19f | 26 | desiredSpeedRight = -15.0f; |
freunjor | 0:be6af08fe19f | 27 | wait_ms(400); |
freunjor | 0:be6af08fe19f | 28 | for (int pos = 1400; pos > 800; pos -= 25) { //Greifer Schliessen |
freunjor | 0:be6af08fe19f | 29 | Greifer.SetPosition(pos); |
freunjor | 0:be6af08fe19f | 30 | wait_ms(80); |
freunjor | 0:be6af08fe19f | 31 | } |
freunjor | 0:be6af08fe19f | 32 | wait_ms(700); |
freunjor | 0:be6af08fe19f | 33 | desiredSpeedLeft =65.0f; //50 RPM |
freunjor | 0:be6af08fe19f | 34 | desiredSpeedRight = -60f; |
freunjor | 0:be6af08fe19f | 35 | |
freunjor | 0:be6af08fe19f | 36 | if (Green>Red){ |
freunjor | 0:be6af08fe19f | 37 | for (int pos = 2300; pos > 440; pos -= 25) { //Arm heben |
freunjor | 0:be6af08fe19f | 38 | Arm.SetPosition(pos); |
freunjor | 0:be6af08fe19f | 39 | wait_ms(5); |
freunjor | 0:be6af08fe19f | 40 | } |
freunjor | 0:be6af08fe19f | 41 | |
freunjor | 0:be6af08fe19f | 42 | for (int pos = 800; pos < 1400; pos += 25) { //Greifer öffnen |
freunjor | 0:be6af08fe19f | 43 | Greifer.SetPosition(pos); |
freunjor | 0:be6af08fe19f | 44 | wait_ms(2); |
freunjor | 0:be6af08fe19f | 45 | } |
freunjor | 0:be6af08fe19f | 46 | |
freunjor | 0:be6af08fe19f | 47 | } |
freunjor | 0:be6af08fe19f | 48 | else if(Red>Green) { |
freunjor | 0:be6af08fe19f | 49 | |
freunjor | 0:be6af08fe19f | 50 | for (int pos = 1300; pos < 2250; pos += 25) { //Deckel schliessen |
freunjor | 0:be6af08fe19f | 51 | Deckel.SetPosition(pos); |
freunjor | 0:be6af08fe19f | 52 | wait_ms(2); |
freunjor | 0:be6af08fe19f | 53 | } |
freunjor | 0:be6af08fe19f | 54 | |
freunjor | 0:be6af08fe19f | 55 | for (int pos = 2300; pos > 440; pos -= 25) { //Arm heben |
freunjor | 0:be6af08fe19f | 56 | Arm.SetPosition(pos); |
freunjor | 0:be6af08fe19f | 57 | wait_ms(7); |
freunjor | 0:be6af08fe19f | 58 | } |
freunjor | 0:be6af08fe19f | 59 | wait_ms(50); |
freunjor | 0:be6af08fe19f | 60 | for (int pos = 800; pos < 1400; pos += 25) { //Greifer öffnen |
freunjor | 0:be6af08fe19f | 61 | Greifer.SetPosition(pos); |
freunjor | 0:be6af08fe19f | 62 | wait_ms(2); |
freunjor | 0:be6af08fe19f | 63 | } |
freunjor | 0:be6af08fe19f | 64 | for (int pos = 440; pos < 700; pos += 25) { //Arm Runter |
freunjor | 0:be6af08fe19f | 65 | Arm.SetPosition(pos); |
freunjor | 0:be6af08fe19f | 66 | wait_ms(5); |
freunjor | 0:be6af08fe19f | 67 | } |
freunjor | 0:be6af08fe19f | 68 | for (int pos = 2250; pos > 1300; pos -= 25) { //Deckel öffnen |
freunjor | 0:be6af08fe19f | 69 | Deckel.SetPosition(pos); |
freunjor | 0:be6af08fe19f | 70 | wait_ms(5); |
freunjor | 0:be6af08fe19f | 71 | } |
freunjor | 0:be6af08fe19f | 72 | for (int pos = 700; pos > 440; pos -= 25) { //Arm heben |
freunjor | 0:be6af08fe19f | 73 | Arm.SetPosition(pos); |
freunjor | 0:be6af08fe19f | 74 | wait_ms(5); |
freunjor | 0:be6af08fe19f | 75 | } |
freunjor | 0:be6af08fe19f | 76 | } |
freunjor | 0:be6af08fe19f | 77 | enableSensor=1; |
freunjor | 0:be6af08fe19f | 78 | } |