File content as of revision 0:4141aef83f4b:

#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
DigitalIn LimSW1(D1);
DigitalIn LimSW2(D0);
DigitalIn HomStart(D3);
Ticker motor_update;
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 scopeTimer;


//-----------------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 = 0.1;
float M1_Ki = 0.1;
float M1_Kd = 0.1;
double M1_e_int=0;
double M1_e_prior=0;

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

float Ts = 0.002; //500hz sample freq

 
 void homing_system () {
    LimSW1.mode(PullDown);
    LimSW2.mode(PullDown);
    M1_speed.write(0);
    M2_speed.write(0);
    M1_direction.write(0);
    M2_direction.write(0);
    
    while (true){
   
        if (HomStart == 0){
          M1_speed.write(0.1);
          }  
 
        if (LimSW1 ==  1){
            M1_speed.write(0);
            M2_speed.write(0.1);
            M1_home = enc1.getPulses()/32.0/131.25*2.0*3.1416; //in radians
            }
        if (LimSW2 == 1) {
            M2_speed.write(0);
            M2_home = enc2.getPulses()/32.0/131.25*2.0*3.1416; //in radians
            }
            
            if (LimSW1 == 1 && LimSW2 ==1) {
                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);}
 }
 
 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_acual_pos-M2_actual_pos //see drawing
    
    double M1_reference_pos = potmeter.read();
    double M2_reference_pos = potmeter2.read();
        
    M1_error_pos = M1_reference_pos - M1_actual_pos;
    M2_error_pos = M2_reference_pos - M2_actual_pos;
}
 
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);
//commence homing procedure
homing_system();
//attach all interrupt
button1.fall(StopMotors);     //stop motor interrupt
motor_update.attach(&M1_control, &M2_control,0.01);
//motor_update.attach(&M2_control,0.01);






pc.printf("initialization complete \n\r");
    
    while(1){   
        
     
    }
 
}