File content as of revision 10:8b0d8d4016a1:

#include "mbed.h"
#include "math.h"
#include "MODSERIAL.h"
#define PI 3.141592f //65358979323846  // pi 

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

Ticker ticker;

volatile bool emg0Bool = false;
volatile bool emg1Bool = false;
volatile bool emg2Bool = false;
volatile bool x_direction = true;
volatile bool a;

const float C1 = 3.0; //motor 1 gear ratio
const float C2 = 0.013; //motor 2 gear ratio/radius of the circular gear in m
const float length = 0.300; //length in m (placeholder)

volatile float x_position = length;
volatile float y_position = 0.0;
volatile float old_y_position;
volatile float old_x_position;
volatile float old_motor1_angle;
volatile float old_motor2_angle;
volatile float motor1_angle = 0.0; //sawtooth gear motor
volatile float motor2_angle = 0.0; //rotational gear motor
volatile float direction;
volatile char c;
      
void yDirection() {
    //direction of the motion
    if (emg0Bool && !emg1Bool) { //if a is pressed and not d, move to the left
        directionpin1 = true;
        directionpin2 = true;
        direction = -1.0;
    }
    else if (!emg0Bool && emg1Bool) { //if d is pressed and not a, move to the right
        directionpin1 = false;
        directionpin2 = false;
        direction = 1.0;
    }

    if (emg0Bool || emg1Bool){
        //calculating the motion
        old_y_position = y_position;
        y_position = old_y_position + (0.1f * direction);
        old_motor2_angle = motor2_angle;
        motor2_angle = asin( y_position / length );     //saw tooth motor angle in rad
        
        //correction from motor 1 to keep x position the same
        old_x_position = x_position;
        x_position = old_x_position + cos( motor2_angle ) - cos( old_motor2_angle );
        old_motor1_angle = motor1_angle;
        motor1_angle = old_motor1_angle + ( x_position - old_x_position) / C1;     //rotational-gear motor angle in rad
    }
    
    //reset the booleans
    emg0Bool = false;
    emg1Bool = false;
}

void xDirection () { 
    //if the button is pressed, reverse the y direction
    if (!button) {
        x_direction = !x_direction;
        wait(0.5f);
        }     
        
    if (emg2Bool) { //if w is pressed, move up/down
        //direction of the motion
        if (x_direction) { 
            directionpin2 = true;
            direction = 1.0;
        }
        else if (!x_direction) {
            directionpin2 = false;
            direction = -1.0;
        }
        
        //calculating the motion
        old_x_position = x_position;
        x_position = old_x_position + (0.1f * direction);
        old_motor1_angle = motor1_angle;
        motor1_angle = old_motor1_angle + ( x_position - length ) / C2; // sawtooth-gear motor angle in degrees
        
        //reset the boolean, for demo purposes
        emg2Bool = false; 
    }
}

int main() {
    pc.baud(115200);
    pc.printf(" ** program reset **\n\r");
    
    while (true) {
        
        //testing the code from keyboard imputs: a-d: left to right, w: forward/backwards
        a = pc.readable();
        if (a) {
            c = pc.getc();
            switch (c){
                case 'a': //move to the left
                    emg0Bool = true; 
                    break;           
                case 'd': //move to the right
                    emg1Bool = true;
                    break;            
                case 'w': //move up/down
                    emg2Bool = true;
                    break;
            }
        }
        yDirection(); //call the function to move in the y direction
        xDirection(); //call the function to move in the x direction
        
        // print the motor angles and coordinates
        pc.printf("position: (%f, %f)\n\r", x_position, y_position);
        pc.printf("motor1 angle: %f degrees\n\r", motor1_angle *180.0f /PI);
        pc.printf("motor2 angle: %f degrees\n\r\n", motor2_angle *180.0f /PI);

        wait(0.5f); //can also be done with ticker, to be sure that it happens exactly every 0.5 seconds
    }
}