Revision 6:040d001acb12, committed 2019-08-24

Comitter:

Tom0108

Date:

Sat Aug 24 06:06:36 2019 +0000

Parent:

5:fcc79e507610

Child:

7:82dad43f2549

Commit message:

Positive control

Changed in this revision

--- a/main.cpp	Sat Aug 24 00:30:20 2019 +0000
+++ b/main.cpp	Sat Aug 24 06:06:36 2019 +0000
@@ -1,56 +1,8 @@
-/*今日 やること*/
-
-/******************************************************************************/
 #include "main.h"
-//#include "gyro.h"
-#include <math.h>
-
-/******************************************************************************/
-#define CIRCLE sbdbt.CIRCLE
-#define SQUARE sbdbt.SQUARE
-#define CROSS  sbdbt.CROSS
-#define TRIANGLE  sbdbt.TRIANGLE
-#define LEFTkey  sbdbt.LEFTkey
-#define RIGHTkey  sbdbt.RIGHTkey
-#define UPkey  sbdbt.UPkey
-#define DOWNkey  sbdbt.DOWNkey
-
-/******************************************************************************/
 
 /* 型定義 --------------------------------------------------------------------*/
 
-MD_I2C_Data_TypeDef MD_Data[Motor_MAX] = {
-    { 8, 0, 0, 0, {0}},     //足回り
-    { 9, 1, 0, 0, {0}},
-    {10, 0, 0, 0, {0}},
-    {11, 1, 0, 0, {0}},
-    {12, 0, 0, 0, {0}},     // 洗濯物回収
-    {13, 1, 0, 0, {0}},     // 上下機構
-    {14, 0, 0, 0, {0}},     // 洗濯物排出
-    {15, 1, 0, 0, {0}}      // 左右移動
-};
-
-IN_I2C_Data_TypeDef In_Data[] = {
-    {1,0,0},        // 洗濯物排出(初期位置)
-    {1,1,0},        // 洗濯物排出(停止)
-    {1,2,0},        // 腕(最大  タオル)
-    {1,3,0},         // 腕(初期位置)
-    {3,0,0},        // 上下機構(上)
-    {3,1,0},        //        (下)
-    {3,2,0},         // 腕(シーツ兼シャツ）
-};
-
-OUT_I2C_Data_TypeDef Out_Data[] = {
-    {0,0,0},        // 腕の上下
-    {0,1,0},        // 爪
-    {0,2,0},        // タオルの先
-    {0,3,0},        // タオル展開
-    {1,0,0},        // タオルの中央
-};
 /* 関数宣言 ------------------------------------------------------------------*/
-void mecanum_Move(void);
-void mecanum_Turn(void);
-void mecanum_Stop(void);
 
 /* 変数宣言 ------------------------------------------------------------------*/
 //かご回転用
@@ -69,14 +21,11 @@
 //タオル横展開用
 int unfold_f=0;
 
-//90度回転のフラグ
-int left_f=0, right_f=0;
+//y_deg: ジャイロの生の値
+//yaw: 実際に使う角度
+double y_deg[2], yaw=0;;
 
-//y_deg: 起動時から絶対の角度
-//yaw: 相対角 初期状態が0になるようにする
-//offset: 初期状態を0にするために引く値
-double y_deg[2], offset;
-double yaw=0;
+//ジャイロさんぷりんぐ用
 double sum=0, ave=0;
 int offset_cnt=0;
 
@@ -89,36 +38,18 @@
 //short_lim: 腕手前側
 int short_lim, max_lim, start_lim;
 
-//機体の前方を変更する
-int dir=0;
+//足回りのpwm値
+int duty[4];
 
-int limit_up,limit_down;            // リミットスイッチ
-int slide_start,slide_stop;     //かごのリミット
+// 足回り
+int lx,ly,rx;
 
-int lx,ly,rx,L,R;        // 足回り
-int g;
+int limit_up,limit_down;        // リミットスイッチ
+int slide_start,slide_stop;     //かごのリミット
 
 int as=0;
 
-/*クラス宣言-------------------------------------------------------------------*/
-Ticker flipper1;                 // 割込み
-Y_I2C i2c(PB_9, PB_8);           // I2C
-DigitalOut myled(LED1);          // オンボードLED
-SBDBT sbdbt(PA_0, PA_1, 9600);   // SDBDT
-MyMPU6050 mpu(PC_9, PA_8);       // ジャイロセンサ
-
-DigitalOut blue(PA_10);
-DigitalOut green(PB_4);
-DigitalOut red(PB_5);
-
-//半自動の時に使う
-Timer tim;
-
 /*----------------------------------- main -----------------------------------*/
-/* @brief  メインプログラム
-* @param  なし
-* @retval なし
-*/
 int main()
 {
     //flipper1.attach(&flip1, 0.001);    // 割り込み
@@ -126,103 +57,53 @@
     while(1) {
         //自動系のタイマースタート
         tim.start();
+        drift_tim.start();
         mpu.loop();
 
+        //ジャイロから値を取得
         y_deg[1] = y_deg[0];
         y_deg[0] = (double)mpu.ypr[0] * 180 / M_PI;
-        
+
         //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(sbdbt.State_check()) {
             for(int i=0; i<7; i++) i2c.In(In_Data,i);
-            
+
             //誤差をさんぷりんぐ
             if(offset_cnt<1000) {
+                PALETTE(RED);
                 if(offset_cnt!=0) {
                     sum+=(y_deg[0]-y_deg[1]);
                     ave=sum/offset_cnt;
                 }
                 offset_cnt++;
-            } else yaw += (y_deg[0]-y_deg[1])-ave;
-//            yaw=90*dir;
-//            yaw%=360;
-            dif_val = y_deg[0]-Turn_val;
+            } else {
+                PALETTE(GREEN);
+                yaw -= (y_deg[0]-y_deg[1])-ave;
+            }
+
+            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)*100/64;
-            L = lx+ly;
-            R = ly-lx;
-
-            if(sbdbt.LX !=64 || sbdbt.LY !=64) {
-
-                mecanum_Move();
+            rx = (sbdbt.RX - 64)*30/64;
 
-                // 以下，ジャイロセンサーのプログラム
-                if(g) {
-                    if(abs(ly) > 10) {
-                        if(ly>10) {
-                            (MD_Data+0)->PWMVal = MD_GET_PWM(MD_Data,0)-dif_val;
-                            (MD_Data+1)->PWMVal = MD_GET_PWM(MD_Data,1)+dif_val;
-                        } else {
-                            (MD_Data+0)->PWMVal = MD_GET_PWM(MD_Data,0)+dif_val;
-                            (MD_Data+1)->PWMVal = MD_GET_PWM(MD_Data,1)-dif_val;
-                        }
-                    }
-
-                    if(abs(lx) > 10) {
-                        if(lx>10) {
-                            (MD_Data+0)->PWMVal = MD_GET_PWM(MD_Data,0)-dif_val;
-                            (MD_Data+2)->PWMVal = MD_GET_PWM(MD_Data,2)+dif_val;
-                        } else {
-                            (MD_Data+0)->PWMVal = MD_GET_PWM(MD_Data,0)+dif_val;
-                            (MD_Data+2)->PWMVal = MD_GET_PWM(MD_Data,2)-dif_val;
-                        }
-                    }
+            //メカナム基本動作
+            mecanum_Move(lx, ly, rx);
 
-                    if((abs(ly)-abs(lx)) > 0) {
-                        (MD_Data+2)->PWMVal = MD_GET_PWM(MD_Data,0);
-                        (MD_Data+3)->PWMVal = MD_GET_PWM(MD_Data,1);
-                    } else {
-                        (MD_Data+1)->PWMVal = MD_GET_PWM(MD_Data,0);
-                        (MD_Data+3)->PWMVal = MD_GET_PWM(MD_Data,2);
-                    }
-                }     // if(g)
-            }// if(LX !=64 || LY !=64)
-            else if(sbdbt.RX != 64) {         // 旋回
-                Turn_val=y_deg[0];
-                mecanum_Turn();
-            } else {
-                mecanum_Stop();
-            }
+            //旋回している間タイマーをリセット
+            if(sbdbt.RX != 64) drift_tim.reset();
 
-            // 90°旋回
-            if(left_f) {
-                if(abs(yaw)<60) {
-                    for(int i=0; i<4; i++) {
-                        MD_SET_PWM(MD_Data, i,60);
-                        MD_SET_DRIVE(MD_Data, i,MD_REVERSE);
-                    }
-                } else {
-                    left_f=0;
-                    Turn_val=y_deg[0];
-                    mecanum_Stop();
-                }
-            }
-            if(right_f) {
-                if(yaw>-60) {
-                    for(int i=0; i<4; i++) {
-                        MD_SET_PWM(MD_Data, i,60);
-                        MD_SET_DRIVE(MD_Data, i,MD_FORWARD);
-                    }
-                } else {
-                    right_f=0;
-                    Turn_val=y_deg[0];
-                    mecanum_Stop();
-                }
-            }
+            //旋回して慣性で動いた後の角度に補正する
+            if(drift_tim.read_ms()<500) Turn_val=yaw;
+
+            //角度補正
+            AngleCorrection(yaw, Turn_val);
 
             /*---ハンガーかけるやつ----------------------------------------------*/
             if(sbdbt.SELECT)as=1;//selectキー
@@ -248,6 +129,7 @@
 
             //タオルつかむ 四角
             if(grab_f || sgrab_f) {
+                PALETTE(BLUE);
                 switch(grab) {
                     //腕を前に動かす
                     case 0:
@@ -255,6 +137,7 @@
                         i2c.Out_Set(Out_Data,1,1);
                         MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
                         MD_SET_PWM(MD_Data,4,40);
+                        //sgrab_fは途中のリミットで止まる
                         if((sgrab_f && short_lim) || max_lim==1) {
                             grab++;
                             tim.reset();
@@ -305,6 +188,7 @@
 
             //タオル引く 丸
             else if(pull_f) {
+                PALETTE(CYAN);
                 switch(pull) {
                     //腕を前に動かす
                     case 0:
@@ -348,6 +232,7 @@
 
             //かごを倒す
             else if(kago_f) {
+                PALETTE(MAGENTA);
                 //腕の先を閉じておく
                 i2c.Out_Set(Out_Data,1,0);
 
@@ -436,7 +321,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("0:%f 1:%f ave:%f yaw:%f",y_deg[0],y_deg[1],ave,yaw);
+//            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("%2d ",dir);
             pc.printf("\n\r");
 
@@ -455,29 +341,13 @@
 
 
             /* ----------------ボタン系--------------- */
-            //補正角リセット
+            //角度リセット
             if(sbdbt.L1) {
-                Turn_val=y_deg[0];
-                offset=y_deg[0];
-            }
-            //90度回転して補正の角度を固定する
-            if(LEFTkey) {
-                if(once==0) {
-                    dir++;
-//                    left_f++;
-//                    left_f%=2;
-                    once++;
-                }
-            } else if(RIGHTkey) {
-                if(once==0) {
-                    dir--;
-//                    right_f++;
-//                    right_f%=2;
-                    once++;
-                }
+                yaw=0;
+                Turn_val=0;
             }
             //かごを倒す
-            else if(CROSS) {
+            if(CROSS) {
                 if(once==0) {
                     kago_f++;
                     kago_f%=2;
@@ -521,63 +391,33 @@
 }       // int main()
 
 /* メカナムの基本移動 */
-void mecanum_Move(void)
+void mecanum_Move(int lx, int ly, int rx)
 {
-    int val1,val2;
-
-    MD_SET_DRIVE(MD_Data, 0,(L > 0) ? MD_FORWARD : MD_REVERSE);
-    MD_SET_DRIVE(MD_Data, 1,(R > 0) ? MD_REVERSE : MD_FORWARD);
-    MD_SET_DRIVE(MD_Data, 2,(R > 0) ? MD_FORWARD : MD_REVERSE);
-    MD_SET_DRIVE(MD_Data, 3,(L > 0) ? MD_REVERSE : MD_FORWARD);
-
-    val1 = (int)sqrt( 1.0*abs( abs(lx)*lx + abs(ly)*ly ) );
-    val2 = (int)sqrt( 1.0*abs( abs(ly)*ly - abs(lx)*lx ) );
-
-    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);
-
+    duty[0]=lx+ly;
+    duty[1]=-(-lx+ly);
+    duty[2]=-lx+ly;
+    duty[3]=-(lx+ly);
 
-    if(MD_GET_PWM(MD_Data,0)*2 < MD_GET_PWM(MD_Data,1) ) {
-        MD_SET_PWM(MD_Data, 0, 0);
-        g = 0;
-    } else if(  MD_GET_PWM(MD_Data,1)*2 < MD_GET_PWM(MD_Data,0) ) {
-        MD_SET_PWM(MD_Data, 1, 0);
-        g = 0;
-    }
-
-    // 斜めじゃないなら大きいほうにPWMをそろえて完全縦横移動
-    else if( MD_GET_PWM(MD_Data,0) < MD_GET_PWM(MD_Data,1) ) {
-        MD_SET_PWM(MD_Data, 0, MD_GET_PWM(MD_Data,1));
-        g = 1;
-
-    } else {
-        MD_SET_PWM(MD_Data, 1, MD_GET_PWM(MD_Data,0));
-        g = 1;
-    }
-
-    MD_SET_PWM(MD_Data, 2, MD_GET_PWM(MD_Data,1));
-    MD_SET_PWM(MD_Data, 3, MD_GET_PWM(MD_Data,0));
-
-}
-
-/* メカナム旋回 */
-void mecanum_Turn(void)
-{
     for(int i=0; i<4; i++) {
-        MD_SET_PWM(MD_Data, i,sbdbt.R1?abs(sbdbt.RX - 64)*50/64:abs(sbdbt.RX - 64)*100/64);
-        MD_SET_DRIVE(MD_Data, i,(sbdbt.RX < 64)? MD_REVERSE: MD_FORWARD);
+        duty[i]+=rx;
+        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]));
     }
 }
 
-/* 停止 */
-void mecanum_Stop(void)
+void AngleCorrection(double n_angle, double t_angle)
 {
+    double dif=-(t_angle-n_angle);
+
+    if(dif>=30) dif=30;
+    else if(dif<=-30) dif=-30;
+
     for(int i=0; i<4; i++) {
-        MD_SET_PWM(MD_Data, i, 0);
-        MD_SET_DRIVE(MD_Data, i, MD_BRAKE);
+        duty[i]+=dif;
+        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]));
     }
+
 }
 
 /*操作法*/
@@ -588,18 +428,15 @@
 □  ボタン  ==   回収（タオル）
 上 ボタン  ==   ハンガーかけ機構 上昇
 下 ボタン  ==   ハンガーかけ機構 下降
-左 ボタン  ==   90回転
-右 ボタン  ==   90回転
 L1 ボタン  ==   リセット（足回り）
 L2 ボタン  ==  タオル掛け
-R1 ボタン  ==  押しながら回転でゆっくりになる
-R2 ボタン  ==  タオルひっぱる奴
+R2 ボタン  ==  タオル真ん中掴む
 select     ==  ハンガー掛け 最大まで上昇
 start      ==  ハンガー展開
 */
 /*その他注意点*/
 /*
-動かすときはジャイロの値を安定させるため15秒から20秒待つ
+動かすときはジャイロの値を安定させるためLEDが赤から緑になるまで待つ
 メカナムは振動が大きいせいかナット等が外れやすいため、定期的に確認をする（特に足回り）
 あまり急発進をさせない（たまに暴走）
 暴走したときはコントローラーを動かしたら治るはず

--- a/main.h	Sat Aug 24 00:30:20 2019 +0000
+++ b/main.h	Sat Aug 24 06:06:36 2019 +0000
@@ -12,18 +12,27 @@
 
 #ifndef _MAIN_H_
 #define _MAIN_H_
-#include "gyro.h"
-#include "YKNCT_I2C.h"
 
 /* ファイル追加 --------------------------------------------------------------*/
 #include "YKNCT.h"
 #include "mbed.h"
+#include "gyro.h"
+#include "YKNCT_I2C.h"
+#include <math.h>
 
 /* 定数定義 ------------------------------------------------------------------*/
-
+#define CIRCLE sbdbt.CIRCLE
+#define SQUARE sbdbt.SQUARE
+#define CROSS  sbdbt.CROSS
+#define TRIANGLE  sbdbt.TRIANGLE
+#define LEFTkey  sbdbt.LEFTkey
+#define RIGHTkey  sbdbt.RIGHTkey
+#define UPkey  sbdbt.UPkey
+#define DOWNkey  sbdbt.DOWNkey
 
 /* マクロ定義 ----------------------------------------------------------------*/
-
+#define PALETTE(color)\
+for(int s=0; s<3; s++) LED[s]=color&1<<s
 
 /* 型定義 --------------------------------------------------------------------*/
 typedef enum
@@ -59,13 +68,71 @@
     ExTowel,
     OUT_MAX
 }_OUT_;
+
+typedef enum {
+    BLACK,
+    BLUE,
+    GREEN,
+    CYAN,
+    RED,
+    MAGENTA,
+    YELLOW,
+    WHITE,
+
+    COLOR_MAX
+
+} _COLOR_;
+
+MD_I2C_Data_TypeDef MD_Data[Motor_MAX] = {
+    { 8, 0, 0, 0, {0}},     //足回り
+    { 9, 1, 0, 0, {0}},
+    {10, 0, 0, 0, {0}},
+    {11, 1, 0, 0, {0}},
+    {12, 0, 0, 0, {0}},     // 洗濯物回収
+    {13, 1, 0, 0, {0}},     // 上下機構
+    {14, 0, 0, 0, {0}},     // 洗濯物排出
+    {15, 1, 0, 0, {0}}      // 左右移動
+};
+
+IN_I2C_Data_TypeDef In_Data[] = {
+    {1,0,0},        // 洗濯物排出(初期位置)
+    {1,1,0},        // 洗濯物排出(停止)
+    {1,2,0},        // 腕(最大  タオル)
+    {1,3,0},         // 腕(初期位置)
+    {3,0,0},        // 上下機構(上)
+    {3,1,0},        //        (下)
+    {3,2,0},         // 腕(シーツ兼シャツ）
+};
+
+OUT_I2C_Data_TypeDef Out_Data[] = {
+    {0,0,0},        // 腕の上下
+    {0,1,0},        // 爪
+    {0,2,0},        // タオルの先
+    {0,3,0},        // タオル展開
+    {1,0,0},        // タオルの中央
+};
 /* 変数宣言 ------------------------------------------------------------------*/
 
+/* 関数宣言 ------------------------------------------------------------------*/
+//メカナム基本動作
+void mecanum_Move(int lx, int ly, int rx);
 
+//角度補正
+//n_angle: 現在の角度
+//t_angle: 目標の角度
+void AngleCorrection(double n_angle, double t_angle);
 
-/* 関数宣言 ------------------------------------------------------------------*/
+/*クラス宣言-------------------------------------------------------------------*/
+Y_I2C i2c(PB_9, PB_8);           // I2C
+SBDBT sbdbt(PA_0, PA_1, 9600);   // SDBDT
+MyMPU6050 mpu(PC_9, PA_8);       // ジャイロセンサ
+DigitalOut LED[]= {PA_10,PB_4,PB_5};
 
+//半自動の時に使う
+Timer tim;
 
+//角度補正の時に使う
+Timer drift_tim;
 
 #endif /* _MAIN_H_ */