File content as of revision 1:3f932e0f693b:

//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
#define AIMTHETA_MAX 120
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);
}