Diff: Sources/main.cpp

Revision:

12:c0bcb95885dd

Parent:

11:292bdbd85a9c

--- a/Sources/main.cpp	Sat May 06 13:33:23 2017 +0000
+++ b/Sources/main.cpp	Wed May 10 13:30:13 2017 +0000
@@ -33,10 +33,11 @@
 ServoArm servoArm(PA_6);
 
 //Greifer:
-Servo servoGreifer(PB_7);
+Servo servoGreifer(PC_7);
 
 //Farbsensor:
 AnalogIn FarbVoltage(A0);
+DigitalOut led(D2);
 
 Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer ); //Implement the Farbsensor into the Robot init function!!
 
@@ -62,7 +63,7 @@
 
     enum states { search = 0, forward, downward, down, upward, color, backwardDrop, readyDrop, backward };
 
-    int state = search;
+    int state = downward;
 
     while( 1 ) {
         
@@ -112,19 +113,23 @@
                 break;
             
             case color: {
+                led = 1;
                 int color = sam.FarbVoltage.read();
                 
-                if( color == -1){
+                if( color == -1 ){
                     //Do nothing
                 }
                 
+                
                 else if( color == 0 || color == GREEN ){
                     state = backwardDrop;
+                    led = 0;
                     timer = 0;
                 }
                 
                 else if( color == RED ){
                     state = readyDrop;
+                    led = 0;
                     timer = 0;
                 }
                 
@@ -152,7 +157,7 @@
                 
             case backward:
                 if( sam.Arm.collectToBack() ){
-                    state = search;
+                    state = downward;
                     timer = 0;
                 }