Diff: Sources/main.cpp

Revision:

11:292bdbd85a9c

Parent:

10:f76476943a6c

Child:

12:c0bcb95885dd

--- a/Sources/main.cpp	Wed May 03 13:54:51 2017 +0000
+++ b/Sources/main.cpp	Sat May 06 13:33:23 2017 +0000
@@ -33,67 +33,189 @@
 ServoArm servoArm(PA_6);
 
 //Greifer:
-Servo servoGreifer(PC_7);
+Servo servoGreifer(PB_7);
 
 //Farbsensor:
 AnalogIn FarbVoltage(A0);
 
 Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer ); //Implement the Farbsensor into the Robot init function!!
 
-void initializeDistanceSensors(){
+void initializeDistanceSensors()
+{
     for( int ii = 0; ii<9; ++ii) {
         sam.sensors[ii].init(&sensorVoltage, &frontS, &leftS, &rightS, &bit0, &bit1, &bit2, ii);
 
-    enable = 1;
+        enable = 1;
     }
 }
 
 
 /* */
-int main(){
+int main()
+{
     initializeDistanceSensors();        //Initialises IR-Sensors.
-    int counter = 0;                    //Counts how many times the robot has turned, before driving
+    //int counter = 0;                    //Counts how many times the robot has turned, before driving
     int timer = 0;                      //Is used, to reset the robot. Returns time in [0.1s]
     //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
-    
-    while( 1 ){
+    //int found = 0;                      //0:= no block available, 1 := a lego is ready to be picked up
+    //int done = 0;
+
+    enum states { search = 0, forward, downward, down, upward, color, backwardDrop, readyDrop, backward };
+
+    int state = search;
+
+    while( 1 ) {
         
-        if ( timer > TIMEOUT ){
+        if ( timer > TIMEOUT ) {
             NVIC_SystemReset();         //Resets Sam.
         }
-        else if (found == 0){
-            sam.search(&counter, &timer, &found);
-        }
-        else{
-            //pick up lego
-            //found = 0;
-            sam.stop(); //Nur zum Testen
+        
+        switch( state ) {
+            case search:
+                if( sam.search(&timer) ){
+                  state = forward;
+                  timer = 0;
+                }
+                
+                break;
+            
+            case forward:
+                if( sam.Arm.backToCollect() ){
+                    state = downward;
+                    timer = 0;
+                }
+                
+                break;
+
+            case downward:
+                if( sam.Arm.collectToDown() ){
+                    state = down;
+                    timer = 0;
+                }
+                
+                break;
+
+            case down:
+                if( sam.Greifer.take() ) {
+                    state = upward;
+                    timer = 0;
+                }
+                
+                break;
+            
+            case upward:
+                if( sam.Arm.downToCollect() ){
+                    state = color;
+                    timer = 0;
+                }
+                
+                break;
+            
+            case color: {
+                int color = sam.FarbVoltage.read();
+                
+                if( color == -1){
+                    //Do nothing
+                }
+                
+                else if( color == 0 || color == GREEN ){
+                    state = backwardDrop;
+                    timer = 0;
+                }
+                
+                else if( color == RED ){
+                    state = readyDrop;
+                    timer = 0;
+                }
+                
+                else{
+                    //Shit...
+                }
+                break;
+            }
+            
+            case backwardDrop:
+                if( sam.Arm.collectToBack() ){
+                    state = readyDrop;
+                    timer = 0;
+                }
+                
+                break;
+                
+            case readyDrop:
+                if( sam.Greifer.leave() ){
+                    state = backward;
+                    timer = 0;
+                }
+                
+                break;
+                
+            case backward:
+                if( sam.Arm.collectToBack() ){
+                    state = search;
+                    timer = 0;
+                }
+                
+                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;
 }
@@ -113,14 +235,14 @@
     sam.stop();
     int done = 0;           //1:= finished process; 0:= not finished
     int fun = 0;            //just to test.
-    int start = 0; 
-              
-   
+    int start = 0;
+
+
     while(1){
         if(fun == 0){
             done = 0;
             sam.Arm.collecttoback(&done);
-            done == 0 ? fun = 0 : fun = 1; 
+            done == 0 ? fun = 0 : fun = 1;
         }
         else if(fun == 1){
             done = 0;