Main control script for biorobotics project
Dependencies: MODSERIAL QEI biquadFilter mbed
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 }
Generated on Thu Aug 11 2022 01:19:45 by
1.7.2