Main control script for biorobotics project

Dependencies:   MODSERIAL QEI biquadFilter mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "QEI.h"
00002 #include "math.h"
00003 #include "mbed.h"
00004 //#include "HIDScope.h" //set mbed library version to 119 for HIDScope to work
00005 #include "MODSERIAL.h"
00006 #include "BiQuad.h"
00007 
00008 //left pot to set reference position.
00009 //right pot to set Kp, right pot sets Ki when (right)button is pressed down
00010 
00011 
00012 //--------------object creation------------------ 
00013 Serial pc(USBTX, USBRX);
00014 //Use X4 encoding.
00015 //QEI wheel(p29, p30, NC, 624, QEI::X4_ENCODING);
00016 //Use X2 encoding by default.
00017 QEI enc1(D13, D12, NC, 32); //enable the encoder
00018 QEI enc2(D15, D14, NC, 32); //enable the encoder
00019 PwmOut M1_speed(D6);
00020 PwmOut M2_speed(D5);
00021 DigitalOut M1_direction(D7);
00022 DigitalOut M2_direction(D4);
00023 AnalogIn potmeter(A0);      //left pot
00024 AnalogIn potmeter2(A1);     //right pot
00025 InterruptIn button1(D2);       //hardware interrupt for stopping motors - right button
00026 DigitalIn LimSW1(D9);
00027 DigitalIn LimSW2(D8);
00028 DigitalIn HomStart(D3); // - left button
00029 
00030 BiQuad bqlowpass(0.4354, 0.8709, 0.4354, 0.5179, 0.2238);
00031 //create scope objects - note: script won't run when HID usb port is not connected
00032 //HIDScope scope(5); //set # of channels
00033 
00034 Ticker motor_update1;
00035 Ticker motor_update2;
00036 Ticker error_update;
00037 
00038 
00039 //-----------------variable decleration----------------
00040 int pwm_freq = 500;
00041 float set_speed;
00042 float reference_pos;
00043 
00044 double M1_home; 
00045 double M1_error_pos = 0;
00046 float M1_Kp = 2.2;
00047 float M1_Ki = 3.8;
00048 float M1_Kd = 0.19;
00049 double M1_e_int=0;
00050 double M1_e_prior=0;
00051 
00052 double M2_home; 
00053 double M2_error_pos = 0;
00054 float M2_Kp = 2;
00055 float M2_Ki = 2.5;
00056 float M2_Kd = 0.1;
00057 double M2_e_int=0;
00058 double M2_e_prior=0;
00059 
00060 double setpoint = 0;
00061 
00062 double M1_rel_pos;
00063 double M2_rel_pos;
00064 double M1_reference_pos;
00065 double M2_reference_pos;
00066 
00067 double q1_step;
00068 double q2_step;
00069 double Arm1_home;
00070 double Arm2_home;
00071 double M1_actual_pos;
00072 double M2_actual_pos;
00073 double q1;
00074 double q2;
00075 double vdx;
00076 double vdy;
00077 
00078 
00079 float Ts = 0.01; //500hz sample freq
00080 
00081 bool M1homflag;
00082 bool M2homflag;
00083 bool Homstartflag;
00084   
00085 
00086 
00087  
00088 void kinematica(double q1, double q2, double vdx, double vdy, double &q1_new, double &q2_new){
00089   double q1_dot = (2.5*cos(q1+q2))/(sin(q2)) *vdx + (2.5*sin(q1+q2))/(sin(q2))*vdy;
00090   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;
00091   q1_new += q1_dot*Ts;
00092   q2_new += (q2_dot-q1_dot)*Ts;
00093   //pc.printf("q1 dot = %f, q2 dot = %f, ",q1_dot,q2_dot);
00094  return;
00095 }
00096 
00097 float PID(double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prior){ //PID calculator
00098     e_int += Ts*e;
00099     double e_diff = (e-e_prior)/Ts;
00100     e_prior = e;
00101     double e_diff_filter = bqlowpass.step(e_diff);
00102     return(Kp*e+Ki*e_int+Kd*e_diff_filter);
00103     }
00104 
00105 void M1_control(){
00106     
00107     //call PID func and set new motor speed
00108     set_speed = PID(M1_error_pos,M1_Kp,M1_Ki,M1_Kd,Ts,M1_e_int,M1_e_prior);
00109     if(set_speed > 0){
00110      M1_speed.write(abs(set_speed));
00111      M1_direction.write(0);
00112      }
00113     else if (set_speed < 0){
00114      M1_speed.write(abs(set_speed));
00115      M1_direction.write(1);
00116      }
00117     else{M1_speed.write(0);}
00118     
00119  }
00120  
00121  void M2_control(){
00122     set_speed = PID(M2_error_pos,M2_Kp,M2_Ki,M2_Kd,Ts,M2_e_int,M2_e_prior);
00123     if(set_speed > 0){
00124      M2_speed.write(abs(set_speed));
00125      M2_direction.write(0);
00126      }
00127     else if (set_speed < 0){
00128      M2_speed.write(abs(set_speed));
00129      M2_direction.write(1);
00130      }
00131     else{M2_speed.write(0);}       
00132     //pc.printf("M2 speed = %f, M2 direction = %i, M2 pos error =  %f, M2 int error = %f\n\r",set_speed,M2_direction.read(),M2_error_pos,M2_e_int);
00133     //pc.printf("M2 integral = %f position error = %f\n\r", M2_e_int,M2_error_pos);
00134   }
00135  
00136  void homing_system () {
00137     LimSW1.mode(PullDown);
00138     LimSW2.mode(PullDown);
00139     M1_speed.write(0);
00140     M2_speed.write(0);
00141     
00142     
00143        while(1){
00144             if (HomStart.read() == 0){
00145                   setpoint += 0.005; //move setpoint 0.2 radian per second (at 100hz)
00146                   //pc.printf("Homing... \n\r");
00147             }     
00148             
00149             double M1_rel_pos = enc1.getPulses()/32.0/131.25*2.0*3.1416; 
00150             double M2_rel_pos = enc2.getPulses()/32.0/131.25*2.0*3.1416;  
00151             if(LimSW1.read() == 0){
00152                   M1_error_pos = setpoint - M1_rel_pos;
00153                   M1_control();
00154             }
00155             if(LimSW2.read() == 0){
00156                 M2_error_pos = - setpoint - M2_rel_pos;
00157                 M2_control();
00158             }
00159             
00160             
00161             
00162         
00163         if(LimSW1.read() ==  1){
00164           M1_error_pos = 0;
00165           M1_speed.write(0);
00166           M1_home = enc1.getPulses()/32.0/131.25*2.0*3.1416; //in radians
00167         }    
00168         if (LimSW2.read() == 1) {
00169           M2_error_pos = 0;
00170           M2_speed.write(0);
00171           M2_home = enc2.getPulses()/32.0/131.25*2.0*3.1416; //in radians
00172         }
00173             
00174         if (LimSW1.read() == 1 && LimSW2.read() ==1) {
00175                 pc.printf("Homing finished \n\r");
00176                 M1_speed.write(0);
00177                 M2_speed.write(0);
00178                 wait(0.1);
00179                 M1_home = enc1.getPulses()/32.0/131.25*2.0*3.1416; //in radians
00180                 M2_home = enc2.getPulses()/32.0/131.25*2.0*3.1416; //in radians
00181                 M1_e_int = 0;
00182                 M2_e_int = 0;
00183                 break;
00184                 //while(1); //stop after homing.
00185         }
00186         pc.printf("M2 error = %f M2 reference = %f,M2_rel = %f\n\r", M2_error_pos,M2_reference_pos,M2_rel_pos);
00187         wait(0.01);
00188        }
00189 
00190 }
00191  
00192  void scopesend(){
00193   
00194      
00195      
00196      }
00197  void StopMotors(){
00198      while(1){
00199          M1_speed.write(0);
00200          M2_speed.write(0);
00201          }
00202     }
00203  
00204 void geterror(){ 
00205     M1_rel_pos = enc1.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians
00206     M2_rel_pos = enc2.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians   
00207 
00208     Arm1_home = 132.0/180.0*3.1416;//home position of large link attached to base in radians - 112 degrees
00209     Arm2_home = 5.0/180.0*3.1416;//home position of small link attached to base
00210     
00211     M1_actual_pos = Arm1_home + (M1_rel_pos - M1_home)*2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!! - Also known as angle theta
00212     M2_actual_pos = Arm2_home + (M2_rel_pos - M2_home)*2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!! - Also known as angle Alpha
00213     
00214     q1 = M1_actual_pos;
00215     q2 = -q1 - M2_actual_pos; //see drawing
00216     
00217     vdx = 0.1*potmeter.read();
00218     vdy = -0.1*potmeter2.read();
00219     
00220     kinematica(q1,q2,vdx,vdy,q1_step,q2_step);
00221     
00222     M1_reference_pos = q1_step; //should cover the right range - radians
00223     M2_reference_pos = -(q1_step+q2_step);
00224     
00225     
00226     
00227         
00228     
00229     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);
00230 
00231     if(M1_reference_pos > Arm1_home){
00232         M1_reference_pos = Arm1_home;
00233     }
00234     else{
00235         M1_error_pos = M1_reference_pos - M1_actual_pos;
00236     }
00237     if(M2_reference_pos < Arm2_home){
00238         M2_reference_pos = Arm2_home;
00239     }
00240     else{
00241         M2_error_pos = M2_reference_pos - M2_actual_pos;
00242     }
00243     
00244     
00245     //---------PID motor control-------------
00246     
00247       set_speed = PID(M1_error_pos,M1_Kp,M1_Ki,M1_Kd,Ts,M1_e_int,M1_e_prior);
00248     if(set_speed > 0){
00249      M1_speed.write(abs(set_speed));
00250      M1_direction.write(0);
00251      }
00252     else if (set_speed < 0){
00253      M1_speed.write(abs(set_speed));
00254      M1_direction.write(1);
00255      }
00256     else{M1_speed.write(0);}
00257      set_speed = PID(M2_error_pos,M2_Kp,M2_Ki,M2_Kd,Ts,M2_e_int,M2_e_prior);
00258     if(set_speed > 0){
00259      M2_speed.write(abs(set_speed));
00260      M2_direction.write(0);
00261      }
00262     else if (set_speed < 0){
00263      M2_speed.write(abs(set_speed));
00264      M2_direction.write(1);
00265      }
00266     else{M2_speed.write(0);}   
00267 }
00268  
00269 int main() {
00270 button1.fall(StopMotors);      
00271     //initialize serial comm and set motor PWM freq
00272 M1_speed.period(1.0/pwm_freq);
00273 M2_speed.period(1.0/pwm_freq);
00274 pc.baud(115200);
00275 pc.printf("starting homing function now. Push button to start procedure \n\r");
00276 //commence homing procedure
00277 homing_system();
00278 pc.printf("Setting home position complete\n\r");
00279 //attach all interrupt
00280 pc.printf("attaching interrupt tickers now \n\r");
00281     M1_rel_pos = enc1.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians
00282     M2_rel_pos = enc2.getPulses()/32.0/131.25*2.0*3.1416; //relative position in radians   
00283 
00284     Arm1_home = 132.0/180.0*3.1416;//home position of large link attached to base in radians - 112 degrees
00285     Arm2_home = 5.0/180.0*3.1416;//home position of small link attached to base
00286     
00287     M1_actual_pos = Arm1_home + (M1_rel_pos - M1_home)*2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!! - Also known as angle theta
00288     M2_actual_pos = Arm2_home + (M2_rel_pos - M2_home)*2; //!!!!!!!Gearing ratio of timing belt = 2!!!!!!!!!! - Also known as angle Alpha
00289     
00290     q1 = M1_actual_pos;
00291     q2 = -q1 - M2_actual_pos; //see drawing
00292     
00293     q1_step = q1;
00294     q2_step = q2;
00295    //stop motor interrupt
00296 //motor_update1.attach(&M1_control,0.01);
00297 //motor_update2.attach(&M2_control,0.01);
00298 error_update.attach(&geterror,0.02);
00299 
00300 pc.printf("initialization complete - position control of motors now active\n\r");
00301     
00302     while(1){   
00303          
00304      
00305     }
00306  
00307 }