Yuki Nakamura / Mbed 2 deprecated syudouki_sensor_fieled1

ジャイロを用いて、フィールド座標でのコントローラー情報を機体座標に変換し、足回りに送信

Dependencies:   BNO055 CalPID Encoder MotorController mbed

syudouki_sensor.cpp@0:cf1fd72e0070, 21 months ago (annotated)

Committer:
c0735080-d3e7-40b8-b1d2-9f5b80c21345
Date:
Thu Sep 15 06:24:44 2022 +0000
Revision:
0:cf1fd72e0070
Child:
1:3f932e0f693b
a;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 1 //R2自動機用 3輪オムニ 測定輪差120[deg]
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 2 #include "mbed.h"
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 3 #include "math.h"
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 4 #include "EC.h"
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 5 #include "MotorController.h"
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 6 #include "CalPID.h"
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 7 #include "BNO055.h"
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 8
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 9 #define RESOLUTION 2048 //AMT102の分解能は2048
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 10 const float Dimameter_sokuteiWheel = 47.15; //測定輪直径[mm]
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 11 const int Teibai = 4; //エンコーダの逓倍
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 12 // const double r_sokuteiWheel = 40.0; //機体回転中心から測定輪の車軸までの長さ[mm]
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 13 const double Phi_sokuteiWheel = 30; //y軸からの取り付け角度[deg](0<Phi<90)
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 14 //角速度を計算する間隔 303k8では0.01以上下げるとうまくいかなかった
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 15 #define DELTA_T 0.01
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 16 //自己位置を格納する配列の要素数
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 17 #define NUM_DATA 500
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 18 //機体の移動速度,角度と機体の移動速度,回転速度を保存する変数と関数
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 19 float fvx_fvy_theta_saved[3][NUM_DATA]= {};
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 20 float vx_vy_aimtheta_saved[3][NUM_DATA]= {};
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 21
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 22 int data_count=0;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 23 int save_count;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 24 #define SAVE_COUNT_THRESHOLD 100 //100回ごとに自己位置を配列に格納
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 25
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 26 BNO055 BNO055(p28,p27); //ジャイロ(SDA,SCL)
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 27 //ジャイロからフィールド座標に対する機体の回転[rad]を求める
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 28 double getGyroYaw(){
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 29 //BNO055(yaw 時計回りに0~360[degree])
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 30 double Y_ = BNO055.euler.yaw;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 31 if(Y_>180) Y_-=360;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 32 return Y_; //-180~180[degree]
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 33 }
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 34 //割り込みタイマー
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 35 Ticker ticker;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 36
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 37 //機体の回転速度のPID 前3つが係数なのでこれを調整 P→D→Iの順
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 38 CalPID aimtheta_pid(0.0,0.0,0.0,DELTA_T,AIMTHETA_MAX);
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 39
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 40 //機体のフィールド座標系での移動速度の目標
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 41 double f_vx = 0;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 42 double f_vy = 0;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 43 //機体の角度
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 44 double theta = 0;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 45 //機体の回転角度の目標
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 46 double target_theta=0;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 47 //機体の機体座標系での移動速度の目標
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 48 double vx = 0;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 49 double vy = 0;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 50 //機体の回転速度[degree/s] 時計回りが正
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 51 double aimtheta = 0;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 52
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 53 void saveData();
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 54 void timercallback();
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 55 void displayData();
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 56 //機体の移動速度をフィールド座標系から機体座標系に変換
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 57 void field_kitai();
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 58
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 59 int main ()
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 60 {
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 61 //waitしている間に緊急停止スイッチ回す
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 62 printf("\r\n");
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 63 wait(3);
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 64 //STARTが書かれた時点で開始する
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 65 printf("START\r\n");
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 66 //機体のフィールド座標系での移動速度を設定
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 67 f_vx = 0;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 68 f_vy = 0;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 69 //機体の回転角度の目標を設定
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 70 target_theta=0;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 71 //割り込みタイマーをオンにする
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 72 ticker.attach(&timercallback,DELTA_T);
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 73 wait(10);
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 74 ticker.detach();
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 75 displayData();
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 76 printf("\r\n");
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 77 }
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 78
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 79 void saveData()
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 80 {
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 81 if(data_count<NUM_DATA) {
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 82 fvx_fvy_theta_saved[0][data_count]=f_vx;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 83 fvx_fvy_theta_saved[1][data_count]=f_vy;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 84 fvx_fvy_theta_saved[2][data_count]=theta;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 85 vx_vy_aimtheta_saved[0][data_count]=vx;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 86 vx_vy_aimtheta_saved[1][data_count]=vy;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 87 vx_vy_aimtheta_saved[2][data_count]=aimtheta;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 88 data_count++;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 89 }
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 90 }
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 91 void timercallback() //自己位置を取得し,一定の間隔で配列に保存
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 92 {
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 93 theta = getGyroYaw(); //初期状態の角度からのずれを計算 [rad]
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 94
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 95 field_kitai();
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 96
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 97 double rotate = target_theta-theta;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 98 double aimtheta = aimtheta_pid.calPID(rotate);
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 99 if(save_count>=SAVE_COUNT_THRESHOLD) {
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 100 saveData();
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 101 save_count=0;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 102 }
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 103 }
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 104 void displayData()
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 105 {
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 106 printf("fvx\r\n");
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 107 for(int i=0; i<data_count; i++) {
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 108 printf("%f,",fvx_fvy_theta_saved[0][i]);
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 109 wait(0.05);//printf重いのでマイコンが落ちないように
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 110 }
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 111 printf("\r\nfvy\r\n");
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 112 for(int i=0; i<data_count; i++) {
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 113 printf("%f,",fvx_fvy_theta_saved[1][i]);
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 114 wait(0.05);//printf重いのでマイコンが落ちないように
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 115 }
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 116 printf("\r\ntheta\r\n");
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 117 for(int i=0; i<data_count; i++) {
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 118 printf("%f,",fvx_fvy_theta_saved[2][i]);
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 119 wait(0.05);//printf重いのでマイコンが落ちないように
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 120 }
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 121 printf("\r\nvx\r\n");
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 122 for(int i=0; i<data_count; i++) {
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 123 printf("%f,",vx_vy_aimtheta_saved[0][i]);
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 124 wait(0.05);//printf重いのでマイコンが落ちないように
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 125 }
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 126 printf("\r\nvy\r\n");
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 127 for(int i=0; i<data_count; i++) {
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 128 printf("%f,",vx_vy_aimtheta_saved[1][i]);
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 129 wait(0.05);//printf重いのでマイコンが落ちないように
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 130 }
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 131 printf("\r\naimtheta\r\n");
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 132 for(int i=0; i<data_count; i++) {
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 133 printf("%f,",vx_vy_aimtheta_saved[2][i]);
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 134 wait(0.05);//printf重いのでマイコンが落ちないように
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 135 }
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 136 data_count=0;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 137 }
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 138 void field_kitai()
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 139 {
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 140 vx = f_vx*cos(theta) - f_vy*sin(theta);
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 141 vy = f_vx*sin(theta) + f_vy*cos(theta);
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 142 }