Diff: main.cpp

Revision:

0:9e851dc42cde

Child:

1:b9f698be841c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Mar 12 02:29:33 2020 +0000
@@ -0,0 +1,254 @@
+/**
+********************************************************************************
+* @file    main.c
+* @author  You
+* @version V?.?.?
+* @date    Today
+* @brief   メインファイル
+******************************************************************************
+*/
+
+/* Includes ------------------------------------------------------------------*/
+#include <main.h>
+
+/* 型定義 --------------------------------------------------------------------*/
+
+/* ロボットの加速度 */
+ROCATION NowAcc;
+/* ロボットの座標 */
+ROCATION NowLoc;
+
+/* 定数定義 ------------------------------------------------------------------*/
+/* マクロ定義 ----------------------------------------------------------------*/
+/* 関数プロトタイプ宣言 -------------------------------------------------------*/
+
+/* タイマ呼び出し用 */
+void IT_CallBack(void);
+
+/* 自己位置推定処理 */
+void LocEstimate(void);
+
+
+/* 変数定義 ------------------------------------------------------------------*/
+
+/* 操作権 0…なし 1…手動 2…自動 */
+int operate=0;
+
+/* 自動シーケンス */
+int auto_mode=0;
+
+/* 直読みエンコーダ角度保存(degree) */
+double EncoderDeg[EncoderMAX] = {0};
+
+/* 足回り値保存変数 */
+int MovMotor[4]={0};
+
+/* 自動yaw補整目標角度 */
+double TarTheta=0;
+
+/* クラス定義 ----------------------------------------------------------------*/
+
+/* 割り込み用クラス */
+Ticker flipper;
+
+/* UART (Tx,Rx) */
+Serial telemetry(USBTX, USBRX, 115200);
+
+/* コントローラー */
+SBDBT DS3(PA_0, PA_1, 9600);
+
+/* オンボードLED */
+DigitalOut led(LED2);
+
+/* USERボタン */
+DigitalIn UB(PC_13,PullDown);
+
+/* エンコーダーピン CS */
+DigitalOut CS[] = {PA_2,PA_3};
+DigitalOut CL[] = {PA_4,PA_5};
+DigitalIn  DO[] = {PA_6,PA_7};
+
+/* 足回り動作クラス定義 */
+Move omuni(MovMotor,NowLoc.theta);
+
+/* I2C MDのクラス定義 */
+YKNCT_MD_I2C MD(PB_9,PB_8);
+
+/* ジャイロのピン設定 */
+BNO055 bno(PB_9, PB_8);
+
+
+/*----------------------------------- main ----------------------------------*/
+int main()
+{
+  telemetry.printf("\n\rMainStart");
+
+  /* 割り込みの設定
+   * IT_CallBack関数を0.1msで割り込み */
+  flipper.attach_us(&IT_CallBack, 100);
+
+  /* ジャイロの設定 */
+  bno.setmode(OPERATION_MODE_IMUPLUS);
+
+  /* I2CMDの設定 */
+  MD.Init(0,MD_SMB);
+  MD.Init(1,MD_SMB);
+  MD.Init(2,MD_SMB);
+  MD.Init(3,MD_SMB);
+
+  telemetry.printf("\n\rMainLoopStart");
+  /* メインループ --------------------------------------------------------------*/
+  while(1) {
+    /* オンボードLED点滅 */
+    led=!led;
+
+    /* 表示改行 */
+    telemetry.printf("\n\r");
+
+    /* 自動処理関連テレメトリ */
+    telemetry.printf("ope:%d ",operate);
+    /* 座標テレメトリ */
+    telemetry.printf("X:%4.0f Y:%4.0f T:%4.0f ",NowLoc.X,NowLoc.Y,NowLoc.theta);
+
+    /* 自己位置推定更新 */
+    LocEstimate();
+
+    /* I2CMD実行 */
+    MD.Exe();
+
+
+    /* 操縦権変更 ×停止 △手動 〇自動 */
+    if(DS3.CROSS)     operate=0;
+    if(DS3.TRIANGLE)  operate=1;
+    if(DS3.CIRCLE)    operate=2;
+
+    /* 操縦権：なし 停止動作 */
+    if(operate==0){
+      /* 足回り停止 */
+      omuni.XmarkOmni_Move(0,0,0);
+
+
+    }
+    /* 操縦権：手動 */
+    else if(operate==1){
+      /* 足回り手動動作 */
+      int x_val = (double)(DS3.LX-64)*100/64;
+      int y_val = (double)(64-DS3.LY)*100/64;
+      int r_val = (double)(DS3.RX-64)*100/64;
+      /**
+       * このr_valとNowLoc.thetaとTarThetaをｺﾈｺﾈしてジャイロ補整するのじゃ
+       * 動けばいいのではなく，見やすく書くのじゃよ
+       **/
+      omuni.XmarkOmni_Move(x_val,y_val,r_val);
+
+
+    }
+    /* 操縦権：自動 */
+    else if(operate==2){
+      switch(auto_mode){
+        /* スタート待機処理 */
+        case 0:
+          /* オンボードSWで次のステップに */
+          if(UB) auto_mode++;
+        break;
+
+        /* 〇〇の処理 */
+        case 1:
+          /* 〇〇の時次のステップに */
+          if(1) auto_mode++;
+        break;
+
+        /* 終了処理 */
+        default:
+          auto_mode=0;
+          operate=0;
+        break;
+      }
+    }
+    
+  }
+}
+
+
+/*******************************************************************************
+* @概要   自己位置推定関数
+* @引数   なし
+* @返り値 なし
+*******************************************************************************/
+void LocEstimate(void){
+  static double GyroDeg[2]={0};
+  static double EncDeg[2][2]={0};
+  static double disp[3]={0};
+
+  /* ジャイロの値取得 */
+  bno.get_angles();
+  GyroDeg[1]=GyroDeg[0];
+  GyroDeg[0]=bno.euler.yaw;
+  if(GyroDeg[0]!=0) {
+    /* 359→0を跨いだ時，前回の値を0から逆回転で負の値で表記 */
+    if(GyroDeg[1]<90 && GyroDeg[0]>270) GyroDeg[1]+=360;
+    /* 0→359を跨いだ時，前回の値を360以上の値で表記 */
+    else if(GyroDeg[1]>270 && GyroDeg[0]<90) GyroDeg[1]-=360;
+    /* 差を求める*/
+    disp[2]=GyroDeg[1]-GyroDeg[0];
+  }
+  /* Enc2つの差分求める */
+  for(int i=0;i<2;i++){
+    EncDeg[i][1]=EncDeg[i][0];
+    EncDeg[i][0]=EncoderDeg[i];
+    disp[i]=EncDeg[i][1]-EncDeg[i][0];
+  }
+  /* 差分を加速度として保存 */
+  NowAcc.theta = disp[2];
+  NowAcc.X =  disp[0] * cos(NowLoc.theta) + disp[1] * sin(NowLoc.theta)+200*disp[2];
+  NowAcc.Y = -disp[0] * sin(NowLoc.theta) + disp[1] * cos(NowLoc.theta)+200*disp[2];
+  /* 差分を累積して現在位置を保存 */
+  NowLoc.X += NowAcc.X;
+  NowLoc.Y += NowAcc.Y;
+  NowLoc.theta += NowAcc.theta;
+}
+
+
+
+
+/* 割り込み(100us) *************************************************************/
+void IT_CallBack(void)
+{
+  static int cnt = 0;
+  static int data[EncoderMAX] = {0};
+  static double EncDeg[EncoderMAX][2] = {0};
+
+  for(int i=0;i<EncoderMAX;i++) 
+  switch(cnt){
+    /* 最初の処理 */
+    case 0:
+      data[i] = 0; CS[i] = 0; CL[i] = 1;
+      break;
+    /* 最後の処理 */
+    case 25:
+      CS[i]=1;
+      /* 前回の値更新 今回の値更新(エンコーダの値(0~4096)を角度(0~360)に) */
+      EncDeg[i][1] = EncDeg[i][0];
+      EncDeg[i][0] = (double)data[i] * 360.0 / 4096;
+      /* 359→0を跨いだ時，前回の値を0から逆回転で負の値で表記 */
+      if ((270 <= EncDeg[i][1]) && (EncDeg[i][0] < 90))
+        EncDeg[i][1] -= 360;
+      /* 0→359を跨いだ時，前回の値を360以上の値で表記 */
+      else if ((EncDeg[i][1] < 90) && (270 <= EncDeg[i][0]))
+        EncDeg[i][1] += 360;
+      /* 差を求める*/
+      EncoderDeg[i] += EncDeg[i][0] - EncDeg[i][1];
+      break;
+    /* 通常の処理 */
+    default:
+      CL[i]=!CL[i];
+      /* 最初でも最後でもなく奇数回で最初以外の時読み取り処理 */
+      if(cnt != 1 && cnt % 2){
+        data[i] |= (DO[i]==1);
+        data[i] = data[i] << 1;
+      }
+      break;
+  }
+  cnt++;
+  cnt%=26;
+}
\ No newline at end of file