Takushima Yukimasa
a
Diff: main.cpp
- Revision:
- 10:7c73e1577742
- Parent:
- 9:64fcbcc182d9
- Child:
- 11:c1a22e2e3534
--- a/main.cpp Tue Aug 27 10:59:38 2019 +0000
+++ b/main.cpp Fri Aug 30 07:47:41 2019 +0000
@@ -18,25 +18,24 @@
//コントローラーの状態を保存するため
int once=0;
-//タオル引く用
-int pull=0, pull_f=0;
-
//タオル掴む用
-//spull_f: 中央のリミットでつかむ
+//pull: 奥のリミットで引く
+//spull: 中央のリミットでつかむ
+//grab: 掴む
//back_f: 真ん中目で下がるとき
-int grab=0, grab_f=0, spull_f=0;
+int pull=0, pull_f=0, grab=0, grab_f=0, spull=0, spull_f=0;
int back_f=0;
//タオル横展開用
int unfold_f=0;
-//y_deg: ジャイロの生の値
//yaw: 実際に使う角度
-double y_deg[2], yaw=def_val;
+double yaw=def_val;
-//ジャイロさんぷりんぐ用
-double sum=0, ave=0;
-int offset_cnt=0;
+//[0]: roll
+//[1]: pitch
+//[2]: yaw
+double rawyaw[2];
//Turn_val: 補正の目標値
//dif_val: 補正値との差
@@ -62,43 +61,36 @@
/*----------------------------------- main -----------------------------------*/
int main()
{
- //flipper1.attach(&flip1, 0.001); // 割り込み
+ bno.reset();
while(1) {
+ //ゾーン対応
+#ifdef _RED_ZONE_
+ PALETTE(RED);
+#else
+ PALETTE(BLUE);
+#endif
+
+ bno.setmode(OPERATION_MODE_IMUPLUS);
+
//自動系のタイマースタート
tim.start();
drift_tim.start();
- mpu.loop();
- //ジャイロから値を取得
- y_deg[1] = y_deg[0];
- y_deg[0] = (double)mpu.ypr[0] * 180 / M_PI;
-
+ //角度の取得
+ bno.get_angles();
+ rawyaw[1]=rawyaw[0];
+ rawyaw[0]=bno.euler.yaw;
+
//180～-180をまたいだ時
- if(y_deg[1]<-90 && y_deg[0]>90) y_deg[1]+=360;
- else if(y_deg[1]>90 && y_deg[0]<-90) y_deg[1]-=360;
+ if(rawyaw[1]<90 && rawyaw[0]>270) rawyaw[1]+=360;
+ else if(rawyaw[1]>270 && rawyaw[0]<90) rawyaw[1]-=360;
+
+ yaw-=rawyaw[0]-rawyaw[1];
if(sbdbt.State_check()) {
for(int i=0; i<7; i++) i2c.In(In_Data,i);
- //誤差をさんぷりんぐ
- if(offset_cnt<1000) {
- PALETTE(YELLOW);
- if(offset_cnt!=0) {
- sum+=(y_deg[0]-y_deg[1]);
- ave=sum/offset_cnt;
- }
- offset_cnt++;
- } else {
- //ゾーン対応
-#ifdef _RED_ZONE_
- PALETTE(RED);
-#else
- PALETTE(BLUE);
-#endif
- yaw -= (y_deg[0]-y_deg[1])-ave;
- }
-
dif_val = yaw-Turn_val;
//制限
if(dif_val>=30) dif_val=30;
@@ -145,10 +137,10 @@
}
//タオルつかむ 四角
- if(grab_f || spull_f) {
+ if(grab_f) {
PALETTE(GREEN);
-
- //真ん中で止まる用s
+
+ //真ん中で止まる用
short_f=1;
switch(grab) {
//腕を前に動かす
@@ -156,22 +148,12 @@
//爪開く
i2c.Out_Set(Out_Data,1,1);
- if(back_f) MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
- else MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
+ MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
MD_SET_PWM (MD_Data,4,60);
- 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();
- }
+ if(max_lim==1) {
+ grab++;
+ tim.reset();
}
break;
@@ -212,13 +194,74 @@
MD_SET_PWM (MD_Data,4,60);
if(start_lim==1) {
grab_f=0;
- spull_f=0;
tim.reset();
}
break;
}
} //if(grab_f)
+ //シーツ用
+ else if(spull_f) {
+ PALETTE(WHITE);
+ short_f=1;
+ switch(spull) {
+ //腕を前に動かす
+ case 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,60);
+
+ if(short_lim==1) {
+ spull++;
+ tim.reset();
+ } else if(max_lim) back_f=1;
+ 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()>200) {
+ spull++;
+ tim.reset();
+ }
+ break;
+
+ case 2:
+ //爪閉じる
+ i2c.Out_Set(Out_Data,1,0);
+ if(tim.read_ms()>0) {
+ spull++;
+ tim.reset();
+ }
+ break;
+
+ case 3:
+ //腕上げる
+ i2c.Out_Set(Out_Data,0,0);
+ if(tim.read_ms()>0) {
+ spull++;
+ tim.reset();
+ }
+ break;
+
+ //腕戻す
+ case 4:
+ MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
+ MD_SET_PWM (MD_Data,4,60);
+ if(start_lim==1) {
+ spull_f=0;
+ tim.reset();
+ }
+ break;
+
+ }
+ } //if(spull_f)
+
//タオル引く 丸
else if(pull_f) {
PALETTE(CYAN);
@@ -234,7 +277,6 @@
if(max_lim==1) {
pull++;
- back_f=0;
tim.reset();
}
break;
@@ -343,7 +385,7 @@
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);
@@ -358,7 +400,7 @@
kago=0;
pull=0;
grab=0;
-
+ spull=0;
tim.reset();
}
}
@@ -390,12 +432,11 @@
// 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("yaw:%.1f dif:%.1f",yaw,dif_val);
- pc.printf("0:%.1f 1:%.1f yaw:%.1f dif:%.1f",y_deg[0],y_deg[1],yaw,dif_val);
+ pc.printf("%.1f %.1f ",rawyaw[0],rawyaw[1]);
+ pc.printf("yaw:%.1f dif:%.1f",yaw,dif_val);
// pc.printf("%2d ",dir);
pc.printf("\n\r");
-
/* --------------電磁弁系-------------------- */
/*タオル掛け*/
if(sbdbt.L2)i2c.Out_Set(Out_Data,2,1);
@@ -408,7 +449,6 @@
if(unfold_f)i2c.Out_Set(Out_Data,3,1);
else i2c.Out_Set(Out_Data,3,0);
-
/* ----------------ボタン系--------------- */
//角度リセット
if(sbdbt.L1) {
@@ -491,7 +531,6 @@
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]));
}
-
}
/*操作法*/
@@ -507,12 +546,4 @@
R2 ボタン == タオル真ん中掴む
select == ハンガー掛け 最大まで上昇
start == ハンガー展開
-*/
-/*その他注意点*/
-/*
-動かすときはジャイロの値を安定させるためLEDが赤から緑になるまで待つ
-メカナムは振動が大きいせいかナット等が外れやすいため、定期的に確認をする（特に足回り）
-あまり急発進をさせない（たまに暴走）
-暴走したときはコントローラーを動かしたら治るはず
-コントローラー接続が切れたらSTMの黒い奴を押す（わからないときは近くの回路班へ）
*/
\ No newline at end of file