Takushima Yukimasa
a
Diff: main.cpp
- Revision:
- 3:1063aa133b62
- Parent:
- 2:47954f05d32d
- Child:
- 4:236e5c58e8ee
--- a/main.cpp Fri Aug 23 03:54:05 2019 +0000
+++ b/main.cpp Fri Aug 23 06:27:19 2019 +0000
@@ -1,11 +1,11 @@
/*今日 やること*/
-/**********************************************************************************************************************************************************************/
+/******************************************************************************/
#include "main.h"
//#include "gyro.h"
#include <math.h>
-/*******************************************************************************************************************************************************************/
+/******************************************************************************/
#define CIRCLE sbdbt.CIRCLE
#define SQUARE sbdbt.SQUARE
#define CROSS sbdbt.CROSS
@@ -15,7 +15,7 @@
#define UPkey sbdbt.UPkey
#define DOWNkey sbdbt.DOWNkey
-/**********************************************************************************************************************************************************************/
+/******************************************************************************/
/* 型定義 --------------------------------------------------------------------*/
@@ -30,18 +30,14 @@
{15, 1, 0, 0, {0}} // 左右移動
};
-IN_I2C_Data_TypeDef In_Data[4] = {
+IN_I2C_Data_TypeDef In_Data[] = {
{1,0,0}, // 洗濯物排出(初期位置)
{1,1,0}, // 洗濯物排出(停止)
{1,2,0}, // 腕(最大 タオル)
- {1,3,0} // 腕(初期位置)
-};
-
-IN_I2C_Data_TypeDef In_Data2[] = {
+ {1,3,0}, // 腕(初期位置)
{3,0,0}, // 上下機構(上)
{3,1,0}, // (下)
{3,2,0}, // 腕(シーツ兼シャツ）
-
};
OUT_I2C_Data_TypeDef Out_Data[] = {
@@ -55,7 +51,6 @@
void flip1(void);
void mecanum_Move(void);
void mecanum_Turn(void);
-void mecanum_TurnMove(void);
void mecanum_Stop(void);
//int sum_deg(double deg,double old);
void big_sort(int16_t data[], uint8_t num);
@@ -63,48 +58,45 @@
/* 変数宣言 ------------------------------------------------------------------*/
//かご回転用
-int kago=0, kago_flag=0;
+int kago=0, kago_f=0;
//コントローラーの状態を保存するため
int once=0;
//タオル引く用
-int pull=0, pull_flag=0;
+int pull=0, pull_f=0;
//タオル掴むよう
-int grab=0, grab_flag=0;
+int grab=0, grab_f=0;
+
+int unfold_f=0;
+
+//90度回転のフラグ
+int left_f=0,right_f=0;
+//y_deg: 起動時から絶対の角度
+//yaw: 相対角 初期状態が0になるようにする
+//offset: 初期状態を0にするために引く値
+double y_deg, yaw, offset;
+
+//Turn_val: 補正の目標値
+//dif_val: 補正値との差
+double Turn_val, dif_val;
+
+//short_lim: 腕中央
+//max_lim: 腕奥側
+//short_lim: 腕手前側
+int short_lim, max_lim, start_lim;
int limit_up,limit_down; // リミットスイッチ
int slide_start,slide_stop; //かごのリミット
-int short_lim,max_lim,start_lim;
-
int lx,ly,rx,L,R; // 足回り
-int L_1,R_1;
int g;
-double y_deg,yaw,offset;
-
-int cross_f;
-int circle_f=0,circle_cnt=0;
-int triangle_f=0,triangle_cnt=0;
-int sel_f = 0,sel_cnt = 0;
-int left_f=0,right_f=0;
-int start_s=0,start_cnt=0,start_f=0;
-int val4;
int as=0;
-int slide_f,slide_st; // 機構
-int back_f=0,start = 0,get=0,www=0;
-int time_f=0,time_cnt=0;
-int link_up=0,link_down=0;
-int tach_f,back_cnt,del;
-int tim_start=0;
-double Turn_val,dif_val;
-Timer tim;
/*クラス宣言-------------------------------------------------------------------*/
-
Ticker flipper1; // 割込み
Y_I2C i2c(PB_9, PB_8); // I2C
DigitalOut myled(LED1); // オンボードLED
@@ -115,6 +107,9 @@
DigitalOut green(PB_4);
DigitalOut red(PB_5);
+//半自動の時に使う
+Timer tim;
+
/*----------------------------------- main -----------------------------------*/
/* @brief メインプログラム
* @param なし
@@ -129,21 +124,11 @@
mpu.loop();
if(sbdbt.State_check()) {
- i2c.In(In_Data,0);
- i2c.In(In_Data,1);
- i2c.In(In_Data,2);
- i2c.In(In_Data,3);
- i2c.In(In_Data2,0);
- i2c.In(In_Data2,1);
- i2c.In(In_Data2,2);
+ for(int i=0; i<7; i++) i2c.In(In_Data,i);
y_deg = (double)mpu.ypr[0] * 180 / M_PI;
-
-
- val4=tim.read_ms();
- slide_start = (In_Data+3)->in_data;
- slide_stop = (In_Data+0)->in_data;
-
+ yaw = offset-y_deg;
+ dif_val = y_deg-Turn_val;
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);
@@ -151,9 +136,6 @@
L = lx+ly;
R = ly-lx;
- yaw = -y_deg+offset;
- dif_val=y_deg-Turn_val;
-
if(sbdbt.LX !=64 || sbdbt.LY !=64) {
mecanum_Move();
@@ -195,7 +177,9 @@
} else {
mecanum_Stop();
}
- if(left_f) { // 90°旋回
+
+ // 90°旋回
+ if(left_f) {
if(abs(yaw)<60) {
for(int i=0; i<4; i++) {
MD_SET_PWM(MD_Data, i,60);
@@ -203,6 +187,7 @@
}
} else {
left_f=0;
+ Turn_val=y_deg;
mecanum_Stop();
}
}
@@ -214,13 +199,12 @@
}
} else {
right_f=0;
+ Turn_val=y_deg;
mecanum_Stop();
}
}
- /*---ハンガーかけるやつ--------------------------------------------------------------------------------------------------------------------------------------------*/
- limit_up = (In_Data2+0)->in_data;
- limit_down = (In_Data2+1)->in_data;
+ /*---ハンガーかけるやつ----------------------------------------------*/
if(sbdbt.SELECT)as=1;//selectキー
if(as==1 && limit_up == 0) {
@@ -242,19 +226,15 @@
MD_SET_DRIVE(MD_Data, 5,MD_BRAKE);
}
- max_lim = (In_Data+1)->in_data;
- start_lim =(In_Data+2)->in_data;
- short_lim = (In_Data2+2)->in_data;
-
//タオルつかむ 四角
- if(grab_flag) {
+ if(grab_f) {
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);
+ MD_SET_PWM(MD_Data,4,60);
if(max_lim==1) {
grab++;
tim.reset();
@@ -264,11 +244,11 @@
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,0,1);
- if(tim.read_ms()>1000) {
+ if(tim.read_ms()>300) {
grab++;
tim.reset();
}
@@ -277,7 +257,7 @@
case 2:
//爪閉じる
i2c.Out_Set(Out_Data,1,0);
- if(tim.read_ms()>500) {
+ if(tim.read_ms()>100) {
grab++;
tim.reset();
}
@@ -286,21 +266,29 @@
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_flag=0;
+ MD_SET_PWM (MD_Data,4,60);
+ if(start_lim==1) grab_f=0;
break;
}
- } //if(grab_flag)
+ } //if(grab_f)
+
//タオル引く 丸
- else if(pull_flag) {
+ else if(pull_f) {
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);
+ MD_SET_PWM (MD_Data,4,60);
if(max_lim==1) {
pull++;
tim.reset();
@@ -314,14 +302,14 @@
//腕おろす
i2c.Out_Set(Out_Data,0,1);
- if(tim.read_ms()>1000) {
+ if(tim.read_ms()>300) {
pull++;
tim.reset();
}
break;
case 2:
- if(tim.read_ms()>500) {
+ if(tim.read_ms()>300) {
pull++;
tim.reset();
}
@@ -329,12 +317,14 @@
case 3:
MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
- MD_SET_PWM (MD_Data,4,40);
- if(start_lim==1) pull_flag=0;
+ MD_SET_PWM (MD_Data,4,60);
+ if(start_lim==1) pull_f=0;
break;
}
- } //if(pull_flag)
- else if(kago_flag) {
+ } //if(pull_f)
+
+ //かごを倒す
+ else if(kago_f) {
//腕の先を閉じておく
i2c.Out_Set(Out_Data,1,0);
@@ -342,14 +332,13 @@
//腕を前に動かす
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();
}
break;
- //かごを倒す
case 1:
//腕停止
MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
@@ -357,7 +346,7 @@
//かご回転
MD_SET_DRIVE(MD_Data,6,MD_FORWARD);
- MD_SET_PWM (MD_Data,6,70);
+ MD_SET_PWM (MD_Data,6,80);
if(slide_stop==1 || tim.read_ms()>3000) {
kago++;
tim.reset();
@@ -367,7 +356,7 @@
break;
case 2:
- if(tim.read_ms()>1500) {
+ if(tim.read_ms()>1000) {
kago++;
tim.reset();
}
@@ -375,22 +364,23 @@
case 3:
MD_SET_DRIVE(MD_Data,6,MD_REVERSE);
- MD_SET_PWM (MD_Data,6,65);
+ 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,40);
- if(start_lim==1) kago_flag=0;
+ MD_SET_PWM (MD_Data,4,60);
+ if(start_lim==1) kago_f=0;
break;
}
} 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);
//腕上げる
@@ -399,75 +389,96 @@
kago=0;
pull=0;
grab=0;
-
+
tim.reset();
}
+ //-------------- 出力 --------------//
+ //第二引数には使う最大の個数
+ i2c.Out(Out_Data,5);
+
+ //モータ出力
+ for(int i=0; i<8; i++) i2c.MD_I2C(MD_Data,i);
+
+ /* ----------- りみっと --------------- */
+ limit_up = (In_Data+4)->in_data;
+ limit_down = (In_Data+5)->in_data;
+ max_lim = (In_Data+1)->in_data;
+ start_lim =(In_Data+2)->in_data;
+ short_lim = (In_Data+6)->in_data;
+ slide_start = (In_Data+3)->in_data;
+ slide_stop = (In_Data+0)->in_data;
+
+ //半自動のデバック
+// pc.printf("pull %2d%2d ",pull,pull_f);
+// pc.printf("grab %2d%2d ",grab,grab_f);
+// pc.printf("kago %2d%2d ",kago,kago_f);
+ pc.printf("deg:%f dif:%f ",y_deg,dif_val);
+ 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(sbdbt.START && start_f == 0) {
- start_cnt++;
- start_f = 1;
- } 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;
- //-------------- 出力 --------------//
- i2c.Out(Out_Data,5);
+ if(unfold_f)i2c.Out_Set(Out_Data,3,1);
+ else i2c.Out_Set(Out_Data,3,0);
- i2c.MD_I2C(MD_Data,0);
- i2c.MD_I2C(MD_Data,1);
- i2c.MD_I2C(MD_Data,2);
- i2c.MD_I2C(MD_Data,3);
- i2c.MD_I2C(MD_Data,4);
- i2c.MD_I2C(MD_Data,5);
- i2c.MD_I2C(MD_Data,6);
- i2c.MD_I2C(MD_Data,7);
- 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(sbdbt.L1) {
Turn_val=y_deg;
offset=y_deg;
}
-
//90度回転して補正の角度を固定する
- if(LEFTkey) offset=y_deg;
- if(RIGHTkey) {
- offset=y_deg;
- right_f=1;
+ if(LEFTkey) {
+ if(once==0) {
+ left_f++;
+ left_f%=2;
+ once++;
+ }
+ } else if(RIGHTkey) {
+ if(once==0) {
+ right_f++;
+ right_f%=2;
+ once++;
+ }
}
- if(CROSS) {
+ //かごを倒す
+ else if(CROSS) {
if(once==0) {
- kago_flag++;
- kago_flag%=2;
+ kago_f++;
+ kago_f%=2;
once++;
}
- } else if(SQUARE) {
+ }
+ //タオルを掴んで動かす
+ else if(SQUARE) {
if(once==0) {
- grab_flag++;
- grab_flag%=2;
+ grab_f++;
+ grab_f%=2;
once++;
}
- } else if(CIRCLE) {
+ }
+ //倒すを引いて動かす
+ else if(CIRCLE) {
if(once==0) {
- pull_flag++;
- pull_flag%=2;
+ pull_f++;
+ pull_f%=2;
+ once++;
+ }
+ }
+ //タオル展開用
+ else if(sbdbt.START) {
+ if(once==0) {
+ unfold_f++;
+ unfold_f%=2;
once++;
}
} else once=0;
@@ -528,24 +539,6 @@
}
}
-/*メカナム旋廻移動*/
-void mecanum_TurnMove(void)
-{
- int val1,val2;
-
- MD_SET_DRIVE(MD_Data, 0,(L_1 > 0) ? MD_FORWARD : MD_REVERSE);
- MD_SET_DRIVE(MD_Data, 1,(R_1 > 0) ? MD_REVERSE : MD_FORWARD);
- MD_SET_DRIVE(MD_Data, 2,(R_1 > 0) ? MD_FORWARD : MD_REVERSE);
- MD_SET_DRIVE(MD_Data, 3,(L_1 > 0) ? MD_REVERSE : MD_FORWARD);
-
- val1 = (int)sqrt( 1.0*abs( abs(lx)*lx + abs(ly)*ly +rx));
- val2 = (int)sqrt( 1.0*abs( abs(ly)*ly - abs(lx)*lx +rx));
-
- MD_SET_PWM(MD_Data,0,val1);
- MD_SET_PWM(MD_Data,1,val2);
- MD_SET_PWM(MD_Data,2,val2);
- MD_SET_PWM(MD_Data,3,val1);
-}
/* 停止 */
void mecanum_Stop(void)
{
@@ -555,26 +548,6 @@
}
}
-
-/*割り込み*/
-// void flip1(void) {
-// static int cnt=0;
-//
-// /* カウントアップ，1sループ */
-// cnt++;
-// cnt%=10000;
-//
-// /* 100msの処理 */
-// if(cnt%100==0) {
-// myled = !myled;
-// if(tim_start==1) time_cnt++;
-// if(time_cnt>10 && time_f==0) {
-// time_f=1;
-// }
-// }
-//}
-
-
/*操作法*/
/*
〇 ボタン == 回収（引きずる）