Diff: main.cpp

Revision:

1:86c4c38abe40

Parent:

0:c1476d342c13

Child:

2:55c616d2e0fe

diff -r c1476d342c13 -r 86c4c38abe40 main.cpp
--- a/main.cpp	Fri Apr 26 11:34:13 2019 +0000
+++ b/main.cpp	Sat Apr 27 04:28:33 2019 +0000
@@ -7,12 +7,21 @@
 ////////////関数
 void setup();
 void can_send();
-
-
+void pid(double,double);
+void out_lo(double);
+void out_li(double);
 
 ////////////定数
+int solution=1000;
+double c_degree=0.36;   //solution=500
 
+double Kp_lo=0.01;
+double Ki_lo=0;
+double Kd_lo=0;
+double Kp_li=0.01;
 
+double Ki_li=0;
+double Kd_li=0;
 
 
 
@@ -20,32 +29,42 @@
 double target_ro=0,target_ri=0;
 double target_lo=0,target_li=0;
 bool hand_mode=0;
+double distance_lo_old=0,distance_li_old=0,pile_lo=0,pile_li=0;
+double posi_lo=0,posi_li=0;
+double pre_time=0;
+
+Timer timer;
+
 
 
 
 /////////////////////////////////////////////
-int main() {
-    
+int main()
+{
+
     setup();
+    
     while(1) {
-        can_send();
+       can_send();
+       pid(target_lo,target_li);
         wait(0.01);
     }
 }
 
 
-void setup(){
+void setup()
+{
     can1.frequency(1000000);
     motor_lo_f.period_us(100);
     motor_lo_b.period_us(100);
     motor_li_f.period_us(100);
     motor_li_b.period_us(100);
-    
+
     hand.mode(PullUp);
     switch2.mode(PullUp);
     switch3.mode(PullUp);
     switch4.mode(PullUp);
-    
+
 
     device.baud(115200);
     device.format(8,Serial::None,1);
@@ -53,20 +72,122 @@
     wait(0.05);
     theta0=degree0;
     check_gyro();
+    timer.start();
 }
 
 
 //////////////////////////////////////can
 void can_send()
 {
-    char data[4]={0};
+    char data[4]= {0};
     int target_ro_send=target_ro+360;
     int target_ri_send=target_ri+360;
     data[0]=target_ro_send & 0b11111111;
     data[1]=target_ri_send & 0b11111111;
     data[2]=(target_ro_send>>8) | ((target_ri_send>>4) & 0b11110000);
     data[3]=hand_mode;
-    
+
     if(can1.write(CANMessage(0,data,4)))led4=1;
     else led4=0;
+}
+
+
+void out_lo(double duty)
+{
+  
+    double dutylimit=0.1;
+
+    if(duty > 0) { //入力duty比が正の場合
+        //if(duty-pre_out_r >accel_max && pre_out_l*duty>0)duty=pre_out_r+accel_max;
+        if( fabs( duty ) < dutylimit ) { //制限値内
+            motor_lo_f = fabs(duty);
+            motor_lo_b = 0;
+        } else { //制限値超
+            motor_lo_f = dutylimit;
+            motor_lo_b = 0;
+        }
+    } else {//入力duty比が負の場合
+        //if(pre_out_r-duty >accel_max  && pre_out_l*duty>0)duty=pre_out_r-accel_max;
+        if( fabs(duty) < dutylimit) { //制限値内
+            motor_lo_f = 0;
+            motor_lo_b = fabs(duty);
+        } else { //制限値超
+            motor_lo_f = 0;
+            motor_lo_b = dutylimit;
+        }
+    }
+    //pre_out_r=duty;
+}
+
+
+void out_li(double duty)
+{
+   
+    double dutylimit=0.1;
+
+    if(duty > 0) { //入力duty比が正の場合
+        //if(duty-pre_out_r >accel_max && pre_out_l*duty>0)duty=pre_out_r+accel_max;
+        if( fabs( duty ) < dutylimit ) { //制限値内
+            motor_li_f = fabs(duty);
+            motor_li_b = 0;
+        } else { //制限値超
+            motor_li_f = dutylimit;
+            motor_li_b = 0;
+        }
+    } else {//入力duty比が負の場合
+        //if(pre_out_r-duty >accel_max  && pre_out_l*duty>0)duty=pre_out_r-accel_max;
+        if( fabs(duty) < dutylimit) { //制限値内
+            motor_li_f = 0;
+            motor_li_b = fabs(duty);
+        } else { //制限値超
+            motor_li_f = 0;
+            motor_li_b = dutylimit;
+        }
+    }
+    //pre_out_r=duty;
+}
+
+void pid(double target_lo_,double target_li_)
+{
+    posi_lo=(ec_lo.getCount()%solution)*c_degree;
+    if(posi_lo<0)posi_lo+=360;
+    posi_li=(ec_li.getCount()%solution)*c_degree;
+    if(posi_li<0)posi_li+=360;
+
+    double now=timer.read();
+    double d_time=now-pre_time;
+
+    double deviation_lo=fabs(target_lo_)-posi_lo;
+    if(fabs(deviation_lo)<90) { //そのまま
+    } else if(deviation_lo>270) {
+        deviation_lo-=360;
+    } else if(deviation_lo<-270) {
+        deviation_lo+=360;
+    } else if(target_lo_>0) {
+        if(deviation_lo<0)deviation_lo+=360;
+    } else {
+        if(deviation_lo>0)deviation_lo-=360;
+    }
+
+    double deviation_li=fabs(target_li_)-posi_li;
+    if(fabs(deviation_li)<90) { //そのまま
+    } else if(deviation_li>270) {
+        deviation_li-=360;
+    } else if(deviation_li<-270) {
+        deviation_li+=360;
+    } else if(target_li_>0) {
+        if(deviation_li<0)deviation_li+=360;
+    } else {
+        if(deviation_li>0)deviation_li-=360;
+    }
+
+    pile_lo+=deviation_lo;
+    pile_li+=deviation_li;
+
+    out_lo(deviation_lo * Kp_lo + (posi_lo - distance_lo_old) / d_time * Kd_lo + pile_lo *  Ki_lo * d_time);
+    out_li(deviation_li * Kp_li + (posi_li - distance_li_old) / d_time * Kd_li + pile_li *  Ki_li * d_time);
+
+    distance_lo_old=deviation_lo;
+    distance_li_old=deviation_li;
+    pre_time=now;
 }
\ No newline at end of file