Diff: main.cpp

Revision:

0:411ab20ce87d

Child:

1:34371ffd3dc0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Apr 26 11:34:44 2019 +0000
@@ -0,0 +1,165 @@
+#include "mbed.h"
+#include "pin.h"
+
+
+
+////////////関数
+void setup();
+void can_receive();
+void pid(double,double);
+void out_ro(double);
+void out_ri(double);
+
+
+////////////定数
+int solution=500;
+int c_degree=360/500;   //solution=500
+
+double Kp_ro=0.01;
+double Ki_ro=0;
+double Kd_ro=0;
+double Kp_ri=0.01;
+double Ki_ri=0;
+double Kd_ri=0;
+
+
+
+
+////////////変数
+double target_ro=0,target_ri=0;
+bool hand_mode=0;
+double distance_ro_old=0,distance_ri_old=0,pile_ro=0,pile_ri=0;
+double posi_ro=0,posi_ri=0;
+double pre_time=0;
+
+
+Timer timer;
+
+
+
+
+//////////////////////////////////////////////
+int main() {
+    
+    setup();
+    while(1) {
+        pid(target_ro,target_ri);
+        wait(0.01);
+    }
+}
+
+//////////////////////////////////////////////
+void setup(){
+    can.frequency(1000000);
+    motor_ro_f.period_us(100);
+    motor_ro_b.period_us(100);
+    motor_ri_f.period_us(100);
+    motor_ri_b.period_us(100);
+    
+    switch1.mode(PullUp);
+    switch2.mode(PullUp);
+    
+    servo.init();
+    timer.start();
+}
+
+
+void pid(double target_ro_,double target_ri_){
+    posi_ro=(ec_ro.getCount()%solution)*c_degree;
+    posi_ri=(ec_ri.getCount()%solution)*c_degree;
+    
+    double now=timer.read();
+    double d_time=now-pre_time;
+    
+    double deviation_ro=fabs(target_ro_)-posi_ro;
+    if(target_ro_>0){
+        if(deviation_ro<0)deviation_ro=360+deviation_ro;
+    }else{
+        if(deviation_ro>0)deviation_ro=deviation_ro-360;
+    }
+    double deviation_ri=fabs(target_ri_)-posi_ri;
+    if(target_ri_>0){
+        if(deviation_ri<0)deviation_ri=360+deviation_ri;
+    }else{
+        if(deviation_ri>0)deviation_ri=deviation_ri-360;
+    }
+    
+    pile_ro+=deviation_ro;
+    pile_ri+=deviation_ri;
+    
+    out_ro(deviation_ro * Kp_ro + (posi_ro - distance_ro_old) / d_time * Kd_ro + pile_ro *  Ki_ro * d_time);
+    out_ri(deviation_ri * Kp_ri + (posi_ri - distance_ri_old) / d_time * Kd_ri + pile_ri *  Ki_ri * d_time);
+    
+    distance_ro_old=deviation_ro;
+    distance_ri_old=deviation_ri;
+    pre_time=now;
+}
+
+
+void out_ro(double duty){
+    double dutylimit=0.4;
+
+    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_ro_f = fabs(duty);
+            motor_ro_b = 0;
+        } else { //制限値超
+            motor_ro_f = dutylimit;
+            motor_ro_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_ro_f = 0;
+            motor_ro_b = fabs(duty);
+        } else { //制限値超
+            motor_ro_f = 0;
+            motor_ro_b = dutylimit;
+        }
+    }
+    //pre_out_r=duty;
+}
+
+
+void out_ri(double duty){
+    double dutylimit=0.4;
+
+    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_ri_f = fabs(duty);
+            motor_ri_b = 0;
+        } else { //制限値超
+            motor_ri_f = dutylimit;
+            motor_ri_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_ri_f = 0;
+            motor_ri_b = fabs(duty);
+        } else { //制限値超
+            motor_ri_f = 0;
+            motor_ri_b = dutylimit;
+        }
+    }
+    //pre_out_r=duty;
+}
+
+//////////////////////////////////////////can
+void can_receive(){
+    CANMessage msg;
+    for(int i=0; i<5; i++) {
+        if(can.read(msg)) {
+            if(msg.id==0) {
+                target_ro= msg.data[0] | ((msg.data[1]&0b1111)<<8) - 360;
+                target_ri= msg.data[2] | ((msg.data[1]&0b11110000)<<4) - 360;
+                hand_mode= msg.data[3];
+                
+                break;
+            }
+                led2=1;
+        } else led2=0;
+    }
+}