main.cpp

00001 #include "mbed.h"
00002 #include "IRSensor.h"
00003 #include "MotorEncoder.h"
00004 #include "LowpassFilter.h"
00005 #include "Servo.h"
00006 
00007 
00008 Servo Arm(PA_10);
00009 Servo Deckel(PB_8);
00010 AnalogIn Red (PB_0);               //Farbauswertung Rot 
00011 AnalogIn Green (PA_4);             //Farbauswertung Grün
00012 
00013 
00014 void aufheben (){
00015      
00016         for (int pos = 800; pos < 1400; pos += 25) {  //Greifer öffnen
00017           Greifer.SetPosition(pos);  
00018           wait_ms(7);
00019       }
00020       
00021       for (int pos = 440; pos < 2300; pos += 25) {      //Arm Runter 
00022          Arm.SetPosition(pos);  
00023           wait_ms(7);
00024    }  
00025         desiredSpeedLeft = 18.0f; 
00026         desiredSpeedRight = -15.0f;
00027     wait_ms(400);
00028        for (int pos = 1400; pos > 800; pos -= 25) {      //Greifer Schliessen
00029           Greifer.SetPosition(pos); 
00030           wait_ms(80); 
00031       }
00032       wait_ms(700);
00033     desiredSpeedLeft =65.0f; //50 RPM
00034     desiredSpeedRight = -60f;
00035     
00036  if (Green>Red){
00037             for (int pos = 2300; pos > 440; pos -= 25) {      //Arm heben
00038           Arm.SetPosition(pos); 
00039           wait_ms(5); 
00040       } 
00041       
00042           for (int pos = 800; pos < 1400; pos += 25) {  //Greifer öffnen
00043           Greifer.SetPosition(pos);  
00044           wait_ms(2);
00045       } 
00046       
00047     }
00048     else if(Red>Green) {
00049        
00050        for (int pos = 1300; pos < 2250; pos += 25) {    //Deckel schliessen
00051           Deckel.SetPosition(pos); 
00052           wait_ms(2); 
00053       } 
00054     
00055       for (int pos = 2300; pos > 440; pos -= 25) {      //Arm heben
00056           Arm.SetPosition(pos); 
00057           wait_ms(7); 
00058       } 
00059       wait_ms(50);
00060           for (int pos = 800; pos < 1400; pos += 25) {  //Greifer öffnen
00061           Greifer.SetPosition(pos);  
00062           wait_ms(2);
00063       }
00064        for (int pos = 440; pos < 700; pos += 25) {      //Arm Runter 
00065          Arm.SetPosition(pos);  
00066           wait_ms(5);
00067       }
00068        for (int pos = 2250; pos > 1300; pos -= 25) {     //Deckel öffnen
00069           Deckel.SetPosition(pos); 
00070           wait_ms(5); 
00071       } 
00072        for (int pos = 700; pos > 440; pos -= 25) {      //Arm heben
00073           Arm.SetPosition(pos); 
00074           wait_ms(5); 
00075       } 
00076       }
00077       enableSensor=1;
00078    }