Yuki Nakamura / Mbed 2 deprecated syudouki_sensor_fieled1

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

Dependencies:   BNO055 CalPID Encoder MotorController mbed

syudouki_sensor.cpp@3:73a1daf5ed2b, 20 months ago (annotated)

Committer:
yn_4009
Date:
Sat Sep 17 11:23:52 2022 +0000
Revision:
3:73a1daf5ed2b
Parent:
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 1:3f932e0f693b 10 #define AIMTHETA_MAX 120
yn_4009 3:73a1daf5ed2b 11
yn_4009 3:73a1daf5ed2b 12
yn_4009 3:73a1daf5ed2b 13 const int PS_CAN_ID = 0x12; // 自身のCAN ID(0~2047の間の好きな数値．）
yn_4009 3:73a1daf5ed2b 14 const int CAN_Hz = 1000000;//CANに使用するクロック周波数[Hz]. CAN通信相手と共通させる
yn_4009 3:73a1daf5ed2b 15 const int TARGET_CAN_ID = 0x10; //データを届けたい相手のCAN ID
yn_4009 3:73a1daf5ed2b 16 const int CAN_LENGTH = 4; //送信データの配列数.最高8まで
yn_4009 3:73a1daf5ed2b 17 CAN can(p30, p29);//CAN_RD, CAN_TDの順
yn_4009 3:73a1daf5ed2b 18
yn_4009 3:73a1daf5ed2b 19
yn_4009 3:73a1daf5ed2b 20 int8_t RX, RY, LX, LY;
yn_4009 3:73a1daf5ed2b 21 uint8_t R2, L2, up, dwn, lft, rit, sqr, crss, cicl, tri, R1, L1, R3, L3, shr, opt, PSb, touc;
yn_4009 3:73a1daf5ed2b 22
yn_4009 3:73a1daf5ed2b 23
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 24 const float Dimameter_sokuteiWheel = 47.15; //測定輪直径[mm]
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 25 const int Teibai = 4; //エンコーダの逓倍
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 26 // const double r_sokuteiWheel = 40.0; //機体回転中心から測定輪の車軸までの長さ[mm]
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 27 const double Phi_sokuteiWheel = 30; //y軸からの取り付け角度[deg](0<Phi<90)
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 28 //角速度を計算する間隔 303k8では0.01以上下げるとうまくいかなかった
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 29 #define DELTA_T 0.01
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 30 //自己位置を格納する配列の要素数
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 31 #define NUM_DATA 500
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 32 //機体の移動速度,角度と機体の移動速度,回転速度を保存する変数と関数
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 33 float fvx_fvy_theta_saved[3][NUM_DATA]= {};
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 34 float vx_vy_aimtheta_saved[3][NUM_DATA]= {};
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 35
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 36 int data_count=0;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 37 int save_count;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 38 #define SAVE_COUNT_THRESHOLD 100 //100回ごとに自己位置を配列に格納
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 39
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 40 BNO055 BNO055(p28,p27); //ジャイロ(SDA,SCL)
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 41 //ジャイロからフィールド座標に対する機体の回転[rad]を求める
yn_4009 3:73a1daf5ed2b 42
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 43 //割り込みタイマー
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 44 Ticker ticker;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 45
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 46 //機体の回転速度のPID 前3つが係数なのでこれを調整 P→D→Iの順
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 47 CalPID aimtheta_pid(0.0,0.0,0.0,DELTA_T,AIMTHETA_MAX);
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 48
yn_4009 3:73a1daf5ed2b 49 const double pi = atan(1.0)*4;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 50 //機体のフィールド座標系での移動速度の目標
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 51 double f_vx = 0;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 52 double f_vy = 0;
yn_4009 3:73a1daf5ed2b 53 //機体の角度(度)
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 54 double theta = 0;
yn_4009 3:73a1daf5ed2b 55 //機体の角度（ラジアン）
yn_4009 3:73a1daf5ed2b 56 double rad_theta = 0;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 57 //機体の回転角度の目標
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 58 double target_theta=0;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 59 //機体の機体座標系での移動速度の目標
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 60 double vx = 0;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 61 double vy = 0;
yn_4009 3:73a1daf5ed2b 62 //足回りへ送る機体の回転速度[degree/s] 時計回りが正
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 63 double aimtheta = 0;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 64
yn_4009 3:73a1daf5ed2b 65 //コントローラーからの機体の回転速度指令
yn_4009 3:73a1daf5ed2b 66 double ctrl_aimtheta = 0;
yn_4009 3:73a1daf5ed2b 67
yn_4009 3:73a1daf5ed2b 68 //機体の最高速度（mm/s)
yn_4009 3:73a1daf5ed2b 69 double maxspeed = 511;
yn_4009 3:73a1daf5ed2b 70
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 71 void saveData();
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 72 void timercallback();
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 73 void displayData();
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 74 //機体の移動速度をフィールド座標系から機体座標系に変換
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 75 void field_kitai();
yn_4009 3:73a1daf5ed2b 76 //コントローラー情報を受信、データ変換
yn_4009 3:73a1daf5ed2b 77 void unzipControl();
yn_4009 3:73a1daf5ed2b 78 //足回りへの送信処理
yn_4009 3:73a1daf5ed2b 79 void asimawari_can_send();
yn_4009 3:73a1daf5ed2b 80 double getGyroYaw();
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 81
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 82 int main ()
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 83 {
yn_4009 3:73a1daf5ed2b 84 BNO055.reset();
yn_4009 3:73a1daf5ed2b 85 wait(0.5);
yn_4009 3:73a1daf5ed2b 86 can.frequency(CAN_Hz);//CANのクロック周波数設定
yn_4009 3:73a1daf5ed2b 87 can.filter(PS_CAN_ID, 0xFFF, CANStandard, 0);////MY_CAN_ID以外のデータを受け取らないよう設定．後半の0xFFF, CANStandard, 0);は基本変えない
yn_4009 3:73a1daf5ed2b 88
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 89 //STARTが書かれた時点で開始する
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 90 printf("START\r\n");
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 91 //機体のフィールド座標系での移動速度を設定
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 92 f_vx = 0;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 93 f_vy = 0;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 94 //機体の回転角度の目標を設定
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 95 target_theta=0;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 96 //割り込みタイマーをオンにする
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 97 ticker.attach(&timercallback,DELTA_T);
yn_4009 3:73a1daf5ed2b 98 /*
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 99 wait(10);
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 100 ticker.detach();
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 101 displayData();
yn_4009 3:73a1daf5ed2b 102 */
yn_4009 3:73a1daf5ed2b 103 while(1) {
yn_4009 3:73a1daf5ed2b 104 }
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 105 }
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 106
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 107 void saveData()
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 108 {
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 109 if(data_count<NUM_DATA) {
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 110 fvx_fvy_theta_saved[0][data_count]=f_vx;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 111 fvx_fvy_theta_saved[1][data_count]=f_vy;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 112 fvx_fvy_theta_saved[2][data_count]=theta;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 113 vx_vy_aimtheta_saved[0][data_count]=vx;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 114 vx_vy_aimtheta_saved[1][data_count]=vy;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 115 vx_vy_aimtheta_saved[2][data_count]=aimtheta;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 116 data_count++;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 117 }
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 118 }
yn_4009 3:73a1daf5ed2b 119
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 120 void timercallback() //自己位置を取得し,一定の間隔で配列に保存
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 121 {
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 122 theta = getGyroYaw(); //初期状態の角度からのずれを計算 [rad]
yn_4009 3:73a1daf5ed2b 123 printf("theta:%11lf, ",theta);
yn_4009 3:73a1daf5ed2b 124 unzipControl();
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 125 field_kitai();
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 126
yn_4009 3:73a1daf5ed2b 127 aimtheta = ctrl_aimtheta;
yn_4009 3:73a1daf5ed2b 128
yn_4009 3:73a1daf5ed2b 129 printf("vx:%11lf, ",vx);
yn_4009 3:73a1daf5ed2b 130 printf("vy:%11lf, ",vy);
yn_4009 3:73a1daf5ed2b 131 printf("f_vx:%11lf, ",f_vx);
yn_4009 3:73a1daf5ed2b 132 printf("f_vy:%11lf, ",f_vy);
yn_4009 3:73a1daf5ed2b 133 printf("aimtheta:%11lf, ",aimtheta);
yn_4009 3:73a1daf5ed2b 134
yn_4009 3:73a1daf5ed2b 135 asimawari_can_send();
yn_4009 3:73a1daf5ed2b 136 printf("\r\n");
yn_4009 3:73a1daf5ed2b 137
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 138 double rotate = target_theta-theta;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 139 double aimtheta = aimtheta_pid.calPID(rotate);
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 140 if(save_count>=SAVE_COUNT_THRESHOLD) {
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 141 saveData();
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 142 save_count=0;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 143 }
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 144 }
yn_4009 3:73a1daf5ed2b 145
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 146 void displayData()
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 147 {
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 148 printf("fvx\r\n");
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 149 for(int i=0; i<data_count; i++) {
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 150 printf("%f,",fvx_fvy_theta_saved[0][i]);
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 151 wait(0.05);//printf重いのでマイコンが落ちないように
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 152 }
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 153 printf("\r\nfvy\r\n");
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 154 for(int i=0; i<data_count; i++) {
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 155 printf("%f,",fvx_fvy_theta_saved[1][i]);
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 156 wait(0.05);//printf重いのでマイコンが落ちないように
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 157 }
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 158 printf("\r\ntheta\r\n");
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 159 for(int i=0; i<data_count; i++) {
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 160 printf("%f,",fvx_fvy_theta_saved[2][i]);
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 161 wait(0.05);//printf重いのでマイコンが落ちないように
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 162 }
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 163 printf("\r\nvx\r\n");
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 164 for(int i=0; i<data_count; i++) {
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 165 printf("%f,",vx_vy_aimtheta_saved[0][i]);
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 166 wait(0.05);//printf重いのでマイコンが落ちないように
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 167 }
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 168 printf("\r\nvy\r\n");
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 169 for(int i=0; i<data_count; i++) {
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 170 printf("%f,",vx_vy_aimtheta_saved[1][i]);
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 171 wait(0.05);//printf重いのでマイコンが落ちないように
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 172 }
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 173 printf("\r\naimtheta\r\n");
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 174 for(int i=0; i<data_count; i++) {
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 175 printf("%f,",vx_vy_aimtheta_saved[2][i]);
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 176 wait(0.05);//printf重いのでマイコンが落ちないように
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 177 }
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 178 data_count=0;
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 179 }
yn_4009 3:73a1daf5ed2b 180
yn_4009 3:73a1daf5ed2b 181 void field_kitai()
yn_4009 3:73a1daf5ed2b 182 {
yn_4009 3:73a1daf5ed2b 183 rad_theta = theta*(pi/180);
yn_4009 3:73a1daf5ed2b 184
yn_4009 3:73a1daf5ed2b 185 vx = f_vx*cos(rad_theta) - f_vy*sin(rad_theta);
yn_4009 3:73a1daf5ed2b 186 vy = f_vx*sin(rad_theta) + f_vy*cos(rad_theta);
yn_4009 3:73a1daf5ed2b 187
yn_4009 3:73a1daf5ed2b 188 if(vx < -(maxspeed)) {
yn_4009 3:73a1daf5ed2b 189 vx = -(maxspeed);
yn_4009 3:73a1daf5ed2b 190 }
yn_4009 3:73a1daf5ed2b 191 if(vx > maxspeed) {
yn_4009 3:73a1daf5ed2b 192 vx = maxspeed;
yn_4009 3:73a1daf5ed2b 193 }
yn_4009 3:73a1daf5ed2b 194 if(vy < -(maxspeed)) {
yn_4009 3:73a1daf5ed2b 195 vy = -(maxspeed);
yn_4009 3:73a1daf5ed2b 196 }
yn_4009 3:73a1daf5ed2b 197 if(vy > maxspeed) {
yn_4009 3:73a1daf5ed2b 198 vy = maxspeed;
yn_4009 3:73a1daf5ed2b 199 }
yn_4009 3:73a1daf5ed2b 200 }
yn_4009 3:73a1daf5ed2b 201
yn_4009 3:73a1daf5ed2b 202 void asimawari_can_send()
yn_4009 3:73a1daf5ed2b 203 {
yn_4009 3:73a1daf5ed2b 204 if(PSb) {
yn_4009 3:73a1daf5ed2b 205 vx=0;
yn_4009 3:73a1daf5ed2b 206 vy=0;
yn_4009 3:73a1daf5ed2b 207 aimtheta=0;
yn_4009 3:73a1daf5ed2b 208 }
yn_4009 3:73a1daf5ed2b 209
yn_4009 3:73a1daf5ed2b 210 char can_data[CAN_LENGTH] = {};//送りたいデータを入れる箱
yn_4009 3:73a1daf5ed2b 211 can_data[0] = vx/4 + 128; //データを入れる. charは8bitなので，ビットシフト等で1要素8bit以内に収める
yn_4009 3:73a1daf5ed2b 212 can_data[1] = vy/4 + 128;
yn_4009 3:73a1daf5ed2b 213 can_data[2] = aimtheta + 128;
yn_4009 3:73a1daf5ed2b 214 can_data[3] = theta/2 +128;
yn_4009 3:73a1daf5ed2b 215 CANMessage msg(TARGET_CAN_ID,can_data,CAN_LENGTH);//CANプロトコルに変換
yn_4009 3:73a1daf5ed2b 216 bool result = can.write(msg);//CAN 送信
yn_4009 3:73a1daf5ed2b 217 if(result == true) {
yn_4009 3:73a1daf5ed2b 218 printf("send data");
yn_4009 3:73a1daf5ed2b 219 } else {
yn_4009 3:73a1daf5ed2b 220 printf("fail");
yn_4009 3:73a1daf5ed2b 221 }
yn_4009 3:73a1daf5ed2b 222 }
yn_4009 3:73a1daf5ed2b 223
yn_4009 3:73a1daf5ed2b 224 void unzipControl()
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 225 {
yn_4009 3:73a1daf5ed2b 226 CANMessage msg;// 送られてきたデータを入れる箱
yn_4009 3:73a1daf5ed2b 227
yn_4009 3:73a1daf5ed2b 228 if(can.read(msg)) {
yn_4009 3:73a1daf5ed2b 229 ;//CANデータの読み取り
yn_4009 3:73a1daf5ed2b 230
yn_4009 3:73a1daf5ed2b 231
yn_4009 3:73a1daf5ed2b 232 RX = msg.data[0] - 128; //<-128~127>に変換
yn_4009 3:73a1daf5ed2b 233 RY = msg.data[1] - 128;
yn_4009 3:73a1daf5ed2b 234 LX = msg.data[2] - 128;
yn_4009 3:73a1daf5ed2b 235 LY = msg.data[3] - 128;
yn_4009 3:73a1daf5ed2b 236 R2 = msg.data[4]; //トリガ値 <0~255>
yn_4009 3:73a1daf5ed2b 237 L2 = msg.data[5];
yn_4009 3:73a1daf5ed2b 238 up = (msg.data[6] & 0b10000000) >> 7; //rx_buf[6]の値はボタン8個の値を合成したものだから、ビットシフトで1桁に解凍
yn_4009 3:73a1daf5ed2b 239 dwn = (msg.data[6] & 0b01000000) >> 6;
yn_4009 3:73a1daf5ed2b 240 lft = (msg.data[6] & 0b00100000) >> 5;
yn_4009 3:73a1daf5ed2b 241 rit = (msg.data[6] & 0b00010000) >> 4;
yn_4009 3:73a1daf5ed2b 242 sqr = (msg.data[6] & 0b00001000) >> 3;
yn_4009 3:73a1daf5ed2b 243 crss = (msg.data[6] & 0b00000100) >> 2;
yn_4009 3:73a1daf5ed2b 244 cicl = (msg.data[6] & 0b00000010) >> 1;
yn_4009 3:73a1daf5ed2b 245 tri = msg.data[6] & 0b00000001;
yn_4009 3:73a1daf5ed2b 246 R1 = (msg.data[7] & 0b10000000) >> 7;
yn_4009 3:73a1daf5ed2b 247 L1 = (msg.data[7] & 0b01000000) >> 6;
yn_4009 3:73a1daf5ed2b 248 R3 = (msg.data[7] & 0b00100000) >> 5;
yn_4009 3:73a1daf5ed2b 249 L3 = (msg.data[7] & 0b00010000) >> 4;
yn_4009 3:73a1daf5ed2b 250 shr = (msg.data[7] & 0b00001000) >> 3;
yn_4009 3:73a1daf5ed2b 251 opt = (msg.data[7] & 0b00000100) >> 2;
yn_4009 3:73a1daf5ed2b 252 PSb = (msg.data[7] & 0b00000010) >> 1;
yn_4009 3:73a1daf5ed2b 253 touc = msg.data[7] & 0b00000001;
yn_4009 3:73a1daf5ed2b 254
yn_4009 3:73a1daf5ed2b 255 if(-10 < LX && LX <10) {
yn_4009 3:73a1daf5ed2b 256 LX = 0;
yn_4009 3:73a1daf5ed2b 257 }
yn_4009 3:73a1daf5ed2b 258
yn_4009 3:73a1daf5ed2b 259 if(-10 < LY && LY < 10) {
yn_4009 3:73a1daf5ed2b 260 LY = 0;
yn_4009 3:73a1daf5ed2b 261 }
yn_4009 3:73a1daf5ed2b 262
yn_4009 3:73a1daf5ed2b 263 f_vx = LX*4;
yn_4009 3:73a1daf5ed2b 264 f_vy = LY*4;
yn_4009 3:73a1daf5ed2b 265 ctrl_aimtheta = (R2 - L2)/3;
yn_4009 3:73a1daf5ed2b 266
yn_4009 3:73a1daf5ed2b 267
yn_4009 3:73a1daf5ed2b 268
yn_4009 3:73a1daf5ed2b 269 /*
yn_4009 3:73a1daf5ed2b 270 printf("LX:%4d, ",LX);
yn_4009 3:73a1daf5ed2b 271 printf("LY:%4d, ",LY);
yn_4009 3:73a1daf5ed2b 272 printf("RX:%4d, ",RX);
yn_4009 3:73a1daf5ed2b 273 printf("RY:%4d, ",RY);
yn_4009 3:73a1daf5ed2b 274 printf("L2:%3d, ",L2);
yn_4009 3:73a1daf5ed2b 275 printf("R2:%3d, ",R2);
yn_4009 3:73a1daf5ed2b 276 printf("L1:%d, ",L1);
yn_4009 3:73a1daf5ed2b 277 printf("R1:%d, ",R1);
yn_4009 3:73a1daf5ed2b 278 printf("L3:%d, ",L3);
yn_4009 3:73a1daf5ed2b 279 printf("R3:%d, ",R3);
yn_4009 3:73a1daf5ed2b 280 printf("↑:%d, ",up);
yn_4009 3:73a1daf5ed2b 281 printf("↓:%d, ",dwn);
yn_4009 3:73a1daf5ed2b 282 printf("←:%d, ",lft);
yn_4009 3:73a1daf5ed2b 283 printf("→:%d, ",rit);
yn_4009 3:73a1daf5ed2b 284 printf("□:%d, ",sqr);
yn_4009 3:73a1daf5ed2b 285 printf("×:%d, ",crss);
yn_4009 3:73a1daf5ed2b 286 printf("〇:%d, ",cicl);
yn_4009 3:73a1daf5ed2b 287 printf("△:%d, ",tri);
yn_4009 3:73a1daf5ed2b 288 printf("shr:%d, ",shr);
yn_4009 3:73a1daf5ed2b 289 printf("opt:%d, ",opt);
yn_4009 3:73a1daf5ed2b 290 printf("PSb:%d, ",PSb);
yn_4009 3:73a1daf5ed2b 291 printf("touc:%d, ",touc);
yn_4009 3:73a1daf5ed2b 292 printf("\r\n");
yn_4009 3:73a1daf5ed2b 293 */
yn_4009 3:73a1daf5ed2b 294
yn_4009 3:73a1daf5ed2b 295 // hypot(x,y);
yn_4009 3:73a1daf5ed2b 296 }
yn_4009 3:73a1daf5ed2b 297 printf("LX:%4d, ",LX);
yn_4009 3:73a1daf5ed2b 298 printf("LY:%4d, ",LY);
yn_4009 3:73a1daf5ed2b 299 printf("ctrl_aimtheta:%11lf, ",ctrl_aimtheta);
c0735080-d3e7-40b8-b1d2-9f5b80c21345 0:cf1fd72e0070 300 }
yn_4009 3:73a1daf5ed2b 301
yn_4009 3:73a1daf5ed2b 302 double getGyroYaw()
yn_4009 3:73a1daf5ed2b 303 {
yn_4009 3:73a1daf5ed2b 304 BNO055.setmode(OPERATION_MODE_NDOF);
yn_4009 3:73a1daf5ed2b 305 BNO055.get_calib();
yn_4009 3:73a1daf5ed2b 306 BNO055.get_angles();
yn_4009 3:73a1daf5ed2b 307 //BNO055(yaw 時計回りに0~360[degree])
yn_4009 3:73a1daf5ed2b 308 double Y_ = BNO055.euler.yaw;
yn_4009 3:73a1daf5ed2b 309 if(Y_ > 180) Y_ -= 360;
yn_4009 3:73a1daf5ed2b 310 return Y_; //-180~180[degree]
yn_4009 3:73a1daf5ed2b 311 }