File content as of revision 1:d7299175a12e:

#include "QEI.h"
#include "math.h"
#include "mbed.h"
//#include "HIDScope.h" //set mbed library version to 119 for HIDScope to work
#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);
//Use X4 encoding.
//QEI wheel(p29, p30, NC, 624, QEI::X4_ENCODING);
//Use X2 encoding by default.
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,    0.611,    0.611,  1,    0.222);
//create scope objects - note: script won't run when HID usb port is not connected
//HIDScope scope(5); //set # of channels

Ticker motor_update1;
Ticker motor_update2;
Ticker error_update;


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


float Arm1_home = 112.0/180.0*3.1416;//home position of large link attached to base in radians - 112 degrees
float Arm2_home = 19.0/180.0*3.1416;//home position of small link attached to base

double M1_home; 
double M1_error_pos = 0;
float M1_Kp = 2;
float M1_Ki = 4;
float M1_Kd = 0;
double M1_e_int=0;
double M1_e_prior=0;

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

float Ts = 0.002; //500hz sample freq

bool M1homflag;
bool M2homflag;
bool Homstartflag;
  
 void homing_system () {
    LimSW1.mode(PullDown);
    LimSW2.mode(PullDown);
    M1_speed.write(0);
    M2_speed.write(0);
    M1_direction.write(0);
    M2_direction.write(1);
    
       while(1){
        if (HomStart.read() == 0){
          M1_speed.write(0.4);
          wait(0.5);
          M2_speed.write(0.3);
          pc.printf("starting M1 \n\r");
        }
        if(LimSW1.read() ==  1){
          
          M1_speed.write(0);
          M1_home = enc1.getPulses()/32.0/131.25*2.0*3.1416; //in radians
        }    
        if (LimSW2.read() == 1) {
          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.5);
                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
                break;
        }
       }

}

 
void kinemtaica(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(q2))/(0.12*sin(q2))*vdy;
  q1_new = q1 +q1_dot*Ts;
  q2_new = q2 +q2_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);}
    pc.printf("Motor1 set speed = %f \n\r",set_speed);
 }
 
 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 scopesend(){
  
     
     
     }
 void StopMotors(){
     while(1){
         M1_speed.write(0);
         M2_speed.write(0);
         }
    }
 
void geterror(){ 
    double M1_rel_pos = enc1.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians
    double M2_rel_pos = enc2.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians
    
    double M1_actual_pos = Arm1_home + (M1_rel_pos - M1_home)/2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!!
    double M2_actual_pos = Arm2_home + (M2_rel_pos - M2_home)/2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!!
    
    double q1 = M1_actual_pos;
    double q2 = 3.1416-M1_actual_pos-M2_actual_pos; //see drawing
    
    //double M1_reference_pos = 1+potmeter.read()*0.5*3.1416; //should cover the right range - radians
    double M1_reference_pos = 0.5*3.1416; //should cover the right range - radians
    double M2_reference_pos = 0.5+potmeter2.read()*0.25*3.1416;
        
    M1_error_pos = M1_reference_pos - M1_actual_pos;
    //M2_error_pos = M2_reference_pos - M2_actual_pos;
    M2_error_pos = 0;
}
 
int main() {
    
    //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");
button1.fall(StopMotors);     //stop motor interrupt
motor_update1.attach(&M1_control,0.01);
motor_update2.attach(&M2_control,0.01);
error_update.attach(&geterror,0.01);

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