Takushima Yukimasa
a
Diff: main.cpp
- Revision:
- 11:c1a22e2e3534
- Parent:
- 10:7c73e1577742
- Child:
- 12:7e33e400a48d
--- a/main.cpp Fri Aug 30 07:47:41 2019 +0000
+++ b/main.cpp Wed Sep 04 05:31:40 2019 +0000
@@ -38,8 +38,7 @@
double rawyaw[2];
//Turn_val: 補正の目標値
-//dif_val: 補正値との差
-double Turn_val=def_val, dif_val;
+double Turn_val=def_val;
//short_lim: 腕中央
//max_lim: 腕奥側
@@ -58,9 +57,12 @@
int as=0;
+int release_f=0;
+
/*----------------------------------- main -----------------------------------*/
int main()
{
+ //ジャイロリセット
bno.reset();
while(1) {
@@ -71,31 +73,25 @@
PALETTE(BLUE);
#endif
- bno.setmode(OPERATION_MODE_IMUPLUS);
-
//自動系のタイマースタート
tim.start();
drift_tim.start();
+ bno.setmode(OPERATION_MODE_IMUPLUS);
//角度の取得
bno.get_angles();
rawyaw[1]=rawyaw[0];
rawyaw[0]=bno.euler.yaw;
-
+
//180～-180をまたいだ時
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);
- dif_val = yaw-Turn_val;
- //制限
- if(dif_val>=30) dif_val=30;
- else if(dif_val<=-30) dif_val=-30;
-
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)*60/64;
@@ -109,25 +105,27 @@
if(sbdbt.RX != 64) drift_tim.reset();
//旋回して慣性で動いた後の角度に補正する
- if(drift_tim.read_ms()<500) Turn_val=yaw;
+ if(drift_tim.read_ms()<1000) Turn_val=yaw;
//角度補正
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(UPkey && limit_up == 0) {
+// 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;
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);
@@ -138,8 +136,6 @@
//タオルつかむ 四角
if(grab_f) {
- PALETTE(GREEN);
-
//真ん中で止まる用
short_f=1;
switch(grab) {
@@ -202,7 +198,6 @@
//シーツ用
else if(spull_f) {
- PALETTE(WHITE);
short_f=1;
switch(spull) {
//腕を前に動かす
@@ -210,15 +205,15 @@
//真ん中のリミットより前に腕があった場合にバックする
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);
+ MD_SET_PWM (MD_Data,4,30);
if(short_lim==1) {
spull++;
tim.reset();
} else if(max_lim) back_f=1;
break;
-
- case 1:
+
+ case 1:
//腕停止
MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
MD_SET_PWM(MD_Data,4,0);
@@ -264,7 +259,6 @@
//タオル引く 丸
else if(pull_f) {
- PALETTE(CYAN);
short_f=1;
switch(pull) {
//腕を前に動かす
@@ -315,7 +309,7 @@
//かごを倒す
else if(kago_f) {
- PALETTE(MAGENTA);
+// PALETTE(MAGENTA);
short_f=1;
//腕の先を閉じておく
i2c.Out_Set(Out_Data,1,0);
@@ -432,9 +426,9 @@
// 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("%.1f %.1f ",rawyaw[0],rawyaw[1]);
- pc.printf("yaw:%.1f dif:%.1f",yaw,dif_val);
-// pc.printf("%2d ",dir);
+// 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("\n\r");
/* --------------電磁弁系-------------------- */
@@ -446,9 +440,16 @@
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);
+ //タオル展開
+ 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) {
@@ -492,6 +493,14 @@
unfold_f%=2;
once++;
}
+ }
+ //タオル展開用
+ else if(sbdbt.SELECT) {
+ if(once==0) {
+ release_f++;
+ release_f%=2;
+ once++;
+ }
} else once=0;