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;
   }