juijiu
Dependencies: HIDScope QEI biquadFilter mbed
Fork of MotorArchitecture1-11 by
Diff: main.cpp
- Revision:
- 9:4f594927cff3
- Parent:
- 8:c7d21f5f87d8
- Child:
- 10:91173ed1e841
--- a/main.cpp Wed Oct 31 09:47:09 2018 +0000 +++ b/main.cpp Thu Nov 01 08:30:56 2018 +0000 @@ -36,12 +36,7 @@ PwmOut motor2_speed_control(D6);//aanstuursnelheid motor 2 DigitalOut motor2_direction(D7);// draairichting motor 2 (0 is CCW ) - float kp1 = 0.1; - float kp2 = 0.1; - float ki1 = 0.01; - float ki2 = 0.01; - float kd1 = 0.0005; - float kd2 = 0.0005; + float int_u1 = 0; float int_u2 = 0; float u1 = 0; @@ -98,8 +93,8 @@ static BiQuad HighPass2(0.95653708, -1.91307417, 0.95653708, -1.91118480, 0.91496354); static BiQuad LowPass1(0.00362164, 0.00724327, 0.00362164, -1.82267251, 0.83715906); static BiQuad LowPass2(0.00362164, 0.00724327, 0.00362164, -1.82267251, 0.83715906); -static BiQuad LowpassFilter(0.0640,0.1279,0.0640,-1.1683,0.4241); - +static BiQuad LowpassFilter1(0.0640,0.1279,0.0640,-1.1683,0.4241); +static BiQuad LowpassFilter2(0.0640,0.1279,0.0640,-1.1683,0.4241); void filterall() { //Notch 50 Hz BW 6 Hz @@ -164,23 +159,19 @@ twistf[0] = 1; twistf[1] = 0; - if (filteredsignal2 > 0.15*max2){ - abs_sig = (filteredsignal2 - (0.15*max2))/(0.85*max2); - + if (but1 == false){ + //abs_sig = (filteredsignal2 - (0.3*max2))/(0.7*max2); + abs_sig = 0.4; } else { abs_sig = 0; } - //if (but1 == false){ - // abs_sig = 0.75; - - - // } + - twist[0] = twistf[0] * abs_sig* gain; - twist[1] = twistf[1] * abs_sig* gain; + twist[0] = twistf[0] * abs_sig ; + twist[1] = twistf[1] * abs_sig ; motor1_speed_control = fabs(u1); @@ -249,12 +240,23 @@ } ///////////END MOVEMENT STATES///////////////////////// ///////////ROBOT ARM STATES /////////////////////////// - + float kp1 = 0.1; + float kp2 = 0.1; + float ki1 = 0.05; + float ki2 = 0.05; + float kd1 = 0.005; + float kd2 = 0.005; + float olderror1; + float olderror2; + float d_error1; + float d_error2; void do_state_failure(){ } + int count1 = 0; int count2 = 0; + void do_state_calib_motor(){ if (state_changed==true) { state_changed = false; @@ -291,7 +293,9 @@ timer.reset(); state_changed = false; } - + float kp11 = 0.1; + float kp12 = 0.1; + float werror1 = deg_m1 - 0; float werror2 = deg_m2 - 0; @@ -300,7 +304,7 @@ if(werror1 < -5){ wu1 = -1; } else{ - wu1 = kp1*werror1; //+ (u1 + werror1*0.002)*ki1; + wu1 = kp11*werror1; //+ (u1 + werror1*0.002)*ki1; } if(werror2 > 5){ @@ -308,7 +312,7 @@ if(werror2 < -5){ wu2 = -1;} else{ - wu2 = kp2*werror2; //+ (u2 + werror2*0.002)*ki2); + wu2 = kp12*werror2; //+ (u2 + werror2*0.002)*ki2); } motor1_speed_control = fabs(wu1)/8; @@ -327,16 +331,19 @@ if ( timer.read() > 4) { motor1_speed_control = 0; - motor2_speed_control = 0; - + motor2_speed_control = 0; + ref_q1 = 0; + ref_q2 = 0; deg_m1 = 0; deg_m2 = 0; + d_error1 = 0; + d_error2 = 0; u1 = 0; u2 = 0; int_u1 = 0; int_u2 = 0; - wait(1); + wait(1); current_state = calib_emg; timer.reset(); state_changed = true; @@ -420,11 +427,11 @@ float inv_jacobian[4]; jacobian[0] = L1; - jacobian[1] = L2*sin(deg_m2)+L1; + jacobian[1] = (L2*cos(deg_m2))+L1; jacobian[2] = -L0; - jacobian[3] = -L0 - L2*cos(deg_m2); + jacobian[3] = -L0 - (L2*sin(deg_m2)); - float det = 1/(jacobian[0]*jacobian[3]-jacobian[1]*jacobian[2]); + float det = 1/((jacobian[0]*jacobian[3])-(jacobian[1]*jacobian[2])); inv_jacobian[0] = det*jacobian[3]; inv_jacobian[1] = -det*jacobian[1]; @@ -442,41 +449,39 @@ error1 = deg_m1 - ref_q1; error2 = deg_m2 - ref_q2; - float olderror1; - float olderror2; - float d_error1; - float d_error2; + + - if(error1 > 5){ - u1 = 1; } - if(error1 < -5){ - u1 = -1; } - else{ + // if(error1 > 5){ + // u1 = 1; } + //if(error1 < -5){ + // u1 = -1; } + //else{ d_error1 = (error1 - olderror1)/Ts; - filtered_d_error1 = LowpassFilter.step(d_error1); + filtered_d_error1 = LowpassFilter1.step(d_error1); int_u1 = int_u1 + error1 * Ts; u1 = kp1*error1 + int_u1*ki1 + kd1*filtered_d_error1; teraterm1 = u1; // to see how big u1 and u2 actually get, they should lie between -1 and 1 for the right gains. if(u1>1){ u1 =1; - } + } if(u1<-1){ u1 = -1; } - } + //} olderror1 = error1; - if(error2 > 5){ - u2 = 1;} - if(error2 < -5){ - u2 = -1;} - else{ + //if(error2 > 5){ + // u2 = 1;} + // if(error2 < -5){ + // u2 = -1;} + //else{ d_error2 = (error2 - olderror2)/Ts; - filtered_d_error2 = LowpassFilter.step(d_error2); + filtered_d_error2 = LowpassFilter2.step(d_error2); int_u2 = int_u2 + error2 * Ts; u2 = kp2*error2 + int_u2*ki2 + kd2*filtered_d_error2; teraterm2 = u2; @@ -486,8 +491,9 @@ if(u2<-1){ u2 = -1; } - } - + // } + olderror2 = error2; + } @@ -535,8 +541,9 @@ loop_timer.attach(&loop_function, Ts); sample_timer.attach(&scopedata, Ts); sample_timer2.attach(&filterall, Ts); - + + pc.baud(115200); while (true) { - printf("%f %f %f %f %f \n\r",ref_q1,ref_q2,error1,int_u1,filtered_d_error1); + printf("%f %f %f %f %f %f \n\r",ref_q1,ref_q2,error1,error2,deg_m1, deg_m2); } } \ No newline at end of file