Revision 7:24a3e797e7a8, committed 2019-03-11

Comitter:

sink

Date:

Mon Mar 11 04:05:57 2019 +0000

Parent:

6:7afdc6a81566

Commit message:

ver2.1

Changed in this revision

--- 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;*/
     }