Kotaro Irisawa / Mbed 2 deprecated 4dof_attitude

4dof attitude control 2022.02.24

Dependencies:   mbed pca9685_2021_12_22 Eigen

main.cpp@1:507dd5045511, 2022-02-24 (annotated)

Committer:
Kotttaro
Date:
Thu Feb 24 06:09:47 2022 +0000
Revision:
1:507dd5045511
Parent:
0:e76f506b9de3 
2022.02.24

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Kotttaro 0:e76f506b9de3 1 //4dofscrewをもとに姿勢制御を行う
Kotttaro 0:e76f506b9de3 2 //9軸センサの値からroll,pitchを算出しさらにロドリゲスの回転公式から1軸中心の回転に変換
Kotttaro 0:e76f506b9de3 3 //回転軸の方向に姿勢角を打ち消すようにねじ運動を入力する
Kotttaro 0:e76f506b9de3 4 //ポテンショメータから取得した値を元に次の角度増分を計算する
Kotttaro 0:e76f506b9de3 5 #include "mbed.h"
Kotttaro 0:e76f506b9de3 6 #include <PCA9685.h>
Kotttaro 0:e76f506b9de3 7 #include "Eigen/Geometry.h"
Kotttaro 0:e76f506b9de3 8 #include "Eigen/Dense.h"
Kotttaro 0:e76f506b9de3 9 #include <math.h>
Kotttaro 0:e76f506b9de3 10
Kotttaro 0:e76f506b9de3 11
Kotttaro 0:e76f506b9de3 12 #define PI 3.14159265358979323846264338327950288
Kotttaro 0:e76f506b9de3 13
Kotttaro 0:e76f506b9de3 14 //最適計算関係
Kotttaro 0:e76f506b9de3 15 #define ITMAX 100 //brent法繰り返し回数
Kotttaro 0:e76f506b9de3 16 #define GLIMIT 100.0
Kotttaro 0:e76f506b9de3 17 #define CGOLD 0.3819660
Kotttaro 0:e76f506b9de3 18 #define SHIFT(a,b,c,d) (a)=(b);(b)=(c);(c)=(d);
Kotttaro 0:e76f506b9de3 19 #define ZEPS 1.0e-10
Kotttaro 0:e76f506b9de3 20 #define GOLD 1.618034
Kotttaro 0:e76f506b9de3 21 #define TINY 1.0e-20
Kotttaro 0:e76f506b9de3 22
Kotttaro 0:e76f506b9de3 23 //サーボ関係
Kotttaro 0:e76f506b9de3 24 #define SERVOMIN 700
Kotttaro 0:e76f506b9de3 25 #define SERVOMAX 2300
Kotttaro 0:e76f506b9de3 26 #define SERVOGAIN 29.6296300
Kotttaro 0:e76f506b9de3 27
Kotttaro 0:e76f506b9de3 28 //9軸センサ関係
Kotttaro 0:e76f506b9de3 29 //mbedではアドレスの指定が7bitなので1つずつずらす
Kotttaro 0:e76f506b9de3 30 // BMX055　加速度センサのI2Cアドレス
Kotttaro 0:e76f506b9de3 31 #define Addr_Accl (0x19<<1) // (JP1,JP2,JP3 = Openの時)
Kotttaro 0:e76f506b9de3 32 // BMX055　ジャイロセンサのI2Cアドレス
Kotttaro 0:e76f506b9de3 33 #define Addr_Gyro (0x69<<1) // (JP1,JP2,JP3 = Openの時)
Kotttaro 0:e76f506b9de3 34 // BMX055　磁気センサのI2Cアドレス
Kotttaro 0:e76f506b9de3 35 #define Addr_Mag (0x13<<1) // (JP1,JP2,JP3 = Openの時)
Kotttaro 0:e76f506b9de3 36
Kotttaro 0:e76f506b9de3 37 #define GYROGAIN 0.0
Kotttaro 0:e76f506b9de3 38 #define ACCELGAIN 1.0
Kotttaro 0:e76f506b9de3 39
Kotttaro 0:e76f506b9de3 40 ////////////////////////////////////////場合に応じて変更する変数///////////////////////////////////////
Kotttaro 0:e76f506b9de3 41 I2C i2c_BMX(p9,p10);
Kotttaro 0:e76f506b9de3 42 double sampling=0.01;//δtの時間[s]
Kotttaro 0:e76f506b9de3 43 int times= 100;//実行回数:実行時間は (sampling)*(times)秒
Kotttaro 0:e76f506b9de3 44 double th_ready[4][4] = { {135 * PI / 180, 30 * PI / 180, -75 * PI / 180, -30 * PI / 180},
Kotttaro 0:e76f506b9de3 45 {45 * PI / 180, 30 * PI / 180, -75 * PI / 180, -30 * PI / 180},
Kotttaro 0:e76f506b9de3 46 {-45 * PI / 180, 30 * PI / 180, -75 * PI / 180, -30 * PI / 180},
Kotttaro 0:e76f506b9de3 47 {-135 * PI / 180, 30 * PI / 180, -75 * PI / 180, -30 * PI / 180} };//初期状態
Kotttaro 0:e76f506b9de3 48 ///////////////////////////////////////////////////////////////////////////////////////////////////////
Kotttaro 0:e76f506b9de3 49
Kotttaro 0:e76f506b9de3 50 PCA9685 pwm;//クラス宣言
Kotttaro 0:e76f506b9de3 51 Serial pc2(USBTX,USBRX);
Kotttaro 0:e76f506b9de3 52 Timer tim;//実行総時間の確認用
Kotttaro 0:e76f506b9de3 53 Timer manage;//samplingを保証するためのタイマー
Kotttaro 0:e76f506b9de3 54 DigitalOut trig1(p27);
Kotttaro 0:e76f506b9de3 55 DigitalOut trig2(p28);
Kotttaro 0:e76f506b9de3 56
Kotttaro 0:e76f506b9de3 57 //ポテンショメータ用
Kotttaro 0:e76f506b9de3 58 DigitalOut sig1(p5);
Kotttaro 0:e76f506b9de3 59 DigitalOut sig2(p6);
Kotttaro 0:e76f506b9de3 60 DigitalOut sig3(p7);
Kotttaro 0:e76f506b9de3 61 DigitalOut sig4(p8);
Kotttaro 0:e76f506b9de3 62 AnalogIn data(p15);
Kotttaro 0:e76f506b9de3 63
Kotttaro 0:e76f506b9de3 64 using namespace Eigen;
Kotttaro 0:e76f506b9de3 65
Kotttaro 0:e76f506b9de3 66 //以下変数定義
Kotttaro 0:e76f506b9de3 67
Kotttaro 0:e76f506b9de3 68 //brentとmnbrakに必要な変数
Kotttaro 0:e76f506b9de3 69 double ax,bx,cx;
Kotttaro 0:e76f506b9de3 70 double fa,fb,fc;
Kotttaro 0:e76f506b9de3 71
Kotttaro 0:e76f506b9de3 72 //サーボの書き込みに必要な変数
Kotttaro 0:e76f506b9de3 73 double servo0[16]={7000.0, 5850.0, 7050.0, 7200.0,
Kotttaro 0:e76f506b9de3 74 6000.0, 6300.0, 5100.0, 6100.0,
Kotttaro 0:e76f506b9de3 75 6700.0, 7000.0, 5650.0, 6000.0,
Kotttaro 0:e76f506b9de3 76 6000.0, 5100.0, 5600.0, 6670.0};//servoの初期値
Kotttaro 0:e76f506b9de3 77 //以下関節角度計算に用いる変数
Kotttaro 0:e76f506b9de3 78 int ch[4][4]={{0 ,1 ,2 ,3} ,
Kotttaro 0:e76f506b9de3 79 {4 ,5 ,6 ,7} ,
Kotttaro 0:e76f506b9de3 80 {8 ,9 ,10,11},
Kotttaro 0:e76f506b9de3 81 {12,13,14,15} };
Kotttaro 0:e76f506b9de3 82
Kotttaro 0:e76f506b9de3 83 double r=50*PI/180;//斜面の傾き[°],(姿勢角を元に算出)
Kotttaro 0:e76f506b9de3 84 double L[4] = {65.0,35.0,110.0,38.0};//4本のリンク長 後から足したのでL[3]を理論中のL0に対応させる
Kotttaro 0:e76f506b9de3 85 double tip[4][3];//足先座標
Kotttaro 0:e76f506b9de3 86 double con[4][3] = { {-55.0, 55.0,0.0},
Kotttaro 0:e76f506b9de3 87 { 55.0, 55.0,0.0},
Kotttaro 0:e76f506b9de3 88 { 55.0,-55.0,0.0},
Kotttaro 0:e76f506b9de3 89 {-55.0,-55.0,0.0}};//脚のコーナー座標,zは必ず0
Kotttaro 0:e76f506b9de3 90 double th0[4][4]= { 0.0,0.0,0.0,0.0,
Kotttaro 0:e76f506b9de3 91 0.0,0.0, 0.0,0.0,
Kotttaro 0:e76f506b9de3 92 0.0,0.0, 0.0,0.0,
Kotttaro 0:e76f506b9de3 93 0.0,0.0, 0.0,0.0 }; //計算用の関節角度
Kotttaro 0:e76f506b9de3 94 double th[4][4],th_write[4][4];//計算用のθと書き込み用のθを別々に用意する
Kotttaro 0:e76f506b9de3 95 double Jacbi[4][3][4];//ヤコビアン 脚数×関節×次元
Kotttaro 0:e76f506b9de3 96 double a,a0, h,fi;//評価関数内の変数 fi=φ
Kotttaro 0:e76f506b9de3 97 double X,tan_u, tan_d;//計算用
Kotttaro 0:e76f506b9de3 98 double dfdth[4];//評価関数のナブラ
Kotttaro 0:e76f506b9de3 99
Kotttaro 0:e76f506b9de3 100 //ねじ軸
Kotttaro 0:e76f506b9de3 101 //Lin:方向, L0:原点座標, vin:ねじ時に沿った速度, win:角速度ベクトルの大きさ
Kotttaro 0:e76f506b9de3 102 double Lin[3], L0[3]={0.0,0.0,0.0}, vin,v[3],wg[3],win,nol;//ねじ軸条件
Kotttaro 0:e76f506b9de3 103 int leg_upperside[2],leg_lowerside[2];
Kotttaro 0:e76f506b9de3 104
Kotttaro 0:e76f506b9de3 105 //9軸センサ関係
Kotttaro 0:e76f506b9de3 106 // センサーの値を保存するグローバル関数
Kotttaro 0:e76f506b9de3 107 float xAccl = 0.00;
Kotttaro 0:e76f506b9de3 108 float yAccl = 0.00;
Kotttaro 0:e76f506b9de3 109 float zAccl = 0.00;
Kotttaro 0:e76f506b9de3 110 float xGyro = 0.00;
Kotttaro 0:e76f506b9de3 111 float yGyro = 0.00;
Kotttaro 0:e76f506b9de3 112 float zGyro = 0.00;
Kotttaro 0:e76f506b9de3 113 int xMag = 0;
Kotttaro 0:e76f506b9de3 114 int yMag = 0;
Kotttaro 0:e76f506b9de3 115 int zMag = 0;
Kotttaro 0:e76f506b9de3 116
Kotttaro 0:e76f506b9de3 117 //姿勢角検出用の各パラメータの定義
Kotttaro 0:e76f506b9de3 118 int count=0;
Kotttaro 0:e76f506b9de3 119 double roll=0.0,pitch=0.0,yaw=0.0;//姿勢角
Kotttaro 0:e76f506b9de3 120 double prfr=0.0,prfp=0.0,prfy=0.0;//修正した姿勢角
Kotttaro 0:e76f506b9de3 121 double sum_roll[10]={0.0},sum_pitch[10]={0.0};//移動平均用の配列
Kotttaro 0:e76f506b9de3 122 double accro,accpi;//加速度センサの値から算出した姿勢角(ロール、ピッチ)
Kotttaro 0:e76f506b9de3 123 double dx,dy,dz;
Kotttaro 0:e76f506b9de3 124 double magdx,magdy,magdz;
Kotttaro 0:e76f506b9de3 125 double gydx=0.0,gydy=0.0,gydz=0.0;//角速度センサの微小時間の変化
Kotttaro 0:e76f506b9de3 126 double gydro=0.0,gydpi=0.0,gydya=0.0;//角速度センサから求めた微小時間の姿勢の変化
Kotttaro 0:e76f506b9de3 127 double gyro_off[3];//角速度センサオフセット保存用変数
Kotttaro 0:e76f506b9de3 128 double theta_rod,theta_rod_1=0.0,nx_rod,ny_rod,nz_rod;//ロドリゲスの回転公式のための4変数
Kotttaro 0:e76f506b9de3 129 double dedt;//偏差の微分
Kotttaro 0:e76f506b9de3 130
Kotttaro 0:e76f506b9de3 131 //ポテンショメータ
Kotttaro 0:e76f506b9de3 132 double data0[16]={0};//初期状態の角度
Kotttaro 0:e76f506b9de3 133 double s[16][10]={{0},{0},{0},{0}, {0},{0},{0},{0}, {0},{0},{0},{0}, {0},{0},{0},{0}};
Kotttaro 0:e76f506b9de3 134 int chPin[16][4]={{0,0,0,0},//ch0
Kotttaro 0:e76f506b9de3 135 {1,0,0,0},//ch1
Kotttaro 0:e76f506b9de3 136 {0,1,0,0},//ch2
Kotttaro 0:e76f506b9de3 137 {1,1,0,0},//ch3
Kotttaro 0:e76f506b9de3 138 {0,0,1,0},//ch4
Kotttaro 0:e76f506b9de3 139 {1,0,1,0},//ch5
Kotttaro 0:e76f506b9de3 140 {0,1,1,0},//ch6
Kotttaro 0:e76f506b9de3 141 {1,1,1,0},//ch7
Kotttaro 0:e76f506b9de3 142 {0,0,0,1},//ch8
Kotttaro 0:e76f506b9de3 143 {1,0,0,1},//ch9
Kotttaro 0:e76f506b9de3 144 {0,1,0,1},//ch10
Kotttaro 0:e76f506b9de3 145 {1,1,0,1},//ch11
Kotttaro 0:e76f506b9de3 146 {0,0,1,1},//ch12
Kotttaro 0:e76f506b9de3 147 {1,0,1,1},//ch13
Kotttaro 0:e76f506b9de3 148 {0,1,1,1},//ch14
Kotttaro 0:e76f506b9de3 149 {1,1,1,1}};//ch15
Kotttaro 0:e76f506b9de3 150
Kotttaro 0:e76f506b9de3 151 //以下行列定義
Kotttaro 0:e76f506b9de3 152 MatrixXd Q(3, 3);//Q行列
Kotttaro 0:e76f506b9de3 153 MatrixXd R(3, 4);//R行列
Kotttaro 0:e76f506b9de3 154 Vector3d vP[4];//各脚の速度ベクトル
Kotttaro 0:e76f506b9de3 155
Kotttaro 0:e76f506b9de3 156 void define_upper_lower();//9軸センサの値から斜面上側の脚と下側の脚を判定する
Kotttaro 0:e76f506b9de3 157 void QR(int leg);//QR分解用関数,引数は脚番号
Kotttaro 0:e76f506b9de3 158 void vp(int leg);//引数は脚番号,与条件から各脚先の速度を導出する
Kotttaro 0:e76f506b9de3 159 void fwd(int leg);//順運動学より脚先の座標を導出する
Kotttaro 0:e76f506b9de3 160 void Jac(int leg);//指定した脚のヤコビアンを計算
Kotttaro 0:e76f506b9de3 161 void solve(double w3,int leg,int det);//theta3の角速度から全関節の関節角度を導き出す
Kotttaro 0:e76f506b9de3 162
Kotttaro 0:e76f506b9de3 163 void deff(int leg);//評価関数計算, legは距離と傾きから指定する
Kotttaro 0:e76f506b9de3 164 void dfd( int leg);//評価関数の勾配をとる
Kotttaro 0:e76f506b9de3 165 double fe(int leg,double dth3);//brent法に合わせてeを関数化
Kotttaro 0:e76f506b9de3 166 double f(int leg,double dth3);//テーラー展開第1項の値を返す, brent法用
Kotttaro 0:e76f506b9de3 167 double num_nolm(int leg , double dth3);//ノルム最小の解を導く際に使用する関数
Kotttaro 0:e76f506b9de3 168
Kotttaro 0:e76f506b9de3 169 void mnbrak(int leg,int discrimination);//brentに必要な極小値の囲い込んだ3点を決定する関数
Kotttaro 0:e76f506b9de3 170 double brent(int leg,double min,double mid,double max,double tol,int discrimination);//brent法により1次元探索するプログラム
Kotttaro 0:e76f506b9de3 171 //discrimination 0:谷側(fe), 1:山側(nolm), 2:谷側(f),3:テスト用の関数(f_test)
Kotttaro 0:e76f506b9de3 172 double SIGN(double x,double y);//xにyの符号をつけたものを返す
Kotttaro 0:e76f506b9de3 173 double FMAX(double x,double y);//大きいほうの値が返される
Kotttaro 0:e76f506b9de3 174 double f_test(double x);//テスト用の関数
Kotttaro 0:e76f506b9de3 175
Kotttaro 0:e76f506b9de3 176 //以下サーボ関係
Kotttaro 0:e76f506b9de3 177 void setup_servo();//サーボセットアップ用関数
Kotttaro 0:e76f506b9de3 178 void servo_write(int ch,double ang);//angに
Kotttaro 0:e76f506b9de3 179 void servo_write7(int ch, double ang);
Kotttaro 0:e76f506b9de3 180 void servo_calib();//全ての角度を0度にする
Kotttaro 0:e76f506b9de3 181 void servo_ready();//初期状態
Kotttaro 0:e76f506b9de3 182
Kotttaro 0:e76f506b9de3 183
Kotttaro 0:e76f506b9de3 184 //以下9軸センサ関係
Kotttaro 0:e76f506b9de3 185 void BMX055_Init();//9軸センサ設定用関数
Kotttaro 0:e76f506b9de3 186 void BMX055_Accl();//加速度の値を得る関数
Kotttaro 0:e76f506b9de3 187 void BMX055_Gyro();//角速度の値を得る関数
Kotttaro 0:e76f506b9de3 188 void BMX055_attitude();//BMX055の値からroll,potchを計算する
Kotttaro 0:e76f506b9de3 189 void make_motion();//ロドリゲスの式をもとにroll,pitchを1軸周りの回転に変換し,ねじ運動を作り出す
Kotttaro 0:e76f506b9de3 190
Kotttaro 0:e76f506b9de3 191 //ポテンショメータ
Kotttaro 0:e76f506b9de3 192 double readarg(int ch,double s[16][10]);//取得するchと移動平均用の配列
Kotttaro 0:e76f506b9de3 193 void setup_th();
Kotttaro 0:e76f506b9de3 194 void update_th();
Kotttaro 0:e76f506b9de3 195 void th_calib();
Kotttaro 0:e76f506b9de3 196
Kotttaro 0:e76f506b9de3 197 int main()
Kotttaro 0:e76f506b9de3 198 {
Kotttaro 0:e76f506b9de3 199 double t=0.0;
Kotttaro 0:e76f506b9de3 200 int dum;
Kotttaro 0:e76f506b9de3 201 //各種設定を行う//
Kotttaro 0:e76f506b9de3 202 pc2.baud(921600);
Kotttaro 0:e76f506b9de3 203 trig1=0;
Kotttaro 0:e76f506b9de3 204 trig2=0;
Kotttaro 0:e76f506b9de3 205 setup_servo();
Kotttaro 0:e76f506b9de3 206 setup_th();
Kotttaro 0:e76f506b9de3 207 BMX055_Init();
Kotttaro 1:507dd5045511 208 //for(int i=0;i<10;i++)update_th();make_motion();
Kotttaro 1:507dd5045511 209 for(int i=0;i<10;i++)make_motion();
Kotttaro 0:e76f506b9de3 210 dedt=0.0;
Kotttaro 0:e76f506b9de3 211 pc2.printf("th_calib will start\r\n");
Kotttaro 0:e76f506b9de3 212 pc2.printf("press keyboad to start\r\n");
Kotttaro 0:e76f506b9de3 213 while(1)
Kotttaro 0:e76f506b9de3 214 {if(pc2.readable()==0)dum=pc2.getc();break;}
Kotttaro 0:e76f506b9de3 215 //th_calib();
Kotttaro 0:e76f506b9de3 216 pc2.printf("th_calib finished\r\n");
Kotttaro 0:e76f506b9de3 217
Kotttaro 0:e76f506b9de3 218 wait(2);
Kotttaro 0:e76f506b9de3 219 pc2.printf("attitude control will start\r\n");
Kotttaro 0:e76f506b9de3 220 pc2.printf("press keyboad to start\r\n");
Kotttaro 0:e76f506b9de3 221 while(1)
Kotttaro 0:e76f506b9de3 222 {if(pc2.readable()==0)dum=pc2.getc();break;}
Kotttaro 0:e76f506b9de3 223 pc2.printf("start\r\n");
Kotttaro 0:e76f506b9de3 224 tim.start();
Kotttaro 0:e76f506b9de3 225 trig1=1;
Kotttaro 0:e76f506b9de3 226 trig2=1;
Kotttaro 0:e76f506b9de3 227 tim.start();
Kotttaro 0:e76f506b9de3 228 //ホワイトノイズ計測用ループ
Kotttaro 0:e76f506b9de3 229 /*while(1)
Kotttaro 0:e76f506b9de3 230 {
Kotttaro 0:e76f506b9de3 231 t=tim.read();
Kotttaro 0:e76f506b9de3 232 BMX055_attitude();
Kotttaro 0:e76f506b9de3 233 pc2.printf("%2.3lf %3.3lf %3.3lf \n\r",t,prfr*180/PI ,prfp*180/PI);
Kotttaro 0:e76f506b9de3 234 wait(0.01);
Kotttaro 0:e76f506b9de3 235 }*/
Kotttaro 0:e76f506b9de3 236
Kotttaro 0:e76f506b9de3 237 //以下姿勢制御ロープ///
Kotttaro 0:e76f506b9de3 238 //while(1){
Kotttaro 0:e76f506b9de3 239 while(tim.read()<30.0){
Kotttaro 0:e76f506b9de3 240 double dth;
Kotttaro 1:507dd5045511 241 //update_th();
Kotttaro 0:e76f506b9de3 242 manage.start();
Kotttaro 0:e76f506b9de3 243 make_motion();//各rodの値を更新
Kotttaro 0:e76f506b9de3 244 define_upper_lower();//脚の上下関係を計算
Kotttaro 0:e76f506b9de3 245
Kotttaro 0:e76f506b9de3 246 //ねじ運動を生成する
Kotttaro 0:e76f506b9de3 247 Lin[0] = nx_rod; //ねじ軸x
Kotttaro 0:e76f506b9de3 248 Lin[1] = ny_rod; //ねじ軸y
Kotttaro 0:e76f506b9de3 249 Lin[2] = nz_rod;//ねじ軸z
Kotttaro 0:e76f506b9de3 250 //win = -theta_rod*110;//p制御のとき
Kotttaro 0:e76f506b9de3 251 win = -theta_rod*110+dedt*0.1;
Kotttaro 0:e76f506b9de3 252 if(theta_rod*180/PI>1.0)
Kotttaro 0:e76f506b9de3 253 {
Kotttaro 0:e76f506b9de3 254 nol = (double)sqrt(Lin[0] * Lin[0] + Lin[1] * Lin[1] + Lin[2] * Lin[2]);
Kotttaro 0:e76f506b9de3 255 for (int i = 0; i < 3; i++)
Kotttaro 0:e76f506b9de3 256 {
Kotttaro 0:e76f506b9de3 257 wg[i] = Lin[i] * win / nol;
Kotttaro 0:e76f506b9de3 258 v[i] = 0.0;
Kotttaro 0:e76f506b9de3 259 }
Kotttaro 0:e76f506b9de3 260
Kotttaro 0:e76f506b9de3 261
Kotttaro 0:e76f506b9de3 262 //谷側の脚の計算
Kotttaro 0:e76f506b9de3 263 for (int i = 0; i < 2;i++){
Kotttaro 0:e76f506b9de3 264 fwd(leg_lowerside[i]);
Kotttaro 0:e76f506b9de3 265 vp(leg_lowerside[i]);
Kotttaro 0:e76f506b9de3 266 Jac(leg_lowerside[i]);
Kotttaro 0:e76f506b9de3 267 QR(leg_lowerside[i]);
Kotttaro 0:e76f506b9de3 268 deff(leg_lowerside[i]);
Kotttaro 0:e76f506b9de3 269 dfd(leg_lowerside[i]);
Kotttaro 0:e76f506b9de3 270 ax=-0.0873*PI/180.0;bx=0.0873*PI/180.0;cx=0.0;
Kotttaro 0:e76f506b9de3 271 mnbrak(leg_lowerside[i],2);
Kotttaro 0:e76f506b9de3 272 dth=brent(leg_lowerside[i],ax,bx,cx,0.0001,2);
Kotttaro 0:e76f506b9de3 273 solve(dth,leg_lowerside[i],1);
Kotttaro 0:e76f506b9de3 274 }
Kotttaro 0:e76f506b9de3 275 //山側の脚の計算
Kotttaro 0:e76f506b9de3 276 for (int i = 0; i < 2;i++){
Kotttaro 0:e76f506b9de3 277 fwd(leg_upperside[i]);
Kotttaro 0:e76f506b9de3 278 vp(leg_upperside[i]);
Kotttaro 0:e76f506b9de3 279 Jac(leg_upperside[i]);
Kotttaro 0:e76f506b9de3 280 QR(leg_upperside[i]);
Kotttaro 0:e76f506b9de3 281 deff(leg_upperside[i]);
Kotttaro 0:e76f506b9de3 282 ax=-0.0873*PI/180.0;bx=0.0873*PI/180.0;cx=0.0;
Kotttaro 0:e76f506b9de3 283 mnbrak(leg_upperside[i],1);
Kotttaro 0:e76f506b9de3 284 dth=brent(leg_upperside[i],ax,bx,cx,0.0001,1);
Kotttaro 0:e76f506b9de3 285 solve(dth, leg_upperside[i], 1);
Kotttaro 0:e76f506b9de3 286 }
Kotttaro 0:e76f506b9de3 287 }
Kotttaro 0:e76f506b9de3 288 ///*
Kotttaro 0:e76f506b9de3 289 //計算結果をサーボに入力
Kotttaro 0:e76f506b9de3 290
Kotttaro 0:e76f506b9de3 291 for(int u=0; u<4; u++)
Kotttaro 0:e76f506b9de3 292 {
Kotttaro 0:e76f506b9de3 293 for(int i=0; i<4; i++)
Kotttaro 0:e76f506b9de3 294 {
Kotttaro 1:507dd5045511 295 //servo_write(ch[u][i],th_write[u][i]);
Kotttaro 1:507dd5045511 296 servo_write(ch[u][i],th[u][i]);
Kotttaro 0:e76f506b9de3 297 }
Kotttaro 0:e76f506b9de3 298 }
Kotttaro 0:e76f506b9de3 299 //*/
Kotttaro 0:e76f506b9de3 300 t=tim.read();
Kotttaro 0:e76f506b9de3 301 pc2.printf("%2.3lf %3.3lf %3.3lf \n\r",t,prfr*180/PI ,prfp*180/PI);
Kotttaro 0:e76f506b9de3 302
Kotttaro 0:e76f506b9de3 303 while(1)
Kotttaro 0:e76f506b9de3 304 {
Kotttaro 0:e76f506b9de3 305 if(manage.read()>=sampling)break;
Kotttaro 0:e76f506b9de3 306 }
Kotttaro 0:e76f506b9de3 307
Kotttaro 0:e76f506b9de3 308
Kotttaro 0:e76f506b9de3 309 manage.reset();
Kotttaro 0:e76f506b9de3 310 }
Kotttaro 0:e76f506b9de3 311 trig1=0;
Kotttaro 0:e76f506b9de3 312 trig2=0;
Kotttaro 0:e76f506b9de3 313 wait(3);
Kotttaro 0:e76f506b9de3 314
Kotttaro 0:e76f506b9de3 315 return 0; // ソフトの終了
Kotttaro 0:e76f506b9de3 316 }
Kotttaro 0:e76f506b9de3 317
Kotttaro 0:e76f506b9de3 318 void define_upper_lower()
Kotttaro 0:e76f506b9de3 319 {
Kotttaro 0:e76f506b9de3 320 double n_angle;
Kotttaro 0:e76f506b9de3 321 n_angle=atan2(ny_rod,nx_rod);//返り値は-PI~PI
Kotttaro 0:e76f506b9de3 322 if((-PI/4 < n_angle)&&(PI/4 > n_angle))
Kotttaro 0:e76f506b9de3 323 {
Kotttaro 0:e76f506b9de3 324 leg_upperside[0]=0;
Kotttaro 0:e76f506b9de3 325 leg_upperside[1]=1;
Kotttaro 0:e76f506b9de3 326 leg_lowerside[0]=2;
Kotttaro 0:e76f506b9de3 327 leg_lowerside[1]=3;
Kotttaro 0:e76f506b9de3 328 }
Kotttaro 0:e76f506b9de3 329 if((PI/4 < n_angle)&&(3*PI/4 > n_angle))
Kotttaro 0:e76f506b9de3 330 {
Kotttaro 0:e76f506b9de3 331 leg_upperside[0]=0;
Kotttaro 0:e76f506b9de3 332 leg_upperside[1]=3;
Kotttaro 0:e76f506b9de3 333 leg_lowerside[0]=1;
Kotttaro 0:e76f506b9de3 334 leg_lowerside[1]=2;
Kotttaro 0:e76f506b9de3 335 }
Kotttaro 0:e76f506b9de3 336 if((-3*PI/4 < n_angle)&&(-PI/4 < n_angle))
Kotttaro 0:e76f506b9de3 337 {
Kotttaro 0:e76f506b9de3 338 leg_upperside[0]=1;
Kotttaro 0:e76f506b9de3 339 leg_upperside[1]=2;
Kotttaro 0:e76f506b9de3 340 leg_lowerside[0]=0;
Kotttaro 0:e76f506b9de3 341 leg_lowerside[1]=3;
Kotttaro 0:e76f506b9de3 342 }
Kotttaro 0:e76f506b9de3 343 if(((-PI > n_angle)&&(-3*PI/4 < n_angle)) || ((PI > n_angle)&&(3*PI/4 < n_angle)))
Kotttaro 0:e76f506b9de3 344 {
Kotttaro 0:e76f506b9de3 345 leg_upperside[0]=2;
Kotttaro 0:e76f506b9de3 346 leg_upperside[1]=3;
Kotttaro 0:e76f506b9de3 347 leg_lowerside[0]=0;
Kotttaro 0:e76f506b9de3 348 leg_lowerside[1]=1;
Kotttaro 0:e76f506b9de3 349 }
Kotttaro 0:e76f506b9de3 350 }
Kotttaro 0:e76f506b9de3 351 void QR(int leg) {
Kotttaro 0:e76f506b9de3 352 double s, t;//要素計算用
Kotttaro 0:e76f506b9de3 353 MatrixXd ma(3, 4), ma1(3, 4);
Kotttaro 0:e76f506b9de3 354
Kotttaro 0:e76f506b9de3 355 ma << Jacbi[leg][0][0], Jacbi[leg][0][1], Jacbi[leg][0][2], Jacbi[leg][0][3],
Kotttaro 0:e76f506b9de3 356 Jacbi[leg][1][0], Jacbi[leg][1][1], Jacbi[leg][1][2], Jacbi[leg][1][3],
Kotttaro 0:e76f506b9de3 357 Jacbi[leg][2][0], Jacbi[leg][2][1], Jacbi[leg][2][2], Jacbi[leg][2][3];
Kotttaro 0:e76f506b9de3 358
Kotttaro 0:e76f506b9de3 359 //ハウスホルダー変換1回目
Kotttaro 0:e76f506b9de3 360 MatrixXd A1(3, 3);
Kotttaro 0:e76f506b9de3 361 A1 << 1.0, 0.0, 0.0,
Kotttaro 0:e76f506b9de3 362 0.0, 1.0, 0.0,
Kotttaro 0:e76f506b9de3 363 0.0, 0.0, 1.0;
Kotttaro 0:e76f506b9de3 364
Kotttaro 0:e76f506b9de3 365 s = (double)sqrt(ma(0, 0) * ma(0, 0) + ma(1, 0) * ma(1, 0) + ma(2, 0) * ma(2, 0));//分母のやつ
Kotttaro 0:e76f506b9de3 366
Kotttaro 0:e76f506b9de3 367 MatrixXd H1(3, 3);//1回目の行列
Kotttaro 0:e76f506b9de3 368 MatrixXd X11(3, 1), X12(1, 3);
Kotttaro 0:e76f506b9de3 369 Vector3d a11, a12;//a11が変換前,a12が変換後
Kotttaro 0:e76f506b9de3 370 a11 << ma(0, 0), ma(1, 0), ma(2, 0);
Kotttaro 0:e76f506b9de3 371 a12 << s, 0.0, 0.0;
Kotttaro 0:e76f506b9de3 372 X11 = a11 - a12;
Kotttaro 0:e76f506b9de3 373 X12 = X11.transpose();
Kotttaro 0:e76f506b9de3 374 t = (double)sqrt(X11(0, 0) * X11(0, 0) + X11(1, 0) * X11(1, 0) + X11(2, 0) * X11(2, 0));
Kotttaro 0:e76f506b9de3 375 H1 = A1 - 2.0 * (X11 * X12) / (t * t);
Kotttaro 0:e76f506b9de3 376 ma1 = H1 * ma;
Kotttaro 0:e76f506b9de3 377 //2回目
Kotttaro 0:e76f506b9de3 378 MatrixXd H2(3, 3), A2(2, 2), h2(2, 2);
Kotttaro 0:e76f506b9de3 379 A2 << 1.0, 0.0,
Kotttaro 0:e76f506b9de3 380 0.0, 1.0;
Kotttaro 0:e76f506b9de3 381 Vector2d a21, a22;
Kotttaro 0:e76f506b9de3 382 MatrixXd X21(2, 1), X22(1, 2);
Kotttaro 0:e76f506b9de3 383 a21 << ma1(1, 1), ma1(2, 1);
Kotttaro 0:e76f506b9de3 384 s = (double)sqrt(ma1(1, 1) * ma1(1, 1) + ma1(2, 1) * ma1(2, 1));
Kotttaro 0:e76f506b9de3 385 a22 << s, 0;
Kotttaro 0:e76f506b9de3 386 X21 = a21 - a22;
Kotttaro 0:e76f506b9de3 387 X22 = X21.transpose();
Kotttaro 0:e76f506b9de3 388 t = (double)sqrt(X21(0, 0) * X21(0, 0) + X21(1, 0) * X21(1, 0));
Kotttaro 0:e76f506b9de3 389 h2 = A2 - 2 * (X21 * X22) / (t * t);
Kotttaro 0:e76f506b9de3 390 H2 << 1.0, 0.0, 0.0,
Kotttaro 0:e76f506b9de3 391 0.0, h2(0, 0), h2(0, 1),
Kotttaro 0:e76f506b9de3 392 0.0, h2(1, 0), h2(1, 1);
Kotttaro 0:e76f506b9de3 393 R = H2 * ma1;
Kotttaro 0:e76f506b9de3 394 MatrixXd H1T(3, 3), H2T(3, 3);
Kotttaro 0:e76f506b9de3 395 H1T = H1.transpose();
Kotttaro 0:e76f506b9de3 396 H2T = H2.transpose();
Kotttaro 0:e76f506b9de3 397 Q = H1T * H2T;
Kotttaro 0:e76f506b9de3 398 }
Kotttaro 0:e76f506b9de3 399
Kotttaro 0:e76f506b9de3 400 void vp(int leg) {//5年生の時に作成したもの
Kotttaro 0:e76f506b9de3 401 double crosx, crosy, crosz;
Kotttaro 0:e76f506b9de3 402 //double wA[3] = { (double)(-wg[0] * PI / 180.0),(double)(-wg[1] * PI / 180.0),(double)(-wg[2] * PI / 180.0) };
Kotttaro 0:e76f506b9de3 403 double wA[3] = { (double)(wg[0] * PI / 180.0),(double)(wg[1] * PI / 180.0),(double)(wg[2] * PI / 180.0) };//実機との辻褄を合わせるため符号を削除
Kotttaro 0:e76f506b9de3 404 double vA[3] = { (-v[0]),(-v[1]) ,(-v[2]) };
Kotttaro 0:e76f506b9de3 405 double AP[3] = { (tip[leg][0] - L0[0]),(tip[leg][1] - L0[1]),tip[leg][2] - L0[2] };
Kotttaro 0:e76f506b9de3 406 if (Lin[2] != 0.0)
Kotttaro 0:e76f506b9de3 407 {
Kotttaro 0:e76f506b9de3 408 double LP[3] = { -(Lin[0] / nol) / (Lin[2] / nol) * tip[leg][2],-(Lin[1] / nol) / (Lin[2] / nol) * tip[leg][2],0.0 };
Kotttaro 0:e76f506b9de3 409 for (int i = 0; i < 3; i++) { AP[i] = AP[i] - LP[i]; }
Kotttaro 0:e76f506b9de3 410 AP[2] = 0.0;
Kotttaro 0:e76f506b9de3 411 }
Kotttaro 0:e76f506b9de3 412 crosx = AP[1] * wA[2] + (-AP[2]) * wA[1];
Kotttaro 0:e76f506b9de3 413 crosy = AP[2] * wA[0] + (-AP[0]) * wA[2];
Kotttaro 0:e76f506b9de3 414 crosz = AP[0] * wA[1] + (-AP[1]) * wA[0];
Kotttaro 0:e76f506b9de3 415 vP[leg] << crosx + vA[0], crosy + vA[1], crosz + vA[2];
Kotttaro 0:e76f506b9de3 416 //printf(" %lf,%lf,%lf\n", -v[0], -v[1], -v[2]);
Kotttaro 0:e76f506b9de3 417 //pc2.printf("input motion %d %lf,%lf,%lf\n\r", leg, vP[leg](0, 0), vP[leg](1, 0), vP[leg](2, 0));
Kotttaro 0:e76f506b9de3 418 //printf("vp finish\n");
Kotttaro 0:e76f506b9de3 419 }
Kotttaro 0:e76f506b9de3 420 void fwd(int leg) {
Kotttaro 0:e76f506b9de3 421 //printf("fwd start\n");
Kotttaro 0:e76f506b9de3 422 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]),
Kotttaro 0:e76f506b9de3 423 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]),
Kotttaro 0:e76f506b9de3 424 s123 = (double)sin(th[leg][1] + th[leg][2] + th[leg][3]);
Kotttaro 0:e76f506b9de3 425 tip[leg][0] = (L[3]+L[0] * c1 + L[1] * c12 + L[2]*c123) * c0 + con[leg][0]; //x
Kotttaro 0:e76f506b9de3 426 tip[leg][1] = (L[3]+L[0] * c1 + L[1] * c12 + L[2] * c123) * s0 + con[leg][1]; //y
Kotttaro 0:e76f506b9de3 427 tip[leg][2] = L[0] * s1 + L[1] * s12+L[2]*s123; //z
Kotttaro 0:e76f506b9de3 428 //printf("fwd finish\n");
Kotttaro 0:e76f506b9de3 429 }
Kotttaro 0:e76f506b9de3 430 void Jac(int leg) {
Kotttaro 0:e76f506b9de3 431 //printf("Jac start\n");
Kotttaro 0:e76f506b9de3 432 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]),
Kotttaro 0:e76f506b9de3 433 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]),
Kotttaro 0:e76f506b9de3 434 s123 = (double)sin(th[leg][1] + th[leg][2] + th[leg][3]);
Kotttaro 0:e76f506b9de3 435 Jacbi[leg][0][0] = -s0 * (L[3]+L[0] * c1 + L[1] * c12 + L[2] * c123);
Kotttaro 0:e76f506b9de3 436 Jacbi[leg][0][1] = (-L[0] * s1 - L[1] * s12 - L[2] * s123) * c0;
Kotttaro 0:e76f506b9de3 437 Jacbi[leg][0][2] = (-L[1] * s12 - L[2] * s123) * c0;
Kotttaro 0:e76f506b9de3 438 Jacbi[leg][0][3] = (-L[2] * s123) * c0;
Kotttaro 0:e76f506b9de3 439
Kotttaro 0:e76f506b9de3 440 Jacbi[leg][1][0] = c0 * (L[3]+L[0] * c1 + L[1] * c12 + L[2] * c123);
Kotttaro 0:e76f506b9de3 441 Jacbi[leg][1][1] = (-L[0] * s1 - L[1] * s12 - L[2] * s123) * s0;
Kotttaro 0:e76f506b9de3 442 Jacbi[leg][1][2] = (-L[1] * s12 - L[2] * s123) * s0;
Kotttaro 0:e76f506b9de3 443 Jacbi[leg][1][3] = (-L[2] * s123) * s0;
Kotttaro 0:e76f506b9de3 444
Kotttaro 0:e76f506b9de3 445 Jacbi[leg][2][0] = 0.0;
Kotttaro 0:e76f506b9de3 446 Jacbi[leg][2][1] = L[0] * c1 + L[1] * c12 + L[2] * c123;
Kotttaro 0:e76f506b9de3 447 Jacbi[leg][2][2] = L[1] * c12 + L[2] * c123;
Kotttaro 0:e76f506b9de3 448 Jacbi[leg][2][3] = L[2] * c123;
Kotttaro 0:e76f506b9de3 449
Kotttaro 0:e76f506b9de3 450
Kotttaro 0:e76f506b9de3 451 //printf("Jac finish\n");
Kotttaro 0:e76f506b9de3 452 }//ok
Kotttaro 0:e76f506b9de3 453 void deff(int leg) {
Kotttaro 0:e76f506b9de3 454 //printf(" 評価関数定義\n");
Kotttaro 0:e76f506b9de3 455 fi = r + atan2(-tip[leg][2], (double)sqrt((tip[leg][0]) * (tip[leg][0]) + (tip[leg][1]) * (tip[leg][1])));//y,xの順
Kotttaro 0:e76f506b9de3 456 a0 = (double)sqrt((tip[leg][0]) * (tip[leg][0]) + (tip[leg][1]) * (tip[leg][1]) + (tip[leg][2]) * (tip[leg][2]));
Kotttaro 0:e76f506b9de3 457 a = a0 * (double)cos(fi);
Kotttaro 0:e76f506b9de3 458 h = a * (1 / (double)cos(fi) - tan(fi));
Kotttaro 0:e76f506b9de3 459 X = tip[leg][2]*(double)sqrt((tip[leg][0]* (tip[leg][0])) + (tip[leg][1]) * (tip[leg][1]));//tan-1の中身
Kotttaro 0:e76f506b9de3 460 //tan-1の分母分子
Kotttaro 0:e76f506b9de3 461 tan_u = tip[leg][2];
Kotttaro 0:e76f506b9de3 462 tan_d = (double)sqrt((tip[leg][0]) * (tip[leg][0]) + (tip[leg][1]) * (tip[leg][1]));
Kotttaro 0:e76f506b9de3 463 //printf("評価関数計算完了\n");
Kotttaro 0:e76f506b9de3 464 }
Kotttaro 0:e76f506b9de3 465 void dfd(int leg) {
Kotttaro 0:e76f506b9de3 466 //printf("評価関数微分\n");
Kotttaro 0:e76f506b9de3 467 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]);
Kotttaro 0:e76f506b9de3 468 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]);
Kotttaro 0:e76f506b9de3 469 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]);
Kotttaro 0:e76f506b9de3 470 double cfi=cos(fi),sfi=sin(fi);
Kotttaro 0:e76f506b9de3 471 double x=tip[leg][0],y=tip[leg][1],z=tip[leg][2];
Kotttaro 0:e76f506b9de3 472
Kotttaro 0:e76f506b9de3 473 double df_da=1/cfi-tan(fi);
Kotttaro 0:e76f506b9de3 474 double df_dfi=a*(-sfi-1)/(cfi*cfi);
Kotttaro 0:e76f506b9de3 475 double da_dx=x*cfi/sqrt(x*x+y*y);
Kotttaro 0:e76f506b9de3 476 double da_dy=y*cfi/sqrt(x*x+y*y);
Kotttaro 0:e76f506b9de3 477 double da_dfi=-sqrt(x*x+y*y)*sfi;
Kotttaro 0:e76f506b9de3 478 double dfi_dx=-x*z/((x*x+y*y+z*z)*sqrt(x*x+y*y));
Kotttaro 0:e76f506b9de3 479 double dfi_dy=-y*z/((x*x+y*y+z*z)*sqrt(x*x+y*y));
Kotttaro 0:e76f506b9de3 480 double dfi_dz=sqrt(x*x+y*y)*z/(x*x+y*y+z*z);
Kotttaro 0:e76f506b9de3 481
Kotttaro 0:e76f506b9de3 482 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]))
Kotttaro 0:e76f506b9de3 483 +df_dfi*(dfi_dx*Jacbi[leg][0][0]+dfi_dy*Jacbi[leg][1][0]+dfi_dz*Jacbi[leg][2][0]);
Kotttaro 0:e76f506b9de3 484
Kotttaro 0:e76f506b9de3 485 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]))
Kotttaro 0:e76f506b9de3 486 +df_dfi*(dfi_dx*Jacbi[leg][0][1]+dfi_dy*Jacbi[leg][1][1]+dfi_dz*Jacbi[leg][2][1]);
Kotttaro 0:e76f506b9de3 487
Kotttaro 0:e76f506b9de3 488 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]))
Kotttaro 0:e76f506b9de3 489 +df_dfi*(dfi_dx*Jacbi[leg][0][2]+dfi_dy*Jacbi[leg][1][2]+dfi_dz*Jacbi[leg][2][2]);
Kotttaro 0:e76f506b9de3 490
Kotttaro 0:e76f506b9de3 491 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]))
Kotttaro 0:e76f506b9de3 492 +df_dfi*(dfi_dx*Jacbi[leg][0][3]+dfi_dy*Jacbi[leg][1][3]+dfi_dz*Jacbi[leg][2][3]);
Kotttaro 0:e76f506b9de3 493
Kotttaro 0:e76f506b9de3 494 //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);
Kotttaro 0:e76f506b9de3 495 }
Kotttaro 0:e76f506b9de3 496 //使わない
Kotttaro 0:e76f506b9de3 497 double fe(int leg,double dth3) {
Kotttaro 0:e76f506b9de3 498 //brent法のための関数, 事前にdfdを実行してから使う
Kotttaro 0:e76f506b9de3 499 double dfd_nolm,th0_nolm,e=0.0;
Kotttaro 0:e76f506b9de3 500 //∇hを正規化する
Kotttaro 0:e76f506b9de3 501 dfd(leg);
Kotttaro 0:e76f506b9de3 502 dfd_nolm = sqrt(dfdth[0]* dfdth[0]+ dfdth[1]* dfdth[1]+ dfdth[2]* dfdth[2]+ dfdth[3]* dfdth[3]);
Kotttaro 0:e76f506b9de3 503 for (int i = 0; i < 4; i++) {
Kotttaro 0:e76f506b9de3 504 dfdth[i]=dfdth[i]/dfd_nolm;
Kotttaro 0:e76f506b9de3 505 }
Kotttaro 0:e76f506b9de3 506
Kotttaro 0:e76f506b9de3 507 solve(dth3, leg, 2);//後退代入でほかの3つのパラメータを導出
Kotttaro 0:e76f506b9de3 508
Kotttaro 0:e76f506b9de3 509 //dthベクトルを正規化
Kotttaro 0:e76f506b9de3 510 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]);
Kotttaro 0:e76f506b9de3 511 for (int i = 0; i < 4; i++) {
Kotttaro 0:e76f506b9de3 512 th0[leg][i] = th0[leg][i] / th0_nolm;
Kotttaro 0:e76f506b9de3 513 }
Kotttaro 0:e76f506b9de3 514 for (int i = 0; i < 4; i++) {
Kotttaro 0:e76f506b9de3 515 e += (th0[leg][i] - dfdth[i]) * (th0[leg][i] - dfdth[i]);
Kotttaro 0:e76f506b9de3 516 }
Kotttaro 0:e76f506b9de3 517 return e;//eベクトルのノルムの2乗を返す
Kotttaro 0:e76f506b9de3 518
Kotttaro 0:e76f506b9de3 519 }
Kotttaro 0:e76f506b9de3 520 double f(int leg,double dth3) {
Kotttaro 0:e76f506b9de3 521 double f_return=0.0;
Kotttaro 0:e76f506b9de3 522 solve(dth3, leg, 2);//後退代入でほかの3つのパラメータを導出
Kotttaro 0:e76f506b9de3 523 f_return=dfdth[0]*th0[leg][0]+dfdth[1]*th0[leg][1]+dfdth[2]*th0[leg][2]+dfdth[3]*th0[leg][3];
Kotttaro 0:e76f506b9de3 524
Kotttaro 0:e76f506b9de3 525 return -f_return;//テイラー展開第二項を返すがbrent法は極小値を求めるため、符号を反転させる
Kotttaro 0:e76f506b9de3 526
Kotttaro 0:e76f506b9de3 527 }
Kotttaro 0:e76f506b9de3 528 void mnbrak(int leg,int discrimination)
Kotttaro 0:e76f506b9de3 529 {
Kotttaro 0:e76f506b9de3 530 double ulim,u,r,q,fu,fu_2,dum,fa,fb,fc;
Kotttaro 0:e76f506b9de3 531 //fa=f(ax);
Kotttaro 0:e76f506b9de3 532 //fb=f(bx);
Kotttaro 0:e76f506b9de3 533 if(discrimination==0){fa=fe(leg,ax);fb=fe(leg,bx);}
Kotttaro 0:e76f506b9de3 534 if(discrimination==1){fa=num_nolm(leg,ax);fb=num_nolm(leg,bx);}
Kotttaro 0:e76f506b9de3 535 if(discrimination==2){fa=f(leg,ax);fb=f(leg,bx);}
Kotttaro 0:e76f506b9de3 536 if(discrimination==3){fa=f_test(ax);fb=f_test(bx);}
Kotttaro 0:e76f506b9de3 537
Kotttaro 0:e76f506b9de3 538 if(fb>fa)
Kotttaro 0:e76f506b9de3 539 {
Kotttaro 0:e76f506b9de3 540 SHIFT(dum,ax,bx,dum);
Kotttaro 0:e76f506b9de3 541 SHIFT(dum,fb,fa,dum);
Kotttaro 0:e76f506b9de3 542 }
Kotttaro 0:e76f506b9de3 543 cx=bx+GOLD*(bx-ax);
Kotttaro 0:e76f506b9de3 544 //fc=f(cx);
Kotttaro 0:e76f506b9de3 545 if(discrimination==0){fc=fe(leg,cx);}
Kotttaro 0:e76f506b9de3 546 if(discrimination==1){fc=num_nolm(leg,cx);}
Kotttaro 0:e76f506b9de3 547 if(discrimination==2){fc=f(leg,cx);}
Kotttaro 0:e76f506b9de3 548 if(discrimination==3){fc=f_test(cx);}
Kotttaro 0:e76f506b9de3 549 while (fb>fc)
Kotttaro 0:e76f506b9de3 550 {
Kotttaro 0:e76f506b9de3 551 r=(bx-ax)*(fb-fc);
Kotttaro 0:e76f506b9de3 552 q=(bx-cx)*(fb-fa);
Kotttaro 0:e76f506b9de3 553 u=bx-((bx-cx)*q-(bx-cx)*r)/
Kotttaro 0:e76f506b9de3 554 (2.0*SIGN(FMAX(fabs(q-r),TINY),q-r));
Kotttaro 0:e76f506b9de3 555 ulim=bx+GLIMIT*(cx-bx);
Kotttaro 0:e76f506b9de3 556
Kotttaro 0:e76f506b9de3 557 if((bx-u)*(u-cx)>0.0)
Kotttaro 0:e76f506b9de3 558 {
Kotttaro 0:e76f506b9de3 559 //fu=f(u);
Kotttaro 0:e76f506b9de3 560 if(discrimination==0){fu=fe(leg,u);}
Kotttaro 0:e76f506b9de3 561 if(discrimination==1){fu=num_nolm(leg,u);}
Kotttaro 0:e76f506b9de3 562 if(discrimination==2){fu=f(leg,u);}
Kotttaro 0:e76f506b9de3 563 if(discrimination==3){fu=f_test(u);}
Kotttaro 0:e76f506b9de3 564 if(fu<fc)
Kotttaro 0:e76f506b9de3 565 {
Kotttaro 0:e76f506b9de3 566 ax=bx;
Kotttaro 0:e76f506b9de3 567 bx=u;
Kotttaro 0:e76f506b9de3 568 fa=fb;
Kotttaro 0:e76f506b9de3 569 fb=fu;
Kotttaro 0:e76f506b9de3 570 return;
Kotttaro 0:e76f506b9de3 571 }
Kotttaro 0:e76f506b9de3 572 else if(fu>fb)
Kotttaro 0:e76f506b9de3 573 {
Kotttaro 0:e76f506b9de3 574 cx=u;
Kotttaro 0:e76f506b9de3 575 fc=fu;
Kotttaro 0:e76f506b9de3 576 return;
Kotttaro 0:e76f506b9de3 577 }
Kotttaro 0:e76f506b9de3 578 u=cx*+GOLD*(cx-bx);
Kotttaro 0:e76f506b9de3 579 //fu=f(u);
Kotttaro 0:e76f506b9de3 580 if(discrimination==0){fu=fe(leg,u);}
Kotttaro 0:e76f506b9de3 581 if(discrimination==1){fu=num_nolm(leg,u);}
Kotttaro 0:e76f506b9de3 582 if(discrimination==2){fu=f(leg,u);}
Kotttaro 0:e76f506b9de3 583 if(discrimination==3){fu=f_test(u);}
Kotttaro 0:e76f506b9de3 584 }
Kotttaro 0:e76f506b9de3 585 else if((cx-u)*(u-ulim)>0.0)
Kotttaro 0:e76f506b9de3 586 {
Kotttaro 0:e76f506b9de3 587 //fu=f(u);
Kotttaro 0:e76f506b9de3 588 if(discrimination==0){fu=fe(leg,u);}
Kotttaro 0:e76f506b9de3 589 if(discrimination==1){fu=num_nolm(leg,u);}
Kotttaro 0:e76f506b9de3 590 if(discrimination==2){fu=f(leg,u);}
Kotttaro 0:e76f506b9de3 591 if(discrimination==3){fu=f_test(u);}
Kotttaro 0:e76f506b9de3 592 if(fu<fc)
Kotttaro 0:e76f506b9de3 593 {
Kotttaro 0:e76f506b9de3 594 SHIFT(bx,cx,u,cx+GOLD*(cx-bx));
Kotttaro 0:e76f506b9de3 595 if(discrimination==0){fu_2=fe(leg,u);}
Kotttaro 0:e76f506b9de3 596 if(discrimination==1){fu_2=num_nolm(leg,u);}
Kotttaro 0:e76f506b9de3 597 if(discrimination==2){fu_2=f(leg,u);}
Kotttaro 0:e76f506b9de3 598 if(discrimination==3){fu_2=f_test(u);}
Kotttaro 0:e76f506b9de3 599 //SHIFT(fb,fc,fu,f(u));
Kotttaro 0:e76f506b9de3 600 SHIFT(fb,fc,fu,fu_2);
Kotttaro 0:e76f506b9de3 601 }
Kotttaro 0:e76f506b9de3 602
Kotttaro 0:e76f506b9de3 603 }
Kotttaro 0:e76f506b9de3 604 else if((u-ulim)*(ulim-cx)>=0.0)
Kotttaro 0:e76f506b9de3 605 {
Kotttaro 0:e76f506b9de3 606 u=ulim;
Kotttaro 0:e76f506b9de3 607 //fu=f(u);
Kotttaro 0:e76f506b9de3 608 if(discrimination==0){fu=fe(leg,u);}
Kotttaro 0:e76f506b9de3 609 if(discrimination==1){fu=num_nolm(leg,u);}
Kotttaro 0:e76f506b9de3 610 if(discrimination==2){fu=f(leg,u);}
Kotttaro 0:e76f506b9de3 611 if(discrimination==3){fu=f_test(u);}
Kotttaro 0:e76f506b9de3 612 }
Kotttaro 0:e76f506b9de3 613 else
Kotttaro 0:e76f506b9de3 614 {
Kotttaro 0:e76f506b9de3 615 u=cx+GOLD*(cx-bx);
Kotttaro 0:e76f506b9de3 616 //fu=f(u);
Kotttaro 0:e76f506b9de3 617 if(discrimination==0){fu=fe(leg,u);}
Kotttaro 0:e76f506b9de3 618 if(discrimination==1){fu=num_nolm(leg,u);}
Kotttaro 0:e76f506b9de3 619 if(discrimination==2){fu=f(leg,u);}
Kotttaro 0:e76f506b9de3 620 if(discrimination==3){fu=f_test(u);}
Kotttaro 0:e76f506b9de3 621 }
Kotttaro 0:e76f506b9de3 622 SHIFT(ax,bx,cx,u);
Kotttaro 0:e76f506b9de3 623 SHIFT(fa,fb,fc,fu);
Kotttaro 0:e76f506b9de3 624 }
Kotttaro 0:e76f506b9de3 625 }
Kotttaro 0:e76f506b9de3 626 double brent(int leg,double min,double mid,double max,double tol,int discrimination)//成功したやつ
Kotttaro 0:e76f506b9de3 627 {
Kotttaro 0:e76f506b9de3 628
Kotttaro 0:e76f506b9de3 629 int iter;
Kotttaro 0:e76f506b9de3 630 double a,b,d,etemp,fu,fv,fw,fx,p,q,r,tol1,tol2,u,v,w,x,xm,xmin;
Kotttaro 0:e76f506b9de3 631 double e=0.0;
Kotttaro 0:e76f506b9de3 632 a=(ax <cx ? ax : cx);
Kotttaro 0:e76f506b9de3 633 b=(ax >cx ? ax : cx);
Kotttaro 0:e76f506b9de3 634 x=w=v=bx;
Kotttaro 0:e76f506b9de3 635 if(discrimination==0){fw=fv=fx=fe(leg,x);}
Kotttaro 0:e76f506b9de3 636 if(discrimination==1){fw=fv=fx=num_nolm(leg,x);}
Kotttaro 0:e76f506b9de3 637 if(discrimination==2){fw=fv=fx=f(leg,x);}
Kotttaro 0:e76f506b9de3 638 if(discrimination==3){fw=fv=fx=f_test(x);}
Kotttaro 0:e76f506b9de3 639 //fw=fv=fx=f(x);//関数部分
Kotttaro 0:e76f506b9de3 640 for(iter=1;iter<=ITMAX;iter++)
Kotttaro 0:e76f506b9de3 641 {
Kotttaro 0:e76f506b9de3 642 xm=0.5*(a+b);
Kotttaro 0:e76f506b9de3 643 tol2=2.0*(tol1=tol*fabs(x)+ZEPS);
Kotttaro 0:e76f506b9de3 644 //pc2.printf("x =%lf,w = %lf,u = %lf\n\r",x,w,u);
Kotttaro 0:e76f506b9de3 645 if(fabs(x-xm)<=(tol2-0.5*(b-a)))
Kotttaro 0:e76f506b9de3 646 {
Kotttaro 0:e76f506b9de3 647 //pc.printf("bernt out");
Kotttaro 0:e76f506b9de3 648 xmin=x;
Kotttaro 0:e76f506b9de3 649 //pc2.printf("xmin=%lf\r\n",x);
Kotttaro 0:e76f506b9de3 650 //pc2.printf("fx=%lf\r\n",fx);
Kotttaro 0:e76f506b9de3 651 return xmin;
Kotttaro 0:e76f506b9de3 652 }
Kotttaro 0:e76f506b9de3 653 if(fabs(e)>tol1)
Kotttaro 0:e76f506b9de3 654 {
Kotttaro 0:e76f506b9de3 655 r=(x-w)*(fx-fv);
Kotttaro 0:e76f506b9de3 656 q=(x-v)*(fx-fw);
Kotttaro 0:e76f506b9de3 657 p=(x-v)*q-(x-w)*r;
Kotttaro 0:e76f506b9de3 658 q=2.0*(q-r);
Kotttaro 0:e76f506b9de3 659 if(q>0.0)p=-p;
Kotttaro 0:e76f506b9de3 660 q=fabs(q);
Kotttaro 0:e76f506b9de3 661 etemp=e;
Kotttaro 0:e76f506b9de3 662 e=d;
Kotttaro 0:e76f506b9de3 663 if(fabs(p)>=fabs(0.5*q*etemp)||p<=q*(a-x)||p>=q*(b-x))
Kotttaro 0:e76f506b9de3 664 { d=CGOLD*(e= (x>=xm ? a-x : b-x));}
Kotttaro 0:e76f506b9de3 665 else
Kotttaro 0:e76f506b9de3 666 {
Kotttaro 0:e76f506b9de3 667 d=p/q;
Kotttaro 0:e76f506b9de3 668 u=x+d;
Kotttaro 0:e76f506b9de3 669 if(u-a < tol2 || b-u < tol2)
Kotttaro 0:e76f506b9de3 670 {d=SIGN(tol1,xm-x);}
Kotttaro 0:e76f506b9de3 671 }
Kotttaro 0:e76f506b9de3 672 }
Kotttaro 0:e76f506b9de3 673 else
Kotttaro 0:e76f506b9de3 674 {
Kotttaro 0:e76f506b9de3 675 d=CGOLD*(e= (x>=xm ? a-x : b-x));
Kotttaro 0:e76f506b9de3 676 }
Kotttaro 0:e76f506b9de3 677 u=(fabs(d) >= tol1 ? x+d : x+SIGN(tol1,d));
Kotttaro 0:e76f506b9de3 678 if(discrimination==0){fu=fe(leg,u);}
Kotttaro 0:e76f506b9de3 679 if(discrimination==1){fu=num_nolm(leg,u);}
Kotttaro 0:e76f506b9de3 680 if(discrimination==2){fu=f(leg,u);}
Kotttaro 0:e76f506b9de3 681 if(discrimination==3){fu=f_test(u);}
Kotttaro 0:e76f506b9de3 682 //fu=f(u);//関数部分
Kotttaro 0:e76f506b9de3 683 if(fu <= fx)
Kotttaro 0:e76f506b9de3 684 {
Kotttaro 0:e76f506b9de3 685 if(u >= x)a=x; else b=x;
Kotttaro 0:e76f506b9de3 686 SHIFT(v,w,x,u);
Kotttaro 0:e76f506b9de3 687 SHIFT(fv,fw,fx,fu);
Kotttaro 0:e76f506b9de3 688 }
Kotttaro 0:e76f506b9de3 689 else{
Kotttaro 0:e76f506b9de3 690 if(u < x){a=u;}
Kotttaro 0:e76f506b9de3 691 else {b=u;}
Kotttaro 0:e76f506b9de3 692 if(fu <= fw || w==x)
Kotttaro 0:e76f506b9de3 693 {
Kotttaro 0:e76f506b9de3 694 v=w;
Kotttaro 0:e76f506b9de3 695 w=u;
Kotttaro 0:e76f506b9de3 696 fv=fw;
Kotttaro 0:e76f506b9de3 697 fw=fu;
Kotttaro 0:e76f506b9de3 698 }
Kotttaro 0:e76f506b9de3 699 else if (fu <= fv || v==x || v==w)
Kotttaro 0:e76f506b9de3 700 {
Kotttaro 0:e76f506b9de3 701 v=u;
Kotttaro 0:e76f506b9de3 702 fv=fu;
Kotttaro 0:e76f506b9de3 703 }
Kotttaro 0:e76f506b9de3 704 }
Kotttaro 0:e76f506b9de3 705
Kotttaro 0:e76f506b9de3 706 }
Kotttaro 0:e76f506b9de3 707 //pc2.printf("xmin=%lf\r\n",x);
Kotttaro 0:e76f506b9de3 708 //pc2.printf("fx=%lf\r\n",fx);
Kotttaro 0:e76f506b9de3 709 return xmin;
Kotttaro 0:e76f506b9de3 710 }
Kotttaro 0:e76f506b9de3 711
Kotttaro 0:e76f506b9de3 712 double SIGN(double x,double y)
Kotttaro 0:e76f506b9de3 713 {
Kotttaro 0:e76f506b9de3 714 double x_return;
Kotttaro 0:e76f506b9de3 715 x_return=abs(x);
Kotttaro 0:e76f506b9de3 716 if(y<0.0)x_return=-x_return;
Kotttaro 0:e76f506b9de3 717
Kotttaro 0:e76f506b9de3 718 return x_return;
Kotttaro 0:e76f506b9de3 719 }
Kotttaro 0:e76f506b9de3 720 double FMAX(double x, double y)
Kotttaro 0:e76f506b9de3 721 {
Kotttaro 0:e76f506b9de3 722 if(x>y){return x;}
Kotttaro 0:e76f506b9de3 723 if(y>x){return y;}
Kotttaro 0:e76f506b9de3 724 return 0;
Kotttaro 0:e76f506b9de3 725 }
Kotttaro 0:e76f506b9de3 726 double num_nolm(int leg,double dth3)
Kotttaro 0:e76f506b9de3 727 {
Kotttaro 0:e76f506b9de3 728
Kotttaro 0:e76f506b9de3 729 double nolm_return=0.0;
Kotttaro 0:e76f506b9de3 730 solve(dth3,leg,2);
Kotttaro 0:e76f506b9de3 731 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];
Kotttaro 0:e76f506b9de3 732
Kotttaro 0:e76f506b9de3 733 return nolm_return;
Kotttaro 0:e76f506b9de3 734 }
Kotttaro 0:e76f506b9de3 735 //brent法のテスト用の関数
Kotttaro 0:e76f506b9de3 736 //極小値を求めたい関数を定義
Kotttaro 0:e76f506b9de3 737 double f_test(double x){
Kotttaro 0:e76f506b9de3 738 double x_return;
Kotttaro 0:e76f506b9de3 739 x_return=x*x-2*x+1;
Kotttaro 0:e76f506b9de3 740 return x_return;
Kotttaro 0:e76f506b9de3 741 }
Kotttaro 0:e76f506b9de3 742 void solve(double w3, int leg,int det) {
Kotttaro 0:e76f506b9de3 743
Kotttaro 0:e76f506b9de3 744 double dth[4];
Kotttaro 0:e76f506b9de3 745 MatrixXd v_Q(3,1),QT(3,3);
Kotttaro 0:e76f506b9de3 746
Kotttaro 0:e76f506b9de3 747 QT = Q.transpose();
Kotttaro 0:e76f506b9de3 748 v_Q = QT * vP[leg]*sampling;
Kotttaro 0:e76f506b9de3 749
Kotttaro 0:e76f506b9de3 750 dth[3] = w3 ;
Kotttaro 0:e76f506b9de3 751 dth[2] = (double)((v_Q(2, 0) - R(2, 3) * dth[3]) / R(2, 2));
Kotttaro 0:e76f506b9de3 752 dth[1] = (double)((v_Q(1, 0) - R(1, 2) * dth[2] - R(1, 3) * dth[3]) / R(1, 1));
Kotttaro 0:e76f506b9de3 753 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));
Kotttaro 0:e76f506b9de3 754 if (det == 1) {
Kotttaro 0:e76f506b9de3 755 for (int i=0; i < 4; i++) {
Kotttaro 1:507dd5045511 756 //th_write[leg][i] = th_write[leg][i] + dth[i];
Kotttaro 1:507dd5045511 757 th[leg][i] = th[leg][i] + dth[i];
Kotttaro 0:e76f506b9de3 758 }
Kotttaro 0:e76f506b9de3 759 }
Kotttaro 0:e76f506b9de3 760 else if (det == 2) {
Kotttaro 0:e76f506b9de3 761 for (int u=0; u < 4; u++) {
Kotttaro 0:e76f506b9de3 762 th0[leg][u] = dth[u];
Kotttaro 0:e76f506b9de3 763 }
Kotttaro 0:e76f506b9de3 764 }
Kotttaro 0:e76f506b9de3 765
Kotttaro 0:e76f506b9de3 766 }
Kotttaro 0:e76f506b9de3 767
Kotttaro 0:e76f506b9de3 768 //以下9軸センサ関係
Kotttaro 0:e76f506b9de3 769 void BMX055_Init() //range 設定用関数
Kotttaro 0:e76f506b9de3 770 {
Kotttaro 0:e76f506b9de3 771 char cmd[3];
Kotttaro 0:e76f506b9de3 772 //------------------------------------------------------------//
Kotttaro 0:e76f506b9de3 773 cmd[0]=0x0F;
Kotttaro 0:e76f506b9de3 774 cmd[1]=0x03;
Kotttaro 0:e76f506b9de3 775 i2c_BMX.write(Addr_Accl,cmd,2,0);
Kotttaro 0:e76f506b9de3 776 wait_ms(100);
Kotttaro 0:e76f506b9de3 777 //------------------------------------------------------------//
Kotttaro 0:e76f506b9de3 778 cmd[0]=0x10;
Kotttaro 0:e76f506b9de3 779 cmd[1]=0x08;
Kotttaro 0:e76f506b9de3 780 i2c_BMX.write(Addr_Accl,cmd,2,0);
Kotttaro 0:e76f506b9de3 781 wait_ms(100);
Kotttaro 0:e76f506b9de3 782 //------------------------------------------------------------//
Kotttaro 0:e76f506b9de3 783 cmd[0]=0x11;
Kotttaro 0:e76f506b9de3 784 cmd[1]=0x00;
Kotttaro 0:e76f506b9de3 785 i2c_BMX.write(Addr_Accl,cmd,2,0);
Kotttaro 0:e76f506b9de3 786 wait_ms(100);
Kotttaro 0:e76f506b9de3 787 //------------------------------------------------------------//
Kotttaro 0:e76f506b9de3 788 cmd[0]=0x0F;
Kotttaro 0:e76f506b9de3 789 cmd[1]=0x04;
Kotttaro 0:e76f506b9de3 790 i2c_BMX.write(Addr_Accl,cmd,2,0);
Kotttaro 0:e76f506b9de3 791 wait_ms(100);
Kotttaro 0:e76f506b9de3 792 //------------------------------------------------------------//
Kotttaro 0:e76f506b9de3 793 cmd[0]=0x10;
Kotttaro 0:e76f506b9de3 794 cmd[1]=0x07;
Kotttaro 0:e76f506b9de3 795 i2c_BMX.write(Addr_Gyro,cmd,2,0);
Kotttaro 0:e76f506b9de3 796 wait_ms(100);
Kotttaro 0:e76f506b9de3 797 //------------------------------------------------------------//
Kotttaro 0:e76f506b9de3 798 cmd[0]=0x11;
Kotttaro 0:e76f506b9de3 799 cmd[1]=0x00;
Kotttaro 0:e76f506b9de3 800 i2c_BMX.write(Addr_Gyro,cmd,2,0);
Kotttaro 0:e76f506b9de3 801 wait_ms(100);
Kotttaro 0:e76f506b9de3 802 //------------------------------------------------------------//
Kotttaro 0:e76f506b9de3 803 cmd[0]=0x4B;
Kotttaro 0:e76f506b9de3 804 cmd[1]=0x83;
Kotttaro 0:e76f506b9de3 805 i2c_BMX.write(Addr_Mag,cmd,2,0);
Kotttaro 0:e76f506b9de3 806 wait_ms(100);
Kotttaro 0:e76f506b9de3 807 //------------------------------------------------------------//
Kotttaro 0:e76f506b9de3 808 cmd[0]=0x4B;
Kotttaro 0:e76f506b9de3 809 cmd[1]=0x01;
Kotttaro 0:e76f506b9de3 810 i2c_BMX.write(Addr_Mag,cmd,2,0);
Kotttaro 0:e76f506b9de3 811 wait_ms(100);
Kotttaro 0:e76f506b9de3 812 //------------------------------------------------------------//
Kotttaro 0:e76f506b9de3 813 cmd[0]=0x4C;
Kotttaro 0:e76f506b9de3 814 cmd[1]=0x00;
Kotttaro 0:e76f506b9de3 815 i2c_BMX.write(Addr_Mag,cmd,2,0);
Kotttaro 0:e76f506b9de3 816 wait_ms(100);
Kotttaro 0:e76f506b9de3 817 //------------------------------------------------------------//
Kotttaro 0:e76f506b9de3 818 cmd[0]=0x4E;
Kotttaro 0:e76f506b9de3 819 cmd[1]=0x84;
Kotttaro 0:e76f506b9de3 820 i2c_BMX.write(Addr_Mag,cmd,2,0);
Kotttaro 0:e76f506b9de3 821 wait_ms(100);
Kotttaro 0:e76f506b9de3 822 //------------------------------------------------------------//
Kotttaro 0:e76f506b9de3 823 cmd[0]=0x51;
Kotttaro 0:e76f506b9de3 824 cmd[1]=0x04;
Kotttaro 0:e76f506b9de3 825 i2c_BMX.write(Addr_Mag,cmd,2,0);
Kotttaro 0:e76f506b9de3 826 wait_ms(100);
Kotttaro 0:e76f506b9de3 827 //------------------------------------------------------------//
Kotttaro 0:e76f506b9de3 828 cmd[0]=0x52;
Kotttaro 0:e76f506b9de3 829 cmd[1]=0x16;
Kotttaro 0:e76f506b9de3 830 i2c_BMX.write(Addr_Mag,cmd,2,0);
Kotttaro 0:e76f506b9de3 831 wait_ms(100);
Kotttaro 0:e76f506b9de3 832 }
Kotttaro 0:e76f506b9de3 833 void BMX055_Accl()
Kotttaro 0:e76f506b9de3 834 {
Kotttaro 0:e76f506b9de3 835 char cmd[1];
Kotttaro 0:e76f506b9de3 836 char rtn[1];
Kotttaro 0:e76f506b9de3 837 int data[6];
Kotttaro 0:e76f506b9de3 838 for (int i = 0; i < 6; i++)
Kotttaro 0:e76f506b9de3 839 {
Kotttaro 0:e76f506b9de3 840 cmd[0]=2+i; //Wire.beginTransmission(Addr_Accl);
Kotttaro 0:e76f506b9de3 841 i2c_BMX.write(Addr_Accl,cmd,1,1); //Wire.write((2 + i));// Select data register
Kotttaro 0:e76f506b9de3 842 //Wire.endTransmission();
Kotttaro 0:e76f506b9de3 843 i2c_BMX.read(Addr_Accl,rtn,1,0); //Wire.requestFrom(Addr_Accl, 1);// Request 1 byte of data
Kotttaro 0:e76f506b9de3 844 data[i]=rtn[0];
Kotttaro 0:e76f506b9de3 845 }
Kotttaro 0:e76f506b9de3 846 // Convert the data to 12-bits
Kotttaro 0:e76f506b9de3 847 xAccl = ((data[1] * 256) + (data[0] & 0xF0)) / 16;
Kotttaro 0:e76f506b9de3 848 if (xAccl > 2047) xAccl -= 4096;
Kotttaro 0:e76f506b9de3 849 yAccl = ((data[3] * 256) + (data[2] & 0xF0)) / 16;
Kotttaro 0:e76f506b9de3 850 if (yAccl > 2047) yAccl -= 4096;
Kotttaro 0:e76f506b9de3 851 zAccl = ((data[5] * 256) + (data[4] & 0xF0)) / 16;
Kotttaro 0:e76f506b9de3 852 if (zAccl > 2047) zAccl -= 4096;
Kotttaro 0:e76f506b9de3 853 xAccl = xAccl * 0.0098; // renge +-2g
Kotttaro 0:e76f506b9de3 854 yAccl = yAccl * 0.0098; // renge +-2g
Kotttaro 0:e76f506b9de3 855 zAccl = zAccl * 0.0098; // renge +-2g
Kotttaro 0:e76f506b9de3 856 }
Kotttaro 0:e76f506b9de3 857 //=====================================================================================//
Kotttaro 0:e76f506b9de3 858 void BMX055_Gyro()
Kotttaro 0:e76f506b9de3 859 {
Kotttaro 0:e76f506b9de3 860 char cmd[1];
Kotttaro 0:e76f506b9de3 861 char rtn[6];
Kotttaro 0:e76f506b9de3 862 int data[6];
Kotttaro 0:e76f506b9de3 863 for (int i = 0; i < 6; i++)
Kotttaro 0:e76f506b9de3 864 {
Kotttaro 0:e76f506b9de3 865 cmd[0]=2+i;
Kotttaro 0:e76f506b9de3 866 i2c_BMX.write(Addr_Gyro,cmd,1,1); //Wire.beginTransmission(Addr_Gyro);
Kotttaro 0:e76f506b9de3 867 //Wire.write((2 + i)); // Select data register
Kotttaro 0:e76f506b9de3 868 //Wire.endTransmission();
Kotttaro 0:e76f506b9de3 869 //Wire.requestFrom(Addr_Gyro, 1); // Request 1 byte of data
Kotttaro 0:e76f506b9de3 870 // Read 6 bytes of data
Kotttaro 0:e76f506b9de3 871 // xGyro lsb, xGyro msb, yGyro lsb, yGyro msb, zGyro lsb, zGyro msb
Kotttaro 0:e76f506b9de3 872 i2c_BMX.read(Addr_Gyro,rtn,1,0);
Kotttaro 0:e76f506b9de3 873 data[i]=rtn[0];
Kotttaro 0:e76f506b9de3 874 //if (Wire.available() == 1)
Kotttaro 0:e76f506b9de3 875 //data[i] = Wire.read();
Kotttaro 0:e76f506b9de3 876 }
Kotttaro 0:e76f506b9de3 877 // Convert the data
Kotttaro 0:e76f506b9de3 878 xGyro = (data[1] * 256) + data[0];
Kotttaro 0:e76f506b9de3 879 if (xGyro > 32767) xGyro -= 65536;
Kotttaro 0:e76f506b9de3 880 yGyro = (data[3] * 256) + data[2];
Kotttaro 0:e76f506b9de3 881 if (yGyro > 32767) yGyro -= 65536;
Kotttaro 0:e76f506b9de3 882 zGyro = (data[5] * 256) + data[4];
Kotttaro 0:e76f506b9de3 883 if (zGyro > 32767) zGyro -= 65536;
Kotttaro 0:e76f506b9de3 884
Kotttaro 0:e76f506b9de3 885 xGyro = xGyro * 0.0038; // Full scale = +/- 125 degree/s
Kotttaro 0:e76f506b9de3 886 yGyro = yGyro * 0.0038; // Full scale = +/- 125 degree/s
Kotttaro 0:e76f506b9de3 887 zGyro = zGyro * 0.0038; // Full scale = +/- 125 degree/s
Kotttaro 0:e76f506b9de3 888 }
Kotttaro 0:e76f506b9de3 889 void BMX055_attitude()
Kotttaro 0:e76f506b9de3 890 {
Kotttaro 0:e76f506b9de3 891 double data1[10],data2[10];
Kotttaro 0:e76f506b9de3 892 //BMX055 加速度の読み取り
Kotttaro 0:e76f506b9de3 893 BMX055_Accl();
Kotttaro 0:e76f506b9de3 894
Kotttaro 0:e76f506b9de3 895 //BMX055 ジャイロの読み取り
Kotttaro 0:e76f506b9de3 896 BMX055_Gyro();
Kotttaro 0:e76f506b9de3 897 gydx=xGyro-gyro_off[0];
Kotttaro 0:e76f506b9de3 898 gydy=yGyro-gyro_off[1];
Kotttaro 0:e76f506b9de3 899 gydz=zGyro-gyro_off[2];
Kotttaro 0:e76f506b9de3 900
Kotttaro 0:e76f506b9de3 901 //加速度からroll,pitch
Kotttaro 0:e76f506b9de3 902 accro=atan2(yAccl,zAccl);
Kotttaro 0:e76f506b9de3 903 accpi=atan2((-xAccl),sqrt((yAccl*yAccl+zAccl*zAccl)));
Kotttaro 0:e76f506b9de3 904
Kotttaro 0:e76f506b9de3 905 //角速度からroll,pitch,yaw
Kotttaro 0:e76f506b9de3 906 gydpi=cos(prfr)*gydy-sin(prfr)*gydz;
Kotttaro 0:e76f506b9de3 907 gydro=gydx+sin(prfr)*(sin(prfp)/cos(prfp))*gydy+cos(prfr)*(sin(prfp)/cos(prfp))*gydz;
Kotttaro 0:e76f506b9de3 908 gydya=(sin(prfr)/cos(prfp))*gydy+(cos(prfr)/cos(prfp))*gydz;
Kotttaro 0:e76f506b9de3 909
Kotttaro 0:e76f506b9de3 910 //角速度のみから姿勢角を算出
Kotttaro 0:e76f506b9de3 911 roll=prfr+gydro;
Kotttaro 0:e76f506b9de3 912 pitch=prfp-gydpi; //ピッチのみ方向が逆
Kotttaro 0:e76f506b9de3 913 yaw=prfy+gydya;
Kotttaro 0:e76f506b9de3 914
Kotttaro 0:e76f506b9de3 915 //角速度:加速度＝GYROGAIN:ACCELGAINで融合
Kotttaro 0:e76f506b9de3 916 prfr=GYROGAIN*(roll)+ACCELGAIN*accro;
Kotttaro 0:e76f506b9de3 917 prfp=GYROGAIN*(pitch)+ACCELGAIN*accpi;
Kotttaro 0:e76f506b9de3 918 /*for(int i=0; i<10 ;i++)
Kotttaro 0:e76f506b9de3 919 {
Kotttaro 0:e76f506b9de3 920 data1[i]=sum_roll[i];
Kotttaro 0:e76f506b9de3 921 data2[i]=sum_pitch[i];
Kotttaro 0:e76f506b9de3 922 }
Kotttaro 0:e76f506b9de3 923 for(int i=0; i<10; i++)
Kotttaro 0:e76f506b9de3 924 {
Kotttaro 0:e76f506b9de3 925 sum_roll[i+1]=data1[i];
Kotttaro 0:e76f506b9de3 926 sum_pitch[i+1]=data2[i];
Kotttaro 0:e76f506b9de3 927
Kotttaro 0:e76f506b9de3 928 }
Kotttaro 0:e76f506b9de3 929 sum_roll[0]=prfr;
Kotttaro 0:e76f506b9de3 930 sum_pitch[0]=prfp;
Kotttaro 0:e76f506b9de3 931
Kotttaro 0:e76f506b9de3 932 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;
Kotttaro 0:e76f506b9de3 933 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;*/
Kotttaro 0:e76f506b9de3 934 }
Kotttaro 0:e76f506b9de3 935
Kotttaro 0:e76f506b9de3 936 //ロドリゲスの式をもとにroll,pitchを1軸周りの回転に変換する
Kotttaro 0:e76f506b9de3 937 void make_motion(){
Kotttaro 0:e76f506b9de3 938
Kotttaro 0:e76f506b9de3 939 BMX055_attitude();
Kotttaro 0:e76f506b9de3 940
Kotttaro 0:e76f506b9de3 941 double Cr,Sr,Cp,Sp,S_rod;
Kotttaro 0:e76f506b9de3 942 double E[3][3];//方向余弦行列
Kotttaro 0:e76f506b9de3 943
Kotttaro 0:e76f506b9de3 944 Cr=cos(prfr);
Kotttaro 0:e76f506b9de3 945 Sr=sin(prfr);
Kotttaro 0:e76f506b9de3 946 Cp=cos(prfp);
Kotttaro 0:e76f506b9de3 947 Sp=sin(prfp);
Kotttaro 0:e76f506b9de3 948
Kotttaro 0:e76f506b9de3 949 //以下方向余弦行列の値の計算
Kotttaro 0:e76f506b9de3 950 //実験で必要ないのでyaw角は無視する
Kotttaro 0:e76f506b9de3 951 E[0][0]=Cp;
Kotttaro 0:e76f506b9de3 952 E[1][0]=-Sr*Sp;
Kotttaro 0:e76f506b9de3 953 E[2][0]=Cr*Sp;
Kotttaro 0:e76f506b9de3 954
Kotttaro 0:e76f506b9de3 955 E[0][1]=0.0;
Kotttaro 0:e76f506b9de3 956 E[1][1]=Cr;
Kotttaro 0:e76f506b9de3 957 E[2][1]=Sr;
Kotttaro 0:e76f506b9de3 958
Kotttaro 0:e76f506b9de3 959 E[0][2]=-Sp;
Kotttaro 0:e76f506b9de3 960 E[1][2]=-Sr*Cp;
Kotttaro 0:e76f506b9de3 961 E[2][2]=Cr*Cp;
Kotttaro 0:e76f506b9de3 962
Kotttaro 0:e76f506b9de3 963 theta_rod_1=theta_rod;//偏差の微分用
Kotttaro 0:e76f506b9de3 964 //方向余弦行列の各成分をもとに1軸周りの回転に変換する
Kotttaro 0:e76f506b9de3 965 theta_rod=acos(((E[0][0]+E[1][1]+E[2][2]-1))/2);
Kotttaro 0:e76f506b9de3 966 S_rod=sin(theta_rod);
Kotttaro 0:e76f506b9de3 967 nx_rod=(E[1][2]-E[2][1])/(2*S_rod);
Kotttaro 0:e76f506b9de3 968 //ny_rod=(E[2][0]-E[0][2])/(2*S_rod);
Kotttaro 0:e76f506b9de3 969 ny_rod=-(E[2][0]-E[0][2])/(2*S_rod);//ロボットの座標系と一致させるため逆方向に
Kotttaro 0:e76f506b9de3 970 nz_rod=(E[0][1]-E[1][0])/(2*S_rod);
Kotttaro 0:e76f506b9de3 971
Kotttaro 0:e76f506b9de3 972 dedt=(theta_rod-theta_rod_1)/sampling;//偏差の時間微分
Kotttaro 0:e76f506b9de3 973 //pc2.printf("rod theta=%05.2lf,n=(%03.2lf,%03.2f,%03.2lf) nolm=%2.3lf\r\n",
Kotttaro 0:e76f506b9de3 974 //theta_rod*180/PI,nx_rod,ny_rod,nz_rod,nx_rod*nx_rod+ny_rod*ny_rod+nz_rod*nz_rod);
Kotttaro 0:e76f506b9de3 975 }
Kotttaro 0:e76f506b9de3 976
Kotttaro 0:e76f506b9de3 977 //サーボ関係
Kotttaro 0:e76f506b9de3 978 void setup_servo() {
Kotttaro 0:e76f506b9de3 979 pwm.begin();
Kotttaro 0:e76f506b9de3 980 pwm.setPWMFreq(200);
Kotttaro 0:e76f506b9de3 981 for(int i=0;i<4;i++)
Kotttaro 0:e76f506b9de3 982 {
Kotttaro 0:e76f506b9de3 983 for(int u=0;u<4;u++)
Kotttaro 0:e76f506b9de3 984 {
Kotttaro 0:e76f506b9de3 985 th[i][u]=th_ready[i][u];
Kotttaro 0:e76f506b9de3 986 th_write[i][u]=th_ready[i][u];
Kotttaro 0:e76f506b9de3 987 }
Kotttaro 0:e76f506b9de3 988 }
Kotttaro 0:e76f506b9de3 989 }
Kotttaro 0:e76f506b9de3 990 void servo_write(int ch,double ang)//引数は[°]
Kotttaro 0:e76f506b9de3 991 {
Kotttaro 0:e76f506b9de3 992 if(ch==0) ang=ang-135*PI/180;
Kotttaro 0:e76f506b9de3 993 if(ch==4) ang=ang-45*PI/180;
Kotttaro 0:e76f506b9de3 994 if(ch==8) ang=ang+45*PI/180;
Kotttaro 0:e76f506b9de3 995 if(ch==12) ang=ang+135*PI/180;
Kotttaro 0:e76f506b9de3 996 if( (ch!=2) && (ch!=5)&&(ch!=7) && (ch!=10) && (ch!=13)&&(ch!=15) )ang=-ang;
Kotttaro 0:e76f506b9de3 997 ang=servo0[ch]+ang*8000/270*180/PI;
Kotttaro 0:e76f506b9de3 998 servo_write7(ch,ang);
Kotttaro 0:e76f506b9de3 999 }
Kotttaro 0:e76f506b9de3 1000
Kotttaro 0:e76f506b9de3 1001 void servo_write7(int ch, double ang){
Kotttaro 0:e76f506b9de3 1002 ang = ((ang-3500)/8000)*1600+700;//サーボモータ内部エンコーダは8000段階
Kotttaro 0:e76f506b9de3 1003 //pc2.printf("%d ang=%5.0lf \r\n ",ch,ang) ; //初期状態を設定するときこの値を参考に設定したためそのまま利用　　
Kotttaro 0:e76f506b9de3 1004 pwm.setPWM(ch, 0, ang);
Kotttaro 0:e76f506b9de3 1005 }
Kotttaro 0:e76f506b9de3 1006 void servo_calib()
Kotttaro 0:e76f506b9de3 1007 {
Kotttaro 0:e76f506b9de3 1008 for(int u=0; u<4; u++)
Kotttaro 0:e76f506b9de3 1009 {
Kotttaro 0:e76f506b9de3 1010 for(int i=0; i<4; i++)
Kotttaro 0:e76f506b9de3 1011 {
Kotttaro 0:e76f506b9de3 1012 servo_write7(ch[u][i],servo0[ch[u][i]]);
Kotttaro 0:e76f506b9de3 1013 }
Kotttaro 0:e76f506b9de3 1014 }
Kotttaro 0:e76f506b9de3 1015 }
Kotttaro 0:e76f506b9de3 1016 //tg_readyで入力した角度に遷移
Kotttaro 0:e76f506b9de3 1017 void servo_ready()
Kotttaro 0:e76f506b9de3 1018 {
Kotttaro 0:e76f506b9de3 1019 for(int u=0; u<4; u++)
Kotttaro 0:e76f506b9de3 1020 {
Kotttaro 0:e76f506b9de3 1021 for(int i=0; i<4; i++)
Kotttaro 0:e76f506b9de3 1022 {
Kotttaro 0:e76f506b9de3 1023 servo_write(ch[u][i],th_ready[u][i]);
Kotttaro 1:507dd5045511 1024 th[u][i]=th_ready[u][i];
Kotttaro 0:e76f506b9de3 1025 }
Kotttaro 0:e76f506b9de3 1026 }
Kotttaro 0:e76f506b9de3 1027 }
Kotttaro 0:e76f506b9de3 1028 //ポテンショメータから角度を所得する関数
Kotttaro 0:e76f506b9de3 1029 double readarg(int ch,double s[16][10])
Kotttaro 0:e76f506b9de3 1030 {
Kotttaro 0:e76f506b9de3 1031 double ave;
Kotttaro 0:e76f506b9de3 1032 sig1=chPin[ch][0];
Kotttaro 0:e76f506b9de3 1033 sig2=chPin[ch][1];
Kotttaro 0:e76f506b9de3 1034 sig3=chPin[ch][2];
Kotttaro 0:e76f506b9de3 1035 sig4=chPin[ch][3];
Kotttaro 0:e76f506b9de3 1036 //移動平均用の準備
Kotttaro 0:e76f506b9de3 1037 if(s[ch][0]==0.0)
Kotttaro 0:e76f506b9de3 1038 {
Kotttaro 0:e76f506b9de3 1039 for(int t=0 ; t<9 ;t++)
Kotttaro 0:e76f506b9de3 1040 {
Kotttaro 0:e76f506b9de3 1041 s[ch][t]=data.read();
Kotttaro 0:e76f506b9de3 1042 }
Kotttaro 0:e76f506b9de3 1043 }
Kotttaro 0:e76f506b9de3 1044
Kotttaro 0:e76f506b9de3 1045 else{
Kotttaro 0:e76f506b9de3 1046 for(int u=0; u<9 ;u++)
Kotttaro 0:e76f506b9de3 1047 {
Kotttaro 0:e76f506b9de3 1048 s[ch][u]=s[ch][u+1];
Kotttaro 0:e76f506b9de3 1049 }
Kotttaro 0:e76f506b9de3 1050
Kotttaro 0:e76f506b9de3 1051 }
Kotttaro 0:e76f506b9de3 1052 //s[ch][9]=data.read_u16();
Kotttaro 0:e76f506b9de3 1053 s[ch][9]=data.read();
Kotttaro 0:e76f506b9de3 1054
Kotttaro 0:e76f506b9de3 1055 //最新の値を優先する重み付け
Kotttaro 0:e76f506b9de3 1056 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;
Kotttaro 0:e76f506b9de3 1057
Kotttaro 0:e76f506b9de3 1058 return ave;
Kotttaro 0:e76f506b9de3 1059
Kotttaro 0:e76f506b9de3 1060 }
Kotttaro 0:e76f506b9de3 1061 void setup_th()
Kotttaro 0:e76f506b9de3 1062 {
Kotttaro 0:e76f506b9de3 1063 servo_calib();
Kotttaro 0:e76f506b9de3 1064 wait(3);
Kotttaro 0:e76f506b9de3 1065 for(int t=0; t<10; t++)
Kotttaro 0:e76f506b9de3 1066 {
Kotttaro 0:e76f506b9de3 1067 for(int i=0; i<4; i++)
Kotttaro 0:e76f506b9de3 1068 {
Kotttaro 0:e76f506b9de3 1069 for(int u=0; u<4; u++)
Kotttaro 0:e76f506b9de3 1070 {
Kotttaro 0:e76f506b9de3 1071 data0[ch[i][u]]+=readarg(ch[i][u],s)/10.0;
Kotttaro 0:e76f506b9de3 1072 }
Kotttaro 0:e76f506b9de3 1073 }
Kotttaro 0:e76f506b9de3 1074 }
Kotttaro 0:e76f506b9de3 1075 servo_ready();
Kotttaro 0:e76f506b9de3 1076 wait(3);
Kotttaro 0:e76f506b9de3 1077 /*for(int i=0; i<4; i++)
Kotttaro 0:e76f506b9de3 1078 {
Kotttaro 0:e76f506b9de3 1079 for(int u=0; u<4; u++)
Kotttaro 0:e76f506b9de3 1080 {
Kotttaro 0:e76f506b9de3 1081 data0[ch[i][u]]=data0[ch[i][u]]/10;
Kotttaro 0:e76f506b9de3 1082 th[i][u]=readarg(ch[i][u],s)-data0[ch[i][u]];
Kotttaro 0:e76f506b9de3 1083 }
Kotttaro 0:e76f506b9de3 1084 }*/
Kotttaro 0:e76f506b9de3 1085 }
Kotttaro 0:e76f506b9de3 1086 void update_th()
Kotttaro 0:e76f506b9de3 1087 {
Kotttaro 0:e76f506b9de3 1088 for(int i=0; i<4; i++)
Kotttaro 0:e76f506b9de3 1089 {
Kotttaro 0:e76f506b9de3 1090 for(int u=0;u<4;u++)
Kotttaro 0:e76f506b9de3 1091 {
Kotttaro 0:e76f506b9de3 1092 th[i][u]=readarg(ch[i][u],s)-data0[ch[i][u]];
Kotttaro 0:e76f506b9de3 1093 if(u==0)
Kotttaro 0:e76f506b9de3 1094 {
Kotttaro 0:e76f506b9de3 1095 if(i==0)th[i][u]+=135*PI/180;
Kotttaro 0:e76f506b9de3 1096 if(i==1)th[i][u]+=45*PI/180;
Kotttaro 0:e76f506b9de3 1097 if(i==2)th[i][u]-=45*PI/180;
Kotttaro 0:e76f506b9de3 1098 if(i==3)th[i][u]-=135*PI/180;
Kotttaro 0:e76f506b9de3 1099 }
Kotttaro 0:e76f506b9de3 1100 //pc2.printf("%d:%3.2lf ",ch[i][u],th[i][u]*180/PI);
Kotttaro 0:e76f506b9de3 1101 }
Kotttaro 0:e76f506b9de3 1102 }
Kotttaro 0:e76f506b9de3 1103 //pc2.printf("\r\n");
Kotttaro 0:e76f506b9de3 1104 }
Kotttaro 0:e76f506b9de3 1105 void th_calib()
Kotttaro 0:e76f506b9de3 1106 {
Kotttaro 0:e76f506b9de3 1107 double th_ready_keep[4][4];
Kotttaro 0:e76f506b9de3 1108 pc2.printf("\r\n th calib start \r\n");
Kotttaro 0:e76f506b9de3 1109 for(int i=0; i<4; i++)
Kotttaro 0:e76f506b9de3 1110 {
Kotttaro 0:e76f506b9de3 1111 for(int u=0; u<4; u++)
Kotttaro 0:e76f506b9de3 1112 {
Kotttaro 0:e76f506b9de3 1113 th_ready_keep[i][u]=th_ready[i][u];
Kotttaro 0:e76f506b9de3 1114 }
Kotttaro 0:e76f506b9de3 1115 }
Kotttaro 0:e76f506b9de3 1116 while(1)
Kotttaro 0:e76f506b9de3 1117 {
Kotttaro 0:e76f506b9de3 1118 int flag=0;
Kotttaro 0:e76f506b9de3 1119
Kotttaro 0:e76f506b9de3 1120 servo_ready();
Kotttaro 0:e76f506b9de3 1121 update_th();
Kotttaro 0:e76f506b9de3 1122
Kotttaro 0:e76f506b9de3 1123 for(int i=0; i<4; i++)
Kotttaro 0:e76f506b9de3 1124 {
Kotttaro 0:e76f506b9de3 1125 for(int u=1; u<4; u++)
Kotttaro 0:e76f506b9de3 1126 {
Kotttaro 0:e76f506b9de3 1127 if(((th[i][u]-th_ready_keep[i][u])*180/PI)>2.0)
Kotttaro 0:e76f506b9de3 1128 {
Kotttaro 0:e76f506b9de3 1129 th_ready[i][u]=th_ready[i][u]-0.5*PI/180;
Kotttaro 0:e76f506b9de3 1130 th_write[i][u]=th_ready[i][u];
Kotttaro 0:e76f506b9de3 1131 flag=1;
Kotttaro 0:e76f506b9de3 1132 pc2.printf("leg %d theta %d ",i,u);
Kotttaro 0:e76f506b9de3 1133 }
Kotttaro 0:e76f506b9de3 1134 if(((th[i][u]-th_ready_keep[i][u])*180/PI)<-2.0)
Kotttaro 0:e76f506b9de3 1135 {
Kotttaro 0:e76f506b9de3 1136 th_ready[i][u]=th_ready[i][u]+0.5*PI/180;
Kotttaro 0:e76f506b9de3 1137 th_write[i][u]=th_ready[i][u];
Kotttaro 0:e76f506b9de3 1138 flag=1;
Kotttaro 0:e76f506b9de3 1139 pc2.printf("leg %d theta %d ",i,u);
Kotttaro 0:e76f506b9de3 1140 }
Kotttaro 0:e76f506b9de3 1141 }
Kotttaro 0:e76f506b9de3 1142 }
Kotttaro 0:e76f506b9de3 1143 pc2.printf("\r\n");
Kotttaro 0:e76f506b9de3 1144 wait(0.1);
Kotttaro 0:e76f506b9de3 1145 if(flag==0)break;
Kotttaro 0:e76f506b9de3 1146 }
Kotttaro 0:e76f506b9de3 1147 }