Diff: main.cpp

Revision:

0:be6af08fe19f

--- /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