Takushima Yukimasa
/
R19_ManuTakukAHWGeo
a
Diff: main.cpp
- Revision:
- 0:761a63c6d020
- Child:
- 1:199c4a71da88
diff -r 000000000000 -r 761a63c6d020 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Aug 22 09:26:20 2019 +0000 @@ -0,0 +1,576 @@ +/*今日 やること*/ + +/**********************************************************************************************************************************************************************/ +#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の黒い奴を押す(わからないときは近くの回路班へ) +*/ \ No newline at end of file