kings / voidaufheben

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?

UserRevisionLine numberNew 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 }