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?
User | Revision | Line number | New 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 | } |