Revision 17:4e1be70bdedb, committed 2017-05-18

Comitter:

beacon

Date:

Thu May 18 14:27:09 2017 +0000

Parent:

15:915f8839fe48

Commit message:

Si compila.

Changed in this revision

--- a/Headers/Declarations.h	Thu May 11 18:57:45 2017 +0000
+++ b/Headers/Declarations.h	Thu May 18 14:27:09 2017 +0000
@@ -2,8 +2,8 @@
 #define DECLARATIONS_H
 
 //DistanceSensors:
-#define NEAR 0.18f          //Used for distance Sensors. If they're to near to a wall -> turn
-#define NEAR_LEGO 0.20f     //If the DistanceSensors are near to a Lego...
+#define NEAR 0.16f          //Used for distance Sensors. If they're to near to a wall -> turn
+#define NEAR_LEGO 0.12f     //If the DistanceSensors are near to a Lego...
 
 #define LEFT_L 5            //Arrayindex of the left LEGO Sensor & left LED
 #define FWD_L 0             //Arrayindex of the front LEGO Sensor & front LED

--- a/Headers/Robot.h	Thu May 11 18:57:45 2017 +0000
+++ b/Headers/Robot.h	Thu May 18 14:27:09 2017 +0000
@@ -5,6 +5,7 @@
 #include <mbed.h>
 #include "Servo.h"
 #include "ServoArm.h"
+#include <Ultraschall.h>
 
 #include "mbed.h"
 
@@ -52,7 +53,7 @@
     
     private:
     
-        AnalogIn*   FarbVoltage;
+        AnalogIn*       FarbVoltage;
 };
 
 class Arm{
@@ -84,8 +85,8 @@
         
         void init(Servo* servoLeiste);
         
-        int UpToDown();
-        int DownToUp();
+        int upToDown();
+        int downToUp();
         
         
     private:
@@ -112,13 +113,27 @@
 
 };
 
+class USsensor
+{
+    public:
+    
+        USsensor();
+        USsensor(Ultraschall* Usensor);
+        void init(Ultraschall* Usensor);
+        float read();
+        operator float();
+    
+    private:
+        Ultraschall* Usensor;
+};
+
 class Robot
 {
     
     public:
         
         //Robot related:
-        Robot(PwmOut* left, PwmOut* right, DigitalOut* powerSignal, DigitalOut* leds, AnalogIn* FarbVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, ServoArm* Arm, Servo* Greifer, Servo* Leiste );
+        Robot(PwmOut* left, PwmOut* right, DigitalOut* powerSignal, DigitalOut* leds, AnalogIn* FarbVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, ServoArm* Arm, Servo* Greifer, Servo* Leiste, Ultraschall* USsensor );
         
         //Drive Functions
         void drive();
@@ -132,6 +147,8 @@
         void stop();
         
         //Functions that use the drive functions
+        void driveSlowly();
+        void driveBackSlowly();/*
         void wallRight(int* counter, int* timer, int* lastAct);
         void wallLeft(int* counter, int* timer, int* lastAct);
         void wallFront(int* counter, int* timer, int* lastAct);
@@ -142,13 +159,15 @@
         void legoLeft(int* counter, int* timer, int* lastAct, int* legoFound);
         
         void nothingFound(int* counter, int* timer, int* lastAct);
-        
+        */
         int search(int* timer);
         
+        float see(int sensor);
         int getErrorMotor();
         
         //DistanceSensors related:
         DistanceSensors sensors[9];
+        USsensor USsensor;
         AnalogIn* frontS;
         AnalogIn* leftS;
         AnalogIn* rightS;
@@ -162,6 +181,7 @@
         
         //Farbsensors related:
         Farbsensor      FarbVoltage;
+        DigitalOut*     led;
         
         //Arm related:
         Arm             Arm;

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Headers/Ultraschall.h	Thu May 18 14:27:09 2017 +0000
@@ -0,0 +1,47 @@
+#ifndef ULTRASCHALL_H
+#define ULTRASCHALL_H
+ 
+#include "mbed.h"
+ 
+/** Ultraschall Class(es)
+ */
+ 
+class Ultraschall
+{
+public:
+    /** Create a Ultraschall object connected to the specified pin
+    * @param pin i/o pin to connect to
+    */
+    Ultraschall();
+    Ultraschall(PinName TrigPin,PinName EchoPin);
+    ~Ultraschall();
+ 
+    /** Return the distance from obstacle in cm
+    * @param distance in cms and returns -1, in case of failure
+    */ 
+    unsigned int get_dist_cm(void);
+    /** Return the pulse duration equal to sonic waves travelling to obstacle and back to receiver.
+    * @param pulse duration in microseconds.
+    */
+    unsigned int get_pulse_us(void);
+    /** Generates the trigger pulse of 10us on the trigger PIN.
+    */
+    void start(void);
+    void isr_rise(void);
+    void isr_fall(void);
+    void fall (void (*fptr)(void));
+    void rise (void (*fptr)(void));
+ 
+ 
+ 
+private:
+ 
+    Timer pulsetime;
+    DigitalOut  trigger;
+    InterruptIn echo;
+    unsigned int pulsedur;
+    unsigned int distance;
+};
+ 
+#endif
+        
\ No newline at end of file

--- a/Sources/Arm.cpp	Thu May 11 18:57:45 2017 +0000
+++ b/Sources/Arm.cpp	Thu May 18 14:27:09 2017 +0000
@@ -13,7 +13,7 @@
     this->arm = arm;
     
     this->arm->calibrate(0.0015f, 180.0f);
-    this->arm->position(COLLECT_POS);
+    this->arm->position(RELEASE_POS);
 }
 
 int Arm::collectToDown(){

--- a/Sources/DistanceSensors.cpp	Thu May 11 18:57:45 2017 +0000
+++ b/Sources/DistanceSensors.cpp	Thu May 18 14:27:09 2017 +0000
@@ -38,7 +38,7 @@
 {
     
     float Usensor;
-  
+
     if (number < 6){
         *bit0 = number & 1; // Set the first bit of the Sensors MUX
         *bit1 = number & 2; // Set the second bit of the Sensors MUX
@@ -61,8 +61,15 @@
     }
     
     //Usensor=sensorVoltage->read(); //Read the Voltage from the selected distance sensor
-    float Distance=-0.38f*sqrt(Usensor)+0.38f;
+    Usensor *= 3.3f;
+    float Distance= 5.906*Usensor*Usensor - 30.831*Usensor + 47.628;
+    Distance /= 100;
+    
+    static float distance_filtered = 0.0f;
+    distance_filtered = 0.55f * distance_filtered + 0.45f * Distance;
+    
     return Distance;
+    return distance_filtered;
 }
 
 DistanceSensors::operator float()

--- a/Sources/Greifer.cpp	Thu May 11 18:57:45 2017 +0000
+++ b/Sources/Greifer.cpp	Thu May 18 14:27:09 2017 +0000
@@ -19,14 +19,14 @@
 void Greifer::init(Servo* greifer)
 {
     this->greifer = greifer;
-    greifer->calibrate(0.0017f, 180.0f);
-    greifer->position(60.0f);
+    greifer->calibrate(0.001f, 90.0f);
+    greifer->position(65.0f);
 }
 
 int Greifer::take()
 
 {
-    this->greifer->position(-155.0f);
+    this->greifer->position(-70.0f);
     static int time = 0;
     if( time < 10 ){
         time++;
@@ -40,7 +40,7 @@
 
 int Greifer::leave()
 {
-    this->greifer->position(60.0f);
+    this->greifer->position(65.0f);
     static int time = 0;
     if( time < 10 ){
         time++;

--- a/Sources/Leiste.cpp	Thu May 11 18:57:45 2017 +0000
+++ b/Sources/Leiste.cpp	Thu May 18 14:27:09 2017 +0000
@@ -15,7 +15,7 @@
     this->leiste->position(UP_POS);
 }
 
-int Leiste::UpToDown(){
+int Leiste::upToDown(){
     static float pos=UP_POS;
     if(pos>DOWN_POS) {
         pos-=3;
@@ -28,7 +28,7 @@
     }
 }
 
-int Leiste::DownToUp(){
+int Leiste::downToUp(){
     static float pos=DOWN_POS;
     if(pos<UP_POS) {
         pos+=3;

--- a/Sources/Robot.cpp	Thu May 11 18:57:45 2017 +0000
+++ b/Sources/Robot.cpp	Thu May 18 14:27:09 2017 +0000
@@ -3,7 +3,7 @@
 
 /* Work in progress -------------------------------------------- */
 
-Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* powerSignal, DigitalOut* leds, AnalogIn* FarbVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, ServoArm* Arm, Servo* Greifer, Servo* Leiste )
+Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* powerSignal, DigitalOut* leds, AnalogIn* FarbVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, ServoArm* Arm, Servo* Greifer, Servo* Leiste, Ultraschall* USsensor )
 {
     this->left =        left;
     this->right =       right;
@@ -23,6 +23,7 @@
     this->Arm =         Arm;
     this->Greifer =     Greifer;
     this->Leiste =      Leiste;
+    this->USsensor =    USsensor;
 
 }
 
@@ -97,7 +98,29 @@
     *right= 0.5f;
 }
 
+void Robot::driveSlowly(){
+    static int i = 0;
+    i++;
+    if( i % 2 ){
+        this->drive();
+    }
+    else{
+        this->stop();
+    }
+}
 
+void Robot::driveBackSlowly(){
+    static int i = 0;
+    i++;
+    if( i % 2 ){
+        this->driveB();
+    }
+    else{
+        this->stop();
+    }
+}
+
+/*
 //Functions that use the drive functions
 void Robot::counterMax(int* counter, int* timer, int* lastAct, int* rando){
     if (*lastAct != 0){                 //If this wasn't the last called action, reset the timer.
@@ -166,7 +189,7 @@
     }
     
     if (this->sensors[FWD_L] > 0.16f){
-        this->drive();
+        this->driveSlowly();
     }
     else{
         this->stop();
@@ -219,42 +242,50 @@
     }
     
     this->drive();
-}
+}*/
 
 
 int Robot::search(int* timer){
-    //return 1;
     enum states {neutral = 0, max, wallF, wallL, wallR, legoF, legoL, legoR };
     
     static int state = neutral;
     
-    static int counter = 0;     //counter is used blabla
+    static int counter =    0;      //counter is used blabla
+    
+    static int rando =      -1;
+    
+    static int lastAct =    0;
     
-    static int rando = -1;
+    //static int stay =       -1;     //Stay is used to remain in a certain state
+    /*
+    this->sensors[FWD_L] < NEAR ?       this->leds[4] = 1         : this->leds[4] = 0;
+    this->sensors[RIGHT_L] < NEAR ?     this->leds[RIGHT_L] = 1   : this->leds[RIGHT_L] = 0;
+    this->sensors[LEFT_L] < NEAR ?      this->leds[LEFT_L] = 1    : this->leds[LEFT_L] = 0;
+    */
     
-    //static int lastAct = 0;
     
+        //printf("\n\rcurrent robot state:  %d", state);
     switch( state ){
         case neutral:
             if( counter > MAX ){
                 state = max;
             }
-            else if( this->sensors[FWD] < NEAR ){
+            else if( this->see(FWD) < NEAR ){
                 state = wallF;
             }
-            else if( this->sensors[LEFT] < NEAR ){
+            else if( this->see(LEFT) < NEAR ){
                 state = wallL;
             }
-            else if( this->sensors[RIGHT] < NEAR ){
+            else if( this->see(RIGHT) < NEAR ){
                 state = wallR;
             }
-            else if( this->sensors[FWD_L] < NEAR ){
+            else if( this->see(FWD_L) < NEAR_LEGO + 0.015f ){
                 state = legoF;
             }
-            else if( this->sensors[LEFT_L] < NEAR ){
+            else if( this->see(LEFT_L) < NEAR_LEGO ){
                 state = legoL;
             }
-            else if( this->sensors[RIGHT_L] < NEAR ){
+            else if( this->see(RIGHT_L) < NEAR_LEGO ){
                 state = legoR;
             }
             else{
@@ -265,7 +296,7 @@
             
         case max: {
             int time = 0;
-            if( time < 15 && this->sensors[FWD] > NEAR ){
+            if( time < 15 && this->see(FWD) > NEAR ){
                 rando == -1 ? rando = rand() % 2 : rando = rando;
                 this->turnAround(rando);
             }
@@ -279,7 +310,7 @@
         
         case wallF:
             counter++;
-            if( this->sensors[FWD] < NEAR ){
+            if( this->see(FWD) < NEAR ){
                 rando == -1 ? rando = rand() % 2 : rando = rando;
                 this->turnAround(rando);
             }
@@ -291,7 +322,7 @@
         
         case wallL:
             counter++;
-            if( this->sensors[LEFT] < NEAR ){
+            if( this->see(LEFT) < NEAR ){
                 this->turnRight();
             }
             else{
@@ -301,7 +332,7 @@
             
         case wallR:
             counter++;
-            if( this->sensors[RIGHT] < NEAR ){
+            if( this->see(RIGHT) < NEAR ){
                 this->turnLeft();
             }
             else{
@@ -311,11 +342,16 @@
         
         case legoF:
             //counter++;
-            if( this->sensors[FWD_L] > 0.17f ){
-                this->drive();
+            if( this->see(FWD) < NEAR ){
+                state = wallF;
+                //stay = -1;
+            }
+            else if( this->see(FWD_L) < 0.19f ){
+                this->driveBackSlowly();
             }
             else{
                 state = neutral;
+                //stay = -1;
                 counter = 0;
                 this->stop();
                 return 1;
@@ -324,21 +360,25 @@
         
         case legoL:
             counter++;
-            if( this->sensors[FWD_L] < NEAR_LEGO + 0.05f ){
+            if( this->see(FWD_L) > NEAR_LEGO + 0.05f ){
                 this->turnLeft();
             }
             else{
-                state = legoF;
+                state = neutral;
+                this->drive();
+                //stay = 1;
             }
             break;
         
         case legoR:
             counter++;
-            if( this->sensors[FWD_L] < NEAR_LEGO + 0.05f ){
+            if( this->see(FWD_L) > NEAR_LEGO + 0.05f ){
                 this->turnRight();
             }
             else{
-                state = legoF;
+                this->drive();
+                state = neutral;
+                //stay = 1;
             }
             break;
         
@@ -346,56 +386,15 @@
     return 0;
 }
 
-int Robot::getErrorMotor(){
-    return 0; //errorMotor;
+float Robot::see(int sensor){
+    if( sensor == FWD_L ){
+        return this->USsensor.read();
+    }
+    else{
+        return this->sensors[sensor].read();
+    }
 }
 
-/*int Robot::searchh(int* timer){
-    enum states {neutral = 0, max, wallF, wallL, wallR, legoF, legoL, legoR };
-    
-    static int state = neutral;
-    
-    static int counter = 0;     //counter is used blabla
-    
-    //static int lastAct = 0;
-    
-    switch( state ){
-        case neutral:
-            if( counter > MAX ){
-                state = max;
-            }
-            else if( this->sensora[FWD] < NEAR ){
-                state = wallF;
-                counter = 0;
-            }
-            else if( this->sensors[LEFT] < NEAR ){
-                state = wallL;
-                counter = 0;
-            }
-            else if( this->sensors[RIGHT] < NEAR ){
-                state = wallR;
-                counter = 0;
-            }
-            else if( this->sensors[FWD_L] < NEAR ){
-                state = legoF;
-                counter = 0;
-            }
-            else if( this->sensors[LEFT_L] < NEAR ){
-                state = legoL;
-                counter = 0;
-            }
-            else if( this->sensors[RIGHT_L] < NEAR ){
-                state = legoR;
-                counter = 0;
-            }
-            break;
-            
-        case max:
-            
-            break;
-  */          
-
-//reset
-//}
-
-//Add management for Overpower!! Pin PB_15
\ No newline at end of file
+int Robot::getErrorMotor(){
+    return 0; //errorMotor;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sources/USsensor.cpp	Thu May 18 14:27:09 2017 +0000
@@ -0,0 +1,37 @@
+#include <Robot.h>
+
+USsensor::USsensor()
+{
+}
+
+USsensor::USsensor(Ultraschall* Usensor)
+{
+    init(Usensor);
+}
+void USsensor::init(Ultraschall* Usensor)
+{
+   this->Usensor = Usensor;   
+}
+
+float USsensor::read()
+{
+    static int i = 0;
+    Usensor->start();
+    if( i ){
+        float dist = Usensor->get_dist_cm();
+        static float distFiltered = dist;
+        
+        distFiltered = 0.05f * distFiltered + 0.95f * dist;
+        return distFiltered / 100;
+        //return dist/100.0f;
+    }
+    else{
+        i++;
+        return 0.25f;
+    }
+}
+
+ USsensor::operator float() 
+{
+    return read();    
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sources/Ultraschall.cpp	Thu May 18 14:27:09 2017 +0000
@@ -0,0 +1,53 @@
+#include "Ultraschall.h"
+ 
+ 
+Ultraschall::Ultraschall(PinName TrigPin,PinName EchoPin):
+    trigger(TrigPin), echo(EchoPin)
+{
+    pulsetime.stop();
+    pulsetime.reset();
+    echo.rise(this,&Ultraschall::isr_rise);
+    echo.fall(this,&Ultraschall::isr_fall);
+    trigger=0;
+}
+ 
+Ultraschall::~Ultraschall()
+{
+}
+ 
+void Ultraschall::isr_rise(void)
+{
+    pulsetime.start();
+}
+void Ultraschall::start(void)
+{
+    trigger=1;
+    wait_us(10);
+    trigger=0;
+}
+ 
+void Ultraschall::isr_fall(void)
+{
+    pulsetime.stop();
+    pulsedur = pulsetime.read_us();
+    distance= (pulsedur*343)/20000;
+    pulsetime.reset();
+}
+ 
+void Ultraschall::rise (void (*fptr)(void))
+{
+    echo.rise(fptr);
+}
+void Ultraschall::fall (void (*fptr)(void))
+{
+    echo.fall(fptr);
+}
+ 
+unsigned int Ultraschall::get_dist_cm()
+{
+    return distance;
+}
+unsigned int Ultraschall::get_pulse_us()
+{
+    return pulsedur;
+}
\ No newline at end of file

--- a/Sources/main.cpp	Thu May 11 18:57:45 2017 +0000
+++ b/Sources/main.cpp	Thu May 18 14:27:09 2017 +0000
@@ -1,6 +1,7 @@
 #include "mbed.h"
 #include "Robot.h"
 #include "Declarations.h"
+#include "Ultraschall.h"
 
 #include <cstdlib>
 
@@ -18,6 +19,10 @@
 AnalogIn leftS(A2);
 AnalogIn rightS(A3);
 
+//Ultraschall
+Ultraschall usensor(D6,D5);
+
+
 //Leds related:
 DigitalOut leds[6] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };
 
@@ -38,11 +43,14 @@
 //Leiste:
 Servo servoLeiste(PB_6);
 
+//Button:
+DigitalIn mybutton(USER_BUTTON);
+
 //Farbsensor:
 AnalogIn FarbVoltage(A0);
 DigitalOut led(D2);
 
-Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer, &servoLeiste ); //Implement the Farbsensor into the Robot init function!!
+Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer, &servoLeiste, &usensor ); //Implement the Farbsensor into the Robot init function!!
 
 void initializeDistanceSensors()
 {
@@ -63,41 +71,101 @@
     //int lastFun = -1;                   //Is used, to check if the same action in Robot::search() was made multiple times.
     //int found = 0;                      //0:= no block available, 1 := a lego is ready to be picked up
     //int done = 0;
-
-    enum states { search = 0, LeisteDown, forward, downward, down, upward, color, backwardDrop, readyDrop, backward, LeisteUp };
+    int color;
+    
+    enum states { search = 0, LeisteDown, turn, push, backOff, forward, downward, down, upward, colorS, readyDrop, backward, LeisteUp };
 
     int state = search;
-    wait(5);
+    
+                static float messung = 0;
+                
+    while( 1 ){
+        printf("\n\r%f", sam.see(FWD_L));
+        
+        wait(1.0f);
+            
+    }
+    
     while( 1 ) {
         
         if ( timer > TIMEOUT ) {
             NVIC_SystemReset();         //Resets Sam.
         }
         
+        //if( timer == 0 )
+        //printf("\n\rLEFT: %.3f,\tFWD: %.3f,\tRIGHT: %.3f", sam.sensors[LEFT].read(), sam.sensors[FWD].read(), sam.sensors[RIGHT].read());
+        
+        
+        //printf("\n\rcurrent main state: %d", state);
+        
+        sam.sensors[FWD_L].read() < NEAR ? sam.leds[1] = 1 : sam.leds[1] = 0;
         switch( state ) {
             case search:
-                if( 1 || sam.search(&timer) ){
-                  state = LeisteDown;
-                  timer = 0;
+                if( sam.search(&timer) ){
+                    //sam.Greifer.nullPos();
+                    state = LeisteDown;
+                    timer = 0;
                 }
-                
                 break;
        
             case LeisteDown:
-                if( sam.Leiste.UpToDown() ){
-                  sam.Greifer.leave();
-                  state = forward;
-                  timer = 0;
+                int count = 0;
+                if( sam.Leiste.upToDown() ){
+                    //sam.Greifer.leave();
+                    state = turn;
+                    timer = 0;
+                }
+                break;
+            
+            case turn:
+                static int i = 0;
+                if( i > 7 ){
+                    sam.stop();
+                    state = push;
+                    i = 0;
+                }
+                else{
+                    i++;
+                    sam.turnRight();
                 }
+                break;
                 
+            case push:{
+                static int i = 0;
+                if( i > 5 ){
+                    sam.stop();
+                    i = 0;
+                    state = backOff;
+                    timer = 0;
+                }
+                else{
+                    sam.driveSlowly();
+                    i++;
+                }
                 break;
+            }
+            
+            case backOff:{
+                static int i = 0;
+                if( i > 1 ){
+                    sam.stop();
+                    i = 0;
+                    state = forward;
+                    timer = 0;
+                }
+                else{
+                    sam.driveBackSlowly();
+                    i++;
+                }
+                break;
+            }
+                
             
             case forward:
                 if( sam.Arm.backToCollect() ){
                     state = downward;
                     timer = 0;
                 }
-                
                 break;
 
             case downward:
@@ -105,7 +173,6 @@
                     state = down;
                     timer = 0;
                 }
-                
                 break;
 
             case down:
@@ -113,20 +180,18 @@
                     state = upward;
                     timer = 0;
                 }
-                
                 break;
             
             case upward:
                 if( sam.Arm.downToCollect() ){
-                    state = color;
+                    state = colorS;
                     timer = 0;
                 }
-                
                 break;
             
-            case color: {
+            case colorS:
                 led = 1;
-                int color = sam.FarbVoltage.read();
+                color = sam.FarbVoltage.read();
                 
                 if( color == -1 ){
                     //Do nothing
@@ -134,7 +199,7 @@
                 
                 
                 else if( color == 0 || color == GREEN ){
-                    state = backwardDrop;
+                    state = backward;
                     led = 0;
                     timer = 0;
                 }
@@ -149,19 +214,15 @@
                     //Shit...
                 }
                 break;
-            }
-            
-            case backwardDrop:
-                if( sam.Arm.collectToBack() ){
-                    state = readyDrop;
-                    timer = 0;
-                }
-                
-                break;
-                
+             
             case readyDrop:
                 if( sam.Greifer.leave() ){
-                    state = backward;
+                    if( color == GREEN || color == 0 ){
+                        state = LeisteUp;
+                    }
+                    else{
+                        state = backward;
+                    }
                     timer = 0;
                 }
                 
@@ -169,124 +230,29 @@
                 
             case backward:
                 if( sam.Arm.collectToBack() ){
-                    sam.Greifer.nullPos();
+                    //sam.Greifer.nullPos();
                     state = LeisteUp;
                     timer = 0;
+                    if( color == GREEN || color == 0 ){
+                        state = readyDrop;
+                        sam.Greifer.leave();
+                    }
+                    else{
+                        state = LeisteUp;
+                    }
                 }
-                
                 break;
                 
             case LeisteUp:
-                if( sam.Leiste.DownToUp() ){
+                if( sam.Leiste.downToUp() ){
                     state = search;
                     timer = 0;
-                    wait(2);
                 }
                 break;
-        }
-        
+        } 
         timer++;
         wait(0.1f);
     }
     
     return 0;
-}
-        
-        
-        
-        /*
-        if (sam.search(&counter, &timer)) {
-
-            while(!done) {
-                sam.Arm.collectToDown(&done);
-                wait(0.1f);
-            }
-            done = 0;
-
-            sam.Greifer.take();
-
-            while(!done) {
-                sam.Arm.downToCollect(&done);
-                sam.leds[1] = 1;
-                wait(0.1f);
-            }
-            done = 0;
-
-            sam.Greifer.leave();
-
-            found = 0;
-        }
-        
-
-        wait( 0.1f );
-    }
-    return 0;
-}
-*/
-
-/* * /
-int main(){
-
-    initializeDistanceSensors();
-    sam.stop();
-
-    while ( 1 ){
-        for (int i=0; i<6; i++){
-            sam.sensors[i] < NEAR ? sam.leds[i] = 1 : sam.leds[i] = 0;
-        }
-
-        sam.turnLeftS();
-
-        while (sam.sensors[FWD] < NEAR){
-            wait(0.05f);
-            sam.stop();
-        }
-
-    }
-    return 0;
-}
-/* */
-
-/* * /
-int main(){
-    for(float p=0; p<1.0; p += 0.1) {
-      //  arm = p;
-        wait(0.2);
-    }
-}
-/* */
-
-/* * /
-int main(){
-    sam.stop();
-    int done = 0;           //1:= finished process; 0:= not finished
-    int fun = 0;            //just to test.
-    int start = 0;
-
-
-    while(1){
-        if(fun == 0){
-            done = 0;
-            sam.Arm.collecttoback(&done);
-            done == 0 ? fun = 0 : fun = 1;
-        }
-        else if(fun == 1){
-            done = 0;
-            sam.Arm.backtocollect(&done);
-            done == 0 ? fun = 1 : fun = 2;
-        }
-        else if(fun == 2){
-            done = 0;
-            sam.Arm.collecttodown(&done);
-            done == 0 ? fun = 2 : fun = 3;
-        }
-        else if(fun == 3){
-            done = 0;
-            sam.Arm.downtocollect(&done);
-            done == 0 ? fun = 3 : fun = 0;
-        }
-        wait(0.1);
-    }
-}
-
-*/
\ No newline at end of file
+}
\ No newline at end of file