Diff: main.cpp

Revision:

3:7a608fbd3bcd

Parent:

2:55c616d2e0fe

Child:

4:96f38805f055

diff -r 55c616d2e0fe -r 7a608fbd3bcd main.cpp
--- a/main.cpp	Sat Apr 27 05:41:33 2019 +0000
+++ b/main.cpp	Sat Apr 27 08:13:01 2019 +0000
@@ -13,8 +13,10 @@
 void reset();
 
 ////////////定数
-int solution=1000;
-double c_degree=0.36;   //solution=500
+int resolution_lo=1000;
+int resolution_li=600;
+double c_degree_lo=0.36;   //solution=500
+double c_degree_li=360.0/600.0;
 
 double Kp_lo=0.01;
 double Ki_lo=0;
@@ -45,11 +47,27 @@
 
     setup();
     reset();
-    while(1) {
-       
+    while(1){
+    target_lo = 180;
+    target_li = 360;
+    target_ri = 360;
+    target_ro = 180;
+    for(int i=0;i<100;++i){
        can_send();
        pid(target_lo,target_li);
-        wait(0.01);
+       //printf("%f,%f\r\n",posi_li,posi_lo);
+       wait(0.01);
+    }
+    target_lo = 360;
+    target_li = 180;
+    target_ri = 180;
+    target_ro = 360;
+    for(int i=0;i<100;++i){
+       can_send();
+       pid(target_lo,target_li);
+       //printf("%f,%f\r\n",posi_li,posi_lo);
+       wait(0.01);
+    }
     }
 }
 
@@ -165,9 +183,9 @@
 
 void pid(double target_lo_,double target_li_)
 {
-    posi_lo=(ec_lo.getCount()%solution)*c_degree;
+    posi_lo=(ec_lo.getCount()%resolution_lo)*c_degree_lo;
     if(posi_lo<0)posi_lo+=360;
-    posi_li=(ec_li.getCount()%solution)*c_degree;
+    posi_li=(ec_li.getCount()%resolution_li)*c_degree_li;
     if(posi_li<0)posi_li+=360;
 
     double now=timer.read();