Takushima Yukimasa
a
Diff: main.cpp
- Revision:
- 1:199c4a71da88
- Parent:
- 0:761a63c6d020
- Child:
- 2:47954f05d32d
--- 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()