Diff: main.cpp

Revision:

3:05d62b46b32c

Parent:

2:d078dc23ebaa

Child:

4:c8461418ac92

--- a/main.cpp	Tue Apr 22 19:52:44 2014 +0000
+++ b/main.cpp	Wed Apr 23 13:13:05 2014 +0000
@@ -11,13 +11,49 @@
 DigitalOut led3(LED3);
 DigitalOut led4(LED4);
 
+int state = 0000;
+int distance = 0; //distance to drive in cm
+
 //value declarations
 float dc_forward = 60;
 float dc_backward = 40;
 float dc_neutral = 50;
 
+//function declarations
+void showstate(void);
 
-void showstate(int state)
+
+
+void sm_drive()
+{
+    showstate();
+    switch(state)
+    {
+        case 0000:                  //standstill
+        
+        // motoren stil
+        l_wheel.write(dc_neutral); 
+        r_wheel.write(dc_neutral);
+        //state transitions
+        if(distance > 0)
+        state = 0001;
+        if(distance < 0)
+        state = 0010;
+        break;
+        
+        case 0001:                  //drive forward
+        //check time of clock
+        //set stopping
+        break;
+        
+        case 0010:                  //drive backward
+        break;
+    }
+}
+
+
+
+void showstate()
 {
     switch(state)
     {
@@ -66,57 +102,6 @@
     }
 }
 
-bool drive(int distance)
-{
-    // check stop of motor
-    while(l_wheel.read() == 50 && r_wheel.read() == 50)
-    {
-        showstate(0001);
-    }
-    showstate(0000);
-    
-    // start motor
-    if(distance > 0)
-    {
-        showstate(0010);
-        l_wheel.write(dc_forward);
-        r_wheel.write(dc_forward);
-    }else if(distance < 0){
-        showstate(0011);
-        l_wheel.write(dc_backward);
-        r_wheel.write(dc_backward);
-    }else{
-        showstate(0000);
-        l_wheel.write(dc_neutral);
-        r_wheel.write(dc_neutral);
-    }
-    // wait for distance
-    wait(second_for_distance);
-    
-    // stop motors
-    l_wheel.write(dc_neutral);
-    r_wheel.write(dc_neutral);
-    showstate(0000);
-    
-    // check stop of motor
-    while(l_wheel.read() == 50 && r_wheel.read() == 50)
-    {
-        showstate(0001);
-    }
-
-    return 0; 
-}
-
-bool turn(int rad)
-{
-    // check stop of motor
-    // start motor
-    // wait for distance
-    // stop motors
-    // check stop of motor
-    return 0;
-}
-
 int main()
 {
     l_wheel.period(0.00001);