File content as of revision 0:8c4c4aac180b:

#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 ()
    }
}