File content as of revision 10:e328acbdf885:

#include "QEI.h"
#include "math.h"
#include "mbed.h"
#include "MODSERIAL.h"
#include "BiQuad.h"

//left pot to set reference position.
//right pot to set Kp, right pot sets Ki when (right)button is pressed down


//--------------object creation------------------ 
Serial pc(USBTX, USBRX);
QEI enc1(D13, D12, NC, 32); //enable the encoder
QEI enc2(D15, D14, NC, 32); //enable the encoder
PwmOut M1_speed(D6);
PwmOut M2_speed(D5);
DigitalOut M1_direction(D7);
DigitalOut M2_direction(D4);
AnalogIn potmeter(A0);      //left pot
AnalogIn potmeter2(A1);     //right pot
InterruptIn button1(D2);       //hardware interrupt for stopping motors - right button
DigitalIn LimSW1(D9);
DigitalIn LimSW2(D8);
DigitalIn HomStart(D3); // - left button

BiQuad bqlowpass(0.4354, 0.8709, 0.4354, 0.5179, 0.2238);

Ticker motor_update1;
Ticker motor_update2;
Ticker error_update;


//-----------------variable decleration----------------
int pwm_freq = 500;
float set_speed;
float reference_pos;

double M1_home; 
double M1_error_pos = 0;
float M1_Kp = 2.2;
float M1_Ki = 3.8;
float M1_Kd = 0.19;
double M1_e_int=0;
double M1_e_prior=0;

double M2_home; 
double M2_error_pos = 0;
float M2_Kp = 2;
float M2_Ki = 2.5;
float M2_Kd = 0.1;
double M2_e_int=0;
double M2_e_prior=0;

double setpoint = 0;

double M1_rel_pos;
double M2_rel_pos;
double M1_reference_pos;
double M2_reference_pos;

double q1_step;
double q2_step;
double Arm1_home;
double Arm2_home;
double M1_actual_pos;
double M2_actual_pos;
double q1;
double q2;
double vdx;
double vdy;


float Ts = 0.01; //500hz sample freq

bool M1homflag;
bool M2homflag;
bool Homstartflag;
  


 
void kinematica(double q1, double q2, double vdx, double vdy, double &q1_new, double &q2_new){
  double q1_dot = (2.5*cos(q1+q2))/(sin(q2)) *vdx + (2.5*sin(q1+q2))/(sin(q2))*vdy;
  double q2_dot = -(0.3*cos(q1+q2)+0.4*cos(q1))/(0.12*sin(q2))*vdx -(0.3*sin(q1+q2)+0.4*sin(q1))/(0.12*sin(q2))*vdy;
  q1_new += q1_dot*Ts;
  q2_new += (q2_dot-q1_dot)*Ts;
 return;
}

float PID(double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prior){ //PID calculator
    e_int += Ts*e;
    double e_diff = (e-e_prior)/Ts;
    e_prior = e;
    double e_diff_filter = bqlowpass.step(e_diff);
    return(Kp*e+Ki*e_int+Kd*e_diff_filter);
    }

void M1_control(){
    
    //call PID func and set new motor speed
    set_speed = PID(M1_error_pos,M1_Kp,M1_Ki,M1_Kd,Ts,M1_e_int,M1_e_prior);
    if(set_speed > 0){
     M1_speed.write(abs(set_speed));
     M1_direction.write(0);
     }
    else if (set_speed < 0){
     M1_speed.write(abs(set_speed));
     M1_direction.write(1);
     }
    else{M1_speed.write(0);}
    
 }
 
 void M2_control(){
    set_speed = PID(M2_error_pos,M2_Kp,M2_Ki,M2_Kd,Ts,M2_e_int,M2_e_prior);
    if(set_speed > 0){
     M2_speed.write(abs(set_speed));
     M2_direction.write(0);
     }
    else if (set_speed < 0){
     M2_speed.write(abs(set_speed));
     M2_direction.write(1);
     }
    else{M2_speed.write(0);}       
  }
 
 void homing_system () {
    LimSW1.mode(PullDown);
    LimSW2.mode(PullDown);
    M1_speed.write(0);
    M2_speed.write(0);
    
    
       while(1){
            if (HomStart.read() == 0){
                  setpoint += 0.005; //move setpoint 0.2 radian per second (at 100hz)
                  //pc.printf("Homing... \n\r");
            }     
            
            double M1_rel_pos = enc1.getPulses()/32.0/131.25*2.0*3.1416; 
            double M2_rel_pos = enc2.getPulses()/32.0/131.25*2.0*3.1416;  
            if(LimSW1.read() == 0){
                  M1_error_pos = setpoint - M1_rel_pos;
                  M1_control();
            }
            if(LimSW2.read() == 0){
                M2_error_pos = - setpoint - M2_rel_pos;
                M2_control();
            }
            
            
            
        
        if(LimSW1.read() ==  1){
          M1_error_pos = 0;
          M1_speed.write(0);
          M1_home = enc1.getPulses()/32.0/131.25*2.0*3.1416; //in radians
        }    
        if (LimSW2.read() == 1) {
          M2_error_pos = 0;
          M2_speed.write(0);
          M2_home = enc2.getPulses()/32.0/131.25*2.0*3.1416; //in radians
        }
            
        if (LimSW1.read() == 1 && LimSW2.read() ==1) {
                pc.printf("Homing finished \n\r");
                M1_speed.write(0);
                M2_speed.write(0);
                wait(0.1);
                M1_home = enc1.getPulses()/32.0/131.25*2.0*3.1416; //in radians
                M2_home = enc2.getPulses()/32.0/131.25*2.0*3.1416; //in radians
                M1_e_int = 0;
                M2_e_int = 0;
                break;
                //while(1); //stop after homing.
        }
        pc.printf("M2 error = %f M2 reference = %f,M2_rel = %f\n\r", M2_error_pos,M2_reference_pos,M2_rel_pos);
        wait(0.01);
       }

}
 
 void scopesend(){
  
     
     
     }
 void StopMotors(){
     while(1){
         M1_speed.write(0);
         M2_speed.write(0);
         }
    }
 
void geterror(){ 
    M1_rel_pos = enc1.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians
    M2_rel_pos = enc2.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians   

    Arm1_home = 132.0/180.0*3.1416;//home position of large link attached to base in radians - 112 degrees
    Arm2_home = 5.0/180.0*3.1416;//home position of small link attached to base
    
    M1_actual_pos = Arm1_home + (M1_rel_pos - M1_home)*2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!! - Also known as angle theta
    M2_actual_pos = Arm2_home + (M2_rel_pos - M2_home)*2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!! - Also known as angle Alpha
    
    q1 = M1_actual_pos;
    q2 = -q1 - M2_actual_pos; //see drawing
    
    vdx = 0.1*potmeter.read();
    vdy = -0.1*potmeter2.read();
    
    kinematica(q1,q2,vdx,vdy,q1_step,q2_step);
    
    M1_reference_pos = q1_step; //should cover the right range - radians
    M2_reference_pos = -(q1_step+q2_step);
    
    
    
        
    
    pc.printf("VDX = %f, q1 = %f, q2 =  %f,q1_step = %f, q2_step =  %f,M1_rel = %f, M2_rel = %f, M2_reference = %f, M1_error_pos = %f, M2_error_pos = %f\n\r",vdx, q1, q2,q1_step,q2_step,M1_rel_pos,M2_rel_pos, M2_reference_pos,M1_error_pos,M2_error_pos);

    if(M1_reference_pos > Arm1_home){
        M1_reference_pos = Arm1_home;
    }
    else{
        M1_error_pos = M1_reference_pos - M1_actual_pos;
    }
    if(M2_reference_pos < Arm2_home){
        M2_reference_pos = Arm2_home;
    }
    else{
        M2_error_pos = M2_reference_pos - M2_actual_pos;
    }
    
    
    //---------PID motor control-------------
    
      set_speed = PID(M1_error_pos,M1_Kp,M1_Ki,M1_Kd,Ts,M1_e_int,M1_e_prior);
    if(set_speed > 0){
     M1_speed.write(abs(set_speed));
     M1_direction.write(0);
     }
    else if (set_speed < 0){
     M1_speed.write(abs(set_speed));
     M1_direction.write(1);
     }
    else{M1_speed.write(0);}
     set_speed = PID(M2_error_pos,M2_Kp,M2_Ki,M2_Kd,Ts,M2_e_int,M2_e_prior);
    if(set_speed > 0){
     M2_speed.write(abs(set_speed));
     M2_direction.write(0);
     }
    else if (set_speed < 0){
     M2_speed.write(abs(set_speed));
     M2_direction.write(1);
     }
    else{M2_speed.write(0);}   
}
 
int main() {
button1.fall(StopMotors);      
    //initialize serial comm and set motor PWM freq
M1_speed.period(1.0/pwm_freq);
M2_speed.period(1.0/pwm_freq);
pc.baud(115200);
pc.printf("starting homing function now. Push button to start procedure \n\r");
//commence homing procedure
homing_system();
pc.printf("Setting home position complete\n\r");
//attach all interrupt
pc.printf("attaching interrupt tickers now \n\r");
    M1_rel_pos = enc1.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians
    M2_rel_pos = enc2.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians   

    Arm1_home = 132.0/180.0*3.1416;//home position of large link attached to base in radians - 112 degrees
    Arm2_home = 5.0/180.0*3.1416;//home position of small link attached to base
    
    M1_actual_pos = Arm1_home + (M1_rel_pos - M1_home)*2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!! - Also known as angle theta
    M2_actual_pos = Arm2_home + (M2_rel_pos - M2_home)*2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!! - Also known as angle Alpha
    
    q1 = M1_actual_pos;
    q2 = -q1 - M2_actual_pos; //see drawing
    
    q1_step = q1;
    q2_step = q2;
error_update.attach(&geterror,0.02);

pc.printf("initialization complete - position control of motors now active\n\r");
    
    while(1){   
         
     
    }
 
}