File content as of revision 1:fc216448bb57:

#include "mbed.h"
#include "math.h"
#include "MODSERIAL.h"

DigitalIn button(SW2);
DigitalOut directionpin1(D7);
DigitalOut directionpin2(D8);
MODSERIAL pc(USBTX, USBRX);

volatile bool emg0;
volatile bool emg1;
volatile bool emg2;
volatile bool y_direction;

volatile double x_position = 0;
volatile double y_position = 0;
volatile double motor1_angle;
volatile double motor2_angle;
volatile double direction;

const float length = 0.300; //length in m (placeholder)
const int C1 = 3; //motor 1 gear ratio (placeholder)
const int C2 = 3; //motor 2 gear ratio
    
void xDirection(double &motor1_angle, double &motor2_angle) {
        //direction of the motion
        if (emg0 && !emg1) {
            directionpin1 = true;
            directionpin2 = true;
            direction = 1;
        }
        else if (!emg0 && emg1) {
            directionpin1 = false;
            directionpin2 = false;
            direction = -1;
        }

    if (emg0 || emg1){
        //calculating the motion
        x_position = x_position + direction;
        motor1_angle = pow(sin( x_position ), -1) / length;
        y_position = y_position + direction;
        motor2_angle = pow(cos( x_position ), -1) / length;
    }
}

void yDirection (int &motor2_angle) {      
    if (emg2) {
        //calculating the motion
        y_position++;
        motor2_angle = C2 * y_position;
        
        //direction of the motion
        if (y_direction) {
            directionpin2 = true;
        }
        else if (!y_direction) {
            directionpin2 = false;
        }
    }
}

int main() {
    pc.printf(" ** program reset **\n\r");
    while (true) {
        xDirection(motor1_angle, motor2_angle);
        yDirection(motor2_angle);
        
        pc.printf("motor1 angle: %i\n\r", motor1_angle);
        pc.printf("motor2 angle: %i\n\r\n", motor2_angle);
        
        wait(0.5f); //can also be done with ticker, to be sure that it happens exactly every 0.5 seconds
    }
}