Takushima Yukimasa
a
Diff: main.cpp
- Revision:
- 8:b79d21c8178b
- Parent:
- 6:040d001acb12
- Child:
- 9:64fcbcc182d9
--- a/main.cpp Sat Aug 24 06:11:03 2019 +0000
+++ b/main.cpp Tue Aug 27 08:54:15 2019 +0000
@@ -1,6 +1,13 @@
#include "main.h"
/* 型定義 --------------------------------------------------------------------*/
+//ゾーン対応
+#define _RED_ZONE_
+#ifdef _RED_ZONE_
+const int def_val=0;
+#else
+const int def_val=180;
+#endif
/* 関数宣言 ------------------------------------------------------------------*/
@@ -15,15 +22,17 @@
int pull=0, pull_f=0;
//タオル掴む用
-//sgrab_f: 中央のリミットでつかむ
-int grab=0, grab_f=0, sgrab_f=0;
+//spull_f: 中央のリミットでつかむ
+//back_f: 真ん中目で下がるとき
+int grab=0, grab_f=0, spull_f=0;
+int back_f=0;
//タオル横展開用
int unfold_f=0;
//y_deg: ジャイロの生の値
//yaw: 実際に使う角度
-double y_deg[2], yaw=0;;
+double y_deg[2], yaw=def_val;
//ジャイロさんぷりんぐ用
double sum=0, ave=0;
@@ -31,12 +40,13 @@
//Turn_val: 補正の目標値
//dif_val: 補正値との差
-double Turn_val, dif_val;
+double Turn_val=def_val, dif_val;
//short_lim: 腕中央
//max_lim: 腕奥側
//short_lim: 腕手前側
int short_lim, max_lim, start_lim;
+int short_f=0;
//足回りのpwm値
int duty[4];
@@ -73,14 +83,19 @@
//誤差をさんぷりんぐ
if(offset_cnt<1000) {
- PALETTE(RED);
+ PALETTE(YELLOW);
if(offset_cnt!=0) {
sum+=(y_deg[0]-y_deg[1]);
ave=sum/offset_cnt;
}
offset_cnt++;
} else {
- PALETTE(GREEN);
+ //ゾーン対応
+#ifdef _RED_ZONE_
+ PALETTE(RED);
+#else
+ PALETTE(BLUE);
+#endif
yaw -= (y_deg[0]-y_deg[1])-ave;
}
@@ -91,219 +106,261 @@
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)*30/64;
+ rx = (sbdbt.RX - 64)*60/64;
+
+ //R1で停止
+ if(sbdbt.R1==0) {
+ //メカナム基本動作
+ mecanum_Move(lx, ly, rx);
- //メカナム基本動作
- mecanum_Move(lx, ly, rx);
+ //旋回している間タイマーをリセット
+ if(sbdbt.RX != 64) drift_tim.reset();
+
+ //旋回して慣性で動いた後の角度に補正する
+ if(drift_tim.read_ms()<500) Turn_val=yaw;
- //旋回している間タイマーをリセット
- if(sbdbt.RX != 64) drift_tim.reset();
+ //角度補正
+ AngleCorrection(yaw, Turn_val);
+
+ /*---ハンガーかけるやつ----------------------------------------------*/
+ if(sbdbt.SELECT)as=1;//selectキー
- //旋回して慣性で動いた後の角度に補正する
- if(drift_tim.read_ms()<500) Turn_val=yaw;
+ if(as==1 && limit_up == 0) {
+ MD_SET_DRIVE(MD_Data, 5,MD_REVERSE);
+ MD_SET_PWM(MD_Data, 5,100);
+ } else if(UPkey && limit_up == 0) {
+ as=0;
+ MD_SET_DRIVE(MD_Data, 5,MD_REVERSE);
+ MD_SET_PWM(MD_Data, 5,100);
+ } 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,100);
+ } else {
+ MD_SET_PWM(MD_Data,5,0);
+ MD_SET_DRIVE(MD_Data, 5,MD_BRAKE);
+ }
- //角度補正
- AngleCorrection(yaw, Turn_val);
+ //タオルつかむ 四角
+ if(grab_f || spull_f) {
+ PALETTE(GREEN);
+ short_f=1;
+ switch(grab) {
+ //腕を前に動かす
+ case 0:
+ //爪開く
+ i2c.Out_Set(Out_Data,1,1);
- /*---ハンガーかけるやつ----------------------------------------------*/
- if(sbdbt.SELECT)as=1;//selectキー
+ 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);
- 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);
- }
+ if(spull_f) {
+ if(short_lim==1) {
+ grab++;
+ back_f=0;
+ tim.reset();
+ } else if(max_lim) back_f=1;
+ } else {
+ if(max_lim==1) {
+ grab++;
+ back_f=0;
+ tim.reset();
+ }
+ }
+ 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()>300) {
+ grab++;
+ tim.reset();
+ }
+ break;
- //タオルつかむ 四角
- if(grab_f || sgrab_f) {
- PALETTE(BLUE);
- 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);
- //sgrab_fは途中のリミットで止まる
- if((sgrab_f && short_lim) || max_lim==1) {
- grab++;
- tim.reset();
- }
- break;
+ case 2:
+ //爪閉じる
+ i2c.Out_Set(Out_Data,1,0);
+ if(tim.read_ms()>100) {
+ grab++;
+ tim.reset();
+ }
+ break;
+
+ 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_f=0;
+ spull_f=0;
+ tim.reset();
+ }
+ break;
+ }
+ } //if(grab_f)
- case 1:
- //腕停止
- MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
- MD_SET_PWM(MD_Data,4,0);
+ //タオル引く 丸
+ else if(pull_f) {
+ PALETTE(CYAN);
+ short_f=1;
+ 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);
+
+ if(max_lim==1) {
+ pull++;
+ back_f=0;
+ tim.reset();
+ }
+ 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()>300) {
- grab++;
- tim.reset();
- }
- break;
+ //腕おろす
+ i2c.Out_Set(Out_Data,0,1);
+ if(tim.read_ms()>300) {
+ pull++;
+ tim.reset();
+ }
+ break;
+
+ case 2:
+ if(tim.read_ms()>300) {
+ pull++;
+ tim.reset();
+ }
+ break;
- case 2:
- //爪閉じる
- i2c.Out_Set(Out_Data,1,0);
- if(tim.read_ms()>100) {
- grab++;
- tim.reset();
- }
- break;
+ case 3:
+ MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
+ MD_SET_PWM (MD_Data,4,40);
+ if(start_lim==1) {
+ pull_f=0;
+ tim.reset();
+ }
+ break;
+ }
+ } //if(pull_f)
+
+ //かごを倒す
+ else if(kago_f) {
+ PALETTE(MAGENTA);
+ short_f=1;
+ //腕の先を閉じておく
+ i2c.Out_Set(Out_Data,1,0);
- case 3:
- //腕上げる
- i2c.Out_Set(Out_Data,0,0);
- if(tim.read_ms()>300) {
- grab++;
- tim.reset();
- }
- break;
+ switch(kago) {
+ //腕を前に動かす
+ case 0:
+ MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
+ MD_SET_PWM (MD_Data,4,40);
+ if(max_lim==1) {
+ kago++;
+ tim.reset();
+ }
+ break;
+
+ case 1:
+ //腕停止
+ MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
+ MD_SET_PWM (MD_Data,4,0);
+
+ //かご回転
+ MD_SET_DRIVE(MD_Data,6,MD_FORWARD);
+ MD_SET_PWM (MD_Data,6,80);
+ if(slide_stop==1) {
+ kago++;
+ tim.reset();
+ MD_SET_DRIVE(MD_Data,6,MD_BRAKE);
+ MD_SET_PWM (MD_Data,6,0);
+ }
+ break;
- case 4:
- MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
- MD_SET_PWM (MD_Data,4,60);
- if(start_lim==1) {
- grab_f=0;
- sgrab_f=0;
- }
- break;
- }
- } //if(grab_f)
+ case 2:
+ if(tim.read_ms()>0) {
+ kago++;
+ tim.reset();
+ }
+ break;
+
+ case 3:
+ MD_SET_DRIVE(MD_Data,6,MD_REVERSE);
+ MD_SET_PWM (MD_Data,6,80);
+ if(slide_start==1) kago++;
+ break;
- //タオル引く 丸
- else if(pull_f) {
- PALETTE(CYAN);
- switch(pull) {
- //腕を前に動かす
- case 0:
- //爪閉じる
- i2c.Out_Set(Out_Data,1,0);
+ case 4:
+ MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
+ MD_SET_PWM (MD_Data,4,40);
+ if(start_lim==1) {
+ kago_f=0;
+ tim.reset();
+ }
+ break;
+ }
+ } else {
+ //爪開く
+ i2c.Out_Set(Out_Data,1,1);
+ //腕上げる
+ i2c.Out_Set(Out_Data,0,0);
+
+ //腕真ん中で停止
+ if(short_f) {
+ if(short_lim==1) short_f=0;
MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
- MD_SET_PWM (MD_Data,4,60);
- if(max_lim==1) {
- pull++;
- tim.reset();
- }
- break;
-
- case 1:
- //腕停止
+ MD_SET_PWM (MD_Data,4,20);
+ } else {
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()>300) {
- pull++;
- tim.reset();
- }
- break;
-
- case 2:
- if(tim.read_ms()>300) {
- pull++;
- tim.reset();
- }
- break;
+ }
+ //かご停止
+ MD_SET_DRIVE(MD_Data,6,MD_BRAKE);
+ MD_SET_PWM (MD_Data,6,0);
- case 3:
- MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
- MD_SET_PWM (MD_Data,4,60);
- if(start_lim==1) pull_f=0;
- break;
- }
- } //if(pull_f)
-
- //かごを倒す
- else if(kago_f) {
- PALETTE(MAGENTA);
- //腕の先を閉じておく
- i2c.Out_Set(Out_Data,1,0);
-
- switch(kago) {
- //腕を前に動かす
- case 0:
- MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
- 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);
- MD_SET_PWM (MD_Data,4,0);
+ kago=0;
+ pull=0;
+ grab=0;
- //かご回転
- MD_SET_DRIVE(MD_Data,6,MD_FORWARD);
- MD_SET_PWM (MD_Data,6,80);
- 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);
- }
- break;
-
- case 2:
- if(tim.read_ms()>1000) {
- kago++;
- tim.reset();
- }
- break;
-
- case 3:
- MD_SET_DRIVE(MD_Data,6,MD_REVERSE);
- 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,60);
- if(start_lim==1) kago_f=0;
- break;
+ tim.reset();
}
- } 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);
- //腕上げる
- i2c.Out_Set(Out_Data,0,0);
-
- kago=0;
- pull=0;
- grab=0;
-
- tim.reset();
+ }
+ //非常停止
+ else {
+ for(int i=0; i<8; i++) {
+ MD_SET_DRIVE(MD_Data,i,MD_BRAKE);
+ MD_SET_PWM (MD_Data,i,0);
+ }
}
//-------------- 出力 --------------//
//第二引数には使う最大の個数
- i2c.Out(Out_Data,5);
+ i2c.Out(Out_Data,8);
//モータ出力
for(int i=0; i<8; i++) i2c.MD_I2C(MD_Data,i);
@@ -343,8 +400,8 @@
/* ----------------ボタン系--------------- */
//角度リセット
if(sbdbt.L1) {
- yaw=0;
- Turn_val=0;
+ yaw=def_val;
+ Turn_val=def_val;
}
//かごを倒す
if(CROSS) {
@@ -363,8 +420,8 @@
}
} else if(TRIANGLE) {
if(once==0) {
- sgrab_f++;
- sgrab_f%=2;
+ spull_f++;
+ spull_f%=2;
once++;
}
}
@@ -399,7 +456,12 @@
duty[3]=-(lx+ly);
for(int i=0; i<4; i++) {
+ //旋回
duty[i]+=rx;
+
+ //制限
+ if(duty[i]>=99) duty[i]=99;
+ else if(duty[i]<=-99) duty[i]=-99;
MD_SET_DRIVE(MD_Data, i, duty[i]==0? MD_BRAKE: (duty[i]>0? MD_FORWARD: MD_REVERSE));
MD_SET_PWM(MD_Data, i, abs(duty[i]));
}