File content as of revision 1:b9f698be841c:

/**
********************************************************************************
* @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;

/* 補正値用定数 */
int cor=4;

/* クラス定義 ----------------------------------------------------------------*/

/* 割り込み用クラス */
Ticker flipper;

/* gyro用タイマ */
Timer yawCnt;

/* 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();
    
    /* タイマ-スタート */
    yawCnt.start();
    /* タイマーリセット */
    yawCnt.reset();


    /* 操縦権変更 ×停止 △手動 〇自動 */
    if(DS3.CROSS)     operate=0;
    if(DS3.TRIANGLE)  operate=1;
    if(DS3.CIRCLE)    operate=2;

    /* 操縦権：なし 停止動作 */
    if(operate==0){
      /* 足回り停止 */
      omuni.XmarkOmni_Move(0,0,0);
      for (int i = 0; i < 4; i++) {
                MD.Set(i,MovMotor[i]);
            }

    }
    /* 操縦権：手動 */
    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;

      /* 目標角更新 */
      if(DS3.RX!=64) yawCnt.reset(); 
      if(yawCnt.read_ms()<1000) TarTheta=NowLoc.theta;
      
      /* gyro値による補正 */
      r_val += (TarTheta-NowLoc.theta)*cor;
       
      omuni.XmarkOmni_Move(x_val,y_val,r_val);
      for (int i = 0; i < 4; i++) {
                MD.Set(i,MovMotor[i]);
            }

    }
    /* 操縦権：自動 */
    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;
}