Kotaro Irisawa
2021.12.22.16:06
Dependencies: mbed pca9685_2021_12_22 Eigen
Diff: main.cpp
- Revision:
- 4:8a50c7822dac
- Parent:
- 3:f824e4d8eef7
- Child:
- 5:f225e0c61cfc
--- a/main.cpp Wed Dec 22 07:06:14 2021 +0000
+++ b/main.cpp Sun Dec 26 07:43:22 2021 +0000
@@ -1,36 +1,55 @@
-#include "mbed.h"
-//特別研究Ⅰで用いたプログラム
+//特別研究Ⅰで用いたプログラムを改良したもの
//ねじ運動を入力し,0.01秒ごとに1脚について各関節角度を出力する
//brent法の部分は『numerical recipes in c』 参照
+#include "mbed.h"
+#include <PCA9685.h>
#include "Eigen/Geometry.h"
#include "Eigen/Dense.h"
#include <math.h>
+
#define ITMAX 100
#define CGOLD 0.3819660
#define SHIFT(a,b,c,d) (a)=(b);(b)=(c);(c)=(d);
#define ZEPS 1.0e-10
+#define GOLD 1.618034
+#define TINY 1.0e-20
+#define GLIMIT 100.0
+#define SERVOMIN 700
+#define SERVOMAX 2300
+#define SERVOGAIN 29.6296300
+#define PI 3.14159265358979323846264338327950288
-//#pragma warning(disable: 4996)
+PCA9685 pwm;//クラス宣言
Serial pc2(USBTX,USBRX);
Timer tim;
-int times= 1;//実行回数:実行時間は??秒
-double PI =3.14159265358979323846264338327950288;
+Timer manage;
+int times= 100;//実行回数:実行時間は (sampling)*(times)秒
using namespace Eigen;
//以下変数定義
+
+//brent法に必要な変数
+double ax=-0.5*PI/180.0,bx=-0.2*PI/180.0,cx=0.0;
+double fa,fb,fc;
+//サーボの書き込みに必要な変数
+double servo0[16]={7800.0, 7250.0, 6000.0, 7150.0, 6100.0, 6300.0, 8400.0, 7200.0, 6800.0, 7000.0, 5700.0, 8350.0, 6100.0, 8500.0, 5600.0, 6570.0};//servoの初期値
+int ch[4][4]={{0 ,1 ,2 ,3} ,
+ {4 ,5 ,6 ,7} ,
+ {8 ,9 ,10,11},
+ {12,13,14,15} };
double r=50*PI/180;//斜面の傾き[°]
-double sampling=0.01;//δtの時間[s]
+double sampling=0.1;//δtの時間[s]
double L[4] = {50.0,50.0,50.0,50.0};//4本のリンク長 後から足したのでL[3]を理論中のL0に対応させる
double tip[4][3];//足先座標
double con[4][3] = { 50.0, 50.0,0,
-50.0, 50.0,0,
-50.0,-50.0,0,
50.0,-50.0,0};//脚のコーナー座標,zは必ず0
-double th[4][4] = { 45 * PI / 180,30 * PI / 180,-30 * PI / 180,-30 * PI / 180,
- 135 * PI / 180,30 * PI / 180,-30 * PI / 180,-15 * PI / 180,
- -135 * PI / 180,30 * PI / 180,-30 * PI / 180,-15 * PI / 180,
- -45 * PI / 180,30 * PI / 180,-30 * PI / 180,-15 * PI / 180 };
+double th[4][4] = { {135 * PI / 180, 60 * PI / 180, -30 * PI / 180, -30 * PI / 180},
+ {45 * PI / 180, 60 * PI / 180, -30 * PI / 180, -30 * PI / 180},
+ {-45 * PI / 180, 60 * PI / 180, -30 * PI / 180, -30 * PI / 180},
+ {-135 * PI / 180, 60 * PI / 180, -30 * PI / 180, -30 * PI / 180} };//サーボに入力する角度
double th0[4][4]= { 0.0,0.0,0.0,0.0,
0.0,0.0, 0.0,0.0,
0.0,0.0, 0.0,0.0,
@@ -62,25 +81,47 @@
double fe(int leg,double dth3);//brent法に合わせてeを関数化,search文を一部抜粋したもの
double num_nolm(int leg , double dth3);//ノルム最小の解を導く際に使用する関数
double f(int leg,double dth3);//テーラー展開第1項の値を返す, brent法用
+void mnbrak(int leg,int discrimination);//brentに必要な極小値の囲い込んだ3点を決定する関数
double brent(int leg,double min,double mid,double max,double tol,int discrimination);//brent法により1次元探索するプログラム
-//discrimination 0:谷側(fe), 1:山側(nolm), 2:谷側(f)
+//discrimination 0:谷側(fe), 1:山側(nolm), 2:谷側(f),3:テスト用の関数(f_test)
double SIGN(double x,double y);//xにyの符号をつけたものを返す
+double FMAX(double x,double y);//大きいほうの値が返される
+double f_test(double x);//テスト用の関数
+
+//以下サーボ関係
+void setup_servo();//サーボセットアップ用関数
+void servo_write(int ch,double ang);//angに
+void servo_write7(int ch, double ang);
+void servo_calib();//全ての角度を0度にする
+//以下9軸センサ関係
int main()
{
double t;
pc2.baud(921600);
+ setup_servo();
+ servo_calib();
+ wait(2);
+
+ for(int u=0; u<4; u++)
+ {
+ for(int i=0; i<4; i++)
+ {
+ servo_write(ch[u][i],th[u][i]);
+ }
+ }
+ wait(2);
int count = 0;
//入力したねじ運動を換算する
Lin[0] = 0.0; //ねじ軸x
- Lin[1] = 1.0; //ねじ軸y
+ Lin[1] = 0.0; //ねじ軸y
Lin[2] = 1.0;//ねじ軸z
L0[0] = 0.0;//ねじ軸原点座標
L0[1] = 0.0;
L0[2] = 0.0;
- vin = 5.0;
- win = 0.0;
+ vin = 0.0;
+ win = 3.0;
printf("\r\n\r\n");
nol = (double)sqrt(Lin[0] * Lin[0] + Lin[1] * Lin[1] + Lin[2] * Lin[2]);
for (int i = 0; i < 3; i++)
@@ -97,75 +138,70 @@
//times*δtの時間だけサーボを動かす
tim.start();
+ manage.start();
for (int i = 0; i < times;i++){
-
+ manage.reset();
count = count + 1;
double dth;
- //printf("%d \n", count);
+ //////////計算部/////////////////
fwd(0);
vp(0);
Jac(0);
QR(0);
deff(0);
- pc2.printf("////////////////\r\n");
- f(0,3000);
- pc2.printf("////////////////\r\n");
- f(0,-3000);
- pc2.printf("////////////////\r\n");
- f(0,-40.0);
- pc2.printf("////////////////\r\n");
- f(0,-500.0985);
- pc2.printf("////////////////\r\n");
- f(0,0.0);
- pc2.printf("////////////////\r\n");
- f(0,500.0985);
- pc2.printf("////////////////\r\n");
- f(0,40.0);
- pc2.printf("////////////////\r\n");
- //dth=brent(0,-500.0985,0.0,500.0985,0.001,0);
- //solve(dth, 0, 1);
-
+ dfd(0);
+ ax=-0.5*PI/180.0;bx=-0.49*PI/180.0;cx=0.0;
+ mnbrak(0,2);
+ dth=brent(0,ax,bx,cx,0.0001,2);
+ solve(dth, 0, 1);
+ ////////////////////////////////
+
+ for(int u=0; u<4; u++)
+ {
+ for(int i=0; i<4; i++)
+ {
+ servo_write(ch[u][i],th[u][i]);
+ }
+ }
t=tim.read();
- //pc2.printf("%2.4lf:(%3.3lf, %3.3lf, %3.3lf, %3.3lf)\n\r",t,th[0][0]*180/PI, th[0][1]*180/PI , th[0][2]*180/PI , th[0][3]*180/PI );
- //pc2.printf("%d %5.8lf\r\n",count ,dth);
+ pc2.printf("%2.4lf:(%3.3lf, %3.3lf, %3.3lf, %3.3lf)\n\r",t,th[0][0]*180/PI, th[0][1]*180/PI , th[0][2]*180/PI , th[0][3]*180/PI );
+
+ while(1)
+ {
+ if(manage.read()>sampling)break;
+ }
+
}
t=tim.read();
- //pc2.printf("%2.4lf\r\n",t);
+ wait(3);
+ servo_calib();
return 0; // ソフトの終了
}
void QR(int leg) {
- //printf("QR start");
-
double s, t;//要素計算用
MatrixXd ma(3, 4), ma1(3, 4);
ma << Jacbi[leg][0][0], Jacbi[leg][0][1], Jacbi[leg][0][2], Jacbi[leg][0][3],
Jacbi[leg][1][0], Jacbi[leg][1][1], Jacbi[leg][1][2], Jacbi[leg][1][3],
Jacbi[leg][2][0], Jacbi[leg][2][1], Jacbi[leg][2][2], Jacbi[leg][2][3];
- /*printf("Jac :%lf %lf %lf %lf\n", ma(0, 0), ma(0, 1), ma(0, 2), ma(0, 3));
- printf(" %lf %lf %lf %lf\n", ma(1, 0), ma(1, 1), ma(1, 2), ma(1, 3));
- printf(" %lf %lf %lf %lf\n", ma(2, 0), ma(2, 1), ma(2, 2), ma(2, 3));*/
- //printf("ma was made\n");
+
//ハウスホルダー変換1回目
MatrixXd A1(3, 3);
A1 << 1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0;
- //printf("A1 was made\n");
+
s = (double)sqrt(ma(0, 0) * ma(0, 0) + ma(1, 0) * ma(1, 0) + ma(2, 0) * ma(2, 0));//分母のやつ
- //printf("%f\n", s);
+
MatrixXd H1(3, 3);//1回目の行列
MatrixXd X11(3, 1), X12(1, 3);
Vector3d a11, a12;//a11が変換前,a12が変換後
- // printf("H1,X11,X12,a11,a12 was made\n");
a11 << ma(0, 0), ma(1, 0), ma(2, 0);
a12 << s, 0.0, 0.0;
X11 = a11 - a12;
X12 = X11.transpose();
- //printf("H1,X11,X12,a11,a12 was calculated\n");
t = (double)sqrt(X11(0, 0) * X11(0, 0) + X11(1, 0) * X11(1, 0) + X11(2, 0) * X11(2, 0));
- //printf("%f\n", t);//ok
H1 = A1 - 2.0 * (X11 * X12) / (t * t);
ma1 = H1 * ma;
//2回目
@@ -176,7 +212,6 @@
MatrixXd X21(2, 1), X22(1, 2);
a21 << ma1(1, 1), ma1(2, 1);
s = (double)sqrt(ma1(1, 1) * ma1(1, 1) + ma1(2, 1) * ma1(2, 1));
- //printf("%f\n", s);//ok
a22 << s, 0;
X21 = a21 - a22;
X22 = X21.transpose();
@@ -186,14 +221,10 @@
0.0, h2(0, 0), h2(0, 1),
0.0, h2(1, 0), h2(1, 1);
R = H2 * ma1;
- //printf("%lf %lf %lf \n,", R0(0,2), R0(1,2), R0(2,2));//r22が0 ok
- //printf("\n");
MatrixXd H1T(3, 3), H2T(3, 3);
H1T = H1.transpose();
H2T = H2.transpose();
- Q = H1T * H2T;
-
-
+ Q = H1T * H2T;
}
void vp(int leg) {//5年生の時に作成したもの
@@ -291,7 +322,7 @@
//pc2.printf("df_da=%lf df_dfi=%lf da_dx=%lf da_dy=%lf da_dfi=%lf dfi_dx=%lf dfi_dy=%lf dfi_dz=%lf\r\n",df_da,df_dfi,da_dx,da_dy,da_dfi,dfi_dx,dfi_dy,dfi_dz);
}
-
+//使わない
double fe(int leg,double dth3) {
//brent法のための関数, 事前にdfdを実行してから使う
double dfd_nolm,th0_nolm,e=0.0;
@@ -301,11 +332,6 @@
for (int i = 0; i < 4; i++) {
dfdth[i]=dfdth[i]/dfd_nolm;
}
- //double nolm;
- //nolm=dfdth[0]*dfdth[0]+dfdth[1]*dfdth[1]+dfdth[2]*dfdth[2]+dfdth[3]*dfdth[3];
- //pc2.printf("nolm=%lf\r\n",nolm);
- //pc2.printf("(dfdth[0],dfdth[1],dfdth[2],dfdth[3],dfd_nolm) = (%lf,%lf,%lf,%lf,%lf)\r\n",dfdth[0],dfdth[1],dfdth[2],dfdth[3],dfd_nolm);
- //正規化完了
solve(dth3, leg, 2);//後退代入でほかの3つのパラメータを導出
@@ -314,47 +340,143 @@
for (int i = 0; i < 4; i++) {
th0[leg][i] = th0[leg][i] / th0_nolm;
}
- //double nolm;
- //nolm=th0[leg][0] * th0[leg][0]+ th0[leg][1]* th0[leg][1]+ th0[leg][2]* th0[leg][2]+ th0[leg][3]*th0[leg][3];
- //pc2.printf("th0 nolm=%lf\r\n",nolm);
for (int i = 0; i < 4; i++) {
e += (th0[leg][i] - dfdth[i]) * (th0[leg][i] - dfdth[i]);
- }
- //pc2.printf("th0=%lf,th1=%lf,th2=%lf,th3=%lf\r\n",th0[leg][0],th0[leg][1],th0[leg][2],th0[leg][3]);
- pc2.printf("(dth,e) = (%lf,%2.10lf)\r\n",dth3,e);
+ }
return e;//eベクトルのノルムの2乗を返す
}
double f(int leg,double dth3) {
double f_return=0.0;
- dfd(leg);
solve(dth3, leg, 2);//後退代入でほかの3つのパラメータを導出
- pc2.printf("dfdth=(%lf,%lf,%lf,%lf)\r\n",dfdth[0],dfdth[1],dfdth[2],dfdth[3]);
- pc2.printf("th0=(%lf,%lf,%lf,%lf)\r\n",th0[leg][0],th0[leg][1],th0[leg][2],th0[leg][3]);
f_return=dfdth[0]*th0[leg][0]+dfdth[1]*th0[leg][1]+dfdth[2]*th0[leg][2]+dfdth[3]*th0[leg][3];
- pc2.printf("(dth,-f)=(%lf,%lf)\r\n",dth3,f_return);
- return f_return;//eベクトルのノルムの2乗を返す
+
+ return -f_return;//テイラー展開第二項を返すがbrent法は極小値を求めるため、符号を反転させる
- }
-double brent(int leg,double min,double mid,double max,double tol,int discrimination)
+ }
+void mnbrak(int leg,int discrimination)
{
+ double ulim,u,r,q,fu,fu_2,dum,fa,fb,fc;
+ //fa=f(ax);
+ //fb=f(bx);
+ if(discrimination==0){fa=fe(leg,ax);fb=fe(leg,bx);}
+ if(discrimination==1){fa=num_nolm(leg,ax);fb=num_nolm(leg,bx);}
+ if(discrimination==2){fa=f(leg,ax);fb=f(leg,bx);}
+ if(discrimination==3){fa=f_test(ax);fb=f_test(bx);}
+
+ if(fb>fa)
+ {
+ SHIFT(dum,ax,bx,dum);
+ SHIFT(dum,fb,fa,dum);
+ }
+ cx=bx+GOLD*(bx-ax);
+ //fc=f(cx);
+ if(discrimination==0){fc=fe(leg,cx);}
+ if(discrimination==1){fc=num_nolm(leg,cx);}
+ if(discrimination==2){fc=f(leg,cx);}
+ if(discrimination==3){fc=f_test(cx);}
+ while (fb>fc)
+ {
+ r=(bx-ax)*(fb-fc);
+ q=(bx-cx)*(fb-fa);
+ u=bx-((bx-cx)*q-(bx-cx)*r)/
+ (2.0*SIGN(FMAX(fabs(q-r),TINY),q-r));
+ ulim=bx+GLIMIT*(cx-bx);
+
+ if((bx-u)*(u-cx)>0.0)
+ {
+ //fu=f(u);
+ if(discrimination==0){fu=fe(leg,u);}
+ if(discrimination==1){fu=num_nolm(leg,u);}
+ if(discrimination==2){fu=f(leg,u);}
+ if(discrimination==3){fu=f_test(u);}
+ if(fu<fc)
+ {
+ ax=bx;
+ bx=u;
+ fa=fb;
+ fb=fu;
+ return;
+ }
+ else if(fu>fb)
+ {
+ cx=u;
+ fc=fu;
+ return;
+ }
+ u=cx*+GOLD*(cx-bx);
+ //fu=f(u);
+ if(discrimination==0){fu=fe(leg,u);}
+ if(discrimination==1){fu=num_nolm(leg,u);}
+ if(discrimination==2){fu=f(leg,u);}
+ if(discrimination==3){fu=f_test(u);}
+ }
+ else if((cx-u)*(u-ulim)>0.0)
+ {
+ //fu=f(u);
+ if(discrimination==0){fu=fe(leg,u);}
+ if(discrimination==1){fu=num_nolm(leg,u);}
+ if(discrimination==2){fu=f(leg,u);}
+ if(discrimination==3){fu=f_test(u);}
+ if(fu<fc)
+ {
+ SHIFT(bx,cx,u,cx+GOLD*(cx-bx));
+ if(discrimination==0){fu_2=fe(leg,u);}
+ if(discrimination==1){fu_2=num_nolm(leg,u);}
+ if(discrimination==2){fu_2=f(leg,u);}
+ if(discrimination==3){fu_2=f_test(u);}
+ //SHIFT(fb,fc,fu,f(u));
+ SHIFT(fb,fc,fu,fu_2);
+ }
+
+ }
+ else if((u-ulim)*(ulim-cx)>=0.0)
+ {
+ u=ulim;
+ //fu=f(u);
+ if(discrimination==0){fu=fe(leg,u);}
+ if(discrimination==1){fu=num_nolm(leg,u);}
+ if(discrimination==2){fu=f(leg,u);}
+ if(discrimination==3){fu=f_test(u);}
+ }
+ else
+ {
+ u=cx+GOLD*(cx-bx);
+ //fu=f(u);
+ if(discrimination==0){fu=fe(leg,u);}
+ if(discrimination==1){fu=num_nolm(leg,u);}
+ if(discrimination==2){fu=f(leg,u);}
+ if(discrimination==3){fu=f_test(u);}
+ }
+ SHIFT(ax,bx,cx,u);
+ SHIFT(fa,fb,fc,fu);
+ }
+ }
+double brent(int leg,double min,double mid,double max,double tol,int discrimination)//成功したやつ
+{
+
int iter;
double a,b,d,etemp,fu,fv,fw,fx,p,q,r,tol1,tol2,u,v,w,x,xm,xmin;
double e=0.0;
- //dfd(leg);
- a=(min < max ? min : max);
- b=(min > max ? min : max);
- x=w=v=mid;
- if(discrimination==0)fw=fv=fu=fe(leg,x);
- else if(discrimination==1)fw=fv=fu=num_nolm(leg,x);
- else if(discrimination==2)fw=fv=fu=-f(leg,x);
+ a=(ax <cx ? ax : cx);
+ b=(ax >cx ? ax : cx);
+ x=w=v=bx;
+ if(discrimination==0){fw=fv=fx=fe(leg,x);}
+ if(discrimination==1){fw=fv=fx=num_nolm(leg,x);}
+ if(discrimination==2){fw=fv=fx=f(leg,x);}
+ if(discrimination==3){fw=fv=fx=f_test(x);}
+ //fw=fv=fx=f(x);//関数部分
for(iter=1;iter<=ITMAX;iter++)
{
xm=0.5*(a+b);
tol2=2.0*(tol1=tol*fabs(x)+ZEPS);
+ //pc2.printf("x =%lf,w = %lf,u = %lf\n\r",x,w,u);
if(fabs(x-xm)<=(tol2-0.5*(b-a)))
{
+ //pc.printf("bernt out");
xmin=x;
+ //pc2.printf("xmin=%lf\r\n",x);
+ //pc2.printf("fx=%lf\r\n",fx);
return xmin;
}
if(fabs(e)>tol1)
@@ -382,9 +504,11 @@
d=CGOLD*(e= (x>=xm ? a-x : b-x));
}
u=(fabs(d) >= tol1 ? x+d : x+SIGN(tol1,d));
- if(discrimination==0)fu=fe(leg,x);
- else if(discrimination==1)fu=num_nolm(leg,x);
- else if(discrimination==2)fu=-f(leg,x);//最大方向にしたいが,brent法は極小なので符号を変える
+ if(discrimination==0){fu=fe(leg,u);}
+ if(discrimination==1){fu=num_nolm(leg,u);}
+ if(discrimination==2){fu=f(leg,u);}
+ if(discrimination==3){fu=f_test(u);}
+ //fu=f(u);//関数部分
if(fu <= fx)
{
if(u >= x)a=x; else b=x;
@@ -409,8 +533,11 @@
}
}
+ //pc2.printf("xmin=%lf\r\n",x);
+ //pc2.printf("fx=%lf\r\n",fx);
return xmin;
- }
+ }
+
double SIGN(double x,double y)
{
double x_return;
@@ -419,51 +546,82 @@
return x_return;
}
+double FMAX(double x, double y)
+{
+ if(x>y){return x;}
+ if(y>x){return y;}
+ return 0;
+ }
double num_nolm(int leg,double dth3)
{
+
double nolm_return=0.0;
- solve(leg,dth3,2);
- for(int i=0; i<4; i++ )
- {
- nolm_return+=th0[leg][i]*th0[leg][i];
- }
- //pc2.printf("%lf\r\n",nolm_return);
- //nolm_return=sqrt(nolm_return);
+ solve(dth3,leg,2);
+ nolm_return=th0[leg][0]*th0[leg][0]+th0[leg][1]*th0[leg][1]+th0[leg][2]*th0[leg][2]+th0[leg][3]*th0[leg][3];
+
return nolm_return;
- }
+ }
+//brent法のテスト用の関数
+//極小値を求めたい関数を定義
+double f_test(double x){
+ double x_return;
+ x_return=x*x-2*x+1;
+ return x_return;
+ }
void solve(double w3, int leg,int det) {
- //printf("後退代入関数開始\n");
+
double dth[4];
MatrixXd v_Q(3,1),QT(3,3);
QT = Q.transpose();
- //printf("Q転地完了\n");
v_Q = QT * vP[leg]*sampling;
- //printf("v_Q(%lf,%lf,%lf)\n", v_Q(0.0), v_Q(1.0), v_Q(2.0));
- //printf("v_Q計算完了\n");
+
dth[3] = w3 ;
- //printf("dth3計算終了\n");
dth[2] = (double)((v_Q(2, 0) - R(2, 3) * dth[3]) / R(2, 2));
- //printf("dth2計算終了\n");
dth[1] = (double)((v_Q(1, 0) - R(1, 2) * dth[2] - R(1, 3) * dth[3]) / R(1, 1));
- //printf("dth1計算終了\n");
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));
- //printf("dth0計算終了\n");
- //printf("dthすべて計算終了\n");
- pc2.printf("1:%lf 2:%lf 3:%lf 4:%lf \r\n",dth[0],dth[1],dth[2],dth[3]);
if (det == 1) {
for (int i=0; i < 4; i++) {
- th[leg][i] = th[leg][i] + dth[i]*sampling;
- //pc2.printf("%d:%lf/ ",i,dth[i]);
-
+ th[leg][i] = th[leg][i] + dth[i];
}
- //pc2.printf("///\r\n");
}
- if (det == 2) {
- for (int u=0; u < 4; u++) {
- th0[leg][u] = dth[u];
- }
- }
- //printf("後退代入終了\n");
+ else if (det == 2) {
+ for (int u=0; u < 4; u++) {
+ th0[leg][u] = dth[u];
+ }
+ }
+ else {pc2.printf("value det in function solve is incorrect\r\n");}
+
}
+//サーボ関係
+void setup_servo() {
+ pwm.begin();
+ pwm.setPWMFreq(200);
+}
+void servo_write(int ch,double ang)//引数は[°]
+{
+ if(ch==0) ang=ang-135*PI/180;
+ if(ch==4) ang=ang-45*PI/180;
+ if(ch==8) ang=ang+45*PI/180;
+ if(ch==12) ang=ang+135*PI/180;
+ if( (ch!=2) && (ch!=5)&&(ch!=7) && (ch!=10) && (ch!=13)&&(ch!=15) )ang=-ang;
+ ang=servo0[ch]+ang*8000/270*180/PI;
+ servo_write7(ch,ang);
+ }
+
+void servo_write7(int ch, double ang){
+ ang = ((ang-3500)/8000)*1600+700;//サーボモータ内部エンコーダは8000段階
+ //pc2.printf("%d ang=%5.0lf \r\n ",ch,ang) ; //初期状態を設定するときこの値を参考に設定したためそのまま利用
+ pwm.setPWM(ch, 0, ang);
+}
+void servo_calib()
+{
+ for(int u=0; u<4; u++)
+ {
+ for(int i=0; i<4; i++)
+ {
+ servo_write7(ch[u][i],servo0[ch[u][i]]);
+ }
+ }
+}