Diff: main.cpp

Revision:

2:47954f05d32d

Parent:

1:199c4a71da88

Child:

3:1063aa133b62

--- a/main.cpp	Fri Aug 23 02:18:37 2019 +0000
+++ b/main.cpp	Fri Aug 23 03:54:05 2019 +0000
@@ -46,10 +46,10 @@
 
 OUT_I2C_Data_TypeDef Out_Data[] = {
     {0,0,0},        // 腕の上下
-    {0,1,0},        // 洗濯物排出
-    {0,2,0},        // バスタオルの先
-    {0,3,0},        // 展開
-    {1,0,0},        // バスタオルの中央
+    {0,1,0},        // 爪
+    {0,2,0},        // タオルの先
+    {0,3,0},        // タオル展開
+    {1,0,0},        // タオルの中央
 };
 /* 関数宣言 ------------------------------------------------------------------*/
 void flip1(void);
@@ -151,19 +151,6 @@
             L = lx+ly;
             R = ly-lx;
 
-            if(sbdbt.L1) {
-                Turn_val=y_deg;
-                offset=y_deg;
-            }
-
-            if(LEFTkey) {
-                offset=y_deg;
-            }
-            if(RIGHTkey) {
-                offset=y_deg;
-                right_f=1;
-            }
-
             yaw = -y_deg+offset;
             dif_val=y_deg-Turn_val;
 
@@ -258,8 +245,7 @@
             max_lim  = (In_Data+1)->in_data;
             start_lim  =(In_Data+2)->in_data;
             short_lim = (In_Data2+2)->in_data;
-            //-------------- 洗濯物回収 --------------//
-            /*---回収機構 ～押し出し～---------------------------------------------------------------------------------------------------------------------------------------*/
+            
             //タオルつかむ 四角
             if(grab_flag) {
                 switch(grab) {
@@ -276,6 +262,10 @@
                         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()>1000) {
@@ -302,7 +292,7 @@
                         break;
                 }
             } //if(grab_flag)
-            //タオル引く 丸   
+            //タオル引く 丸
             else if(pull_flag) {
                 switch(pull) {
                     //腕を前に動かす
@@ -318,6 +308,10 @@
                         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()>1000) {
@@ -349,7 +343,10 @@
                     case 0:
                         MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
                         MD_SET_PWM  (MD_Data,4,40);
-                        if(max_lim==1) kago++;
+                        if(max_lim==1) {
+                            kago++;
+                            tim.reset();
+                        }
                         break;
 
                     //かごを倒す
@@ -361,22 +358,25 @@
                         //かご回転
                         MD_SET_DRIVE(MD_Data,6,MD_FORWARD);
                         MD_SET_PWM  (MD_Data,6,70);
-                        if(slide_stop==1) {
+                        if(slide_stop==1 || tim.read_ms()>3000) {
                             kago++;
+                            tim.reset();
                             MD_SET_DRIVE(MD_Data,6,MD_BRAKE);
                             MD_SET_PWM  (MD_Data,6,0);
                         }
-                        tim.reset();
                         break;
 
                     case 2:
-                        if(tim.read_ms()>1500)kago++;
+                        if(tim.read_ms()>1500) {
+                            kago++;
+                            tim.reset();
+                        }
                         break;
 
                     case 3:
                         MD_SET_DRIVE(MD_Data,6,MD_REVERSE);
                         MD_SET_PWM  (MD_Data,6,65);
-                        if(slide_start==1) kago++;
+                        if(slide_start==1 || tim.read_ms()>3000) kago++;
                         break;
 
                     case 4:
@@ -392,13 +392,15 @@
                 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);
-                
+                i2c.Out_Set(Out_Data,0,0);
+
                 kago=0;
                 pull=0;
                 grab=0;
+                
+                tim.reset();
             }
 
             /*タオル掛け*/
@@ -438,7 +440,18 @@
 //            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(CROSS) {
                 if(once==0) {
                     kago_flag++;