TOUTEKI
Dependencies: mbed QEI2 UnderBody Filter
Diff: main.cpp
- Revision:
- 7:24a3e797e7a8
- Parent:
- 6:7afdc6a81566
--- a/main.cpp Fri Mar 08 07:29:25 2019 +0000 +++ b/main.cpp Mon Mar 11 04:05:57 2019 +0000 @@ -7,9 +7,7 @@ Ticker timer; Timer T; -/*QEI Enc2(p7,p8,NC,RESOLUTION,&T,QEI::X4_ENCODING); -QEI Enc3(p5,p6,NC,RESOLUTION,&T,QEI::X4_ENCODING); -QEI Enc (p12,p11,NC,RESOLUTION,&T,QEI::X4_ENCODING);*/ + RoboClaw MD(115200,p9,p10); Serial Saber(p13,p14); Serial pc(USBTX,USBRX); @@ -30,29 +28,11 @@ DigitalIn G_limit2(p10); int cmd,A; -int SA1,B_SA1,LIM1,LIM2; +int LIM1,LIM2; int S1,S2; -float angle,pre_angle,SOKUDO,e_D,pre_e_D,ed_D,ei_D,e_V,ed_V,pre_e_V,bcmd; -float goal_D=0,Kp=5,Ki=0.01,Kd=0.1; -float Ksp2 = 6.5, Ksd2 = 0.4; -float Ksp3 = 6.5, Ksd3 = 0.4; - -//float encount,b_encount; - char mode = 0x00; -/*int cmd2 = 0; -int cmd3 = 0; -float spd2=0; -float spd3=0; - -float spd_err2=0; -float spd_err3=0; - -int tmp1; -int tmp2; -*/ double filtered_ref_qpps; int G_LIM1=0,G_LIM2=0; @@ -115,62 +95,6 @@ S2=SENS2.read(); static char slave_mode = 0x00; static int spd_count = 0; - /*encount=Enc.getPulses()-b_encount; - - float ppr = 1.0; - - static float pre_spd2 = 0.0; - static float pre_spd3 = 0.0; - - static float pre_err2 = 0.0; - static float pre_err3 = 0.0; - - static float ref_spd = 0.0; - - static int lim_cmd2 = 87; - static int lim_cmd3 = 92; - - static int count = 0; - static int count3 = 0; - - angle=(float)(encount)*(360.0/48.0)/4.0; - SOKUDO=(angle-pre_angle)/INT_TIME; - - e_D=(goal_D-angle); - ed_D=(e_D-pre_e_D)/INT_TIME; - ei_D+=(e_D+pre_e_D)*INT_TIME/2.0; - - cmd=(int)((e_D*Kp)+(ed_D*Kd)+(ei_D*Ki)); - - float encount2 = Enc2.getPulses(); - float encount3 = Enc3.getPulses(); - - float rot_sp2 = encount2/MULTIPLU/ppr; - spd2 = (rot_sp2 - pre_spd2)/INT_TIME/(48*4); - float rot_sp3 = encount3/MULTIPLU/ppr; - spd3 = (rot_sp3 - pre_spd3)/INT_TIME/(48*4); - - spd_err2 = filtered_ref_spd - spd2; - float spd_d2 = (spd_err2 - pre_err2)/INT_TIME; - tmp1 = (int)((spd_err2 * Ksp2) + (spd_d2 * Ksd2)); - if(tmp1>=127)tmp1=127; - if(tmp1<=-127)tmp1=-127; - cmd2 += tmp1; - - spd_err3 = filtered_ref_spd - spd3; - float spd_d3 = (spd_err3 - pre_err3)/INT_TIME; - tmp2 = (int)((spd_err3 * Ksp3) + (spd_d3 * Ksd3)); - if(tmp2>=127)tmp2=127; - if(tmp2<=-127)tmp2=-127; - cmd3 += tmp2; - - if (cmd2 > lim_cmd2) cmd2 = lim_cmd2; - if (cmd2 < -lim_cmd2) cmd2 = -lim_cmd2; - - if (cmd3 > lim_cmd3) cmd3 = lim_cmd3; - if (cmd3 < -lim_cmd3) cmd3 = -lim_cmd3;*/ - - G_LIM1=!G_limit1.read();//pullupなので逆 G_LIM2=!G_limit2.read();//pullupなので逆 @@ -223,7 +147,6 @@ case 0x90://全停止 ref_qpps1 = 0.0; ref_qpps2 = 0.0; - //cmd = 0; G_cmd = 0; break; @@ -248,23 +171,7 @@ fet2 = 0; if (slave_mode != 0x60) spd_count = 0; } - /* - if(angle>=124 && goal_D >= 120){ - cmd=0; - goal_D = angle; - } - if(angle <= 2 && goal_D <= 10) { - cmd = 0; - goal_D = angle; - } - - if(cmd>20) cmd=20; - if(cmd<-15)cmd=-15; - - if(cmd>=0) Saber_Serial (SABER_ADDR, 1, cmd); - else Saber_Serial (SABER_ADDR, 0, cmd); - */ qpps1 = limit_MD( qpps1, MAX_QPPS1); qpps2 = limit_MD( qpps2, MAX_QPPS2); @@ -272,21 +179,7 @@ if (G_cmd > 0) Saber_Serial (SABER_ADDR, 4, G_cmd); else Saber_Serial (SABER_ADDR, 5, G_cmd); - /* - if (cmd3 > 0) Saber_Serial (SB_ADRS, 0, cmd3); - else Saber_Serial (SB_ADRS, 1, cmd3); - if (cmd2 > 0) Saber_Serial (SB_ADRS, 4, cmd2); - else Saber_Serial (SB_ADRS, 5, cmd2);*/ - - /*pre_spd2 = rot_sp2; - pre_err2 = spd_err2; - pre_spd3 = rot_sp3; - pre_err3 = spd_err3; - pre_e_D = e_D; - pre_angle = angle; - pre_e_V = e_V; - B_SA1 = SA1;*/ }