Takushima Yukimasa
a
Diff: main.cpp
- Revision:
- 0:761a63c6d020
- Child:
- 1:199c4a71da88
--- /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