File content as of revision 1:507dd5045511:

//4dofscrewをもとに姿勢制御を行う
//9軸センサの値からroll,pitchを算出しさらにロドリゲスの回転公式から1軸中心の回転に変換
//回転軸の方向に姿勢角を打ち消すようにねじ運動を入力する
//ポテンショメータから取得した値を元に次の角度増分を計算する
#include "mbed.h"
#include <PCA9685.h> 
#include "Eigen/Geometry.h"
#include "Eigen/Dense.h"
#include <math.h>


#define PI 3.14159265358979323846264338327950288

//最適計算関係
#define ITMAX 100     //brent法繰り返し回数
#define GLIMIT 100.0  
#define CGOLD 0.3819660
#define SHIFT(a,b,c,d) (a)=(b);(b)=(c);(c)=(d);
#define ZEPS 1.0e-10
#define GOLD 1.618034
#define TINY 1.0e-20

//サーボ関係
#define SERVOMIN 700       
#define SERVOMAX 2300
#define SERVOGAIN 29.6296300

//9軸センサ関係
//mbedではアドレスの指定が7bitなので1つずつずらす
// BMX055　加速度センサのI2Cアドレス  
#define Addr_Accl (0x19<<1)  // (JP1,JP2,JP3 = Openの時)
// BMX055　ジャイロセンサのI2Cアドレス
#define Addr_Gyro (0x69<<1)  // (JP1,JP2,JP3 = Openの時)
// BMX055　磁気センサのI2Cアドレス
#define Addr_Mag (0x13<<1)   // (JP1,JP2,JP3 = Openの時)

#define GYROGAIN 0.0
#define ACCELGAIN 1.0

////////////////////////////////////////場合に応じて変更する変数///////////////////////////////////////
I2C i2c_BMX(p9,p10);
double sampling=0.01;//δtの時間[s]
int times= 100;//実行回数:実行時間は (sampling)*(times)秒                      
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} };//初期状態                      
///////////////////////////////////////////////////////////////////////////////////////////////////////

PCA9685 pwm;//クラス宣言
Serial pc2(USBTX,USBRX);
Timer tim;//実行総時間の確認用
Timer manage;//samplingを保証するためのタイマー
DigitalOut trig1(p27);
DigitalOut trig2(p28);

//ポテンショメータ用 
DigitalOut sig1(p5);
DigitalOut sig2(p6);
DigitalOut sig3(p7);
DigitalOut sig4(p8);
AnalogIn data(p15);

using namespace Eigen;

//以下変数定義

//brentとmnbrakに必要な変数
double ax,bx,cx;
double fa,fb,fc;

//サーボの書き込みに必要な変数
double servo0[16]={7000.0, 5850.0, 7050.0, 7200.0,
                   6000.0, 6300.0, 5100.0, 6100.0, 
                   6700.0, 7000.0, 5650.0, 6000.0, 
                   6000.0, 5100.0, 5600.0, 6670.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 L[4] = {65.0,35.0,110.0,38.0};//4本のリンク長 後から足したのでL[3]を理論中のL0に対応させる
double tip[4][3];//足先座標
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 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],th_write[4][4];//計算用のθと書き込み用のθを別々に用意する                    
double Jacbi[4][3][4];//ヤコビアン 脚数×関節×次元
double a,a0, h,fi;//評価関数内の変数 fi=φ
double X,tan_u, tan_d;//計算用
double dfdth[4];//評価関数のナブラ

//ねじ軸
//Lin:方向, L0:原点座標, vin:ねじ時に沿った速度, win:角速度ベクトルの大きさ
double Lin[3], L0[3]={0.0,0.0,0.0}, vin,v[3],wg[3],win,nol;//ねじ軸条件
int leg_upperside[2],leg_lowerside[2]; 

//9軸センサ関係
// センサーの値を保存するグローバル関数
float xAccl = 0.00;
float yAccl = 0.00;
float zAccl = 0.00;
float xGyro = 0.00;
float yGyro = 0.00;
float zGyro = 0.00;
int   xMag  = 0;
int   yMag  = 0;
int   zMag  = 0;

//姿勢角検出用の各パラメータの定義
  int count=0;
  double roll=0.0,pitch=0.0,yaw=0.0;//姿勢角
  double prfr=0.0,prfp=0.0,prfy=0.0;//修正した姿勢角
  double sum_roll[10]={0.0},sum_pitch[10]={0.0};//移動平均用の配列
  double accro,accpi;//加速度センサの値から算出した姿勢角(ロール、ピッチ)
  double dx,dy,dz;
  double magdx,magdy,magdz;
  double gydx=0.0,gydy=0.0,gydz=0.0;//角速度センサの微小時間の変化
  double gydro=0.0,gydpi=0.0,gydya=0.0;//角速度センサから求めた微小時間の姿勢の変化
  double gyro_off[3];//角速度センサオフセット保存用変数
  double theta_rod,theta_rod_1=0.0,nx_rod,ny_rod,nz_rod;//ロドリゲスの回転公式のための4変数
  double dedt;//偏差の微分

//ポテンショメータ
double data0[16]={0};//初期状態の角度
double s[16][10]={{0},{0},{0},{0},  {0},{0},{0},{0},  {0},{0},{0},{0},  {0},{0},{0},{0}};
int chPin[16][4]={{0,0,0,0},//ch0
{1,0,0,0},//ch1
{0,1,0,0},//ch2
{1,1,0,0},//ch3
{0,0,1,0},//ch4
{1,0,1,0},//ch5
{0,1,1,0},//ch6
{1,1,1,0},//ch7
{0,0,0,1},//ch8
{1,0,0,1},//ch9
{0,1,0,1},//ch10
{1,1,0,1},//ch11
{0,0,1,1},//ch12
{1,0,1,1},//ch13
{0,1,1,1},//ch14
{1,1,1,1}};//ch15

//以下行列定義
MatrixXd Q(3, 3);//Q行列
MatrixXd R(3, 4);//R行列
Vector3d vP[4];//各脚の速度ベクトル

void define_upper_lower();//9軸センサの値から斜面上側の脚と下側の脚を判定する
void QR(int leg);//QR分解用関数,引数は脚番号
void vp(int leg);//引数は脚番号,与条件から各脚先の速度を導出する
void fwd(int leg);//順運動学より脚先の座標を導出する
void Jac(int leg);//指定した脚のヤコビアンを計算
void solve(double w3,int leg,int det);//theta3の角速度から全関節の関節角度を導き出す

void deff(int leg);//評価関数計算, legは距離と傾きから指定する
void dfd( int leg);//評価関数の勾配をとる
double fe(int leg,double dth3);//brent法に合わせてeを関数化
double f(int leg,double dth3);//テーラー展開第1項の値を返す, brent法用
double num_nolm(int leg , double dth3);//ノルム最小の解を導く際に使用する関数

void mnbrak(int leg,int discrimination);//brentに必要な極小値の囲い込んだ3点を決定する関数
double brent(int leg,double min,double mid,double max,double tol,int discrimination);//brent法により1次元探索するプログラム 
//discrimination 0:谷側(fe), 1:山側(nolm), 2:谷側(f),3:テスト用の関数(f_test)
double SIGN(double x,double y);//xにyの符号をつけたものを返す
double FMAX(double x,double y);//大きいほうの値が返される
double f_test(double x);//テスト用の関数

//以下サーボ関係
void setup_servo();//サーボセットアップ用関数
void servo_write(int ch,double ang);//angに
void servo_write7(int ch, double ang);
void servo_calib();//全ての角度を0度にする
void servo_ready();//初期状態


//以下9軸センサ関係
void BMX055_Init();//9軸センサ設定用関数
void BMX055_Accl();//加速度の値を得る関数
void BMX055_Gyro();//角速度の値を得る関数
void BMX055_attitude();//BMX055の値からroll,potchを計算する
void make_motion();//ロドリゲスの式をもとにroll,pitchを1軸周りの回転に変換し,ねじ運動を作り出す

//ポテンショメータ
double readarg(int ch,double s[16][10]);//取得するchと移動平均用の配列
void setup_th();
void update_th();
void th_calib();

int main()
{ 
    double t=0.0;
    int dum;
    //各種設定を行う//
    pc2.baud(921600);
    trig1=0;
    trig2=0;   
    setup_servo();
    setup_th();
    BMX055_Init(); 
    //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");
    while(1)
    {if(pc2.readable()==0)dum=pc2.getc();break;}
    //th_calib();
    pc2.printf("th_calib finished\r\n");
    
    wait(2);
    pc2.printf("attitude control will start\r\n");
    pc2.printf("press keyboad to start\r\n");
    while(1)
    {if(pc2.readable()==0)dum=pc2.getc();break;}
    pc2.printf("start\r\n");
    tim.start();
    trig1=1;
    trig2=1;    
    tim.start();
    //ホワイトノイズ計測用ループ
    /*while(1)
    {
        t=tim.read();
        BMX055_attitude();
        pc2.printf("%2.3lf %3.3lf %3.3lf \n\r",t,prfr*180/PI ,prfp*180/PI);
        wait(0.01);
        }*/
      
    //以下姿勢制御ロープ///  
    //while(1){
    while(tim.read()<30.0){      
    double dth;    
    //update_th();    
    manage.start();    
    make_motion();//各rodの値を更新
    define_upper_lower();//脚の上下関係を計算
        
    //ねじ運動を生成する
    Lin[0] = nx_rod; //ねじ軸x
    Lin[1] = ny_rod; //ねじ軸y
    Lin[2] = nz_rod;//ねじ軸z
    //win = -theta_rod*110;//p制御のとき
    win = -theta_rod*110+dedt*0.1;
    if(theta_rod*180/PI>1.0)
    {
    nol = (double)sqrt(Lin[0] * Lin[0] + Lin[1] * Lin[1] + Lin[2] * Lin[2]);
    for (int i = 0; i < 3; i++)
    {
        wg[i] = Lin[i] * win / nol;
        v[i] = 0.0;
    }
    
    
    //谷側の脚の計算
    for (int i = 0; i < 2;i++){  
            fwd(leg_lowerside[i]);
            vp(leg_lowerside[i]);
            Jac(leg_lowerside[i]);
            QR(leg_lowerside[i]);
            deff(leg_lowerside[i]);
            dfd(leg_lowerside[i]);
            ax=-0.0873*PI/180.0;bx=0.0873*PI/180.0;cx=0.0;
            mnbrak(leg_lowerside[i],2);
            dth=brent(leg_lowerside[i],ax,bx,cx,0.0001,2);
            solve(dth,leg_lowerside[i],1);
     }
     //山側の脚の計算
     for (int i = 0; i < 2;i++){  
            fwd(leg_upperside[i]);
            vp(leg_upperside[i]);
            Jac(leg_upperside[i]);
            QR(leg_upperside[i]);
            deff(leg_upperside[i]);
            ax=-0.0873*PI/180.0;bx=0.0873*PI/180.0;cx=0.0;
            mnbrak(leg_upperside[i],1);
            dth=brent(leg_upperside[i],ax,bx,cx,0.0001,1);
            solve(dth, leg_upperside[i], 1);
     }
     }         
            ///* 
            //計算結果をサーボに入力
            
            for(int u=0; u<4; u++)
            {
              for(int i=0; i<4; i++)
              { 
               //servo_write(ch[u][i],th_write[u][i]);
               servo_write(ch[u][i],th[u][i]);
               }
             }
             //*/    
        t=tim.read();
        pc2.printf("%2.3lf %3.3lf %3.3lf \n\r",t,prfr*180/PI ,prfp*180/PI);

        while(1)
        {
            if(manage.read()>=sampling)break;
            }
            
        
        manage.reset();
      }  
        trig1=0;
        trig2=0;
        wait(3);
        
    return 0;               // ソフトの終了 
}

void define_upper_lower()
{
    double n_angle;
    n_angle=atan2(ny_rod,nx_rod);//返り値は-PI~PI
    if((-PI/4 < n_angle)&&(PI/4 > n_angle))
    {
     leg_upperside[0]=0;
     leg_upperside[1]=1;
     leg_lowerside[0]=2;
     leg_lowerside[1]=3;
    }
    if((PI/4 < n_angle)&&(3*PI/4 > n_angle))
    {
     leg_upperside[0]=0;
     leg_upperside[1]=3;
     leg_lowerside[0]=1;
     leg_lowerside[1]=2;
    }
    if((-3*PI/4 < n_angle)&&(-PI/4 < n_angle))
    {
     leg_upperside[0]=1;
     leg_upperside[1]=2;
     leg_lowerside[0]=0;
     leg_lowerside[1]=3;
    }
    if(((-PI > n_angle)&&(-3*PI/4 < n_angle)) || ((PI > n_angle)&&(3*PI/4 < n_angle)))
    {
     leg_upperside[0]=2;
     leg_upperside[1]=3;
     leg_lowerside[0]=0;
     leg_lowerside[1]=1;
    }
    }
void QR(int leg) {
    double s, t;//要素計算用
    MatrixXd ma(3, 4), ma1(3, 4);
    
    ma << Jacbi[leg][0][0], Jacbi[leg][0][1], Jacbi[leg][0][2], Jacbi[leg][0][3],
        Jacbi[leg][1][0], Jacbi[leg][1][1], Jacbi[leg][1][2], Jacbi[leg][1][3],
        Jacbi[leg][2][0], Jacbi[leg][2][1], Jacbi[leg][2][2], Jacbi[leg][2][3];
    
    //ハウスホルダー変換1回目
    MatrixXd A1(3, 3);
    A1 << 1.0, 0.0, 0.0,
        0.0, 1.0, 0.0,
        0.0, 0.0, 1.0;

    s = (double)sqrt(ma(0, 0) * ma(0, 0) + ma(1, 0) * ma(1, 0) + ma(2, 0) * ma(2, 0));//分母のやつ

    MatrixXd H1(3, 3);//1回目の行列
    MatrixXd X11(3, 1), X12(1, 3);
    Vector3d a11, a12;//a11が変換前,a12が変換後
    a11 << ma(0, 0), ma(1, 0), ma(2, 0);
    a12 << s, 0.0, 0.0;
    X11 = a11 - a12;
    X12 = X11.transpose();
    t = (double)sqrt(X11(0, 0) * X11(0, 0) + X11(1, 0) * X11(1, 0) + X11(2, 0) * X11(2, 0));
    H1 = A1 - 2.0 * (X11 * X12) / (t * t);
    ma1 = H1 * ma;
    //2回目
    MatrixXd H2(3, 3), A2(2, 2), h2(2, 2);
    A2 << 1.0, 0.0,
        0.0, 1.0;
    Vector2d a21, a22;
    MatrixXd X21(2, 1), X22(1, 2);
    a21 << ma1(1, 1), ma1(2, 1);
    s = (double)sqrt(ma1(1, 1) * ma1(1, 1) + ma1(2, 1) * ma1(2, 1));
    a22 << s, 0;
    X21 = a21 - a22;
    X22 = X21.transpose();
    t = (double)sqrt(X21(0, 0) * X21(0, 0) + X21(1, 0) * X21(1, 0));
    h2 = A2 - 2 * (X21 * X22) / (t * t);
    H2 << 1.0, 0.0, 0.0,
        0.0, h2(0, 0), h2(0, 1),
        0.0, h2(1, 0), h2(1, 1);
    R = H2 * ma1;
    MatrixXd H1T(3, 3), H2T(3, 3);
    H1T = H1.transpose();
    H2T = H2.transpose();
    Q = H1T * H2T;    
}

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)
    {
        double LP[3] = { -(Lin[0] / nol) / (Lin[2] / nol) * tip[leg][2],-(Lin[1] / nol) / (Lin[2] / nol) * tip[leg][2],0.0 };
        for (int i = 0; i < 3; i++) { AP[i] = AP[i] - LP[i]; }
        AP[2] = 0.0;
    }
    crosx = AP[1] * wA[2] + (-AP[2]) * wA[1];
    crosy = AP[2] * wA[0] + (-AP[0]) * wA[2];
    crosz = AP[0] * wA[1] + (-AP[1]) * wA[0];
    vP[leg] << crosx + vA[0], crosy + vA[1], crosz + vA[2];
    //printf(" %lf,%lf,%lf\n", -v[0], -v[1], -v[2]);
    //pc2.printf("input motion %d %lf,%lf,%lf\n\r", leg, vP[leg](0, 0), vP[leg](1, 0), vP[leg](2, 0));
    //printf("vp finish\n");
}
void fwd(int leg) {
    //printf("fwd start\n");
    double c0 = (double)cos(th[leg][0]), s0 = (double)sin(th[leg][0]), c1 = (double)cos(th[leg][1]), s1 = (double)sin(th[leg][1]),
        c12 = (double)cos(th[leg][1] + th[leg][2]), s12 = (double)sin(th[leg][1] + th[leg][2]), c123 = (double)cos(th[leg][1] + th[leg][2] + th[leg][3]),
        s123 = (double)sin(th[leg][1] + th[leg][2] + th[leg][3]);
    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");
}
void Jac(int leg) {
    //printf("Jac start\n");
    double c0 = (double)cos(th[leg][0]), s0 = (double)sin(th[leg][0]), c1 = (double)cos(th[leg][1]), s1 = (double)sin(th[leg][1]),
        c12 = (double)cos(th[leg][1] + th[leg][2]), s12 = (double)sin(th[leg][1] + th[leg][2]), c123 = (double)cos(th[leg][1] + th[leg][2] + th[leg][3]),
        s123 = (double)sin(th[leg][1] + th[leg][2] + th[leg][3]);
    Jacbi[leg][0][0] = -s0 * (L[3]+L[0] * c1 + L[1] * c12 + L[2] * c123);
    Jacbi[leg][0][1] = (-L[0] * s1 - L[1] * s12 - L[2] * s123) * c0;
    Jacbi[leg][0][2] = (-L[1] * s12 - L[2] * s123) * c0;
    Jacbi[leg][0][3] = (-L[2] * s123) * c0;

    Jacbi[leg][1][0] = c0 * (L[3]+L[0] * c1 + L[1] * c12 + L[2] * c123);
    Jacbi[leg][1][1] = (-L[0] * s1 - L[1] * s12 - L[2] * s123) * s0;
    Jacbi[leg][1][2] = (-L[1] * s12 - L[2] * s123) * s0;
    Jacbi[leg][1][3] = (-L[2] * s123) * s0;

    Jacbi[leg][2][0] = 0.0;
    Jacbi[leg][2][1] = L[0] * c1 + L[1] * c12 + L[2] * c123;
    Jacbi[leg][2][2] = L[1] * c12 + L[2] * c123;
    Jacbi[leg][2][3] = L[2] * c123;


    //printf("Jac finish\n");
}//ok
void deff(int leg) {
    //printf(" 評価関数定義\n");
    fi = r  + atan2(-tip[leg][2], (double)sqrt((tip[leg][0]) * (tip[leg][0]) + (tip[leg][1]) * (tip[leg][1])));//y,xの順
    a0 = (double)sqrt((tip[leg][0]) * (tip[leg][0]) + (tip[leg][1]) * (tip[leg][1]) + (tip[leg][2]) * (tip[leg][2]));
    a = a0 * (double)cos(fi);
    h = a * (1 / (double)cos(fi) - tan(fi));
    X = tip[leg][2]*(double)sqrt((tip[leg][0]* (tip[leg][0])) + (tip[leg][1]) * (tip[leg][1]));//tan-1の中身
    //tan-1の分母分子
    tan_u = tip[leg][2];
    tan_d = (double)sqrt((tip[leg][0]) * (tip[leg][0]) + (tip[leg][1]) * (tip[leg][1]));
    //printf("評価関数計算完了\n");
}
void dfd(int leg) {
    //printf("評価関数微分\n");
    double c0 = (double)cos(th[leg][0]), s0 = (double)sin(th[leg][0]), c1 = (double)cos(th[leg][1]), s1 = (double)sin(th[leg][1]), s2 = (double)sin(th[leg][2]), s3 = (double)sin(th[leg][2]);
    double c12 = (double)cos(th[leg][1] + th[leg][2]), s12 = (double)sin(th[leg][1] + th[leg][2]), s23 = (double)sin(th[leg][2] + th[leg][3]), c23 = (double)cos(th[leg][2] + th[leg][3]);
    double c123 = (double)cos(th[leg][1] + th[leg][2] + th[leg][3]), s123 = (double)sin(th[leg][1] + th[leg][2] + th[leg][3]);
    double cfi=cos(fi),sfi=sin(fi);
    double x=tip[leg][0],y=tip[leg][1],z=tip[leg][2];
    
    double df_da=1/cfi-tan(fi);
    double df_dfi=a*(-sfi-1)/(cfi*cfi);
    double da_dx=x*cfi/sqrt(x*x+y*y);
    double da_dy=y*cfi/sqrt(x*x+y*y);
    double da_dfi=-sqrt(x*x+y*y)*sfi;
    double dfi_dx=-x*z/((x*x+y*y+z*z)*sqrt(x*x+y*y));
    double dfi_dy=-y*z/((x*x+y*y+z*z)*sqrt(x*x+y*y));
    double dfi_dz=sqrt(x*x+y*y)*z/(x*x+y*y+z*z);
   
    dfdth[0]=df_da*(da_dx*Jacbi[leg][0][0]+da_dy*Jacbi[leg][1][0]+da_dfi*(dfi_dx*Jacbi[leg][0][0]+dfi_dy*Jacbi[leg][1][0]+dfi_dz*Jacbi[leg][2][0]))
             +df_dfi*(dfi_dx*Jacbi[leg][0][0]+dfi_dy*Jacbi[leg][1][0]+dfi_dz*Jacbi[leg][2][0]); 
             
    dfdth[1]=df_da*(da_dx*Jacbi[leg][0][1]+da_dy*Jacbi[leg][1][1]+da_dfi*(dfi_dx*Jacbi[leg][0][1]+dfi_dy*Jacbi[leg][1][1]+dfi_dz*Jacbi[leg][2][1]))
             +df_dfi*(dfi_dx*Jacbi[leg][0][1]+dfi_dy*Jacbi[leg][1][1]+dfi_dz*Jacbi[leg][2][1]);
              
    dfdth[2]=df_da*(da_dx*Jacbi[leg][0][2]+da_dy*Jacbi[leg][1][2]+da_dfi*(dfi_dx*Jacbi[leg][0][2]+dfi_dy*Jacbi[leg][1][2]+dfi_dz*Jacbi[leg][2][2]))
             +df_dfi*(dfi_dx*Jacbi[leg][0][2]+dfi_dy*Jacbi[leg][1][2]+dfi_dz*Jacbi[leg][2][2]); 
    
    dfdth[3]=df_da*(da_dx*Jacbi[leg][0][3]+da_dy*Jacbi[leg][1][3]+da_dfi*(dfi_dx*Jacbi[leg][0][3]+dfi_dy*Jacbi[leg][1][3]+dfi_dz*Jacbi[leg][2][3]))
             +df_dfi*(dfi_dx*Jacbi[leg][0][3]+dfi_dy*Jacbi[leg][1][3]+dfi_dz*Jacbi[leg][2][3]);                              
   
   //pc2.printf("df_da=%lf  df_dfi=%lf  da_dx=%lf  da_dy=%lf  da_dfi=%lf  dfi_dx=%lf  dfi_dy=%lf  dfi_dz=%lf\r\n",df_da,df_dfi,da_dx,da_dy,da_dfi,dfi_dx,dfi_dy,dfi_dz);  
}
//使わない
double fe(int leg,double dth3) {
    //brent法のための関数, 事前にdfdを実行してから使う
    double dfd_nolm,th0_nolm,e=0.0;
    //∇hを正規化する
    dfd(leg);
    dfd_nolm = sqrt(dfdth[0]* dfdth[0]+ dfdth[1]* dfdth[1]+ dfdth[2]* dfdth[2]+ dfdth[3]* dfdth[3]);
    for (int i = 0; i < 4; i++) {
        dfdth[i]=dfdth[i]/dfd_nolm;
    }
    
    solve(dth3, leg, 2);//後退代入でほかの3つのパラメータを導出
    
    //dthベクトルを正規化
    th0_nolm = sqrt(th0[leg][0] * th0[leg][0]+ th0[leg][1]* th0[leg][1]+ th0[leg][2]* th0[leg][2]+ th0[leg][3]*th0[leg][3]);
    for (int i = 0; i < 4; i++) {
        th0[leg][i] = th0[leg][i] / th0_nolm;
    }
    for (int i = 0; i < 4; i++) {
         e += (th0[leg][i] - dfdth[i]) * (th0[leg][i] - dfdth[i]);
    }    
    return e;//eベクトルのノルムの2乗を返す

    }
double f(int leg,double dth3) {
    double f_return=0.0;
    solve(dth3, leg, 2);//後退代入でほかの3つのパラメータを導出
    f_return=dfdth[0]*th0[leg][0]+dfdth[1]*th0[leg][1]+dfdth[2]*th0[leg][2]+dfdth[3]*th0[leg][3];   
    
    return -f_return;//テイラー展開第二項を返すがbrent法は極小値を求めるため、符号を反転させる

    } 
void mnbrak(int leg,int discrimination)
{
    double ulim,u,r,q,fu,fu_2,dum,fa,fb,fc;
    //fa=f(ax);
    //fb=f(bx);
    if(discrimination==0){fa=fe(leg,ax);fb=fe(leg,bx);}
    if(discrimination==1){fa=num_nolm(leg,ax);fb=num_nolm(leg,bx);}
    if(discrimination==2){fa=f(leg,ax);fb=f(leg,bx);}
    if(discrimination==3){fa=f_test(ax);fb=f_test(bx);}
    
    if(fb>fa)
    {
        SHIFT(dum,ax,bx,dum);
        SHIFT(dum,fb,fa,dum);
        }
    cx=bx+GOLD*(bx-ax);
    //fc=f(cx);
    if(discrimination==0){fc=fe(leg,cx);}
    if(discrimination==1){fc=num_nolm(leg,cx);}
    if(discrimination==2){fc=f(leg,cx);}
    if(discrimination==3){fc=f_test(cx);}
    while (fb>fc)
    {
        r=(bx-ax)*(fb-fc);
        q=(bx-cx)*(fb-fa);
        u=bx-((bx-cx)*q-(bx-cx)*r)/
        (2.0*SIGN(FMAX(fabs(q-r),TINY),q-r));
        ulim=bx+GLIMIT*(cx-bx);
        
        if((bx-u)*(u-cx)>0.0)
        {
            //fu=f(u);
            if(discrimination==0){fu=fe(leg,u);}
            if(discrimination==1){fu=num_nolm(leg,u);}
            if(discrimination==2){fu=f(leg,u);}
            if(discrimination==3){fu=f_test(u);}
            if(fu<fc)
            {
                ax=bx;
                bx=u;
                fa=fb;
                fb=fu;
                return;
                }
            else if(fu>fb)
            {
                cx=u;
                fc=fu;
                return;
                }
             u=cx*+GOLD*(cx-bx);
             //fu=f(u);
             if(discrimination==0){fu=fe(leg,u);}
             if(discrimination==1){fu=num_nolm(leg,u);}
             if(discrimination==2){fu=f(leg,u);}
             if(discrimination==3){fu=f_test(u);}       
            }
        else if((cx-u)*(u-ulim)>0.0)
        {
            //fu=f(u);
            if(discrimination==0){fu=fe(leg,u);}
            if(discrimination==1){fu=num_nolm(leg,u);}
            if(discrimination==2){fu=f(leg,u);}
            if(discrimination==3){fu=f_test(u);}
            if(fu<fc)
            {
                SHIFT(bx,cx,u,cx+GOLD*(cx-bx));
                if(discrimination==0){fu_2=fe(leg,u);}
                if(discrimination==1){fu_2=num_nolm(leg,u);}
                if(discrimination==2){fu_2=f(leg,u);}
                if(discrimination==3){fu_2=f_test(u);}
                //SHIFT(fb,fc,fu,f(u));
                SHIFT(fb,fc,fu,fu_2);
                }
            
            }
        else if((u-ulim)*(ulim-cx)>=0.0)
        {
            u=ulim;
            //fu=f(u);
            if(discrimination==0){fu=fe(leg,u);}
            if(discrimination==1){fu=num_nolm(leg,u);}
            if(discrimination==2){fu=f(leg,u);}
            if(discrimination==3){fu=f_test(u);}
            }
        else
        {
            u=cx+GOLD*(cx-bx);
            //fu=f(u);
            if(discrimination==0){fu=fe(leg,u);}
            if(discrimination==1){fu=num_nolm(leg,u);}
            if(discrimination==2){fu=f(leg,u);}
            if(discrimination==3){fu=f_test(u);}
            }
        SHIFT(ax,bx,cx,u);
        SHIFT(fa,fb,fc,fu);
        }
    }
double brent(int leg,double min,double mid,double max,double tol,int discrimination)//成功したやつ
{

    int iter;
    double a,b,d,etemp,fu,fv,fw,fx,p,q,r,tol1,tol2,u,v,w,x,xm,xmin;
    double e=0.0;
    a=(ax <cx ? ax : cx);
    b=(ax >cx ? ax : cx);
    x=w=v=bx;
    if(discrimination==0){fw=fv=fx=fe(leg,x);}
    if(discrimination==1){fw=fv=fx=num_nolm(leg,x);}
    if(discrimination==2){fw=fv=fx=f(leg,x);}
    if(discrimination==3){fw=fv=fx=f_test(x);}
    //fw=fv=fx=f(x);//関数部分
    for(iter=1;iter<=ITMAX;iter++)
    {
        xm=0.5*(a+b);
        tol2=2.0*(tol1=tol*fabs(x)+ZEPS);
        //pc2.printf("x =%lf,w = %lf,u = %lf\n\r",x,w,u);
        if(fabs(x-xm)<=(tol2-0.5*(b-a)))
        {
            //pc.printf("bernt out");
            xmin=x;
             //pc2.printf("xmin=%lf\r\n",x);
             //pc2.printf("fx=%lf\r\n",fx);
            return xmin;
        }
        if(fabs(e)>tol1)
        {
            r=(x-w)*(fx-fv);
            q=(x-v)*(fx-fw);
            p=(x-v)*q-(x-w)*r;
            q=2.0*(q-r);
            if(q>0.0)p=-p;
            q=fabs(q);
            etemp=e;
            e=d;
            if(fabs(p)>=fabs(0.5*q*etemp)||p<=q*(a-x)||p>=q*(b-x))
            {  d=CGOLD*(e= (x>=xm ? a-x : b-x));}
            else
            {
               d=p/q;
               u=x+d;
               if(u-a < tol2 || b-u < tol2)
               {d=SIGN(tol1,xm-x);}
            }
        }
        else
        {
            d=CGOLD*(e= (x>=xm ? a-x : b-x));
            }    
        u=(fabs(d) >= tol1 ? x+d : x+SIGN(tol1,d));
        if(discrimination==0){fu=fe(leg,u);}
        if(discrimination==1){fu=num_nolm(leg,u);}
        if(discrimination==2){fu=f(leg,u);}
        if(discrimination==3){fu=f_test(u);}
        //fu=f(u);//関数部分
         if(fu <= fx)
         {
            if(u >= x)a=x; else b=x;
            SHIFT(v,w,x,u);
            SHIFT(fv,fw,fx,fu);
          }
         else{
          if(u < x){a=u;} 
          else {b=u;}
          if(fu <= fw || w==x)
          {
              v=w;
              w=u;
              fv=fw;
              fw=fu;
              }    
          else if (fu <= fv || v==x || v==w)
          {
            v=u;
            fv=fu;
          }
        }
        
        }
        //pc2.printf("xmin=%lf\r\n",x);
        //pc2.printf("fx=%lf\r\n",fx);
    return xmin;
    }           

double SIGN(double x,double y)
{
    double x_return;
    x_return=abs(x);
    if(y<0.0)x_return=-x_return;
    
    return x_return; 
    }
double FMAX(double x, double y)
{
    if(x>y){return x;}
    if(y>x){return y;}
    return 0;
    }      
double num_nolm(int leg,double dth3)
{
    
    double nolm_return=0.0;
    solve(dth3,leg,2);
    nolm_return=th0[leg][0]*th0[leg][0]+th0[leg][1]*th0[leg][1]+th0[leg][2]*th0[leg][2]+th0[leg][3]*th0[leg][3];
    
    return nolm_return;    
    }
//brent法のテスト用の関数    
//極小値を求めたい関数を定義    
double f_test(double x){
    double x_return;
    x_return=x*x-2*x+1;
    return x_return;
    }              
void solve(double w3, int leg,int det) {
   
    double dth[4];
    MatrixXd v_Q(3,1),QT(3,3);

    QT = Q.transpose();
    v_Q = QT * vP[leg]*sampling;
   
    dth[3] = w3 ;
    dth[2] = (double)((v_Q(2, 0) - R(2, 3) * dth[3]) / R(2, 2));
    dth[1] = (double)((v_Q(1, 0) - R(1, 2) * dth[2] - R(1, 3) * dth[3]) / R(1, 1));
    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[leg][i] = th[leg][i] + dth[i];
        }
    }
    else if (det == 2) {
        for (int u=0; u < 4; u++) {
            th0[leg][u] = dth[u];
        }
    }
    
}

//以下9軸センサ関係
void BMX055_Init()   //range 設定用関数
{
  char cmd[3];
  //------------------------------------------------------------//
  cmd[0]=0x0F;
  cmd[1]=0x03;
  i2c_BMX.write(Addr_Accl,cmd,2,0);
  wait_ms(100);
 //------------------------------------------------------------//
  cmd[0]=0x10;
  cmd[1]=0x08;
  i2c_BMX.write(Addr_Accl,cmd,2,0);
  wait_ms(100);
  //------------------------------------------------------------//
  cmd[0]=0x11;
  cmd[1]=0x00;
  i2c_BMX.write(Addr_Accl,cmd,2,0);
  wait_ms(100);
 //------------------------------------------------------------//
  cmd[0]=0x0F;
  cmd[1]=0x04;
  i2c_BMX.write(Addr_Accl,cmd,2,0);
  wait_ms(100);
 //------------------------------------------------------------//
  cmd[0]=0x10;
  cmd[1]=0x07;
  i2c_BMX.write(Addr_Gyro,cmd,2,0);
  wait_ms(100);
 //------------------------------------------------------------//
  cmd[0]=0x11;
  cmd[1]=0x00;
  i2c_BMX.write(Addr_Gyro,cmd,2,0);
  wait_ms(100);
 //------------------------------------------------------------//
  cmd[0]=0x4B;
  cmd[1]=0x83;
  i2c_BMX.write(Addr_Mag,cmd,2,0);
  wait_ms(100);
  //------------------------------------------------------------//
  cmd[0]=0x4B;
  cmd[1]=0x01;
  i2c_BMX.write(Addr_Mag,cmd,2,0);
  wait_ms(100);
  //------------------------------------------------------------//
  cmd[0]=0x4C;
  cmd[1]=0x00;
  i2c_BMX.write(Addr_Mag,cmd,2,0);
  wait_ms(100);
 //------------------------------------------------------------//
  cmd[0]=0x4E;
  cmd[1]=0x84;
  i2c_BMX.write(Addr_Mag,cmd,2,0);
  wait_ms(100);
 //------------------------------------------------------------//
  cmd[0]=0x51;
  cmd[1]=0x04;
  i2c_BMX.write(Addr_Mag,cmd,2,0);
  wait_ms(100);
 //------------------------------------------------------------//
  cmd[0]=0x52;
  cmd[1]=0x16;
  i2c_BMX.write(Addr_Mag,cmd,2,0);
  wait_ms(100);
}
void BMX055_Accl()
{
  char cmd[1];
  char rtn[1];
  int data[6];
  for (int i = 0; i < 6; i++)
  {
    cmd[0]=2+i;                  //Wire.beginTransmission(Addr_Accl);
    i2c_BMX.write(Addr_Accl,cmd,1,1);  //Wire.write((2 + i));// Select data register
                                 //Wire.endTransmission();
    i2c_BMX.read(Addr_Accl,rtn,1,0);   //Wire.requestFrom(Addr_Accl, 1);// Request 1 byte of data
    data[i]=rtn[0];
  }
  // Convert the data to 12-bits
  xAccl = ((data[1] * 256) + (data[0] & 0xF0)) / 16;
  if (xAccl > 2047)  xAccl -= 4096;
  yAccl = ((data[3] * 256) + (data[2] & 0xF0)) / 16;
  if (yAccl > 2047)  yAccl -= 4096;
  zAccl = ((data[5] * 256) + (data[4] & 0xF0)) / 16;
  if (zAccl > 2047)  zAccl -= 4096;
  xAccl = xAccl * 0.0098; // renge +-2g
  yAccl = yAccl * 0.0098; // renge +-2g
  zAccl = zAccl * 0.0098; // renge +-2g
}
//=====================================================================================//
void BMX055_Gyro()
{
  char cmd[1];
  char rtn[6];
  int data[6];
  for (int i = 0; i < 6; i++)
  {
    cmd[0]=2+i;
    i2c_BMX.write(Addr_Gyro,cmd,1,1);    //Wire.beginTransmission(Addr_Gyro);
                                   //Wire.write((2 + i));    // Select data register
                                   //Wire.endTransmission();
                                    //Wire.requestFrom(Addr_Gyro, 1);    // Request 1 byte of data
    // Read 6 bytes of data
    // xGyro lsb, xGyro msb, yGyro lsb, yGyro msb, zGyro lsb, zGyro msb
    i2c_BMX.read(Addr_Gyro,rtn,1,0);
    data[i]=rtn[0]; 
                                   //if (Wire.available() == 1)
                                //data[i] = Wire.read();
  }
  // Convert the data
  xGyro = (data[1] * 256) + data[0];
  if (xGyro > 32767)  xGyro -= 65536;
  yGyro = (data[3] * 256) + data[2];
  if (yGyro > 32767)  yGyro -= 65536;
  zGyro = (data[5] * 256) + data[4];
  if (zGyro > 32767)  zGyro -= 65536;

  xGyro = xGyro * 0.0038; //  Full scale = +/- 125 degree/s
  yGyro = yGyro * 0.0038; //  Full scale = +/- 125 degree/s
  zGyro = zGyro * 0.0038; //  Full scale = +/- 125 degree/s
}
void BMX055_attitude()
{
  double data1[10],data2[10];  
//BMX055 加速度の読み取り
  BMX055_Accl();
  
  //BMX055 ジャイロの読み取り
  BMX055_Gyro();
  gydx=xGyro-gyro_off[0];
  gydy=yGyro-gyro_off[1];
  gydz=zGyro-gyro_off[2];
  
  //加速度からroll,pitch
  accro=atan2(yAccl,zAccl);
  accpi=atan2((-xAccl),sqrt((yAccl*yAccl+zAccl*zAccl)));
  
  //角速度からroll,pitch,yaw
  gydpi=cos(prfr)*gydy-sin(prfr)*gydz;
  gydro=gydx+sin(prfr)*(sin(prfp)/cos(prfp))*gydy+cos(prfr)*(sin(prfp)/cos(prfp))*gydz;
  gydya=(sin(prfr)/cos(prfp))*gydy+(cos(prfr)/cos(prfp))*gydz;

  //角速度のみから姿勢角を算出
  roll=prfr+gydro;
  pitch=prfp-gydpi; //ピッチのみ方向が逆
  yaw=prfy+gydya;
  
  //角速度:加速度＝GYROGAIN:ACCELGAINで融合
  prfr=GYROGAIN*(roll)+ACCELGAIN*accro;
  prfp=GYROGAIN*(pitch)+ACCELGAIN*accpi;
  /*for(int i=0; i<10 ;i++)
  {
      data1[i]=sum_roll[i];
      data2[i]=sum_pitch[i];
   }
  for(int i=0; i<10; i++)
  {
      sum_roll[i+1]=data1[i];
      sum_pitch[i+1]=data2[i];
      
      } 
  sum_roll[0]=prfr;
  sum_pitch[0]=prfp;
  
  prfr=(10*sum_roll[0]+9*sum_roll[1]+8*sum_roll[2]+7*sum_roll[3]+6*sum_roll[4]+5*sum_roll[5]+4*sum_roll[6]+3*sum_roll[7]+2*sum_roll[8]+1*sum_roll[9])/55;
  prfp=(10*sum_pitch[0]+9*sum_pitch[1]+8*sum_pitch[2]+7*sum_pitch[3]+6*sum_pitch[4]+5*sum_pitch[5]+4*sum_pitch[6]+3*sum_pitch[7]+2*sum_pitch[8]+1*sum_pitch[9])/55;*/
  }
  
//ロドリゲスの式をもとにroll,pitchを1軸周りの回転に変換する
void make_motion(){
    
    BMX055_attitude();
    
    double Cr,Sr,Cp,Sp,S_rod;
    double E[3][3];//方向余弦行列
    
    Cr=cos(prfr);
    Sr=sin(prfr);
    Cp=cos(prfp);
    Sp=sin(prfp);
    
    //以下方向余弦行列の値の計算
    //実験で必要ないのでyaw角は無視する
    E[0][0]=Cp;
    E[1][0]=-Sr*Sp;
    E[2][0]=Cr*Sp;
    
    E[0][1]=0.0;
    E[1][1]=Cr;
    E[2][1]=Sr;
    
    E[0][2]=-Sp;
    E[1][2]=-Sr*Cp;
    E[2][2]=Cr*Cp;
    
    theta_rod_1=theta_rod;//偏差の微分用
    //方向余弦行列の各成分をもとに1軸周りの回転に変換する
    theta_rod=acos(((E[0][0]+E[1][1]+E[2][2]-1))/2);
    S_rod=sin(theta_rod);
    nx_rod=(E[1][2]-E[2][1])/(2*S_rod);
    //ny_rod=(E[2][0]-E[0][2])/(2*S_rod);
    ny_rod=-(E[2][0]-E[0][2])/(2*S_rod);//ロボットの座標系と一致させるため逆方向に
    nz_rod=(E[0][1]-E[1][0])/(2*S_rod);
    
    dedt=(theta_rod-theta_rod_1)/sampling;//偏差の時間微分
    //pc2.printf("rod theta=%05.2lf,n=(%03.2lf,%03.2f,%03.2lf) nolm=%2.3lf\r\n",
    //theta_rod*180/PI,nx_rod,ny_rod,nz_rod,nx_rod*nx_rod+ny_rod*ny_rod+nz_rod*nz_rod);
    }

//サーボ関係
void setup_servo() { 
 pwm.begin();               
 pwm.setPWMFreq(200); 
 for(int i=0;i<4;i++)
 {
     for(int u=0;u<4;u++)
     {
         th[i][u]=th_ready[i][u];
         th_write[i][u]=th_ready[i][u];
         }
     }               
}
void servo_write(int ch,double ang)//引数は[°]
{
    if(ch==0)  ang=ang-135*PI/180;
    if(ch==4)  ang=ang-45*PI/180;
    if(ch==8)  ang=ang+45*PI/180;
    if(ch==12) ang=ang+135*PI/180;
    if( (ch!=2) && (ch!=5)&&(ch!=7) && (ch!=10) && (ch!=13)&&(ch!=15) )ang=-ang;
    ang=servo0[ch]+ang*8000/270*180/PI;
    servo_write7(ch,ang);
    }

void servo_write7(int ch, double ang){ 
  ang = ((ang-3500)/8000)*1600+700;//サーボモータ内部エンコーダは8000段階
  //pc2.printf("%d ang=%5.0lf \r\n ",ch,ang) ;                                               //初期状態を設定するときこの値を参考に設定したためそのまま利用　　 
  pwm.setPWM(ch, 0, ang);
}
void servo_calib()
{
    for(int u=0; u<4; u++)
    {
     for(int i=0; i<4; i++)
     {
        servo_write7(ch[u][i],servo0[ch[u][i]]);
        }
     }
}
//tg_readyで入力した角度に遷移
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]);
               th[u][i]=th_ready[u][i];
               }
             }
}      
//ポテンショメータから角度を所得する関数
double readarg(int ch,double s[16][10])
{
    double ave;
    sig1=chPin[ch][0];
    sig2=chPin[ch][1];
    sig3=chPin[ch][2];
    sig4=chPin[ch][3];
  //移動平均用の準備
  if(s[ch][0]==0.0)
  {
    for(int t=0 ; t<9 ;t++)
    {
       s[ch][t]=data.read(); 
    }
  }
  
  else{ 
  for(int u=0; u<9 ;u++)
  {
      s[ch][u]=s[ch][u+1];
      }
      
  }
  //s[ch][9]=data.read_u16();
  s[ch][9]=data.read();
  
  //最新の値を優先する重み付け
  ave=333.3*(PI/180)*(s[ch][0]+s[ch][1]*2+s[ch][2]*3+s[ch][3]*4+s[ch][4]*5+s[ch][5]*6+s[ch][6]*7+s[ch][7]*8+s[ch][8]*9+s[ch][9]*10)/55;
  
  return ave;
  
}
void setup_th()
{
    servo_calib();
    wait(3);
    for(int t=0; t<10; t++)
    {
    for(int i=0; i<4; i++)
    {
        for(int u=0; u<4; u++)
        {
            data0[ch[i][u]]+=readarg(ch[i][u],s)/10.0;
            }
        }
    }
    servo_ready();
    wait(3);
    /*for(int i=0; i<4; i++)
    {
        for(int u=0; u<4; u++)
        {
            data0[ch[i][u]]=data0[ch[i][u]]/10;
            th[i][u]=readarg(ch[i][u],s)-data0[ch[i][u]];
            }
        }*/
}
void update_th()
{
    for(int i=0; i<4; i++)
    {
        for(int u=0;u<4;u++)
        {
            th[i][u]=readarg(ch[i][u],s)-data0[ch[i][u]];
            if(u==0)
            {
                if(i==0)th[i][u]+=135*PI/180;
                if(i==1)th[i][u]+=45*PI/180;
                if(i==2)th[i][u]-=45*PI/180;
                if(i==3)th[i][u]-=135*PI/180;
                }
            //pc2.printf("%d:%3.2lf ",ch[i][u],th[i][u]*180/PI);
            }
        }
        //pc2.printf("\r\n");
}
void th_calib()
{
    double th_ready_keep[4][4];
    pc2.printf("\r\n th calib start \r\n");
    for(int i=0; i<4; i++)
    {
     for(int u=0; u<4; u++)
      {
       th_ready_keep[i][u]=th_ready[i][u];
       }
    }
    while(1)
    {
    int flag=0;
    
    servo_ready();
    update_th();
    
    for(int i=0; i<4; i++)
    {
        for(int u=1; u<4; u++)
        {
            if(((th[i][u]-th_ready_keep[i][u])*180/PI)>2.0)
            {
             th_ready[i][u]=th_ready[i][u]-0.5*PI/180;
             th_write[i][u]=th_ready[i][u];
             flag=1;
             pc2.printf("leg %d theta %d  ",i,u);
             }
            if(((th[i][u]-th_ready_keep[i][u])*180/PI)<-2.0)
             {
             th_ready[i][u]=th_ready[i][u]+0.5*PI/180;
             th_write[i][u]=th_ready[i][u];
             flag=1;
             pc2.printf("leg %d theta %d  ",i,u);
             }
         }
      }
      pc2.printf("\r\n");
    wait(0.1);  
    if(flag==0)break;
    }
}