Diff: syudouki_sensor.cpp

Revision:

0:cf1fd72e0070

Child:

1:3f932e0f693b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/syudouki_sensor.cpp	Thu Sep 15 06:24:44 2022 +0000
@@ -0,0 +1,142 @@
+//R2自動機用 3輪オムニ 測定輪差120[deg]
+#include "mbed.h"
+#include "math.h"
+#include "EC.h"
+#include "MotorController.h"
+#include "CalPID.h"
+#include "BNO055.h"
+
+#define RESOLUTION 2048 //AMT102の分解能は2048
+const float Dimameter_sokuteiWheel = 47.15; //測定輪直径[mm]
+const int Teibai = 4; //エンコーダの逓倍
+// const double r_sokuteiWheel = 40.0; //機体回転中心から測定輪の車軸までの長さ[mm]
+const double Phi_sokuteiWheel = 30;   //y軸からの取り付け角度[deg](0<Phi<90)
+//角速度を計算する間隔 303k8では0.01以上下げるとうまくいかなかった
+#define DELTA_T 0.01
+//自己位置を格納する配列の要素数
+#define NUM_DATA 500
+//機体の移動速度,角度と機体の移動速度,回転速度を保存する変数と関数
+float fvx_fvy_theta_saved[3][NUM_DATA]= {};
+float vx_vy_aimtheta_saved[3][NUM_DATA]= {};
+
+int data_count=0;
+int save_count;
+#define SAVE_COUNT_THRESHOLD 100 //100回ごとに自己位置を配列に格納
+
+BNO055 BNO055(p28,p27); //ジャイロ(SDA,SCL)
+//ジャイロからフィールド座標に対する機体の回転[rad]を求める
+double getGyroYaw(){
+    //BNO055(yaw 時計回りに0~360[degree])
+    double Y_ = BNO055.euler.yaw;
+    if(Y_>180) Y_-=360;
+    return Y_; //-180~180[degree]
+}
+//割り込みタイマー
+Ticker ticker;
+
+//機体の回転速度のPID 前3つが係数なのでこれを調整 P→D→Iの順
+CalPID aimtheta_pid(0.0,0.0,0.0,DELTA_T,AIMTHETA_MAX);
+
+//機体のフィールド座標系での移動速度の目標
+double f_vx = 0;
+double f_vy = 0;
+//機体の角度
+double theta = 0;
+//機体の回転角度の目標
+double target_theta=0;
+//機体の機体座標系での移動速度の目標
+double vx = 0;
+double vy = 0;
+//機体の回転速度[degree/s] 時計回りが正
+double aimtheta = 0;
+
+void saveData();
+void timercallback();
+void displayData();
+//機体の移動速度をフィールド座標系から機体座標系に変換
+void field_kitai();
+
+int main ()
+{
+    //waitしている間に緊急停止スイッチ回す
+    printf("\r\n");
+    wait(3);
+    //STARTが書かれた時点で開始する
+    printf("START\r\n");
+    //機体のフィールド座標系での移動速度を設定
+    f_vx = 0;
+    f_vy = 0;
+    //機体の回転角度の目標を設定
+    target_theta=0;
+    //割り込みタイマーをオンにする
+    ticker.attach(&timercallback,DELTA_T);
+    wait(10);
+    ticker.detach();
+    displayData();
+    printf("\r\n");
+}
+
+void saveData()
+{
+    if(data_count<NUM_DATA) {
+        fvx_fvy_theta_saved[0][data_count]=f_vx;
+        fvx_fvy_theta_saved[1][data_count]=f_vy;
+        fvx_fvy_theta_saved[2][data_count]=theta;
+        vx_vy_aimtheta_saved[0][data_count]=vx;
+        vx_vy_aimtheta_saved[1][data_count]=vy;
+        vx_vy_aimtheta_saved[2][data_count]=aimtheta;
+        data_count++;
+    }
+}
+void timercallback() //自己位置を取得し,一定の間隔で配列に保存
+{
+    theta = getGyroYaw(); //初期状態の角度からのずれを計算 [rad]
+
+    field_kitai();
+
+    double rotate = target_theta-theta;
+    double aimtheta = aimtheta_pid.calPID(rotate);
+    if(save_count>=SAVE_COUNT_THRESHOLD) {
+        saveData();
+        save_count=0;
+    }
+}
+void displayData()
+{
+    printf("fvx\r\n");
+    for(int i=0; i<data_count; i++) {
+        printf("%f,",fvx_fvy_theta_saved[0][i]);
+        wait(0.05);//printf重いのでマイコンが落ちないように
+    }
+    printf("\r\nfvy\r\n");
+    for(int i=0; i<data_count; i++) {
+        printf("%f,",fvx_fvy_theta_saved[1][i]);
+        wait(0.05);//printf重いのでマイコンが落ちないように
+    }
+    printf("\r\ntheta\r\n");
+    for(int i=0; i<data_count; i++) {
+        printf("%f,",fvx_fvy_theta_saved[2][i]);
+        wait(0.05);//printf重いのでマイコンが落ちないように
+    }
+    printf("\r\nvx\r\n");
+    for(int i=0; i<data_count; i++) {
+        printf("%f,",vx_vy_aimtheta_saved[0][i]);
+        wait(0.05);//printf重いのでマイコンが落ちないように
+    }
+    printf("\r\nvy\r\n");
+    for(int i=0; i<data_count; i++) {
+        printf("%f,",vx_vy_aimtheta_saved[1][i]);
+        wait(0.05);//printf重いのでマイコンが落ちないように
+    }
+    printf("\r\naimtheta\r\n");
+    for(int i=0; i<data_count; i++) {
+        printf("%f,",vx_vy_aimtheta_saved[2][i]);
+        wait(0.05);//printf重いのでマイコンが落ちないように
+    }
+    data_count=0;
+}
+void field_kitai() 
+{
+    vx = f_vx*cos(theta) - f_vy*sin(theta);
+    vy = f_vx*sin(theta) + f_vy*cos(theta);
+}