Takushima Yukimasa
/
R19_ManuTakukAHWGeo
a
Diff: main.cpp
- Revision:
- 2:47954f05d32d
- Parent:
- 1:199c4a71da88
- Child:
- 3:1063aa133b62
diff -r 199c4a71da88 -r 47954f05d32d main.cpp --- a/main.cpp Fri Aug 23 02:18:37 2019 +0000 +++ b/main.cpp Fri Aug 23 03:54:05 2019 +0000 @@ -46,10 +46,10 @@ OUT_I2C_Data_TypeDef Out_Data[] = { {0,0,0}, // 腕の上下 - {0,1,0}, // 洗濯物排出 - {0,2,0}, // バスタオルの先 - {0,3,0}, // 展開 - {1,0,0}, // バスタオルの中央 + {0,1,0}, // 爪 + {0,2,0}, // タオルの先 + {0,3,0}, // タオル展開 + {1,0,0}, // タオルの中央 }; /* 関数宣言 ------------------------------------------------------------------*/ void flip1(void); @@ -151,19 +151,6 @@ L = lx+ly; R = ly-lx; - if(sbdbt.L1) { - Turn_val=y_deg; - offset=y_deg; - } - - if(LEFTkey) { - offset=y_deg; - } - if(RIGHTkey) { - offset=y_deg; - right_f=1; - } - yaw = -y_deg+offset; dif_val=y_deg-Turn_val; @@ -258,8 +245,7 @@ max_lim = (In_Data+1)->in_data; start_lim =(In_Data+2)->in_data; short_lim = (In_Data2+2)->in_data; - //-------------- 洗濯物回収 --------------// - /*---回収機構 ~押し出し~---------------------------------------------------------------------------------------------------------------------------------------*/ + //タオルつかむ 四角 if(grab_flag) { switch(grab) { @@ -276,6 +262,10 @@ break; case 1: + //腕停止 + MD_SET_DRIVE(MD_Data,4,MD_BRAKE); + MD_SET_PWM (MD_Data,4,0); + //腕おろす i2c.Out_Set(Out_Data,0,1); if(tim.read_ms()>1000) { @@ -302,7 +292,7 @@ break; } } //if(grab_flag) - //タオル引く 丸 + //タオル引く 丸 else if(pull_flag) { switch(pull) { //腕を前に動かす @@ -318,6 +308,10 @@ break; case 1: + //腕停止 + MD_SET_DRIVE(MD_Data,4,MD_BRAKE); + MD_SET_PWM (MD_Data,4,0); + //腕おろす i2c.Out_Set(Out_Data,0,1); if(tim.read_ms()>1000) { @@ -349,7 +343,10 @@ case 0: MD_SET_DRIVE(MD_Data,4,MD_REVERSE); MD_SET_PWM (MD_Data,4,40); - if(max_lim==1) kago++; + if(max_lim==1) { + kago++; + tim.reset(); + } break; //かごを倒す @@ -361,22 +358,25 @@ //かご回転 MD_SET_DRIVE(MD_Data,6,MD_FORWARD); MD_SET_PWM (MD_Data,6,70); - if(slide_stop==1) { + if(slide_stop==1 || tim.read_ms()>3000) { kago++; + tim.reset(); MD_SET_DRIVE(MD_Data,6,MD_BRAKE); MD_SET_PWM (MD_Data,6,0); } - tim.reset(); break; case 2: - if(tim.read_ms()>1500)kago++; + if(tim.read_ms()>1500) { + kago++; + tim.reset(); + } break; case 3: MD_SET_DRIVE(MD_Data,6,MD_REVERSE); MD_SET_PWM (MD_Data,6,65); - if(slide_start==1) kago++; + if(slide_start==1 || tim.read_ms()>3000) kago++; break; case 4: @@ -392,13 +392,15 @@ 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); - + i2c.Out_Set(Out_Data,0,0); + kago=0; pull=0; grab=0; + + tim.reset(); } /*タオル掛け*/ @@ -438,7 +440,18 @@ // 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(CROSS) { if(once==0) { kago_flag++;