Takushima Yukimasa
a
Diff: main.cpp
- Revision:
- 12:7e33e400a48d
- Parent:
- 11:c1a22e2e3534
- Child:
- 13:48c4a4c95e77
--- a/main.cpp Wed Sep 04 05:31:40 2019 +0000
+++ b/main.cpp Sun Sep 08 09:57:22 2019 +0000
@@ -2,7 +2,7 @@
/* 型定義 --------------------------------------------------------------------*/
//ゾーン対応
-#define _RED_ZONE_
+//#define _RED_ZONE_
#ifdef _RED_ZONE_
const int def_val=0;
#else
@@ -12,29 +12,30 @@
/* 関数宣言 ------------------------------------------------------------------*/
/* 変数宣言 ------------------------------------------------------------------*/
-//かご回転用
-int kago=0, kago_f=0;
//コントローラーの状態を保存するため
int once=0;
-//タオル掴む用
+//kogo: かごをひっくり返す
//pull: 奥のリミットで引く
-//spull: 中央のリミットでつかむ
+//spull: 中央のリミットでつかむ(short_pull)
//grab: 掴む
//back_f: 真ん中目で下がるとき
-int pull=0, pull_f=0, grab=0, grab_f=0, spull=0, spull_f=0;
-int back_f=0;
+//release_f: 全体のエアー開放用
+//unfold_f: タオル横展開用
+int kago=0, kago_f=0, pull=0, pull_f=0, grab=0, grab_f=0, spull=0, spull_f=0;
+int back_f=0, release_f=0, unfold_f=0, short_f=0;
-//タオル横展開用
-int unfold_f=0;
+//上下動作を保持
+//[0]: 上
+//[1]: 下
+int keep_f[2]= {0};
//yaw: 実際に使う角度
double yaw=def_val;
-//[0]: roll
-//[1]: pitch
-//[2]: yaw
+//[0]: 今回の値
+//[1]: 前回の値
double rawyaw[2];
//Turn_val: 補正の目標値
@@ -44,7 +45,8 @@
//max_lim: 腕奥側
//short_lim: 腕手前側
int short_lim, max_lim, start_lim;
-int short_f=0;
+int limit_up,limit_down; // リミットスイッチ
+int slide_start,slide_stop; //かごのリミット
//足回りのpwm値
int duty[4];
@@ -52,13 +54,6 @@
// 足回り
int lx,ly,rx;
-int limit_up,limit_down; // リミットスイッチ
-int slide_start,slide_stop; //かごのリミット
-
-int as=0;
-
-int release_f=0;
-
/*----------------------------------- main -----------------------------------*/
int main()
{
@@ -66,16 +61,11 @@
bno.reset();
while(1) {
- //ゾーン対応
-#ifdef _RED_ZONE_
- PALETTE(RED);
-#else
- PALETTE(BLUE);
-#endif
-
//自動系のタイマースタート
tim.start();
drift_tim.start();
+ keep_tim[0].start();
+ keep_tim[1].start();
bno.setmode(OPERATION_MODE_IMUPLUS);
//角度の取得
@@ -89,7 +79,15 @@
yaw-=rawyaw[0]-rawyaw[1];
+ //ゾーン対応
+#ifdef _RED_ZONE_
+ PALETTE(RED);
+#else
+ PALETTE(BLUE);
+#endif
+
if(sbdbt.State_check()) {
+ //リミット
for(int i=0; i<7; i++) i2c.In(In_Data,i);
lx = ((64-sbdbt.LY)*100.0/64)*sin(yaw*M_PI/180)+((sbdbt.LX-64)*100.0/64)*cos(yaw*M_PI/180);
@@ -111,25 +109,31 @@
AngleCorrection(yaw, Turn_val);
/*---ハンガーかけるやつ----------------------------------------------*/
-// if(sbdbt.SELECT)as=1;//selectキー
-//
-// if(as==1 && limit_up == 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);
-// }
- if(UPkey && limit_up == 0) {
- as=0;
+ //上方向
+ if(!DOWNkey && ((UPkey || keep_f[0]) && limit_up == 0)) {
+ //一定秒以上押されたら保持
+ if(keep_tim[0].read_ms()>500) keep_f[0]=1;
+ //下方向の保持を解除
+ keep_f[1]=0;
+ keep_tim[1].reset();
+
+ MD_SET_DRIVE(MD_Data, 5,MD_FORWARD);
+ MD_SET_PWM(MD_Data, 5,100);
+ }
+ //下方向
+ else if(!UPkey && ((DOWNkey || keep_f[1]) && limit_down == 0)) {
+ if(keep_tim[1].read_ms()>500) keep_f[1]=1;
+ keep_f[0]=0;
+ keep_tim[0].reset();
+
MD_SET_DRIVE(MD_Data, 5,MD_REVERSE);
MD_SET_PWM(MD_Data, 5,100);
- } else if(DOWNkey && limit_down == 0) {
- MD_SET_DRIVE(MD_Data, 5,MD_FORWARD);
- MD_SET_PWM(MD_Data, 5,100);
} else {
+ keep_f[0]=0;
+ keep_tim[0].reset();
+ keep_f[1]=0;
+ keep_tim[1].reset();
+
MD_SET_PWM(MD_Data,5,0);
MD_SET_DRIVE(MD_Data, 5,MD_BRAKE);
}
@@ -406,9 +410,28 @@
}
}
+ /* --------------電磁弁系-------------------- */
+ /*タオル掛け*/
+ 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(unfold_f) i2c.Out_Set(Out_Data,3,1);
+ else i2c.Out_Set(Out_Data,3,0);
+
+ //エアー解放
+ if(release_f) {
+ PALETTE(YELLOW);
+ i2c.Out_Set(Out_Data,5,1);
+ } else i2c.Out_Set(Out_Data,5,0);
+
//-------------- 出力 --------------//
//第二引数には使う最大の個数
- i2c.Out(Out_Data,8);
+ i2c.Out(Out_Data,9);
//モータ出力
for(int i=0; i<8; i++) i2c.MD_I2C(MD_Data,i);
@@ -428,28 +451,10 @@
// pc.printf("kago %2d%2d ",kago,kago_f);
// pc.printf("%.1f %.1f ",rawyaw[0],rawyaw[1]);
// pc.printf("yaw:%.1f %.1f",yaw, bno.euler.yaw);
- pc.printf("lx %d ly %d rx %d",lx, ly, rx);
+// pc.printf("lx %d ly %d rx %d",lx, ly, rx);
+ pc.printf("%d %d",keep_tim[0].read_ms(),keep_f[0]);
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(unfold_f) i2c.Out_Set(Out_Data,3,1);
- else i2c.Out_Set(Out_Data,3,0);
-
- //エアー解放
- if(release_f) {
- PALETTE(MAGENTA);
- i2c.Out_Set(Out_Data,5,1);
- } else i2c.Out_Set(Out_Data,5,0);
-
/* ----------------ボタン系--------------- */
//角度リセット
if(sbdbt.L1) {
@@ -505,6 +510,7 @@
} // if(sbdbt.State_check())
+ else PALETTE(BLACK);
} // while(1)
} // int main()