Diff: main.cpp

Revision:

10:f5dfd33d5431

Parent:

9:a6bed03648e5

Child:

11:524d13eb7d5e

--- a/main.cpp	Fri Jan 05 02:29:59 2018 +0000
+++ b/main.cpp	Mon Jan 15 04:11:02 2018 +0000
@@ -1,12 +1,97 @@
 #include "Pokitto.h"
 
+#define STATE_IDLE    0
+#define STATE_RUNNING 1
+
 // the pokitto core library
 Pokitto::Core pokitto;
 
-// create a 'DigitalOut' object to represent
-// the digital output pin used to communicated with the LED
-// the object is set to use the EXT0 pin for output
-DigitalOut led0 = DigitalOut(EXT0);
+/* Map EXTn outputs to inputs on L298 Full Bridge Driver */
+DigitalOut ena = DigitalOut(EXT0);
+DigitalOut enb = DigitalOut(EXT1);
+DigitalOut in1 = DigitalOut(EXT2);
+DigitalOut in2 = DigitalOut(EXT3);
+DigitalOut in3 = DigitalOut(EXT4);
+DigitalOut in4 = DigitalOut(EXT4);
+
+int state;
+
+int script[] = {1, 1, 4, -1, -1, 4, 1, -1, 4, -1, 1, 4, 0, 0, 0};
+int step;
+int substep;
+
+void init(void)
+{
+    state = STATE_IDLE;
+    ena.write(0);
+    enb.write(0);
+    in1.write(0);
+    in2.write(0);
+    in3.write(0);
+    in4.write(0);
+}
+
+void set_motors(int a, int b)
+{
+    ena.write(1);
+    enb.write(1);
+    if (a == -1)
+    {
+        in1.write(0);
+        in2.write(1);
+    }
+    else if (a == 1)
+    {
+        in1.write(1);
+        in2.write(0);
+    }
+    else
+    {
+        in1.write(1);
+        in2.write(1);
+    }
+    if (b == -1)
+    {
+        in3.write(0);
+        in4.write(1);
+    }
+    else if (b == 1)
+    {
+        in3.write(1);
+        in4.write(0);
+    }
+    else
+    {
+        in3.write(1);
+        in4.write(1);
+    }
+}
+
+void run(void)
+{
+    if (substep == 0)
+    {
+        set_motors(script[step], script[step + 1]);
+        substep = script[step + 2];
+    }
+    substep--;
+    if (substep == 0)
+    {
+        step +=  3;
+        if (script[step] == 0)
+            state = STATE_IDLE;
+    }
+}
+
+void stop(void)
+{
+    ena.write(0);
+    enb.write(0);
+    in1.write(1);
+    in2.write(1);
+    in3.write(1);
+    in4.write(1);
+}
 
 int main ()
 {
@@ -19,27 +104,27 @@
         // update the Pokitto's state
         if (pokitto.update())
         {
-            // write 'Hello World!' to the screen
-                        // this is so you can tell the Pokitto is running
-            pokitto.display.color=led0+1;
-            pokitto.display.setCursor(0,0);
-            if (led0==0) pokitto.display.print("Hello World!");
-            else pokitto.display.print("Hei Etlari!");
-
-            // if the A button is pressed
-            if(pokitto.buttons.aBtn())
+            switch (state)
             {
-                // send a high signal to turn the LED on
-                led0.write(1);
-            }
-
-            // if the B button is pressed
-            if(pokitto.buttons.bBtn())
-            {
-                // send a low signal to turn the LED off
-                led0.write(0);
+            case STATE_IDLE:
+                pokitto.display.print("Idle");
+                stop();
+                if (pokitto.buttons.aBtn())
+                {
+                    state = STATE_RUNNING;
+                    step = 0;
+                    substep = 0;
+                }
+                break;
+            case STATE_RUNNING:
+                pokitto.display.print("Running");
+                run();
+                if (pokitto.buttons.aBtn())
+                    state = STATE_IDLE;
+                break;
             }
         }
+        wait_ms(100);
     }
 
     return 1;