Takushima Yukimasa
/
R19_ManuTakukAHWGeo
a
Diff: main.cpp
- Revision:
- 9:64fcbcc182d9
- Parent:
- 8:b79d21c8178b
- Child:
- 10:7c73e1577742
diff -r b79d21c8178b -r 64fcbcc182d9 main.cpp --- a/main.cpp Tue Aug 27 08:54:15 2019 +0000 +++ b/main.cpp Tue Aug 27 10:59:38 2019 +0000 @@ -147,6 +147,8 @@ //タオルつかむ 四角 if(grab_f || spull_f) { PALETTE(GREEN); + + //真ん中で止まる用s short_f=1; switch(grab) { //腕を前に動かす @@ -156,7 +158,7 @@ if(back_f) MD_SET_DRIVE(MD_Data,4,MD_FORWARD); else MD_SET_DRIVE(MD_Data,4,MD_REVERSE); - MD_SET_PWM (MD_Data,4,40); + MD_SET_PWM (MD_Data,4,60); if(spull_f) { if(short_lim==1) { @@ -180,7 +182,7 @@ //腕おろす i2c.Out_Set(Out_Data,0,1); - if(tim.read_ms()>300) { + if(tim.read_ms()>200) { grab++; tim.reset(); } @@ -189,7 +191,7 @@ case 2: //爪閉じる i2c.Out_Set(Out_Data,1,0); - if(tim.read_ms()>100) { + if(tim.read_ms()>0) { grab++; tim.reset(); } @@ -198,7 +200,7 @@ case 3: //腕上げる i2c.Out_Set(Out_Data,0,0); - if(tim.read_ms()>300) { + if(tim.read_ms()>0) { grab++; tim.reset(); } @@ -207,7 +209,7 @@ //腕戻す case 4: MD_SET_DRIVE(MD_Data,4,MD_FORWARD); - MD_SET_PWM (MD_Data,4,40); + MD_SET_PWM (MD_Data,4,60); if(start_lim==1) { grab_f=0; spull_f=0; @@ -228,7 +230,7 @@ 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++; @@ -244,14 +246,15 @@ //腕おろす i2c.Out_Set(Out_Data,0,1); - if(tim.read_ms()>300) { + if(tim.read_ms()>100) { pull++; tim.reset(); } break; + //停止 case 2: - if(tim.read_ms()>300) { + if(tim.read_ms()>0) { pull++; tim.reset(); } @@ -259,7 +262,7 @@ case 3: MD_SET_DRIVE(MD_Data,4,MD_FORWARD); - MD_SET_PWM (MD_Data,4,40); + MD_SET_PWM (MD_Data,4,60); if(start_lim==1) { pull_f=0; tim.reset(); @@ -279,7 +282,7 @@ //腕を前に動かす 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(); @@ -293,7 +296,7 @@ //かご回転 MD_SET_DRIVE(MD_Data,6,MD_FORWARD); - MD_SET_PWM (MD_Data,6,80); + MD_SET_PWM (MD_Data,6,100); if(slide_stop==1) { kago++; tim.reset(); @@ -311,13 +314,13 @@ case 3: MD_SET_DRIVE(MD_Data,6,MD_REVERSE); - MD_SET_PWM (MD_Data,6,80); + MD_SET_PWM (MD_Data,6,100); if(slide_start==1) kago++; break; case 4: MD_SET_DRIVE(MD_Data,4,MD_FORWARD); - MD_SET_PWM (MD_Data,4,40); + MD_SET_PWM (MD_Data,4,60); if(start_lim==1) { kago_f=0; tim.reset(); @@ -332,9 +335,18 @@ //腕真ん中で停止 if(short_f) { - if(short_lim==1) short_f=0; - MD_SET_DRIVE(MD_Data,4,MD_REVERSE); - MD_SET_PWM (MD_Data,4,20); + if(short_lim==1) { + short_f=0; + back_f=0; + } + //奥のリミットに当たったら逆回転 + if(max_lim==1) back_f=1; + //手前のリミットに当たったら正回転 + else if(start_lim==1) back_f=0; + + if(back_f) MD_SET_DRIVE(MD_Data,4,MD_FORWARD); + else MD_SET_DRIVE(MD_Data,4,MD_REVERSE); + MD_SET_PWM (MD_Data,4,30); } else { MD_SET_DRIVE(MD_Data,4,MD_BRAKE); MD_SET_PWM (MD_Data,4,0);