Takushima Yukimasa
/
R19_ManuTakukAHWGeo
a
Diff: main.cpp
- Revision:
- 1:199c4a71da88
- Parent:
- 0:761a63c6d020
- Child:
- 2:47954f05d32d
diff -r 761a63c6d020 -r 199c4a71da88 main.cpp --- a/main.cpp Thu Aug 22 09:26:20 2019 +0000 +++ b/main.cpp Fri Aug 23 02:18:37 2019 +0000 @@ -14,8 +14,6 @@ #define RIGHTkey sbdbt.RIGHTkey #define UPkey sbdbt.UPkey #define DOWNkey sbdbt.DOWNkey -#define SELECTkey LEFTkey&&RIGHTkey -#define STARTkey UPkey&&UPkey /**********************************************************************************************************************************************************************/ @@ -49,9 +47,9 @@ OUT_I2C_Data_TypeDef Out_Data[] = { {0,0,0}, // 腕の上下 {0,1,0}, // 洗濯物排出 - {0,2,0}, // バスタオル - {0,3,0}, // 挟む機構 - {1,0,0}, // 展開 + {0,2,0}, // バスタオルの先 + {0,3,0}, // 展開 + {1,0,0}, // バスタオルの中央 }; /* 関数宣言 ------------------------------------------------------------------*/ void flip1(void); @@ -64,8 +62,21 @@ int median_filter(int16_t data[], int16_t add_data, const uint8_t num); /* 変数宣言 ------------------------------------------------------------------*/ +//かご回転用 +int kago=0, kago_flag=0; + +//コントローラーの状態を保存するため +int once=0; + +//タオル引く用 +int pull=0, pull_flag=0; + +//タオル掴むよう +int grab=0, grab_flag=0; + + int limit_up,limit_down; // リミットスイッチ -int slide_start,slide_stop; +int slide_start,slide_stop; //かごのリミット int short_lim,max_lim,start_lim; @@ -75,7 +86,6 @@ 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; @@ -114,6 +124,8 @@ { //flipper1.attach(&flip1, 0.001); // 割り込み while(1) { + //自動系のタイマースタート + tim.start(); mpu.loop(); if(sbdbt.State_check()) { @@ -129,8 +141,8 @@ val4=tim.read_ms(); - slide_start = (In_Data+0)->in_data; - slide_stop = (In_Data+3)->in_data; + slide_start = (In_Data+3)->in_data; + slide_stop = (In_Data+0)->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); @@ -146,7 +158,6 @@ if(LEFTkey) { offset=y_deg; - left_f=1; } if(RIGHTkey) { offset=y_deg; @@ -223,7 +234,7 @@ /*---ハンガーかけるやつ--------------------------------------------------------------------------------------------------------------------------------------------*/ limit_up = (In_Data2+0)->in_data; limit_down = (In_Data2+1)->in_data; - if(SELECTkey)as=1;//selectキー + if(sbdbt.SELECT)as=1;//selectキー if(as==1 && limit_up == 0) { MD_SET_DRIVE(MD_Data, 5,MD_REVERSE); @@ -246,192 +257,168 @@ max_lim = (In_Data+1)->in_data; start_lim =(In_Data+2)->in_data; - short_lim= (In_Data2+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(grab_flag) { + 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); + if(max_lim==1) { + grab++; + tim.reset(); + } + break; - // 洗濯物挟んで回収 - 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); - - + case 1: + //腕おろす i2c.Out_Set(Out_Data,0,1); - tim_start = 1; - tim.start(); + if(tim.read_ms()>1000) { + grab++; + tim.reset(); + } + break; - 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; + case 2: + //爪閉じる + i2c.Out_Set(Out_Data,1,0); + if(tim.read_ms()>500) { + grab++; tim.reset(); - tim.stop(); - i2c.Out_Set(Out_Data,1,1); } - } + break; + + case 3: + //腕上げる + i2c.Out_Set(Out_Data,0,0); + MD_SET_DRIVE(MD_Data,4,MD_FORWARD); + MD_SET_PWM (MD_Data,4,40); + if(start_lim==1) grab_flag=0; + break; } - // 閉じて洗濯物回収 - if(www) { - if(short_lim==0) { + } //if(grab_flag) + //タオル引く 丸 + else if(pull_flag) { + 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,30); - } - // モーター止めて腕下げる - else if(back_f==0 && short_lim==1) { - MD_SET_DRIVE(MD_Data,4,MD_BRAKE); - MD_SET_PWM(MD_Data,4,0); + MD_SET_PWM (MD_Data,4,40); + if(max_lim==1) { + pull++; + tim.reset(); + } + break; - + case 1: + //腕おろす 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(tim.read_ms()>1000) { + pull++; + tim.reset(); } - } - 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; + break; + + case 2: + if(tim.read_ms()>500) { + pull++; tim.reset(); - tim.stop(); - i2c.Out_Set(Out_Data,1,1); } - } + break; + + case 3: + MD_SET_DRIVE(MD_Data,4,MD_FORWARD); + MD_SET_PWM (MD_Data,4,40); + if(start_lim==1) pull_flag=0; + break; } - + } //if(pull_flag) + else if(kago_flag) { + //腕の先を閉じておく + i2c.Out_Set(Out_Data,1,0); - if(get) { - // 腕伸ばす - if(max_lim==0) { + switch(kago) { + //腕を前に動かす + case 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_PWM (MD_Data,4,40); + if(max_lim==1) kago++; + break; + + //かごを倒す + 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,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,6,MD_FORWARD); + MD_SET_PWM (MD_Data,6,70); + if(slide_stop==1) { + kago++; + MD_SET_DRIVE(MD_Data,6,MD_BRAKE); + MD_SET_PWM (MD_Data,6,0); + } + tim.reset(); + break; - 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; - } - } + case 2: + if(tim.read_ms()>1500)kago++; + break; + + case 3: + MD_SET_DRIVE(MD_Data,6,MD_REVERSE); + MD_SET_PWM (MD_Data,6,65); + if(slide_start==1) 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; + break; } } else { MD_SET_DRIVE(MD_Data,4,MD_BRAKE); - MD_SET_PWM(MD_Data,4,0); + 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,0,0); - i2c.Out_Set(Out_Data,1,1); + //腕上げる + i2c.Out_Set(Out_Data,1,0); + + kago=0; + pull=0; + grab=0; } - /*---回収機構 ~押し出し~---------------------------------------------------------------------------------------------------------------------------------------*/ - - - - 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(sbdbt.R2)i2c.Out_Set(Out_Data,4,1); + else i2c.Out_Set(Out_Data,4,0); /*展開*/ - if(STARTkey && start_f == 0) { + if(sbdbt.START && start_f == 0) { start_cnt++; start_f = 1; - } else if(STARTkey==0 && start_f == 1) start_f =0; + } 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; + 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); @@ -444,9 +431,35 @@ 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); + 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(CROSS) { + if(once==0) { + kago_flag++; + kago_flag%=2; + once++; + } + } else if(SQUARE) { + if(once==0) { + grab_flag++; + grab_flag%=2; + once++; + } + } else if(CIRCLE) { + if(once==0) { + pull_flag++; + pull_flag%=2; + once++; + } + } else once=0; + + } // if(sbdbt.State_check()) } // while(1) } // int main()