Takushima Yukimasa
/
R19_ManuTakukAHWGeo
a
Diff: main.cpp
- Revision:
- 6:040d001acb12
- Parent:
- 5:fcc79e507610
- Child:
- 8:b79d21c8178b
diff -r fcc79e507610 -r 040d001acb12 main.cpp --- a/main.cpp Sat Aug 24 00:30:20 2019 +0000 +++ b/main.cpp Sat Aug 24 06:06:36 2019 +0000 @@ -1,56 +1,8 @@ -/*今日 やること*/ - -/******************************************************************************/ #include "main.h" -//#include "gyro.h" -#include <math.h> - -/******************************************************************************/ -#define CIRCLE sbdbt.CIRCLE -#define SQUARE sbdbt.SQUARE -#define CROSS sbdbt.CROSS -#define TRIANGLE sbdbt.TRIANGLE -#define LEFTkey sbdbt.LEFTkey -#define RIGHTkey sbdbt.RIGHTkey -#define UPkey sbdbt.UPkey -#define DOWNkey sbdbt.DOWNkey - -/******************************************************************************/ /* 型定義 --------------------------------------------------------------------*/ -MD_I2C_Data_TypeDef MD_Data[Motor_MAX] = { - { 8, 0, 0, 0, {0}}, //足回り - { 9, 1, 0, 0, {0}}, - {10, 0, 0, 0, {0}}, - {11, 1, 0, 0, {0}}, - {12, 0, 0, 0, {0}}, // 洗濯物回収 - {13, 1, 0, 0, {0}}, // 上下機構 - {14, 0, 0, 0, {0}}, // 洗濯物排出 - {15, 1, 0, 0, {0}} // 左右移動 -}; - -IN_I2C_Data_TypeDef In_Data[] = { - {1,0,0}, // 洗濯物排出(初期位置) - {1,1,0}, // 洗濯物排出(停止) - {1,2,0}, // 腕(最大 タオル) - {1,3,0}, // 腕(初期位置) - {3,0,0}, // 上下機構(上) - {3,1,0}, // (下) - {3,2,0}, // 腕(シーツ兼シャツ) -}; - -OUT_I2C_Data_TypeDef Out_Data[] = { - {0,0,0}, // 腕の上下 - {0,1,0}, // 爪 - {0,2,0}, // タオルの先 - {0,3,0}, // タオル展開 - {1,0,0}, // タオルの中央 -}; /* 関数宣言 ------------------------------------------------------------------*/ -void mecanum_Move(void); -void mecanum_Turn(void); -void mecanum_Stop(void); /* 変数宣言 ------------------------------------------------------------------*/ //かご回転用 @@ -69,14 +21,11 @@ //タオル横展開用 int unfold_f=0; -//90度回転のフラグ -int left_f=0, right_f=0; +//y_deg: ジャイロの生の値 +//yaw: 実際に使う角度 +double y_deg[2], yaw=0;; -//y_deg: 起動時から絶対の角度 -//yaw: 相対角 初期状態が0になるようにする -//offset: 初期状態を0にするために引く値 -double y_deg[2], offset; -double yaw=0; +//ジャイロさんぷりんぐ用 double sum=0, ave=0; int offset_cnt=0; @@ -89,36 +38,18 @@ //short_lim: 腕手前側 int short_lim, max_lim, start_lim; -//機体の前方を変更する -int dir=0; +//足回りのpwm値 +int duty[4]; -int limit_up,limit_down; // リミットスイッチ -int slide_start,slide_stop; //かごのリミット +// 足回り +int lx,ly,rx; -int lx,ly,rx,L,R; // 足回り -int g; +int limit_up,limit_down; // リミットスイッチ +int slide_start,slide_stop; //かごのリミット int as=0; -/*クラス宣言-------------------------------------------------------------------*/ -Ticker flipper1; // 割込み -Y_I2C i2c(PB_9, PB_8); // I2C -DigitalOut myled(LED1); // オンボードLED -SBDBT sbdbt(PA_0, PA_1, 9600); // SDBDT -MyMPU6050 mpu(PC_9, PA_8); // ジャイロセンサ - -DigitalOut blue(PA_10); -DigitalOut green(PB_4); -DigitalOut red(PB_5); - -//半自動の時に使う -Timer tim; - /*----------------------------------- main -----------------------------------*/ -/* @brief メインプログラム -* @param なし -* @retval なし -*/ int main() { //flipper1.attach(&flip1, 0.001); // 割り込み @@ -126,103 +57,53 @@ while(1) { //自動系のタイマースタート tim.start(); + drift_tim.start(); mpu.loop(); + //ジャイロから値を取得 y_deg[1] = y_deg[0]; y_deg[0] = (double)mpu.ypr[0] * 180 / M_PI; - + //180~-180をまたいだ時 if(y_deg[1]<-90 && y_deg[0]>90) y_deg[1]+=360; else if(y_deg[1]>90 && y_deg[0]<-90) y_deg[1]-=360; if(sbdbt.State_check()) { for(int i=0; i<7; i++) i2c.In(In_Data,i); - + //誤差をさんぷりんぐ if(offset_cnt<1000) { + PALETTE(RED); if(offset_cnt!=0) { sum+=(y_deg[0]-y_deg[1]); ave=sum/offset_cnt; } offset_cnt++; - } else yaw += (y_deg[0]-y_deg[1])-ave; -// yaw=90*dir; -// yaw%=360; - dif_val = y_deg[0]-Turn_val; + } else { + PALETTE(GREEN); + yaw -= (y_deg[0]-y_deg[1])-ave; + } + + dif_val = yaw-Turn_val; + //制限 + if(dif_val>=30) dif_val=30; + else if(dif_val<=-30) dif_val=-30; lx = ((64-sbdbt.LY)*100.0/64)*sin(yaw*M_PI/180)+((sbdbt.LX-64)*100.0/64)*cos(yaw*M_PI/180); ly = ((64-sbdbt.LY)*100.0/64)*cos(yaw*M_PI/180)-((sbdbt.LX-64)*100.0/64)*sin(yaw*M_PI/180); - rx = (sbdbt.RX - 64)*100/64; - L = lx+ly; - R = ly-lx; - - if(sbdbt.LX !=64 || sbdbt.LY !=64) { - - mecanum_Move(); + rx = (sbdbt.RX - 64)*30/64; - // 以下,ジャイロセンサーのプログラム - if(g) { - if(abs(ly) > 10) { - if(ly>10) { - (MD_Data+0)->PWMVal = MD_GET_PWM(MD_Data,0)-dif_val; - (MD_Data+1)->PWMVal = MD_GET_PWM(MD_Data,1)+dif_val; - } else { - (MD_Data+0)->PWMVal = MD_GET_PWM(MD_Data,0)+dif_val; - (MD_Data+1)->PWMVal = MD_GET_PWM(MD_Data,1)-dif_val; - } - } - - if(abs(lx) > 10) { - if(lx>10) { - (MD_Data+0)->PWMVal = MD_GET_PWM(MD_Data,0)-dif_val; - (MD_Data+2)->PWMVal = MD_GET_PWM(MD_Data,2)+dif_val; - } else { - (MD_Data+0)->PWMVal = MD_GET_PWM(MD_Data,0)+dif_val; - (MD_Data+2)->PWMVal = MD_GET_PWM(MD_Data,2)-dif_val; - } - } + //メカナム基本動作 + mecanum_Move(lx, ly, rx); - if((abs(ly)-abs(lx)) > 0) { - (MD_Data+2)->PWMVal = MD_GET_PWM(MD_Data,0); - (MD_Data+3)->PWMVal = MD_GET_PWM(MD_Data,1); - } else { - (MD_Data+1)->PWMVal = MD_GET_PWM(MD_Data,0); - (MD_Data+3)->PWMVal = MD_GET_PWM(MD_Data,2); - } - } // if(g) - }// if(LX !=64 || LY !=64) - else if(sbdbt.RX != 64) { // 旋回 - Turn_val=y_deg[0]; - mecanum_Turn(); - } else { - mecanum_Stop(); - } + //旋回している間タイマーをリセット + if(sbdbt.RX != 64) drift_tim.reset(); - // 90°旋回 - if(left_f) { - if(abs(yaw)<60) { - for(int i=0; i<4; i++) { - MD_SET_PWM(MD_Data, i,60); - MD_SET_DRIVE(MD_Data, i,MD_REVERSE); - } - } else { - left_f=0; - Turn_val=y_deg[0]; - mecanum_Stop(); - } - } - if(right_f) { - if(yaw>-60) { - for(int i=0; i<4; i++) { - MD_SET_PWM(MD_Data, i,60); - MD_SET_DRIVE(MD_Data, i,MD_FORWARD); - } - } else { - right_f=0; - Turn_val=y_deg[0]; - mecanum_Stop(); - } - } + //旋回して慣性で動いた後の角度に補正する + if(drift_tim.read_ms()<500) Turn_val=yaw; + + //角度補正 + AngleCorrection(yaw, Turn_val); /*---ハンガーかけるやつ----------------------------------------------*/ if(sbdbt.SELECT)as=1;//selectキー @@ -248,6 +129,7 @@ //タオルつかむ 四角 if(grab_f || sgrab_f) { + PALETTE(BLUE); switch(grab) { //腕を前に動かす case 0: @@ -255,6 +137,7 @@ i2c.Out_Set(Out_Data,1,1); MD_SET_DRIVE(MD_Data,4,MD_REVERSE); MD_SET_PWM(MD_Data,4,40); + //sgrab_fは途中のリミットで止まる if((sgrab_f && short_lim) || max_lim==1) { grab++; tim.reset(); @@ -305,6 +188,7 @@ //タオル引く 丸 else if(pull_f) { + PALETTE(CYAN); switch(pull) { //腕を前に動かす case 0: @@ -348,6 +232,7 @@ //かごを倒す else if(kago_f) { + PALETTE(MAGENTA); //腕の先を閉じておく i2c.Out_Set(Out_Data,1,0); @@ -436,7 +321,8 @@ // pc.printf("pull %2d%2d ",pull,pull_f); // pc.printf("grab %2d%2d ",grab,grab_f); // pc.printf("kago %2d%2d ",kago,kago_f); - pc.printf("0:%f 1:%f ave:%f yaw:%f",y_deg[0],y_deg[1],ave,yaw); +// pc.printf("yaw:%.1f dif:%.1f",yaw,dif_val); + pc.printf("0:%.1f 1:%.1f yaw:%.1f dif:%.1f",y_deg[0],y_deg[1],yaw,dif_val); // pc.printf("%2d ",dir); pc.printf("\n\r"); @@ -455,29 +341,13 @@ /* ----------------ボタン系--------------- */ - //補正角リセット + //角度リセット if(sbdbt.L1) { - Turn_val=y_deg[0]; - offset=y_deg[0]; - } - //90度回転して補正の角度を固定する - if(LEFTkey) { - if(once==0) { - dir++; -// left_f++; -// left_f%=2; - once++; - } - } else if(RIGHTkey) { - if(once==0) { - dir--; -// right_f++; -// right_f%=2; - once++; - } + yaw=0; + Turn_val=0; } //かごを倒す - else if(CROSS) { + if(CROSS) { if(once==0) { kago_f++; kago_f%=2; @@ -521,63 +391,33 @@ } // int main() /* メカナムの基本移動 */ -void mecanum_Move(void) +void mecanum_Move(int lx, int ly, int rx) { - int val1,val2; - - MD_SET_DRIVE(MD_Data, 0,(L > 0) ? MD_FORWARD : MD_REVERSE); - MD_SET_DRIVE(MD_Data, 1,(R > 0) ? MD_REVERSE : MD_FORWARD); - MD_SET_DRIVE(MD_Data, 2,(R > 0) ? MD_FORWARD : MD_REVERSE); - MD_SET_DRIVE(MD_Data, 3,(L > 0) ? MD_REVERSE : MD_FORWARD); - - val1 = (int)sqrt( 1.0*abs( abs(lx)*lx + abs(ly)*ly ) ); - val2 = (int)sqrt( 1.0*abs( abs(ly)*ly - abs(lx)*lx ) ); - - MD_SET_PWM(MD_Data,0,val1); - MD_SET_PWM(MD_Data,1,val2); - MD_SET_PWM(MD_Data,2,val2); - MD_SET_PWM(MD_Data,3,val1); - + duty[0]=lx+ly; + duty[1]=-(-lx+ly); + duty[2]=-lx+ly; + duty[3]=-(lx+ly); - if(MD_GET_PWM(MD_Data,0)*2 < MD_GET_PWM(MD_Data,1) ) { - MD_SET_PWM(MD_Data, 0, 0); - g = 0; - } else if( MD_GET_PWM(MD_Data,1)*2 < MD_GET_PWM(MD_Data,0) ) { - MD_SET_PWM(MD_Data, 1, 0); - g = 0; - } - - // 斜めじゃないなら大きいほうにPWMをそろえて完全縦横移動 - else if( MD_GET_PWM(MD_Data,0) < MD_GET_PWM(MD_Data,1) ) { - MD_SET_PWM(MD_Data, 0, MD_GET_PWM(MD_Data,1)); - g = 1; - - } else { - MD_SET_PWM(MD_Data, 1, MD_GET_PWM(MD_Data,0)); - g = 1; - } - - MD_SET_PWM(MD_Data, 2, MD_GET_PWM(MD_Data,1)); - MD_SET_PWM(MD_Data, 3, MD_GET_PWM(MD_Data,0)); - -} - -/* メカナム旋回 */ -void mecanum_Turn(void) -{ for(int i=0; i<4; i++) { - MD_SET_PWM(MD_Data, i,sbdbt.R1?abs(sbdbt.RX - 64)*50/64:abs(sbdbt.RX - 64)*100/64); - MD_SET_DRIVE(MD_Data, i,(sbdbt.RX < 64)? MD_REVERSE: MD_FORWARD); + duty[i]+=rx; + MD_SET_DRIVE(MD_Data, i, duty[i]==0? MD_BRAKE: (duty[i]>0? MD_FORWARD: MD_REVERSE)); + MD_SET_PWM(MD_Data, i, abs(duty[i])); } } -/* 停止 */ -void mecanum_Stop(void) +void AngleCorrection(double n_angle, double t_angle) { + double dif=-(t_angle-n_angle); + + if(dif>=30) dif=30; + else if(dif<=-30) dif=-30; + for(int i=0; i<4; i++) { - MD_SET_PWM(MD_Data, i, 0); - MD_SET_DRIVE(MD_Data, i, MD_BRAKE); + duty[i]+=dif; + MD_SET_DRIVE(MD_Data, i, duty[i]==0? MD_BRAKE: (duty[i]>0? MD_FORWARD: MD_REVERSE)); + MD_SET_PWM(MD_Data, i, abs(duty[i])); } + } /*操作法*/ @@ -588,18 +428,15 @@ □ ボタン == 回収(タオル) 上 ボタン == ハンガーかけ機構 上昇 下 ボタン == ハンガーかけ機構 下降 -左 ボタン == 90回転 -右 ボタン == 90回転 L1 ボタン == リセット(足回り) L2 ボタン == タオル掛け -R1 ボタン == 押しながら回転でゆっくりになる -R2 ボタン == タオルひっぱる奴 +R2 ボタン == タオル真ん中掴む select == ハンガー掛け 最大まで上昇 start == ハンガー展開 */ /*その他注意点*/ /* -動かすときはジャイロの値を安定させるため15秒から20秒待つ +動かすときはジャイロの値を安定させるためLEDが赤から緑になるまで待つ メカナムは振動が大きいせいかナット等が外れやすいため、定期的に確認をする(特に足回り) あまり急発進をさせない(たまに暴走) 暴走したときはコントローラーを動かしたら治るはず