![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
a
main.cpp
- Committer:
- Tom0108
- Date:
- 2019-08-22
- Revision:
- 0:761a63c6d020
- Child:
- 1:199c4a71da88
File content as of revision 0:761a63c6d020:
/*今日 やること*/ /**********************************************************************************************************************************************************************/ #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 #define SELECTkey LEFTkey&&RIGHTkey #define STARTkey UPkey&&UPkey /**********************************************************************************************************************************************************************/ /* 型定義 --------------------------------------------------------------------*/ 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[4] = { {1,0,0}, // 洗濯物排出(初期位置) {1,1,0}, // 洗濯物排出(停止) {1,2,0}, // 腕(最大 タオル) {1,3,0} // 腕(初期位置) }; IN_I2C_Data_TypeDef In_Data2[] = { {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 flip1(void); void mecanum_Move(void); void mecanum_Turn(void); void mecanum_TurnMove(void); void mecanum_Stop(void); //int sum_deg(double deg,double old); void big_sort(int16_t data[], uint8_t num); int median_filter(int16_t data[], int16_t add_data, const uint8_t num); /* 変数宣言 ------------------------------------------------------------------*/ int limit_up,limit_down; // リミットスイッチ int slide_start,slide_stop; int short_lim,max_lim,start_lim; int lx,ly,rx,L,R; // 足回り int L_1,R_1; int g; double y_deg,yaw,offset; int cross_f; int kago=0,kago2=0; // コントローラー int circle_f=0,circle_cnt=0; int triangle_f=0,triangle_cnt=0; int sel_f = 0,sel_cnt = 0; int left_f=0,right_f=0; int start_s=0,start_cnt=0,start_f=0; int val4; int as=0; int slide_f,slide_st; // 機構 int back_f=0,start = 0,get=0,www=0; int time_f=0,time_cnt=0; int link_up=0,link_down=0; int tach_f,back_cnt,del; int tim_start=0; double Turn_val,dif_val; Timer tim; /*クラス宣言-------------------------------------------------------------------*/ 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); /*----------------------------------- main -----------------------------------*/ /* @brief メインプログラム * @param なし * @retval なし */ int main() { //flipper1.attach(&flip1, 0.001); // 割り込み while(1) { mpu.loop(); if(sbdbt.State_check()) { i2c.In(In_Data,0); i2c.In(In_Data,1); i2c.In(In_Data,2); i2c.In(In_Data,3); i2c.In(In_Data2,0); i2c.In(In_Data2,1); i2c.In(In_Data2,2); y_deg = (double)mpu.ypr[0] * 180 / M_PI; val4=tim.read_ms(); slide_start = (In_Data+0)->in_data; slide_stop = (In_Data+3)->in_data; 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.L1) { Turn_val=y_deg; offset=y_deg; } if(LEFTkey) { offset=y_deg; left_f=1; } if(RIGHTkey) { offset=y_deg; right_f=1; } yaw = -y_deg+offset; dif_val=y_deg-Turn_val; if(sbdbt.LX !=64 || sbdbt.LY !=64) { mecanum_Move(); // 以下,ジャイロセンサーのプログラム 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; } } 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; mecanum_Turn(); } else { mecanum_Stop(); } if(left_f) { // 90°旋回 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; 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; mecanum_Stop(); } } /*---ハンガーかけるやつ--------------------------------------------------------------------------------------------------------------------------------------------*/ limit_up = (In_Data2+0)->in_data; limit_down = (In_Data2+1)->in_data; if(SELECTkey)as=1;//selectキー if(as==1 && limit_up == 0) { MD_SET_DRIVE(MD_Data, 5,MD_REVERSE); MD_SET_PWM(MD_Data, 5,50); } else if(UPkey && limit_up == 0) { as=0; MD_SET_DRIVE(MD_Data, 5,MD_REVERSE); MD_SET_PWM(MD_Data, 5,50); } else if(as==1 && limit_up == 1) { as=0; MD_SET_PWM(MD_Data,5,0); MD_SET_DRIVE(MD_Data, 5,MD_BRAKE); } else if(DOWNkey && limit_down == 0) { MD_SET_DRIVE(MD_Data, 5,MD_FORWARD); MD_SET_PWM(MD_Data, 5,50); } else { MD_SET_PWM(MD_Data,5,0); MD_SET_DRIVE(MD_Data, 5,MD_BRAKE); } max_lim = (In_Data+1)->in_data; start_lim =(In_Data+2)->in_data; short_lim= (In_Data2+2)->in_data; //-------------- 洗濯物回収 --------------// if(SQUARE) start = 1; if(CIRCLE) get = 1; if(CROSS) www=1; if(start || www|| get) { // 洗濯物挟んで回収 if(start) { // 腕伸ばす if(max_lim==0) { MD_SET_DRIVE(MD_Data,4,MD_REVERSE); MD_SET_PWM (MD_Data,4,50); } // モーター止めて腕下げる else if(back_f==0 && max_lim==1) { MD_SET_DRIVE(MD_Data,4,MD_BRAKE); MD_SET_PWM(MD_Data,4,0); i2c.Out_Set(Out_Data,0,1); tim_start = 1; tim.start(); if(tim.read_ms()>1000 && max_lim == 1) { green = 1; i2c.Out_Set(Out_Data,1,0); i2c.Out_Set(Out_Data,0,0); back_f=1; } } if(back_f==1) { // 初期位置まで腕縮める if(start_lim==0) { MD_SET_DRIVE(MD_Data,4,MD_FORWARD); MD_SET_PWM (MD_Data,4,50); } else { back_f=0; start = 0; tim.reset(); tim.stop(); i2c.Out_Set(Out_Data,1,1); } } } // 閉じて洗濯物回収 if(www) { if(short_lim==0) { MD_SET_DRIVE(MD_Data,4,MD_REVERSE); MD_SET_PWM (MD_Data,4,30); } // モーター止めて腕下げる else if(back_f==0 && short_lim==1) { MD_SET_DRIVE(MD_Data,4,MD_BRAKE); MD_SET_PWM(MD_Data,4,0); i2c.Out_Set(Out_Data,0,1); tim.start(); if(tim.read_ms()>1000 && short_lim == 1) { green = 1; i2c.Out_Set(Out_Data,1,0); i2c.Out_Set(Out_Data,0,0); back_f=1; } } if(back_f==1) { // 初期位置まで腕縮める if(start_lim==0) { MD_SET_DRIVE(MD_Data,4,MD_FORWARD); MD_SET_PWM (MD_Data,4,40); } else { back_f=0; www = 0; tim.reset(); tim.stop(); i2c.Out_Set(Out_Data,1,1); } } } if(get) { // 腕伸ばす if(max_lim==0) { MD_SET_DRIVE(MD_Data,4,MD_REVERSE); MD_SET_PWM (MD_Data,4,55); } // モーター止めて腕下げる else if(back_f==0 && max_lim==1) { tim.start(); MD_SET_DRIVE(MD_Data,4,MD_BRAKE); MD_SET_PWM(MD_Data,4,0); i2c.Out_Set(Out_Data,1,0); i2c.Out_Set(Out_Data,0,1); if(tim.read_ms()>1000)back_f=1; // タイマで少し待つ } if(back_f==1) { // 初期位置まで腕縮める if(start_lim==0) { MD_SET_DRIVE(MD_Data,4,MD_FORWARD); MD_SET_PWM (MD_Data,4,60); } else { tim.reset(); tim.stop(); back_f=0; get = 0; } } } } else { MD_SET_DRIVE(MD_Data,4,MD_BRAKE); MD_SET_PWM(MD_Data,4,0); i2c.Out_Set(Out_Data,0,0); i2c.Out_Set(Out_Data,1,1); } /*---回収機構 ~押し出し~---------------------------------------------------------------------------------------------------------------------------------------*/ if(TRIANGLE) { if(max_lim==0) { MD_SET_DRIVE(MD_Data,4,MD_REVERSE); MD_SET_PWM (MD_Data,4,40); } else if(max_lim==1) { MD_SET_DRIVE(MD_Data,4,MD_BRAKE); MD_SET_PWM (MD_Data,4,0); kago=1; if(kago==1 && slide_start==0) { MD_SET_DRIVE(MD_Data,6,MD_FORWARD); MD_SET_PWM (MD_Data,6,70); } else if(slide_start==1) { tim.start(); if(tim.read_ms()>1500 && slide_stop==0) { tim.stop(); tim.reset(); MD_SET_DRIVE(MD_Data,6,MD_REVERSE); MD_SET_PWM (MD_Data,6,65); } else if(slide_stop==1) { kago2=1; MD_SET_DRIVE(MD_Data,6,MD_BRAKE); MD_SET_PWM (MD_Data,6,0); } else { MD_SET_DRIVE(MD_Data,6,MD_BRAKE); MD_SET_PWM (MD_Data,6,0); } } } else if(kago2==1 && start_lim==0) { MD_SET_DRIVE(MD_Data,4,MD_FORWARD); MD_SET_PWM (MD_Data,4,40); } else if(start_lim==1) { MD_SET_DRIVE(MD_Data,4,MD_BRAKE); MD_SET_PWM (MD_Data,4,0); } else { MD_SET_DRIVE(MD_Data,4,MD_BRAKE); MD_SET_PWM (MD_Data,4,0); } } else { kago=0; kago2=0; } /*タオル掛け*/ if(sbdbt.L2)i2c.Out_Set(Out_Data,2,1); else i2c.Out_Set(Out_Data,2,0); /*タオルを引っ張るやつ*/ if(sbdbt.R2)i2c.Out_Set(Out_Data,3,1); else i2c.Out_Set(Out_Data,3,0); /*展開*/ if(STARTkey && start_f == 0) { start_cnt++; start_f = 1; } else if(STARTkey==0 && start_f == 1) start_f =0; if(start_cnt%3==1) start_s=0; else start_s=1; // if(start_s==0)i2c.Out_Set(Out_Data,3,1); // else if(start_s==1)i2c.Out_Set(Out_Data,3,0); // else start_cnt=2; //-------------- 出力 --------------// i2c.Out(Out_Data,5); i2c.MD_I2C(MD_Data,0); i2c.MD_I2C(MD_Data,1); i2c.MD_I2C(MD_Data,2); i2c.MD_I2C(MD_Data,3); i2c.MD_I2C(MD_Data,4); i2c.MD_I2C(MD_Data,5); i2c.MD_I2C(MD_Data,6); i2c.MD_I2C(MD_Data,7); pc.printf("%2d%2d%2d%2d%2d%2d\n\r",UPkey,RIGHTkey,DOWNkey,LEFTkey,sbdbt.SELECT,sbdbt.START); // printf("y_deg%2f offset%2f yaw%2f Turn_val%2f dif_val%2f\r\n",y_deg,offset,yaw,Turn_val,dif_val); } // if(sbdbt.State_check()) } // while(1) } // int main() /* メカナムの基本移動 */ void mecanum_Move(void) { 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); 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); } } /*メカナム旋廻移動*/ void mecanum_TurnMove(void) { int val1,val2; MD_SET_DRIVE(MD_Data, 0,(L_1 > 0) ? MD_FORWARD : MD_REVERSE); MD_SET_DRIVE(MD_Data, 1,(R_1 > 0) ? MD_REVERSE : MD_FORWARD); MD_SET_DRIVE(MD_Data, 2,(R_1 > 0) ? MD_FORWARD : MD_REVERSE); MD_SET_DRIVE(MD_Data, 3,(L_1 > 0) ? MD_REVERSE : MD_FORWARD); val1 = (int)sqrt( 1.0*abs( abs(lx)*lx + abs(ly)*ly +rx)); val2 = (int)sqrt( 1.0*abs( abs(ly)*ly - abs(lx)*lx +rx)); 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); } /* 停止 */ void mecanum_Stop(void) { for(int i=0; i<4; i++) { MD_SET_PWM(MD_Data, i, 0); MD_SET_DRIVE(MD_Data, i, MD_BRAKE); } } /*割り込み*/ // void flip1(void) { // static int cnt=0; // // /* カウントアップ,1sループ */ // cnt++; // cnt%=10000; // // /* 100msの処理 */ // if(cnt%100==0) { // myled = !myled; // if(tim_start==1) time_cnt++; // if(time_cnt>10 && time_f==0) { // time_f=1; // } // } //} /*操作法*/ /* 〇 ボタン == 回収(引きずる) ? ボタン == 籠 △ ボタン == 回収(シーツ兼シャツ) □ ボタン == 回収(タオル) 上 ボタン == ハンガーかけ機構 上昇 下 ボタン == ハンガーかけ機構 下降 左 ボタン == 90回転 右 ボタン == 90回転 L1 ボタン == リセット(足回り) L2 ボタン == タオル掛け R1 ボタン == 押しながら回転でゆっくりになる R2 ボタン == タオルひっぱる奴 select == ハンガー掛け 最大まで上昇 start == ハンガー展開 */ /*その他注意点*/ /* 動かすときはジャイロの値を安定させるため15秒から20秒待つ メカナムは振動が大きいせいかナット等が外れやすいため、定期的に確認をする(特に足回り) あまり急発進をさせない(たまに暴走) 暴走したときはコントローラーを動かしたら治るはず コントローラー接続が切れたらSTMの黒い奴を押す(わからないときは近くの回路班へ) */