Diff: main.cpp

Revision:

1:34371ffd3dc0

Parent:

0:411ab20ce87d

Child:

2:db2bc2ae4d20

--- a/main.cpp	Fri Apr 26 11:34:44 2019 +0000
+++ b/main.cpp	Sat Apr 27 05:34:43 2019 +0000
@@ -3,22 +3,25 @@
 
 
 
+
 ////////////関数
 void setup();
 void can_receive();
 void pid(double,double);
 void out_ro(double);
 void out_ri(double);
+void reset();
 
 
 ////////////定数
-int solution=500;
-int c_degree=360/500;   //solution=500
+int solution=1000;
+double c_degree=0.36;   //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;
 
@@ -39,65 +42,98 @@
 
 
 //////////////////////////////////////////////
-int main() {
-    
+int main()
+{
+
     setup();
+    reset();
     while(1) {
+        can_receive();
         pid(target_ro,target_ri);
         wait(0.01);
     }
 }
 
 //////////////////////////////////////////////
-void setup(){
+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);
-    
+
+    switch_ro.mode(PullUp);
+    switch_ri.mode(PullUp);
+
     servo.init();
     timer.start();
 }
 
+void reset()
+{
+    while(switch_ro.read()) {
+        out_ro(0.05);
+    }
+     ec_ro.reset();
+     out_ro(0);
+    while(switch_ri.read()) {
+        out_ri(0.05);
+    }
+    
+     ec_ri.reset();
+     out_ri(0);
+}
 
-void pid(double target_ro_,double target_ri_){
+void pid(double target_ro_,double target_ri_)
+{
     posi_ro=(ec_ro.getCount()%solution)*c_degree;
+    if(posi_ro<0)posi_ro+=360;
     posi_ri=(ec_ri.getCount()%solution)*c_degree;
-    
+    if(posi_ri<0)posi_ri+=360;
+
     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;
+    if(fabs(deviation_ro)<90) { //そのまま
+    } else if(deviation_ro>270) {
+        deviation_ro-=360;
+    } else if(deviation_ro<-270) {
+        deviation_ro+=360;
+    } else if(target_ro_>0) {
+        if(deviation_ro<0)deviation_ro+=360;
+    } else {
+        if(deviation_ro>0)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;
+    if(fabs(deviation_ri)<90) { //そのまま
+    } else if(deviation_ri>270) {
+        deviation_ri-=360;
+    } else if(deviation_ri<-270) {
+        deviation_ri+=360;
+    } else if(target_ri_>0) {
+        if(deviation_ri<0)deviation_ri+=360;
+    } else {
+        if(deviation_ri>0)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;
+void out_ro(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;
@@ -122,8 +158,9 @@
 }
 
 
-void out_ri(double duty){
-    double dutylimit=0.4;
+void out_ri(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;
@@ -148,18 +185,19 @@
 }
 
 //////////////////////////////////////////can
-void can_receive(){
+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;
+                target_ro= msg.data[0] + ((msg.data[2]&0b1111)<<8) - 360;
+                target_ri= msg.data[1] + ((msg.data[2]&0b11110000)<<4) - 360;
                 hand_mode= msg.data[3];
-                
+
                 break;
             }
-                led2=1;
+            led2=1;
         } else led2=0;
     }
 }