Diff: main.cpp

Revision:

0:8c4c4aac180b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Oct 31 11:18:52 2017 +0000
@@ -0,0 +1,72 @@
+#include "mbed.h"
+
+//global variabelen
+
+DigitalOut gpo(D0);
+DigitalOut led(LED_RED);
+const int depth =20; 
+const int height = 20;
+int z, y, Goalz, Goaly, MemPrePos;
+dir = enum eDirection {STOP = 0, LEFT, RIGHT, UP, DOWN, SERVOAAN, SERVOUIT} ;
+
+void setup()
+{
+    bool outofreach = false;
+    dir = 0;
+    //z = startpositie van z. bijv. z = depth/2;
+    //y = startpositie van y.       y = height/2;
+    // Set Goalz en Goaly based on EMG signals --> maybe not in here
+    MemPrePos = 0; 
+} 
+
+void MovementSpace()
+{
+    // bounderies robot
+    }
+
+void Logic ()
+{
+    switch (dir)
+    {
+    case LEFT:
+    Goalz++;
+    case RIGHT:
+    Goalz--;
+    case UP:
+    Goaly++;
+    case DOWN:
+    Goaly--;
+    case SERVOAAN:
+    //;
+    case SERVOUIT:
+    //;
+    default:
+    break;
+    }
+    
+    // RKI: q1(t + T) = q1(t) + 
+    //     (sin(q1)L1+sin(q2)L2)(Rdoely - cos(q1)L1-cos(q2)L2)K -
+    //     (cos(q1)L1 +cos(q2)L2)K(T/B1)(Rdoelx - sin(q1)L1 - sin(q2)L2)
+    //
+    //      q2(t + T) = q2(t) +
+    //      (2*sin(q1)L1 + sin(q2)L2)(Rdoely - cos(q1)L1-cos(q2)L2)K -
+    //      cos(q2)L2(Rdoelx - sin(q1)L1-sin(q2)L2)K(T/B2)
+    //
+    // lengte van de armen = L1 en L2
+    // q zijn de hoeken van de joint dus de hoeken van de motor
+    
+    if (y of z = bounderies robot)
+    {
+    outofreach=true;
+    }
+        
+}
+
+int main()
+{
+    setup ()
+    while (!outofreach) 
+    {
+         ticker.emg ()
+    }
+}
\ No newline at end of file