ryota yoshikawa / Mbed 2 deprecated somaomuni_Gyro

ジャイロ補正を追加したプログラム

Dependencies:   mbed YKNCT_Movement SBDBT BNO055 YKNCT_MD YKNCT_I2C

main.cpp@1:b9f698be841c, 2020-03-14 (annotated)

Committer:
yoshikawaryota
Date:
Sat Mar 14 05:29:14 2020 +0000
Revision:
1:b9f698be841c
Parent:
0:9e851dc42cde 
add Gyro;Ver.Yoshikawa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TakushimaYukimasa 0:9e851dc42cde 1 /**
TakushimaYukimasa 0:9e851dc42cde 2 ********************************************************************************
TakushimaYukimasa 0:9e851dc42cde 3 * @file main.c
TakushimaYukimasa 0:9e851dc42cde 4 * @author You
TakushimaYukimasa 0:9e851dc42cde 5 * @version V?.?.?
TakushimaYukimasa 0:9e851dc42cde 6 * @date Today
TakushimaYukimasa 0:9e851dc42cde 7 * @brief メインファイル
TakushimaYukimasa 0:9e851dc42cde 8 ******************************************************************************
TakushimaYukimasa 0:9e851dc42cde 9 */
TakushimaYukimasa 0:9e851dc42cde 10
TakushimaYukimasa 0:9e851dc42cde 11 /* Includes ------------------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 12 #include <main.h>
TakushimaYukimasa 0:9e851dc42cde 13
TakushimaYukimasa 0:9e851dc42cde 14 /* 型定義 --------------------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 15
TakushimaYukimasa 0:9e851dc42cde 16 /* ロボットの加速度 */
TakushimaYukimasa 0:9e851dc42cde 17 ROCATION NowAcc;
TakushimaYukimasa 0:9e851dc42cde 18 /* ロボットの座標 */
TakushimaYukimasa 0:9e851dc42cde 19 ROCATION NowLoc;
TakushimaYukimasa 0:9e851dc42cde 20
TakushimaYukimasa 0:9e851dc42cde 21 /* 定数定義 ------------------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 22 /* マクロ定義 ----------------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 23 /* 関数プロトタイプ宣言 -------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 24
TakushimaYukimasa 0:9e851dc42cde 25 /* タイマ呼び出し用 */
TakushimaYukimasa 0:9e851dc42cde 26 void IT_CallBack(void);
TakushimaYukimasa 0:9e851dc42cde 27
TakushimaYukimasa 0:9e851dc42cde 28 /* 自己位置推定処理 */
TakushimaYukimasa 0:9e851dc42cde 29 void LocEstimate(void);
TakushimaYukimasa 0:9e851dc42cde 30
TakushimaYukimasa 0:9e851dc42cde 31
TakushimaYukimasa 0:9e851dc42cde 32 /* 変数定義 ------------------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 33
TakushimaYukimasa 0:9e851dc42cde 34 /* 操作権 0…なし 1…手動 2…自動 */
TakushimaYukimasa 0:9e851dc42cde 35 int operate=0;
TakushimaYukimasa 0:9e851dc42cde 36
TakushimaYukimasa 0:9e851dc42cde 37 /* 自動シーケンス */
TakushimaYukimasa 0:9e851dc42cde 38 int auto_mode=0;
TakushimaYukimasa 0:9e851dc42cde 39
TakushimaYukimasa 0:9e851dc42cde 40 /* 直読みエンコーダ角度保存(degree) */
TakushimaYukimasa 0:9e851dc42cde 41 double EncoderDeg[EncoderMAX] = {0};
TakushimaYukimasa 0:9e851dc42cde 42
TakushimaYukimasa 0:9e851dc42cde 43 /* 足回り値保存変数 */
TakushimaYukimasa 0:9e851dc42cde 44 int MovMotor[4]={0};
TakushimaYukimasa 0:9e851dc42cde 45
TakushimaYukimasa 0:9e851dc42cde 46 /* 自動yaw補整目標角度 */
TakushimaYukimasa 0:9e851dc42cde 47 double TarTheta=0;
TakushimaYukimasa 0:9e851dc42cde 48
yoshikawaryota 1:b9f698be841c 49 /* 補正値用定数 */
yoshikawaryota 1:b9f698be841c 50 int cor=4;
yoshikawaryota 1:b9f698be841c 51
TakushimaYukimasa 0:9e851dc42cde 52 /* クラス定義 ----------------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 53
TakushimaYukimasa 0:9e851dc42cde 54 /* 割り込み用クラス */
TakushimaYukimasa 0:9e851dc42cde 55 Ticker flipper;
TakushimaYukimasa 0:9e851dc42cde 56
yoshikawaryota 1:b9f698be841c 57 /* gyro用タイマ */
yoshikawaryota 1:b9f698be841c 58 Timer yawCnt;
yoshikawaryota 1:b9f698be841c 59
TakushimaYukimasa 0:9e851dc42cde 60 /* UART (Tx,Rx) */
TakushimaYukimasa 0:9e851dc42cde 61 Serial telemetry(USBTX, USBRX, 115200);
TakushimaYukimasa 0:9e851dc42cde 62
TakushimaYukimasa 0:9e851dc42cde 63 /* コントローラー */
TakushimaYukimasa 0:9e851dc42cde 64 SBDBT DS3(PA_0, PA_1, 9600);
TakushimaYukimasa 0:9e851dc42cde 65
TakushimaYukimasa 0:9e851dc42cde 66 /* オンボードLED */
TakushimaYukimasa 0:9e851dc42cde 67 DigitalOut led(LED2);
TakushimaYukimasa 0:9e851dc42cde 68
TakushimaYukimasa 0:9e851dc42cde 69 /* USERボタン */
TakushimaYukimasa 0:9e851dc42cde 70 DigitalIn UB(PC_13,PullDown);
TakushimaYukimasa 0:9e851dc42cde 71
TakushimaYukimasa 0:9e851dc42cde 72 /* エンコーダーピン CS */
TakushimaYukimasa 0:9e851dc42cde 73 DigitalOut CS[] = {PA_2,PA_3};
TakushimaYukimasa 0:9e851dc42cde 74 DigitalOut CL[] = {PA_4,PA_5};
TakushimaYukimasa 0:9e851dc42cde 75 DigitalIn DO[] = {PA_6,PA_7};
TakushimaYukimasa 0:9e851dc42cde 76
TakushimaYukimasa 0:9e851dc42cde 77 /* 足回り動作クラス定義 */
TakushimaYukimasa 0:9e851dc42cde 78 Move omuni(MovMotor,NowLoc.theta);
TakushimaYukimasa 0:9e851dc42cde 79
TakushimaYukimasa 0:9e851dc42cde 80 /* I2C MDのクラス定義 */
TakushimaYukimasa 0:9e851dc42cde 81 YKNCT_MD_I2C MD(PB_9,PB_8);
TakushimaYukimasa 0:9e851dc42cde 82
TakushimaYukimasa 0:9e851dc42cde 83 /* ジャイロのピン設定 */
TakushimaYukimasa 0:9e851dc42cde 84 BNO055 bno(PB_9, PB_8);
TakushimaYukimasa 0:9e851dc42cde 85
TakushimaYukimasa 0:9e851dc42cde 86
TakushimaYukimasa 0:9e851dc42cde 87 /*----------------------------------- main ----------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 88 int main()
TakushimaYukimasa 0:9e851dc42cde 89 {
TakushimaYukimasa 0:9e851dc42cde 90 telemetry.printf("\n\rMainStart");
TakushimaYukimasa 0:9e851dc42cde 91
TakushimaYukimasa 0:9e851dc42cde 92 /* 割り込みの設定
TakushimaYukimasa 0:9e851dc42cde 93 * IT_CallBack関数を0.1msで割り込み */
TakushimaYukimasa 0:9e851dc42cde 94 flipper.attach_us(&IT_CallBack, 100);
TakushimaYukimasa 0:9e851dc42cde 95
TakushimaYukimasa 0:9e851dc42cde 96 /* ジャイロの設定 */
TakushimaYukimasa 0:9e851dc42cde 97 bno.setmode(OPERATION_MODE_IMUPLUS);
TakushimaYukimasa 0:9e851dc42cde 98
TakushimaYukimasa 0:9e851dc42cde 99 /* I2CMDの設定 */
TakushimaYukimasa 0:9e851dc42cde 100 MD.Init(0,MD_SMB);
TakushimaYukimasa 0:9e851dc42cde 101 MD.Init(1,MD_SMB);
TakushimaYukimasa 0:9e851dc42cde 102 MD.Init(2,MD_SMB);
TakushimaYukimasa 0:9e851dc42cde 103 MD.Init(3,MD_SMB);
TakushimaYukimasa 0:9e851dc42cde 104
TakushimaYukimasa 0:9e851dc42cde 105 telemetry.printf("\n\rMainLoopStart");
TakushimaYukimasa 0:9e851dc42cde 106 /* メインループ --------------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 107 while(1) {
TakushimaYukimasa 0:9e851dc42cde 108 /* オンボードLED点滅 */
TakushimaYukimasa 0:9e851dc42cde 109 led=!led;
TakushimaYukimasa 0:9e851dc42cde 110
TakushimaYukimasa 0:9e851dc42cde 111 /* 表示改行 */
TakushimaYukimasa 0:9e851dc42cde 112 telemetry.printf("\n\r");
TakushimaYukimasa 0:9e851dc42cde 113
TakushimaYukimasa 0:9e851dc42cde 114 /* 自動処理関連テレメトリ */
TakushimaYukimasa 0:9e851dc42cde 115 telemetry.printf("ope:%d ",operate);
TakushimaYukimasa 0:9e851dc42cde 116 /* 座標テレメトリ */
TakushimaYukimasa 0:9e851dc42cde 117 telemetry.printf("X:%4.0f Y:%4.0f T:%4.0f ",NowLoc.X,NowLoc.Y,NowLoc.theta);
TakushimaYukimasa 0:9e851dc42cde 118
TakushimaYukimasa 0:9e851dc42cde 119 /* 自己位置推定更新 */
TakushimaYukimasa 0:9e851dc42cde 120 LocEstimate();
TakushimaYukimasa 0:9e851dc42cde 121
TakushimaYukimasa 0:9e851dc42cde 122 /* I2CMD実行 */
TakushimaYukimasa 0:9e851dc42cde 123 MD.Exe();
yoshikawaryota 1:b9f698be841c 124
yoshikawaryota 1:b9f698be841c 125 /* タイマ-スタート */
yoshikawaryota 1:b9f698be841c 126 yawCnt.start();
yoshikawaryota 1:b9f698be841c 127 /* タイマーリセット */
yoshikawaryota 1:b9f698be841c 128 yawCnt.reset();
TakushimaYukimasa 0:9e851dc42cde 129
TakushimaYukimasa 0:9e851dc42cde 130
TakushimaYukimasa 0:9e851dc42cde 131 /* 操縦権変更 ×停止 △手動 〇自動 */
TakushimaYukimasa 0:9e851dc42cde 132 if(DS3.CROSS) operate=0;
TakushimaYukimasa 0:9e851dc42cde 133 if(DS3.TRIANGLE) operate=1;
TakushimaYukimasa 0:9e851dc42cde 134 if(DS3.CIRCLE) operate=2;
TakushimaYukimasa 0:9e851dc42cde 135
TakushimaYukimasa 0:9e851dc42cde 136 /* 操縦権：なし 停止動作 */
TakushimaYukimasa 0:9e851dc42cde 137 if(operate==0){
TakushimaYukimasa 0:9e851dc42cde 138 /* 足回り停止 */
TakushimaYukimasa 0:9e851dc42cde 139 omuni.XmarkOmni_Move(0,0,0);
yoshikawaryota 1:b9f698be841c 140 for (int i = 0; i < 4; i++) {
yoshikawaryota 1:b9f698be841c 141 MD.Set(i,MovMotor[i]);
yoshikawaryota 1:b9f698be841c 142 }
TakushimaYukimasa 0:9e851dc42cde 143
TakushimaYukimasa 0:9e851dc42cde 144 }
TakushimaYukimasa 0:9e851dc42cde 145 /* 操縦権：手動 */
TakushimaYukimasa 0:9e851dc42cde 146 else if(operate==1){
TakushimaYukimasa 0:9e851dc42cde 147 /* 足回り手動動作 */
TakushimaYukimasa 0:9e851dc42cde 148 int x_val = (double)(DS3.LX-64)*100/64;
TakushimaYukimasa 0:9e851dc42cde 149 int y_val = (double)(64-DS3.LY)*100/64;
TakushimaYukimasa 0:9e851dc42cde 150 int r_val = (double)(DS3.RX-64)*100/64;
yoshikawaryota 1:b9f698be841c 151
yoshikawaryota 1:b9f698be841c 152 /* 目標角更新 */
yoshikawaryota 1:b9f698be841c 153 if(DS3.RX!=64) yawCnt.reset();
yoshikawaryota 1:b9f698be841c 154 if(yawCnt.read_ms()<1000) TarTheta=NowLoc.theta;
yoshikawaryota 1:b9f698be841c 155
yoshikawaryota 1:b9f698be841c 156 /* gyro値による補正 */
yoshikawaryota 1:b9f698be841c 157 r_val += (TarTheta-NowLoc.theta)*cor;
yoshikawaryota 1:b9f698be841c 158
TakushimaYukimasa 0:9e851dc42cde 159 omuni.XmarkOmni_Move(x_val,y_val,r_val);
yoshikawaryota 1:b9f698be841c 160 for (int i = 0; i < 4; i++) {
yoshikawaryota 1:b9f698be841c 161 MD.Set(i,MovMotor[i]);
yoshikawaryota 1:b9f698be841c 162 }
TakushimaYukimasa 0:9e851dc42cde 163
TakushimaYukimasa 0:9e851dc42cde 164 }
TakushimaYukimasa 0:9e851dc42cde 165 /* 操縦権：自動 */
TakushimaYukimasa 0:9e851dc42cde 166 else if(operate==2){
TakushimaYukimasa 0:9e851dc42cde 167 switch(auto_mode){
TakushimaYukimasa 0:9e851dc42cde 168 /* スタート待機処理 */
TakushimaYukimasa 0:9e851dc42cde 169 case 0:
TakushimaYukimasa 0:9e851dc42cde 170 /* オンボードSWで次のステップに */
TakushimaYukimasa 0:9e851dc42cde 171 if(UB) auto_mode++;
TakushimaYukimasa 0:9e851dc42cde 172 break;
TakushimaYukimasa 0:9e851dc42cde 173
TakushimaYukimasa 0:9e851dc42cde 174 /* 〇〇の処理 */
TakushimaYukimasa 0:9e851dc42cde 175 case 1:
TakushimaYukimasa 0:9e851dc42cde 176 /* 〇〇の時次のステップに */
TakushimaYukimasa 0:9e851dc42cde 177 if(1) auto_mode++;
TakushimaYukimasa 0:9e851dc42cde 178 break;
TakushimaYukimasa 0:9e851dc42cde 179
TakushimaYukimasa 0:9e851dc42cde 180 /* 終了処理 */
TakushimaYukimasa 0:9e851dc42cde 181 default:
TakushimaYukimasa 0:9e851dc42cde 182 auto_mode=0;
TakushimaYukimasa 0:9e851dc42cde 183 operate=0;
TakushimaYukimasa 0:9e851dc42cde 184 break;
TakushimaYukimasa 0:9e851dc42cde 185 }
TakushimaYukimasa 0:9e851dc42cde 186 }
TakushimaYukimasa 0:9e851dc42cde 187
TakushimaYukimasa 0:9e851dc42cde 188 }
TakushimaYukimasa 0:9e851dc42cde 189 }
TakushimaYukimasa 0:9e851dc42cde 190
TakushimaYukimasa 0:9e851dc42cde 191
TakushimaYukimasa 0:9e851dc42cde 192 /*******************************************************************************
TakushimaYukimasa 0:9e851dc42cde 193 * @概要 自己位置推定関数
TakushimaYukimasa 0:9e851dc42cde 194 * @引数 なし
TakushimaYukimasa 0:9e851dc42cde 195 * @返り値 なし
TakushimaYukimasa 0:9e851dc42cde 196 *******************************************************************************/
TakushimaYukimasa 0:9e851dc42cde 197 void LocEstimate(void){
TakushimaYukimasa 0:9e851dc42cde 198 static double GyroDeg[2]={0};
TakushimaYukimasa 0:9e851dc42cde 199 static double EncDeg[2][2]={0};
TakushimaYukimasa 0:9e851dc42cde 200 static double disp[3]={0};
TakushimaYukimasa 0:9e851dc42cde 201
TakushimaYukimasa 0:9e851dc42cde 202 /* ジャイロの値取得 */
TakushimaYukimasa 0:9e851dc42cde 203 bno.get_angles();
TakushimaYukimasa 0:9e851dc42cde 204 GyroDeg[1]=GyroDeg[0];
TakushimaYukimasa 0:9e851dc42cde 205 GyroDeg[0]=bno.euler.yaw;
TakushimaYukimasa 0:9e851dc42cde 206 if(GyroDeg[0]!=0) {
TakushimaYukimasa 0:9e851dc42cde 207 /* 359→0を跨いだ時，前回の値を0から逆回転で負の値で表記 */
TakushimaYukimasa 0:9e851dc42cde 208 if(GyroDeg[1]<90 && GyroDeg[0]>270) GyroDeg[1]+=360;
TakushimaYukimasa 0:9e851dc42cde 209 /* 0→359を跨いだ時，前回の値を360以上の値で表記 */
TakushimaYukimasa 0:9e851dc42cde 210 else if(GyroDeg[1]>270 && GyroDeg[0]<90) GyroDeg[1]-=360;
TakushimaYukimasa 0:9e851dc42cde 211 /* 差を求める*/
TakushimaYukimasa 0:9e851dc42cde 212 disp[2]=GyroDeg[1]-GyroDeg[0];
TakushimaYukimasa 0:9e851dc42cde 213 }
TakushimaYukimasa 0:9e851dc42cde 214 /* Enc2つの差分求める */
TakushimaYukimasa 0:9e851dc42cde 215 for(int i=0;i<2;i++){
TakushimaYukimasa 0:9e851dc42cde 216 EncDeg[i][1]=EncDeg[i][0];
TakushimaYukimasa 0:9e851dc42cde 217 EncDeg[i][0]=EncoderDeg[i];
TakushimaYukimasa 0:9e851dc42cde 218 disp[i]=EncDeg[i][1]-EncDeg[i][0];
TakushimaYukimasa 0:9e851dc42cde 219 }
TakushimaYukimasa 0:9e851dc42cde 220 /* 差分を加速度として保存 */
TakushimaYukimasa 0:9e851dc42cde 221 NowAcc.theta = disp[2];
TakushimaYukimasa 0:9e851dc42cde 222 NowAcc.X = disp[0] * cos(NowLoc.theta) + disp[1] * sin(NowLoc.theta)+200*disp[2];
TakushimaYukimasa 0:9e851dc42cde 223 NowAcc.Y = -disp[0] * sin(NowLoc.theta) + disp[1] * cos(NowLoc.theta)+200*disp[2];
TakushimaYukimasa 0:9e851dc42cde 224 /* 差分を累積して現在位置を保存 */
TakushimaYukimasa 0:9e851dc42cde 225 NowLoc.X += NowAcc.X;
TakushimaYukimasa 0:9e851dc42cde 226 NowLoc.Y += NowAcc.Y;
TakushimaYukimasa 0:9e851dc42cde 227 NowLoc.theta += NowAcc.theta;
TakushimaYukimasa 0:9e851dc42cde 228 }
TakushimaYukimasa 0:9e851dc42cde 229
TakushimaYukimasa 0:9e851dc42cde 230
TakushimaYukimasa 0:9e851dc42cde 231
TakushimaYukimasa 0:9e851dc42cde 232
TakushimaYukimasa 0:9e851dc42cde 233 /* 割り込み(100us) *************************************************************/
TakushimaYukimasa 0:9e851dc42cde 234 void IT_CallBack(void)
TakushimaYukimasa 0:9e851dc42cde 235 {
TakushimaYukimasa 0:9e851dc42cde 236 static int cnt = 0;
TakushimaYukimasa 0:9e851dc42cde 237 static int data[EncoderMAX] = {0};
TakushimaYukimasa 0:9e851dc42cde 238 static double EncDeg[EncoderMAX][2] = {0};
TakushimaYukimasa 0:9e851dc42cde 239
TakushimaYukimasa 0:9e851dc42cde 240 for(int i=0;i<EncoderMAX;i++)
TakushimaYukimasa 0:9e851dc42cde 241 switch(cnt){
TakushimaYukimasa 0:9e851dc42cde 242 /* 最初の処理 */
TakushimaYukimasa 0:9e851dc42cde 243 case 0:
TakushimaYukimasa 0:9e851dc42cde 244 data[i] = 0; CS[i] = 0; CL[i] = 1;
TakushimaYukimasa 0:9e851dc42cde 245 break;
TakushimaYukimasa 0:9e851dc42cde 246 /* 最後の処理 */
TakushimaYukimasa 0:9e851dc42cde 247 case 25:
TakushimaYukimasa 0:9e851dc42cde 248 CS[i]=1;
TakushimaYukimasa 0:9e851dc42cde 249 /* 前回の値更新 今回の値更新(エンコーダの値(0~4096)を角度(0~360)に) */
TakushimaYukimasa 0:9e851dc42cde 250 EncDeg[i][1] = EncDeg[i][0];
TakushimaYukimasa 0:9e851dc42cde 251 EncDeg[i][0] = (double)data[i] * 360.0 / 4096;
TakushimaYukimasa 0:9e851dc42cde 252 /* 359→0を跨いだ時，前回の値を0から逆回転で負の値で表記 */
TakushimaYukimasa 0:9e851dc42cde 253 if ((270 <= EncDeg[i][1]) && (EncDeg[i][0] < 90))
TakushimaYukimasa 0:9e851dc42cde 254 EncDeg[i][1] -= 360;
TakushimaYukimasa 0:9e851dc42cde 255 /* 0→359を跨いだ時，前回の値を360以上の値で表記 */
TakushimaYukimasa 0:9e851dc42cde 256 else if ((EncDeg[i][1] < 90) && (270 <= EncDeg[i][0]))
TakushimaYukimasa 0:9e851dc42cde 257 EncDeg[i][1] += 360;
TakushimaYukimasa 0:9e851dc42cde 258 /* 差を求める*/
TakushimaYukimasa 0:9e851dc42cde 259 EncoderDeg[i] += EncDeg[i][0] - EncDeg[i][1];
TakushimaYukimasa 0:9e851dc42cde 260 break;
TakushimaYukimasa 0:9e851dc42cde 261 /* 通常の処理 */
TakushimaYukimasa 0:9e851dc42cde 262 default:
TakushimaYukimasa 0:9e851dc42cde 263 CL[i]=!CL[i];
TakushimaYukimasa 0:9e851dc42cde 264 /* 最初でも最後でもなく奇数回で最初以外の時読み取り処理 */
TakushimaYukimasa 0:9e851dc42cde 265 if(cnt != 1 && cnt % 2){
TakushimaYukimasa 0:9e851dc42cde 266 data[i] |= (DO[i]==1);
TakushimaYukimasa 0:9e851dc42cde 267 data[i] = data[i] << 1;
TakushimaYukimasa 0:9e851dc42cde 268 }
TakushimaYukimasa 0:9e851dc42cde 269 break;
TakushimaYukimasa 0:9e851dc42cde 270 }
TakushimaYukimasa 0:9e851dc42cde 271 cnt++;
TakushimaYukimasa 0:9e851dc42cde 272 cnt%=26;
TakushimaYukimasa 0:9e851dc42cde 273 }