Diff: include/EngineDriver.h

Revision:

2:09cc6b203fe4

Parent:

1:90d75436758b

Child:

4:cc391b3f3e57

--- a/include/EngineDriver.h	Mon Jun 22 15:47:43 2015 +0000
+++ b/include/EngineDriver.h	Tue Jun 23 08:32:15 2015 +0000
@@ -7,19 +7,51 @@
 DigitalOut in2(D5);
 DigitalOut in3(D6);
 DigitalOut in4(D10);
+int inState[4] = {0,0,0,0};
+void applyStates()
+{
+    in1 = inState[0];
+    in2 = inState[1];
+    in3 = inState[2];
+    in4 = inState[3];
+}
+void resetEngines()
+{
+    for(int i = 0; i < 4; ++i)
+    {
+        inState[i] = 0;
+    }
+    applyStates();
+}
+
 void testEngines()
 {
-        in1 = 1;
-        in2 = 0;
-        in3 = 1;
-        in4 = 0;
-        wait(1);
-        in1 = 0;
-        in2 = 1;
-        in3 = 0;
-        in4 = 1;
-        wait(1);
+    int i = 0;
+    while(i < 4)
+    {
+        if(i % 2 == 0)
+        {
+            inState[i] = 1;
+        }
+        i++;
+    }
+    applyStates();
+    wait(5);
+    resetEngines();
+    int j = 0;
+    while(j < 4)
+    {
+        if(j % 2 != 0)
+        {
+            inState[j] = 1;
+        }
+        j++;
+    }
+    applyStates();
+    wait(5);
+    resetEngines();
 }
+
 void enableAll()
 {
     en1 = 1;