Diff: main.cpp

Revision:

4:236e5c58e8ee

Parent:

3:1063aa133b62

Child:

5:fcc79e507610

--- a/main.cpp	Fri Aug 23 06:27:19 2019 +0000
+++ b/main.cpp	Fri Aug 23 09:20:04 2019 +0000
@@ -48,13 +48,9 @@
     {1,0,0},        // タオルの中央
 };
 /* 関数宣言 ------------------------------------------------------------------*/
-void flip1(void);
 void mecanum_Move(void);
 void mecanum_Turn(void);
 void mecanum_Stop(void);
-//int sum_deg(double deg,double old);
-void big_sort(int16_t data[], uint8_t num);
-int median_filter(int16_t data[], int16_t add_data, const uint8_t num);
 
 /* 変数宣言 ------------------------------------------------------------------*/
 //かご回転用
@@ -66,18 +62,21 @@
 //タオル引く用
 int pull=0, pull_f=0;
 
-//タオル掴むよう
-int grab=0, grab_f=0;
+//タオル掴む用
+//sgrab_f: 中央のリミットでつかむ
+int grab=0, grab_f=0, sgrab_f=0;
 
+//タオル横展開用
 int unfold_f=0;
 
 //90度回転のフラグ
-int left_f=0,right_f=0;
+int left_f=0, right_f=0;
 
 //y_deg: 起動時から絶対の角度
 //yaw: 相対角 初期状態が0になるようにする
 //offset: 初期状態を0にするために引く値
-double y_deg, yaw, offset;
+double y_deg, offset;
+int yaw=0;
 
 //Turn_val: 補正の目標値
 //dif_val: 補正値との差
@@ -88,6 +87,9 @@
 //short_lim: 腕手前側
 int short_lim, max_lim, start_lim;
 
+//機体の前方を変更する
+int dir=0;
+
 int limit_up,limit_down;            // リミットスイッチ
 int slide_start,slide_stop;     //かごのリミット
 
@@ -126,8 +128,10 @@
 
             for(int i=0; i<7; i++) i2c.In(In_Data,i);
 
-            y_deg = (double)mpu.ypr[0] * 180 / M_PI;
-            yaw = offset-y_deg;
+//            y_deg = (double)mpu.ypr[0] * 180 / M_PI;
+//            yaw = offset-y_deg;
+            yaw=90*dir;
+            yaw%=360;
             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);
@@ -227,15 +231,15 @@
             }
 
             //タオルつかむ 四角
-            if(grab_f) {
+            if(grab_f || sgrab_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,60);
-                        if(max_lim==1) {
+                        MD_SET_PWM(MD_Data,4,40);
+                        if((sgrab_f && short_lim) || max_lim==1) {
                             grab++;
                             tim.reset();
                         }
@@ -275,7 +279,10 @@
                     case 4:
                         MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
                         MD_SET_PWM  (MD_Data,4,60);
-                        if(start_lim==1)    grab_f=0;
+                        if(start_lim==1) {
+                            grab_f=0;
+                            sgrab_f=0;
+                        }
                         break;
                 }
             } //if(grab_f)
@@ -413,7 +420,8 @@
 //            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("deg:%f dif:%f ",y_deg,dif_val);
+//            pc.printf("%2d ",dir);
             pc.printf("\n\r");
 
 
@@ -439,14 +447,16 @@
             //90度回転して補正の角度を固定する
             if(LEFTkey) {
                 if(once==0) {
-                    left_f++;
-                    left_f%=2;
+                    dir++;
+//                    left_f++;
+//                    left_f%=2;
                     once++;
                 }
             } else if(RIGHTkey) {
                 if(once==0) {
-                    right_f++;
-                    right_f%=2;
+                    dir--;
+//                    right_f++;
+//                    right_f%=2;
                     once++;
                 }
             }
@@ -465,6 +475,12 @@
                     grab_f%=2;
                     once++;
                 }
+            } else if(TRIANGLE) {
+                if(once==0) {
+                    sgrab_f++;
+                    sgrab_f%=2;
+                    once++;
+                }
             }
             //倒すを引いて動かす
             else if(CIRCLE) {