Diff: main.cpp

Revision:

5:f225e0c61cfc

Parent:

4:8a50c7822dac

--- a/main.cpp	Sun Dec 26 07:43:22 2021 +0000
+++ b/main.cpp	Thu Feb 24 06:12:53 2022 +0000
@@ -23,44 +23,45 @@
 Serial pc2(USBTX,USBRX);
 Timer tim;
 Timer manage;
-int times= 100;//実行回数:実行時間は (sampling)*(times)秒
+int times= 200;//実行回数:実行時間は (sampling)*(times)秒
 
 using namespace Eigen;
 
 //以下変数定義
 
 //brent法に必要な変数
-double ax=-0.5*PI/180.0,bx=-0.2*PI/180.0,cx=0.0;
+double ax=-0.1*PI/180.0,bx=0.1*PI/180.0,cx=0.0;
 double fa,fb,fc;
 //サーボの書き込みに必要な変数
-double servo0[16]={7800.0, 7250.0, 6000.0, 7150.0, 6100.0, 6300.0, 8400.0, 7200.0, 6800.0, 7000.0, 5700.0, 8350.0, 6100.0, 8500.0, 5600.0, 6570.0};//servoの初期値
+double servo0[16]={7700.0, 5850.0, 7050.0, 6850.0, 6000.0, 6300.0, 8400.0, 7200.0, 6700.0, 7000.0, 5650.0, 8400.0, 6000.0, 5100.0, 5600.0, 6570.0};//servoの初期値
 int ch[4][4]={{0 ,1 ,2 ,3} ,
               {4 ,5 ,6 ,7} ,
               {8 ,9 ,10,11},
               {12,13,14,15} };
-double r=50*PI/180;//斜面の傾き[°]
-double sampling=0.1;//δtの時間[s]
-double L[4] = {50.0,50.0,50.0,50.0};//4本のリンク長 後から足したのでL[3]を理論中のL0に対応させる
+double r=0.0*PI/180;//斜面の傾き[°]
+double sampling=0.01;//δtの時間[s]
+double L[4] = {65.0,35.0,110.0,38.0};//4本のリンク長 後から足したのでL[3]を理論中のL0に対応させる
 double tip[4][3];//足先座標
-double con[4][3] = {  50.0, 50.0,0,
-                     -50.0, 50.0,0,
-                     -50.0,-50.0,0,
-                      50.0,-50.0,0};//脚のコーナー座標,zは必ず0
-double th[4][4] = { {135 * PI / 180, 60 * PI / 180, -30 * PI / 180, -30 * PI / 180},
-                    {45  * PI / 180, 60 * PI / 180, -30 * PI / 180, -30 * PI / 180},
-                   {-45  * PI / 180, 60 * PI / 180, -30 * PI / 180, -30 * PI / 180},
-                   {-135 * PI / 180, 60 * PI / 180, -30 * PI / 180, -30 * PI / 180} };//サーボに入力する角度
+double con[4][3] = {  {-55.0, 55.0,0.0},
+                      { 55.0, 55.0,0.0},
+                      { 55.0,-55.0,0.0},
+                      {-55.0,-55.0,0.0}};//脚のコーナー座標,zは必ず0
+double th_ready[4][4] = { {135 * PI / 180, 30 * PI / 180, -75 * PI / 180, -30 * PI / 180},
+                          {45  * PI / 180, 30 * PI / 180, -75 * PI / 180, -30 * PI / 180},
+                          {-45  * PI / 180, 30 * PI / 180, -75 * PI / 180, -30 * PI / 180},
+                          {-135 * PI / 180, 30 * PI / 180, -75 * PI / 180, -30 * PI / 180} };//初期状態                   
 double th0[4][4]= { 0.0,0.0,0.0,0.0, 
                     0.0,0.0, 0.0,0.0, 
                     0.0,0.0, 0.0,0.0, 
                     0.0,0.0, 0.0,0.0 }; //計算用の関節角度
+double th[4][4];//サーボに入力する角度                    
 double Jacbi[4][3][4];//ヤコビアン 脚数×関節×次元
 double a,a0, h,fi;//評価関数内の変数 fi=φ
 double X,tan_u, tan_d;//計算用
 
 //ねじ軸
 //Lin:方向, L0:原点座標, vin:ねじ時に沿った速度, win:角速度ベクトルの大きさ
-double Lin[3], L0[3], vin,v[3],wg[3],win,nol;//ねじ軸条件 
+double Lin[3], L0[3]={0.0,0.0,0.0}, vin,v[3],wg[3],win,nol;//ねじ軸条件 
 double dfdth[4];//評価関数のナブラ
 
 
@@ -93,35 +94,46 @@
 void servo_write(int ch,double ang);//angに
 void servo_write7(int ch, double ang);
 void servo_calib();//全ての角度を0度にする
-
+void servo_ready();//初期状態
 
-//以下9軸センサ関係
 int main()
 { 
     double t;
     pc2.baud(921600);
-    setup_servo();
-    servo_calib();
+    //setup_servo();
+    for(int u=0; u<4; u++)
+    {
+     for(int i=0; i<4; i++)
+     {
+        th[u][i]=th_ready[u][i];
+        }
+    }
+    for(int i=0; i<4; i++)
+     {
+        fwd(i);
+        }
+    //servo_ready();
     wait(2);
-    
-    for(int u=0; u<4; u++)
+    //while(1);
+    /*for(int u=0; u<4; u++)
     {
      for(int i=0; i<4; i++)
      {
         servo_write(ch[u][i],th[u][i]);
         }
-        }
+        }*/
+    //while(1);    
     wait(2);    
     int count = 0;
     //入力したねじ運動を換算する
-    Lin[0] = 0.0; //ねじ軸x
-    Lin[1] = 0.0; //ねじ軸y
-    Lin[2] = 1.0;//ねじ軸z
+    Lin[0] = cos(30.0*PI/180); //ねじ軸x
+    Lin[1] = sin(30.0*PI/180); //ねじ軸y
+    Lin[2] = 0.0;//ねじ軸z
     L0[0] = 0.0;//ねじ軸原点座標
     L0[1] = 0.0;
     L0[2] = 0.0;
     vin = 0.0;
-    win = 3.0;
+    win = 2.0;
     printf("\r\n\r\n");
     nol = (double)sqrt(Lin[0] * Lin[0] + Lin[1] * Lin[1] + Lin[2] * Lin[2]);
     for (int i = 0; i < 3; i++)
@@ -129,52 +141,61 @@
         wg[i] = Lin[i] * win / nol;
         v[i] = Lin[i] * vin / nol;
     }
-    
-    for (int i=0; i < 4; i++) {
-        fwd(i);
-        vp(i);
-    }
     //printf("%lf , %lf , %lf",vP[0](0,0), vP[0](1, 0), vP[0](2, 0));
 
     //times*δtの時間だけサーボを動かす
     tim.start();
+    
     manage.start();
-    for (int i = 0; i < times;i++){
-        manage.reset();
-        count = count + 1;
+    //for (int i = 0; i < times;i++){
+    while(tim.read()<=5.0) {   
+        int l=2;
+        //count = count + 1;
         double dth;
+        //for(int l=0;l<4;l++)
+        //{
             //////////計算部/////////////////
-            fwd(0);
-            vp(0);
-            Jac(0);
-            QR(0);
-            deff(0);
-            dfd(0);
-            ax=-0.5*PI/180.0;bx=-0.49*PI/180.0;cx=0.0;
-            mnbrak(0,2);
-            dth=brent(0,ax,bx,cx,0.0001,2);
-            solve(dth, 0, 1);
+            t=tim.read();
+            fwd(l);
+            pc2.printf("%3.3lf %3.4lf %3.4lf %3.4lf\r\n",t,tip[l][0],tip[l][1],tip[l][2]);
+            vp(l);
+            Jac(l);
+            QR(l);
+            deff(l);
+            dfd(l);
+            ax=-0.15*PI/180.0;bx=0.15*PI/180.0;cx=0.0;
+            //2:谷,1:山
+            mnbrak(l,2);
+            dth=brent(l,ax,bx,cx,0.01,2);
+            solve(dth, l, 1);
             ////////////////////////////////
-            
-            for(int u=0; u<4; u++)
+            ///*
+      //}      
+            /*for(int u=0; u<4; u++)
             {
               for(int i=0; i<4; i++)
               { 
                servo_write(ch[u][i],th[u][i]);
                }
-             }    
-        t=tim.read();
-        pc2.printf("%2.4lf:(%3.3lf, %3.3lf, %3.3lf, %3.3lf)\n\r",t,th[0][0]*180/PI,  th[0][1]*180/PI , th[0][2]*180/PI , th[0][3]*180/PI );
-
+             }//*/    
+        //t=tim.read();
+        //pc2.printf("%2.4lf:( %3.3lf, %3.3lf, %3.3lf, %3.3lf )\n\r",t,th[0][0]*180/PI,  th[1][0]*180/PI , th[2][0]*180/PI , th[3][0]*180/PI );
+        //pc2.printf("%2.4lf:( %3.3lf, %3.3lf, %3.3lf, %3.3lf )\n\r",t,th[0][1]*180/PI,  th[1][1]*180/PI , th[2][1]*180/PI , th[3][1]*180/PI );
+        //pc2.printf("%2.4lf:( %3.3lf, %3.3lf, %3.3lf, %3.3lf )\n\r",t,th[0][2]*180/PI,  th[1][2]*180/PI , th[2][2]*180/PI , th[3][2]*180/PI );
+        //pc2.printf("%2.4lf:( %3.3lf, %3.3lf, %3.3lf, %3.3lf )\n\r",t,th[0][3]*180/PI,  th[1][3]*180/PI , th[2][3]*180/PI , th[3][3]*180/PI );
+        //pc2.printf("%2.4lf:x( %3.3lf, %3.3lf, %3.3lf, %3.3lf )y( %3.3lf, %3.3lf, %3.3lf, %3.3lf )z( %3.3lf, %3.3lf, %3.3lf, %3.3lf )\n\r",
+        //t,tip[0][0],tip[1][0],tip[2][0],tip[3][0],tip[0][1],tip[1][1],tip[2][1],tip[3][1],tip[0][2],tip[1][2],tip[2][2],,tip[3][2]);
+        //pc2.printf("\r\n");
         while(1)
         {
             if(manage.read()>sampling)break;
             }
+        manage.reset();    
             
         }
         t=tim.read();
         wait(3);
-        servo_calib();
+        servo_ready();
     return 0;               // ソフトの終了 
 }
 
@@ -229,7 +250,7 @@
 
 void vp(int leg) {//5年生の時に作成したもの
     double crosx, crosy, crosz;
-    double wA[3] = { (double)(-wg[0] * PI / 180.0),(double)(-wg[1] * PI / 180.0),(double)(-wg[2] * PI / 180.0) };
+    double wA[3] = { (double)(wg[0] * PI / 180.0),(double)(wg[1] * PI / 180.0),(double)(wg[2] * PI / 180.0) };
     double vA[3] = { (-v[0]),(-v[1]) ,(-v[2]) };
     double AP[3] = { (tip[leg][0] - L0[0]),(tip[leg][1] - L0[1]),tip[leg][2] - L0[2] };
     if (Lin[2] != 0.0)
@@ -254,7 +275,7 @@
     tip[leg][0] = (L[3]+L[0] * c1 + L[1] * c12 + L[2]*c123) * c0 + con[leg][0];  //x
     tip[leg][1] = (L[3]+L[0] * c1 + L[1] * c12 + L[2] * c123) * s0 + con[leg][1];  //y
     tip[leg][2] = L[0] * s1 + L[1] * s12+L[2]*s123; //z
-     //printf("fwd finish\n");
+    //pc2.printf("leg=%d,(  %3.3lf,  %3.3lf,  %3.3lf))  ",leg,tip[leg][0],tip[leg][1],tip[leg][2]);
 }
 void Jac(int leg) {
     //printf("Jac start\n");
@@ -590,7 +611,6 @@
             th0[leg][u] = dth[u];
         }
     }
-    else {pc2.printf("value det　in function solve is incorrect\r\n");}
             
 }
 
@@ -625,3 +645,13 @@
         }
      }
 }
+void servo_ready()
+{
+    for(int u=0; u<4; u++)
+            {
+              for(int i=0; i<4; i++)
+              { 
+               servo_write(ch[u][i],th_ready[u][i]);
+               }
+             }
+    }