Diff: main.cpp

Revision:

3:5483d7c18c34

Parent:

2:365bf16abbf6

Child:

4:85b8b4aa97a3

--- a/main.cpp	Tue May 16 14:14:08 2017 +0000
+++ b/main.cpp	Tue May 16 15:44:05 2017 +0000
@@ -1,5 +1,6 @@
 #include "mbed.h"
 #include "IRSensor.h"
+#include "Servo.h"
 #include "MotorEncoder.h"
 #include "LowpassFilter.h"
 #include <cmath>
@@ -12,6 +13,12 @@
 DigitalOut bit1(PC_2);
 DigitalOut bit2(PC_3);
 IRSensor sensors[6];            //Erstellt 6 IRSensor-Objekte als Array
+Servo Arm(PA_10);
+Servo Greifer(PC_4);
+Servo Deckel(PB_8);
+AnalogIn Red (PB_0);               //Farbauswertung Rot 
+AnalogIn Green (PA_4);             //Farbauswertung Grün
+
 
 DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };    //LED-Outputs der Sensoren
 
@@ -139,11 +146,77 @@
     SONST
 
 };
-
+int aufheben (){
+      
+      enableSensor=0;
+      desiredSpeedLeft =65.0f; //50 RPM
+      desiredSpeedRight = -60.0f;
+    
+     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); 
+      }
+      
+    if (Green>Red){
+            for (int pos = 2300; pos > 440; pos -= 25) {      //Arm heben
+          Arm.SetPosition(pos); 
+          wait_ms(8); 
+      } 
+      
+          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); 
+      } 
+    }
+    return 0;
+}
 
 int main()
-{
-
+{   
+    Greifer.Enable(1500,20000);
+    Arm.Enable (1500,20000);
+    Deckel.Enable(1500,20000);
     // Initialisieren der PWM Ausgaenge pwmLeft.period(0.00005f); // PWM Periode von 50 us
     pwmLeft.period(0.00005f); // Setzt die Periode auf 50 μs
     pwmRight.period(0.00005f);
@@ -212,8 +285,8 @@
 
             case VORSUCHEN: {
                 my_led = 1;
-                desiredSpeedLeft = 15.0f; //Drehung
-                desiredSpeedRight = 15.0f;
+                desiredSpeedLeft = 25.0f; //Drehung
+                desiredSpeedRight = 25.0f;
                 if(z<12) { //Schreibt die Sensorwerte in ein Array
                     sensorOben[z] = sensorZentimeter[0];
                     sensorLinks[z] = sensorZentimeter[5];
@@ -226,8 +299,8 @@
                 my_led = 1;
 
                 //for(int i = 0; i < 10; i++)
-                desiredSpeedLeft = 15.0f; //Drehung
-                desiredSpeedRight = 15.0f;
+                desiredSpeedLeft = 25.0f; //Drehung
+                desiredSpeedRight = 25.0f;
 
                 if(z < 100) {
                     sensorOben[z] = sensorZentimeter[0];
@@ -251,16 +324,17 @@
                 break;
             }
             case 3:
-
-                desiredSpeedLeft = -20.0f; //Stillstand
-                desiredSpeedRight = 15.0f;
-                wait(2.0f);
+                desiredSpeedLeft = -15.0f;
+                desiredSpeedRight = -15.0f;
+                wait(1.0f);
+                aufheben();
+                wait(1.0f);
                 status = VORSUCHEN;
                 break;
             case 4:
 
-                desiredSpeedLeft = 15.0f; //Stillstand
-                desiredSpeedRight = -20.0f;
+                desiredSpeedLeft = 11.75f; //Stillstand
+                desiredSpeedRight = -15.0f;
                 wait(2.0f);
                 status = VORSUCHEN;
                 break;