File content as of revision 4:1e8da6b5f147:

#include "mbed.h"
#include "HIDScope.h"
#include "QEI.h"
#include "MODSERIAL.h"
#include "BiQuad.h"
#include "FastPWM.h"

// Button and potmeter1 control
InterruptIn button1(D11);
InterruptIn button2(D10);
InterruptIn buttonsw2(SW2);
InterruptIn buttonsw3(SW3);
AnalogIn potmeter1(A0);
AnalogIn potmeter2(A1);
AnalogIn potmeter3(A2);
AnalogIn potmeter4(A3);

// Encoder
DigitalIn encA1(D9);
DigitalIn encB1(D8);
DigitalIn encA2(D13);
DigitalIn encB2(D13);
QEI encoder1(D9,D8,NC,64,QEI::X4_ENCODING);     //Encoding motor 1
QEI encoder2(D13,D12,NC,64,QEI::X4_ENCODING);   //Encoding motor 2
float Ts = 0.01;                                //Sample time
float motor1angle;                              //Measured angle motor 1
float motor2angle;                              //Measured angle motor 2
float potmeter;
float omega1;                                   //velocity rad/s motor 1
float omega2;                                   //Velocity rad/s motor2
float deg2rad=0.0174532;                        //Conversion factor degree to rad
float rad2deg=57.29578;                         //Conversion factor rad to degree


// Motor
DigitalOut motor2Direction(D4);
FastPWM motor2Power(D5);
DigitalOut motor1Direction(D7);
FastPWM motor1Power(D6);

volatile int motortoggle = 1;                   //Toggle to stop motors

//Motorcontrol
bool motordir1;
bool motordir2;
float motor1ref=0.1745;
float motor2ref=0.0873;
double controlsignal1;
double controlsignal2;
double pi2= 6.283185;
float motor1error;                              //motor 1 error
float motor2error;
float Kp=0.27;
float Ki=0.35;
float Kd=0.1;
float u_p1;
float u_p2;
float u_i1;
float u_i2;

//Windup control
float ux1;
float ux2;
float up1;                                      //Proportional contribution motor 1
float up2;                                      //Proportional contribution motor 2
float ek1;                                          
float ek2;
float ei1= 0.0;                                 //Error integral motor 1
float ei2=0.0;                                  //Error integral motor 2
float Ka= 1.0;                                  //Integral windup gain

//RKI 
float Vx=0.0;                                   //Desired linear velocity x direction
float Vy=0.0;                                   //Desired linear velocity y direction
float q1=0.0f*deg2rad;                          //Angle of first joint [rad]
float q2=-135.0f*deg2rad;                       //Angle of second joint [rad]
float q1dot;                                    //Velocity of first joint [rad/s]
float q2dot;                                    //Velocity of second joint [rad/s]
float l1=26.0;                                  //Distance base-link [cm]
float l2=62.0;                                  //Distance link-endpoint [cm]
float xe;                                       //Endpoint x position [cm]
float ye;                                       //Endpoint y position [cm]

//Hidscope
HIDScope scope(6);                              //Going to send x channels of data. To access data go to 'http:/localhost:18082/' after starting HIDScope application.

//State maschine
enum Motor_States{motor_wait , motor_cal1_start , motor_cal1 , motor_cal2_start , motor_cal2 , motor_encoderset};
Motor_States motor_curr_state;
bool motor_state_changed = true;
bool motor_cal1_done = false;
bool motor_cal2_done = false;

bool button1_pressed = false;
bool button2_pressed = false;

// PC connection
MODSERIAL pc(USBTX, USBRX);

// Intializing tickers
Ticker motorTicker;
Ticker controlTicker;
Ticker directionTicker;
Ticker encoderTicker;
Ticker scopeTicker;
Ticker tickGlobal;      //Global ticker

const float PWM_period = 1e-6;

volatile int counts1; // Encoder counts
volatile int counts2;
volatile int countsPrev1 = 0;
volatile int countsPrev2 = 0;
volatile int deltaCounts1;
volatile int deltaCounts2;

float factorin = 6.23185/64; // Convert encoder counts to angle in rad
float gearratio = 131.25; // Gear ratio of gearbox

void button1Press()
{
    button1_pressed = true;
}


// Ticker Functions
void readEncoder()
{
    counts1 = encoder1.getPulses();        
    deltaCounts1 = counts1 - countsPrev1;
    countsPrev1 = counts1;
    
    counts2 = encoder2.getPulses();
    deltaCounts2 = counts2 - countsPrev2;
    countsPrev2 = counts2;    
}

void do_motor_cal1_start(){
    // Entry function
    if ( motor_state_changed == true ) {
        motor_state_changed = false;
        // More functions
    }
    
    //Do stuff    
    motor1angle = (counts1 * factorin / gearratio); // Angle of motor shaft in rad
    omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
    controlsignal1=0.10f;
    motor1Power.write(abs(controlsignal1*motortoggle));
    motor1Direction=0;

    // State transition guard
    if ( abs(omega1) >= 1.5f ){
        motor_curr_state = motor_cal1;
        motor_state_changed = true;
        // More functions
    }
    
}

void do_motorCalibration1() {
    // Entry function
    if ( motor_state_changed == true ) {
        motor_state_changed = false;
        // More functions
    }

    // Do stuff until end condition is met
    motor1angle = (counts1 * factorin / gearratio); // Angle of motor shaft in rad
    omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
    float potmeter=potmeter1.read();
    controlsignal1=0.0980f;
    motor1Power.write(abs(controlsignal1*motortoggle));
    motor1Direction=0;

    // State transition guard
    if ( abs(omega1) <= 0.5f ) {
        motor_curr_state = motor_cal2_start;
        motor_state_changed = true;
        // More functions
    }
}

void do_motor_cal2_start()

void do_motorCalibration2(){
    // Entry function
    if ( motor_state_changed == true ) {
        motor_state_changed = false;
        // More functions
    }

    // Do stuff until end condition is met    
    potmeter=potmeter1.read();
    motor1angle = (counts1 * factorin / gearratio); // Angle of motor shaft in rad
    omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
    controlsignal1=potmeter;
    motor1Power.write(abs(controlsignal1*motortoggle));
    motor1Direction=0;    
    
    motor2angle = (counts2 * factorin / gearratio); // Angle of motor shaft in rad
    omega2 = deltaCounts2 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s 
    controlsignal2=potmeter;
    motor2Power.write(abs(controlsignal2*motortoggle));
    motor2Direction=1;  
    //Dit moet je doen zolang omega van motor 2 > 0.iets  
    
    // State transition guard
    if ( omega2 <= 0.5f ) {
        motor_curr_state = motor_encoderset;
        motor_state_changed = true;
        // More functions
    }    
}

void do_motor_Encoder_Set(){
    // Entry function
    if ( motor_state_changed == true ) {
        motor_state_changed = false;
        // More functions
    }

    // Do stuff until end condition is met 
    
    
    // State transition guard
    if ( omega2 <= 0.5f ) {         
        motor_curr_state = motor_encoderset;
        motor_state_changed = true;
        // More functions
    }        
}

void do_motor_wait(){
    // Entry function
    if ( motor_state_changed == true ) {
        motor_state_changed = false;
        // More functions
    }

    // Do nothing until end condition is met 
    
    // State transition guard
    if ( button1_pressed ) {   
        button1_pressed = false;      
        motor_curr_state = motor_cal1_start;           //Beginnen met calibratie
        motor_state_changed = true;
        // More functions
    }        
}    

void toggleMotor()
{
    motortoggle = !motortoggle;
}

void motor_state_machine()
{
    switch(motor_curr_state) {
        case motor_wait:
            do_motor_wait();
            break;
        case motor_cal1_start:
            do_motor_cal1_start();
            break;
        case motor_cal1:
            do_motorCalibration1();
            break;
        case motor_cal2_start:
            do_motor_cal2_start();    
            break;
        case motor_cal2:
            do_motorCalibration2();
            break;
        case motor_encoderset:
            do_motor_Encoder_Set();
            break;
    }
}

// Global loop of program
void tickGlobalFunc()
{
    //sampleSignal();
    //emg_state_machine();
    motor_state_machine();
    readEncoder();
    // controller();
    // outputToMotors();
}


int main()
{
    pc.baud(115200);
    pc.printf("\r\nStarting...\r\n\r\n");
    motor1Power.period(PWM_period);
    motor2Power.period(PWM_period);
    
    motor_curr_state = motor_wait;                          // Start off in EMG Wait state
    tickGlobal.attach( &tickGlobalFunc, Ts );
    
    button1.fall(&button1Press);
    
    while (true) {
            potmeter=potmeter1.read();
            pc.printf("Omega1: %f Omega 2: %f  controlsignal1: %f \r\n", omega1, omega2, controlsignal1);
            pc.printf("Currentstate: %i motor_cal1: %i\r\n",motor_curr_state, motor_cal1);
        wait(0.5);
    }
}