File content as of revision 7:9e0ded88fe60:

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

DigitalIn button(SW2);
DigitalOut directionpin1(D7);
DigitalOut directionpin2(D8);
MODSERIAL pc(USBTX, USBRX);
HIDScope scope(2);
Encoder motor1(D13,D12);
Encoder motor2(D13,D12); //has to be changed!!!

//values of inverse kinematics
volatile bool emg0Bool = false;
volatile bool emg1Bool = false;
volatile bool emg2Bool = false;
volatile bool y_direction = true;
volatile bool a;

volatile float x_position = 0.0;
volatile float y_position = 0.0;
volatile float y_position2 = 0.0;
volatile float old_y;
volatile float old_y2;
volatile float old_x;
volatile float motor1_angle = 0.0; //circular gear motor
volatile float motor2_angle = 0.0; //sawtooth gear motor
volatile float direction;
volatile char c;

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

//values of PID controller
const float Kp = 2;
const float Ki = 0.2;
const float Kd = 0;
const float Timestep = 0.001;
float G ;               //desired motor angle
float Output1 = 0 ;      //Starting value
float Output2 = 0 ;      //Starting value
float P = 0;           //encoder value
float e1 = 0 ;          //Starting value 
float e2 = 0 ;          //Starting value
float e3;  
float f1 = 0 ;          //Starting value 
float f2 = 0 ;          //Starting value
float f3;     
float Output_Last1;      // Remember previous position
float Output_Last2;      // Remember previous position
float Y;                // Value that is outputted to motor control
float P_Last = 0;       // Starting position
const float Max_Speed = 400;      //Max speed of the motor
      
void xDirection() {
    //direction of the motion
    if (emg0Bool && !emg1Bool) { //if a is pressed and not d, move to the left
        directionpin1 = true;
        directionpin2 = true;
        direction = -1;
    }
    else if (!emg0Bool && emg1Bool) { //if d is pressed and not a, move to the right
        directionpin1 = false;
        directionpin2 = false;
        direction = 1;
    }

    if (emg0Bool || emg1Bool){
        //calculating the motion
        old_x = x_position;
        x_position = old_x + (0.1f * direction);
        motor1_angle = asin( x_position / (length * C1 ));     //rotational motor angle in rad
        
        old_y2 = y_position2;
        y_position2 = old_y2 + (0.1f * direction);
        motor2_angle = ( y_position / C2) + acos( x_position / length );     //sawtooth-gear motor angle in rad
    }
    
    //reset the booleans
    emg0Bool = false;
    emg1Bool = false;
}

void yDirection () {      
    if (emg2Bool) { //if w is pressed, move up/down
        //direction of the motion
        if (y_direction) { 
            directionpin2 = true;
            direction = 1;
        }
        else if (!y_direction) {
            directionpin2 = false;
            direction = -1;
        }
        
        //calculating the motion
        old_y = y_position;
        y_position = old_y + (0.1f * direction);
        motor2_angle = y_position / C2; // sawtooth-gear motor angle in rad
        
        //reset the boolean
        emg2Bool = false; 
    }
}

void PIDController1() {   
    P = motor1.getPosition() / 8400 * 360;        
    e1 = e2;
    e2 = e3;
    e3 = G - P;
    Output_Last1 = Output1;
    Output1 = Kp * (e3 - e2) + Output_Last1 +Ki * e3 + Kd * (e3 - 2*e2 + e1);
    Y = Output1;
     
    if (Output1 >= 1){
        Y = 1;
    }
    else if (Output1 <= -1){
        Y = -1;
    }    
    P = P_Last + Y * Timestep * Max_Speed;

    scope.set(0,Output1);
    scope.set(1,P);
    scope.send();
}

void PIDController2() {   
    P = motor2.getPosition() / 8400 * 360;        
    f1 = f2;
    f2 = f3;
    f3 = G - P;
    Output_Last2 = Output2;
    Output2 = Kp * (f3 - f2) + Output_Last2 +Ki * f3 + Kd * (f3 - 2*f2 + f1);
    Y = Output2;
     
    if (Output2 >= 1){
        Y = 1;
    }
    else if (Output2 <= -1){
        Y = -1;
    }    
    P = P_Last + Y * Timestep * Max_Speed;

    scope.set(0,Output2);
    scope.set(1,P);
    scope.send();
}

int main() {
    pc.baud(115200);
    pc.printf(" ** program reset **\n\r");
    
    while (true) {
        //if the button is pressed, reverse the y direction
        if (!button) {
            y_direction = !y_direction;
            wait(0.5);
        }
        
        //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
        PIDController1();
        PIDController2();
        
        // print the motor angles and coordinates
        pc.printf("position: (%f, %f)\n\r", x_position, y_position);
        pc.printf("motor1 angle: %f\n\r", motor1_angle);
        pc.printf("motor2 angle: %f\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
    }
}