File content as of revision 11:cd7be08f46b0:

#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 deg2rad=0.0174532;                        //Conversion factor degree to rad
float rad2deg=57.29578;                         //Conversion factor rad to degree
float motor1angle=(-135.0-10.0)*deg2rad*5.5;    //Measured angle motor 1
float motor2angle=(-10.0)*deg2rad*2.75;         //Measured angle motor 2
float motor1offset;                             //Offset bij calibratie
float motor2offset;
float potmeter;
float omega1;                                   //velocity rad/s motor 1
float omega2;                                   //Velocity rad/s motor2



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

//Motorcontrol
bool motordir1;
bool motordir2;
float motor1ref=(-135.0-10.0)*deg2rad*5.5;
float motor2ref=(-10.0)*deg2rad*2.75;
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=-10.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_encoderset , motor_wait2 , motor_bewegen};
Motor_States motor_curr_state;
bool motor_state_changed = true;
bool motor_calibration_done = false;
bool motor_RKI=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 counts1af;
volatile int counts2af;
int counts1offset;
int counts2offset;
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;
}

void button2Press()
{
    button2_pressed = true;
}

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

void PID_controller(){

    static float error_integral1=0;
    static float e_prev1=motor1error;
    
    //Proportional part:
    u_p1=Kp*motor1error;
    
    //Integral part
    error_integral1=error_integral1+ei1*Ts;
    u_i1=Ki*error_integral1;
    
    //Derivative part
    float error_derivative1=(motor1error-e_prev1)/Ts;
    float u_d1=Kd*error_derivative1;
    e_prev1=motor1error;
    
    // Sum and limit
    up1= u_p1+u_i1+u_d1;
    if (up1>1){
        controlsignal1=1;}
    else if (up1<-1){
        controlsignal1=-1;}
    else {
        controlsignal1=up1;}
    
    // To prevent windup
    ux1= up1-controlsignal1;
    ek1= Ka*ux1;
    ei1= motor1error-ek1;

// Motor 2
    
    static float error_integral2=0;
    static float e_prev2=motor2error;
    
    //Proportional part:
    u_p2=Kp*motor2error;
    
    //Integral part
    error_integral2=error_integral2+ei2*Ts;
    u_i2=Ki*error_integral2;
    
    //Derivative part
    float error_derivative2=(motor2error-e_prev2)/Ts;
    float u_d2=Kd*error_derivative2;
    e_prev2=motor2error;
    
    // Sum and limit
    up2= u_p2+u_i2+u_d2;
    if (up2>1.0f){
        controlsignal2=1.0f;}
    else if (up2<-1){
        controlsignal2=-1.0f;}
    else {
        controlsignal2=up2;}
    
    // To prevent windup
    ux2= up2-controlsignal2;
    ek2= Ka*ux2;
    ei2= motor2error-ek2;
}

void RKI(){
  
  if (motor_RKI==true){
    //Vy=potmeter1.read()*10.0*motortoggle;
    //Vy=potmeter2.read()*10*motortoggle;
    static float t=0;
    Vx=6.0f*sin(1.0f*t);
    Vy=0.0f;
    t+=Ts;
    q1dot=(l2*cos(q1+q2)*Vx+l2*sin(q1+q2)*Vy)/((-l1*sin(q1)-l2*sin(q1+q2))*l2*cos(q1+q2)+l2*sin(q1+q2)*(l1*cos(q1)+l2*cos(q1+q2)));
    q2dot=((-l1*cos(q1)-l2*cos(q1+q2))*Vx+(-l1*sin(q1)-l2*sin(q1+q2))*Vy)/((-l1*sin(q1)-l2*sin(q1+q2))*l2*cos(q1+q2)+l2*sin(q1+q2)*(l1*cos(q1)+l2*cos(q1+q2)));
    q1=q1+q1dot*Ts;
    q2=q2+q2dot*Ts;  
    
    xe=l1*cos(q1)+l2*cos(q1+q2);
    ye=l1*sin(q1)+l2*sin(q1+q2);
    
    
    
    if (q1<0.0f){
        q1=0.0;}
    else if (q1>90.0f*deg2rad){
        q1=90.0f*deg2rad;}    
    else{
        q1=q1;}
    
    if (q2>-45.0*deg2rad){
        q2=-45.0*deg2rad;}
    else if (q2<-135.0*deg2rad){
        q2=-135.0*deg2rad;}
    else{
        q2=q2;}
    
    motor1ref=5.5f*q1+5.5f*q2;
    motor2ref=2.75f*q1;
    }
}    
    

void motorControl(){
    counts1= counts1af-counts1offset;
    motor1angle = (counts1 * factorin / gearratio)-((135.0*5.5*deg2rad)+(10.0*5.5*deg2rad)); // Angle of motor shaft in rad + correctie voor q1 en q2
    omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
    motor1error=motor1ref-motor1angle;         
    if (controlsignal1<0){
        motordir1= 0;}
    else {
         motordir1= 1;}

    motor1Power.write(abs(controlsignal1));
    motor1Direction= motordir1;
    
    counts2= counts2af - counts2offset;
    motor2angle = (counts2 * factorin / gearratio)-(10.0*2.75*deg2rad); // Angle of motor shaft in rad + correctie voor q1
    omega2 = deltaCounts2 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
    motor2error=motor2ref-motor2angle;         
    if (controlsignal2<0){
        motordir2= 0;}
    else {
         motordir2= 1;}
    if (motor_calibration_done == true){
    motor2Power.write(abs(controlsignal2));}
    motor2Direction= motordir2;    
}

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 ( button2_pressed ) {   
        button2_pressed = false;      
        motor_curr_state = motor_encoderset;           //Beginnen met calibratie
        motor_state_changed = true;
        // More functions
    }    
   
}    

void do_motor_Encoder_Set(){
    // Entry function
    if ( motor_state_changed == true ) {
        motor_state_changed = false;
        // More functions
    }
    motor1Power.write(0.0f);
    motor2Power.write(0.0f);
    counts1offset = counts1af ;
    counts2offset = counts2af;
      
    // State transition guard
if ( button2_pressed ) {   
        button2_pressed = false;
        motor_calibration_done = true;        
        motor_curr_state = motor_wait2;
        motor_state_changed = true;
    }        
}

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

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

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

if ( button2_pressed ) {   
        button2_pressed = false;   
        motor_RKI = false;  
        motor_curr_state = motor_wait2;          
        motor_state_changed = true;
        // More functions
    }    
   
    
}

void motor_state_machine()
{
    switch(motor_curr_state) {
        case motor_wait:
            do_motor_wait();
            break;
        case motor_encoderset:
            do_motor_Encoder_Set();
            break;
        case motor_wait2:
            do_motor_wait2();
            break;
        case motor_bewegen:
            do_motor_bewegen();
            break;
    }
}

// Global loop of program
void tickGlobalFunc()
{
    //sampleSignal();
    //emg_state_machine();
    motor_state_machine();
    readEncoder();
    PID_controller();
    motorControl();
    RKI();
    
    // 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);
    button2.fall(&button2Press);
    
    while (true) {
            pc.printf("counts1offset %i counts1af: %i counts1: %i\r\n",counts1offset,counts1af,counts1);
            pc.printf("counts2offset %i counts2af: %i counts2: %i\r\n",counts2offset,counts2af,counts2);
            pc.printf("state: %i motor1ref: %f motor1angle: %f\r\n",motor_curr_state,motor1ref,motor1angle);
            pc.printf("q1: %f q2: %f motor1error: %f \r\n",q1*rad2deg,q2*rad2deg, motor1error);
            
        wait(0.5);
    }
}