Takushima Yukimasa
/
R19_ManuTakukAHWGeo
a
Diff: main.cpp
- Revision:
- 3:1063aa133b62
- Parent:
- 2:47954f05d32d
- Child:
- 4:236e5c58e8ee
diff -r 47954f05d32d -r 1063aa133b62 main.cpp --- a/main.cpp Fri Aug 23 03:54:05 2019 +0000 +++ b/main.cpp Fri Aug 23 06:27:19 2019 +0000 @@ -1,11 +1,11 @@ /*今日 やること*/ -/**********************************************************************************************************************************************************************/ +/******************************************************************************/ #include "main.h" //#include "gyro.h" #include <math.h> -/*******************************************************************************************************************************************************************/ +/******************************************************************************/ #define CIRCLE sbdbt.CIRCLE #define SQUARE sbdbt.SQUARE #define CROSS sbdbt.CROSS @@ -15,7 +15,7 @@ #define UPkey sbdbt.UPkey #define DOWNkey sbdbt.DOWNkey -/**********************************************************************************************************************************************************************/ +/******************************************************************************/ /* 型定義 --------------------------------------------------------------------*/ @@ -30,18 +30,14 @@ {15, 1, 0, 0, {0}} // 左右移動 }; -IN_I2C_Data_TypeDef In_Data[4] = { +IN_I2C_Data_TypeDef In_Data[] = { {1,0,0}, // 洗濯物排出(初期位置) {1,1,0}, // 洗濯物排出(停止) {1,2,0}, // 腕(最大 タオル) - {1,3,0} // 腕(初期位置) -}; - -IN_I2C_Data_TypeDef In_Data2[] = { + {1,3,0}, // 腕(初期位置) {3,0,0}, // 上下機構(上) {3,1,0}, // (下) {3,2,0}, // 腕(シーツ兼シャツ) - }; OUT_I2C_Data_TypeDef Out_Data[] = { @@ -55,7 +51,6 @@ 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); @@ -63,48 +58,45 @@ /* 変数宣言 ------------------------------------------------------------------*/ //かご回転用 -int kago=0, kago_flag=0; +int kago=0, kago_f=0; //コントローラーの状態を保存するため int once=0; //タオル引く用 -int pull=0, pull_flag=0; +int pull=0, pull_f=0; //タオル掴むよう -int grab=0, grab_flag=0; +int grab=0, grab_f=0; + +int unfold_f=0; + +//90度回転のフラグ +int left_f=0,right_f=0; +//y_deg: 起動時から絶対の角度 +//yaw: 相対角 初期状態が0になるようにする +//offset: 初期状態を0にするために引く値 +double y_deg, yaw, offset; + +//Turn_val: 補正の目標値 +//dif_val: 補正値との差 +double Turn_val, dif_val; + +//short_lim: 腕中央 +//max_lim: 腕奥側 +//short_lim: 腕手前側 +int short_lim, max_lim, start_lim; 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 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 @@ -115,6 +107,9 @@ DigitalOut green(PB_4); DigitalOut red(PB_5); +//半自動の時に使う +Timer tim; + /*----------------------------------- main -----------------------------------*/ /* @brief メインプログラム * @param なし @@ -129,21 +124,11 @@ 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); + for(int i=0; i<7; i++) i2c.In(In_Data,i); y_deg = (double)mpu.ypr[0] * 180 / M_PI; - - - val4=tim.read_ms(); - slide_start = (In_Data+3)->in_data; - slide_stop = (In_Data+0)->in_data; - + yaw = offset-y_deg; + dif_val = y_deg-Turn_val; 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); @@ -151,9 +136,6 @@ L = lx+ly; R = ly-lx; - yaw = -y_deg+offset; - dif_val=y_deg-Turn_val; - if(sbdbt.LX !=64 || sbdbt.LY !=64) { mecanum_Move(); @@ -195,7 +177,9 @@ } else { mecanum_Stop(); } - if(left_f) { // 90°旋回 + + // 90°旋回 + if(left_f) { if(abs(yaw)<60) { for(int i=0; i<4; i++) { MD_SET_PWM(MD_Data, i,60); @@ -203,6 +187,7 @@ } } else { left_f=0; + Turn_val=y_deg; mecanum_Stop(); } } @@ -214,13 +199,12 @@ } } else { right_f=0; + Turn_val=y_deg; mecanum_Stop(); } } - /*---ハンガーかけるやつ--------------------------------------------------------------------------------------------------------------------------------------------*/ - limit_up = (In_Data2+0)->in_data; - limit_down = (In_Data2+1)->in_data; + /*---ハンガーかけるやつ----------------------------------------------*/ if(sbdbt.SELECT)as=1;//selectキー if(as==1 && limit_up == 0) { @@ -242,19 +226,15 @@ 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(grab_flag) { + if(grab_f) { switch(grab) { //腕を前に動かす case 0: //爪開く i2c.Out_Set(Out_Data,1,1); MD_SET_DRIVE(MD_Data,4,MD_REVERSE); - MD_SET_PWM (MD_Data,4,40); + MD_SET_PWM(MD_Data,4,60); if(max_lim==1) { grab++; tim.reset(); @@ -264,11 +244,11 @@ case 1: //腕停止 MD_SET_DRIVE(MD_Data,4,MD_BRAKE); - MD_SET_PWM (MD_Data,4,0); + MD_SET_PWM(MD_Data,4,0); //腕おろす i2c.Out_Set(Out_Data,0,1); - if(tim.read_ms()>1000) { + if(tim.read_ms()>300) { grab++; tim.reset(); } @@ -277,7 +257,7 @@ case 2: //爪閉じる i2c.Out_Set(Out_Data,1,0); - if(tim.read_ms()>500) { + if(tim.read_ms()>100) { grab++; tim.reset(); } @@ -286,21 +266,29 @@ case 3: //腕上げる i2c.Out_Set(Out_Data,0,0); + if(tim.read_ms()>300) { + grab++; + tim.reset(); + } + break; + + case 4: MD_SET_DRIVE(MD_Data,4,MD_FORWARD); - MD_SET_PWM (MD_Data,4,40); - if(start_lim==1) grab_flag=0; + MD_SET_PWM (MD_Data,4,60); + if(start_lim==1) grab_f=0; break; } - } //if(grab_flag) + } //if(grab_f) + //タオル引く 丸 - else if(pull_flag) { + else if(pull_f) { switch(pull) { //腕を前に動かす case 0: //爪閉じる i2c.Out_Set(Out_Data,1,0); MD_SET_DRIVE(MD_Data,4,MD_REVERSE); - MD_SET_PWM (MD_Data,4,40); + MD_SET_PWM (MD_Data,4,60); if(max_lim==1) { pull++; tim.reset(); @@ -314,14 +302,14 @@ //腕おろす i2c.Out_Set(Out_Data,0,1); - if(tim.read_ms()>1000) { + if(tim.read_ms()>300) { pull++; tim.reset(); } break; case 2: - if(tim.read_ms()>500) { + if(tim.read_ms()>300) { pull++; tim.reset(); } @@ -329,12 +317,14 @@ case 3: MD_SET_DRIVE(MD_Data,4,MD_FORWARD); - MD_SET_PWM (MD_Data,4,40); - if(start_lim==1) pull_flag=0; + MD_SET_PWM (MD_Data,4,60); + if(start_lim==1) pull_f=0; break; } - } //if(pull_flag) - else if(kago_flag) { + } //if(pull_f) + + //かごを倒す + else if(kago_f) { //腕の先を閉じておく i2c.Out_Set(Out_Data,1,0); @@ -342,14 +332,13 @@ //腕を前に動かす case 0: MD_SET_DRIVE(MD_Data,4,MD_REVERSE); - MD_SET_PWM (MD_Data,4,40); + MD_SET_PWM (MD_Data,4,60); if(max_lim==1) { kago++; tim.reset(); } break; - //かごを倒す case 1: //腕停止 MD_SET_DRIVE(MD_Data,4,MD_BRAKE); @@ -357,7 +346,7 @@ //かご回転 MD_SET_DRIVE(MD_Data,6,MD_FORWARD); - MD_SET_PWM (MD_Data,6,70); + MD_SET_PWM (MD_Data,6,80); if(slide_stop==1 || tim.read_ms()>3000) { kago++; tim.reset(); @@ -367,7 +356,7 @@ break; case 2: - if(tim.read_ms()>1500) { + if(tim.read_ms()>1000) { kago++; tim.reset(); } @@ -375,22 +364,23 @@ case 3: MD_SET_DRIVE(MD_Data,6,MD_REVERSE); - MD_SET_PWM (MD_Data,6,65); + MD_SET_PWM (MD_Data,6,80); if(slide_start==1 || tim.read_ms()>3000) kago++; break; case 4: MD_SET_DRIVE(MD_Data,4,MD_FORWARD); - MD_SET_PWM (MD_Data,4,40); - if(start_lim==1) kago_flag=0; + MD_SET_PWM (MD_Data,4,60); + if(start_lim==1) kago_f=0; break; } } else { + //かごと腕を停止 MD_SET_DRIVE(MD_Data,4,MD_BRAKE); MD_SET_PWM (MD_Data,4,0); MD_SET_DRIVE(MD_Data,6,MD_BRAKE); MD_SET_PWM (MD_Data,6,0); - + //爪開く i2c.Out_Set(Out_Data,1,1); //腕上げる @@ -399,75 +389,96 @@ kago=0; pull=0; grab=0; - + tim.reset(); } + //-------------- 出力 --------------// + //第二引数には使う最大の個数 + i2c.Out(Out_Data,5); + + //モータ出力 + for(int i=0; i<8; i++) i2c.MD_I2C(MD_Data,i); + + /* ----------- りみっと --------------- */ + limit_up = (In_Data+4)->in_data; + limit_down = (In_Data+5)->in_data; + max_lim = (In_Data+1)->in_data; + start_lim =(In_Data+2)->in_data; + short_lim = (In_Data+6)->in_data; + slide_start = (In_Data+3)->in_data; + slide_stop = (In_Data+0)->in_data; + + //半自動のデバック +// 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("deg:%f dif:%f ",y_deg,dif_val); + pc.printf("\n\r"); + + + /* --------------電磁弁系-------------------- */ /*タオル掛け*/ 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,4,1); else i2c.Out_Set(Out_Data,4,0); - /*展開*/ - if(sbdbt.START && start_f == 0) { - start_cnt++; - start_f = 1; - } else if(sbdbt.START==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); + if(unfold_f)i2c.Out_Set(Out_Data,3,1); + else i2c.Out_Set(Out_Data,3,0); - 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("pull %2d%2d ",pull,pull_flag); - pc.printf("grab %2d%2d ",grab,grab_flag); - pc.printf("kago %2d%2d ",kago,kago_flag); - pc.printf("\n\r"); -// 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.L1) { Turn_val=y_deg; offset=y_deg; } - //90度回転して補正の角度を固定する - if(LEFTkey) offset=y_deg; - if(RIGHTkey) { - offset=y_deg; - right_f=1; + if(LEFTkey) { + if(once==0) { + left_f++; + left_f%=2; + once++; + } + } else if(RIGHTkey) { + if(once==0) { + right_f++; + right_f%=2; + once++; + } } - if(CROSS) { + //かごを倒す + else if(CROSS) { if(once==0) { - kago_flag++; - kago_flag%=2; + kago_f++; + kago_f%=2; once++; } - } else if(SQUARE) { + } + //タオルを掴んで動かす + else if(SQUARE) { if(once==0) { - grab_flag++; - grab_flag%=2; + grab_f++; + grab_f%=2; once++; } - } else if(CIRCLE) { + } + //倒すを引いて動かす + else if(CIRCLE) { if(once==0) { - pull_flag++; - pull_flag%=2; + pull_f++; + pull_f%=2; + once++; + } + } + //タオル展開用 + else if(sbdbt.START) { + if(once==0) { + unfold_f++; + unfold_f%=2; once++; } } else once=0; @@ -528,24 +539,6 @@ } } -/*メカナム旋廻移動*/ -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) { @@ -555,26 +548,6 @@ } } - -/*割り込み*/ -// 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; -// } -// } -//} - - /*操作法*/ /* 〇 ボタン == 回収(引きずる)