Diff: Sources/main.cpp

Revision:

6:ba26dd3251b3

Parent:

5:1aaf5de776ff

Child:

7:6fed0dcae9c1

--- a/Sources/main.cpp	Wed Apr 26 08:05:25 2017 +0000
+++ b/Sources/main.cpp	Wed Apr 26 14:09:08 2017 +0000
@@ -30,12 +30,12 @@
 DigitalIn       overtemperatur(PB_15);
 
 //Arm:
-//Servo arm(PC_7);
+Servo servoArm(PA_6);
 
 //Farbsensor:
 AnalogIn FarbVoltage(A0);
 
-Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS ); //Implement the Farbsensor into the Robot init function!!
+Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm); //Implement the Farbsensor into the Robot init function!!
 
 void initializeDistanceSensors(){
     for( int ii = 0; ii<9; ++ii) {
@@ -45,7 +45,7 @@
     }
 }
 
-/* */
+/* * /
 int main(){
     initializeDistanceSensors();        //Initialises IR-Sensors.
     int counter = 0;                    //Counts how many times the robot has turned, before driving
@@ -101,4 +101,35 @@
         wait(0.2);
     }
 }
-/* */
\ No newline at end of file
+/* */
+
+/* */
+int main(){
+    sam.stop();
+    int done = 0;           //1:= finished process; 0:= not finished
+    int fun = 0;            //just to test.
+    
+    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