File content as of revision 12:9a2d3d544426:

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

PwmOut pwmpin1(D6);
PwmOut pwmpin2(D5);
DigitalOut directionpin2(D4);
DigitalOut directionpin1(D7);
QEI motor2(D13,D12,NC, 32);
QEI motor1(D11,D10,NC, 32);
DigitalOut ledred(LED_RED);

DigitalIn KillSwitch(SW2);
DigitalIn button(SW3);
MODSERIAL pc(USBTX, USBRX);
HIDScope scope(6);

//values of inverse kinematics
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)
const float Timestep = 0.1;

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;

//values of PID controller
const float Kp = 1;
const float Ki = 0.2;
const float Kd = 0;
float Output1 = 0 ;      //Starting value
float Output2 = 0 ;      //Starting value
float P1 = 0;           //encoder value
float P2 = 0;
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 Y1;               // Value that is outputted to motor control
float Y2;               // Value that is outputted to motor control
float pwm1;
float pwm2;
float P_Last = 0;       // Starting position
      
void yDirection() {
    //direction of the motion
    if (emg0Bool && !emg1Bool) { //if a is pressed and not d, move to the left
        direction = -1;
    }
    else if (!emg0Bool && emg1Bool) { //if d is pressed and not a, move to the right
        direction = 1;
    }

    if (emg0Bool || emg1Bool){  
        //correction from motor 1 to keep x position the same
        
        
        //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 )* C1;     //saw tooth motor angle in rad
        //correction on x- axis
        old_x_position = x_position;
        x_position = old_x_position + (cos(motor2_angle/ C1)-cos(old_motor2_angle/ C1)); // old x + correction
        old_motor1_angle = motor1_angle;
        motor1_angle = old_motor1_angle + ( x_position - length ) / C2;
        
    }
    
    //reset the booleans, only for demo purposes
    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) { 
            direction = 1.0;
        }
        else if (!x_direction) {
            direction = -1.0;
        }
        
        //calculating the motion
        old_x_position = x_position;
        x_position = old_x_position + (0.01f * direction);
        old_motor1_angle = motor1_angle;
        motor1_angle = old_motor1_angle + ( x_position - length ) / C2; // sawtooth-gear motor angle in rad
        
        //reset the boolean, for demo purposes
        emg2Bool = false; 
    }
}

volatile float Plek1;
void PIDController1() { 
    Plek1 = motor1.getPulses();  
    P1 = motor1.getPulses() / 8400 * 2*PI;    //actual motor angle in rad  
    e1 = e2;
    e2 = e3;
    e3 = motor1_angle - P1;
    float de3 = (e3-e2)/Timestep;
    float ie3 = ie3 + e3*Timestep;
    Output2 = Kp * e3 + Ki * ie3 + Kd * de3;
    
//    Output_Last1 = Output1;
//    Output1 = Kp * (e3 - e2) + Output_Last1 +Ki * e3 + Kd * (e3 - 2*e2 + e1);
    Y1 = 0.5f * Output1;
    
    if (Y1 >= 1){
        Y1 = 1;
    }
    else if (Y1 <= -1){
        Y1 = -1;
    }    
}
volatile float Plek2;
void PIDController2() {   
    Plek2 = motor2.getPulses();
    P2 = motor2.getPulses() / 8400.0f * 2.0f*PI; // actual motor angle in rad
    f2 = f3;
    f3 = motor2_angle - P2;
    float df3 = (f3-f2)/Timestep;
    float if3 = if3 + f3*Timestep;
    Output2 = Kp * f3 + Ki * if3 + Kd * df3;
  //  Output_Last2 = Output2;
  //  Output2 = Kp * (f3 - f2) + Output_Last2 +Ki * f3 + Kd * (f3 - 2*f2 + f1);
    Y2 = 0.5f * Output2;
     
    if (Y2 >= 1){
        Y2 = 1;
    }
    else if (Y2 <= -1){
        Y2 = -1;
    }   
}

void ControlMotor1() {
    if (Y1 > 0) {
        Y1 = 0.6f * Y1 + 0.4f;
        directionpin1 = true;
    } 
    else if(Y1 < 0){
        Y1 = 0.6f - 0.4f * Y1;
        directionpin1 = false;
    }    
    pwmpin2 = abs(Y1);
}

void ControlMotor2() {
    if (Y2 > 0) {
        Y2 = 0.5f * Y2 + 0.5f;
        directionpin2 = true;
    } 
    else if(Y2 < 0){
        Y2 = 0.5f - 0.5f * Y2;
        directionpin2 = false;
    }
    pwmpin1 = abs(Y2);
}

int main() {
    pc.baud(115200);
    pc.printf(" ** program reset **\n\r");
    ledred = true;
    
    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;
            }
        }
        xDirection(); //call the function to move in the y direction
        yDirection(); //call the function to move in the x direction
        PIDController1();
        PIDController2();
        ControlMotor1();
        ControlMotor2();
        
        if (!KillSwitch) { //even in mekaar gebeund voor het hebben van een stop knop
            ledred = false;
            pwmpin1 = 0;
            pwmpin2 = 0;
            //for fun blink sos maybe???
            wait(2.0f);
            bool u = true;

            while(u) {
                if (!KillSwitch) {
                    u = false;
                    ledred = true;
                    wait(1.0f);
                }
            }
        }
        scope.set(0, Y1);
        scope.set(1, Y2);
        scope.set(2, motor1_angle);
        scope.set(3, motor2_angle);
        scope.set(4, x_position);
        scope.set(5, y_position);
        scope.send(); 
                      
        // 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", motor2_angle);
        pc.printf("output1: %f\r\n", Output1);
        pc.printf("output2: %f\r\n", Output2);
        pc.printf("P1: %f\r\n", P1);
        pc.printf("P2: %f\r\n", P2);
    
        pc.printf("dirctionpin: %f\r\n\n", (float)directionpin1);
        pc.printf("plek1: %f\r\n", Plek1);
        pc.printf("plek2: %f\r\n", Plek2);

        wait(Timestep); //can also be done with ticker, to be sure that it happens exactly every 0.5 seconds
        pwmpin1 = 0;
        pwmpin2 = 0;
    }
}