Diff: main.cpp

Revision:

1:507dd5045511

Parent:

0:e76f506b9de3

diff -r e76f506b9de3 -r 507dd5045511 main.cpp
--- a/main.cpp	Sun Jan 23 09:05:26 2022 +0000
+++ b/main.cpp	Thu Feb 24 06:09:47 2022 +0000
@@ -205,7 +205,8 @@
     setup_servo();
     setup_th();
     BMX055_Init(); 
-    for(int i=0;i<10;i++)update_th();make_motion();
+    //for(int i=0;i<10;i++)update_th();make_motion();
+    for(int i=0;i<10;i++)make_motion();
     dedt=0.0;
     pc2.printf("th_calib will start\r\n");
     pc2.printf("press keyboad to start\r\n");
@@ -237,7 +238,7 @@
     //while(1){
     while(tim.read()<30.0){      
     double dth;    
-    update_th();    
+    //update_th();    
     manage.start();    
     make_motion();//各rodの値を更新
     define_upper_lower();//脚の上下関係を計算
@@ -291,7 +292,8 @@
             {
               for(int i=0; i<4; i++)
               { 
-               servo_write(ch[u][i],th_write[u][i]);
+               //servo_write(ch[u][i],th_write[u][i]);
+               servo_write(ch[u][i],th[u][i]);
                }
              }
              //*/    
@@ -751,7 +753,8 @@
     dth[0] = (double)((v_Q(0, 0) - R(0, 1) * dth[1] - R(0, 2)*dth[2] - R(0, 3) * dth[3])/R(0,0));
     if (det == 1) {
         for (int i=0; i < 4; i++) {
-            th_write[leg][i] = th_write[leg][i] + dth[i];
+            //th_write[leg][i] = th_write[leg][i] + dth[i];
+            th[leg][i] = th[leg][i] + dth[i];
         }
     }
     else if (det == 2) {
@@ -1018,6 +1021,7 @@
               for(int i=0; i<4; i++)
               { 
                servo_write(ch[u][i],th_ready[u][i]);
+               th[u][i]=th_ready[u][i];
                }
              }
 }