File content as of revision 3:73a1daf5ed2b:

//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 int PS_CAN_ID = 0x12; // 自身のCAN ID(0~2047の間の好きな数値．）
const int CAN_Hz = 1000000;//CANに使用するクロック周波数[Hz]. CAN通信相手と共通させる
const int TARGET_CAN_ID = 0x10; //データを届けたい相手のCAN ID
const int CAN_LENGTH = 4; //送信データの配列数.最高8まで
CAN can(p30, p29);//CAN_RD, CAN_TDの順


int8_t RX, RY, LX, LY;
uint8_t R2, L2, up, dwn, lft, rit, sqr, crss, cicl, tri, R1, L1, R3, L3, shr, opt, PSb, touc;


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]を求める

//割り込みタイマー
Ticker ticker;

//機体の回転速度のPID 前3つが係数なのでこれを調整 P→D→Iの順
CalPID aimtheta_pid(0.0,0.0,0.0,DELTA_T,AIMTHETA_MAX);

const double pi = atan(1.0)*4;
//機体のフィールド座標系での移動速度の目標
double f_vx = 0;
double f_vy = 0;
//機体の角度(度)
double theta = 0;
//機体の角度（ラジアン）
double rad_theta = 0;
//機体の回転角度の目標
double target_theta=0;
//機体の機体座標系での移動速度の目標
double vx = 0;
double vy = 0;
//足回りへ送る機体の回転速度[degree/s] 時計回りが正
double aimtheta = 0;

//コントローラーからの機体の回転速度指令
double ctrl_aimtheta = 0;

//機体の最高速度（mm/s)
double maxspeed = 511;

void saveData();
void timercallback();
void displayData();
//機体の移動速度をフィールド座標系から機体座標系に変換
void field_kitai();
//コントローラー情報を受信、データ変換
void unzipControl();
//足回りへの送信処理
void asimawari_can_send();
double getGyroYaw();

int main ()
{
    BNO055.reset();
    wait(0.5);
    can.frequency(CAN_Hz);//CANのクロック周波数設定
    can.filter(PS_CAN_ID, 0xFFF, CANStandard, 0);////MY_CAN_ID以外のデータを受け取らないよう設定．後半の0xFFF, CANStandard, 0);は基本変えない

    //STARTが書かれた時点で開始する
    printf("START\r\n");
    //機体のフィールド座標系での移動速度を設定
    f_vx = 0;
    f_vy = 0;
    //機体の回転角度の目標を設定
    target_theta=0;
    //割り込みタイマーをオンにする
    ticker.attach(&timercallback,DELTA_T);
    /*
    wait(10);
    ticker.detach();
    displayData();
    */
    while(1) {
    }
}

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]
    printf("theta:%11lf, ",theta);
    unzipControl();
    field_kitai();

    aimtheta = ctrl_aimtheta;

    printf("vx:%11lf, ",vx);
    printf("vy:%11lf, ",vy);
    printf("f_vx:%11lf, ",f_vx);
    printf("f_vy:%11lf, ",f_vy);
    printf("aimtheta:%11lf, ",aimtheta);

    asimawari_can_send();
    printf("\r\n");
        
    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()
{
    rad_theta = theta*(pi/180);

    vx = f_vx*cos(rad_theta) - f_vy*sin(rad_theta);
    vy = f_vx*sin(rad_theta) + f_vy*cos(rad_theta);

    if(vx < -(maxspeed)) {
        vx = -(maxspeed);
    }
    if(vx > maxspeed) {
        vx = maxspeed;
    }
    if(vy < -(maxspeed)) {
        vy = -(maxspeed);
    }
    if(vy > maxspeed) {
        vy = maxspeed;
    }
}

void asimawari_can_send()
{
    if(PSb) {
        vx=0;
        vy=0;
        aimtheta=0;
    }

    char can_data[CAN_LENGTH] = {};//送りたいデータを入れる箱
    can_data[0] = vx/4 + 128; //データを入れる. charは8bitなので，ビットシフト等で1要素8bit以内に収める
    can_data[1] = vy/4 + 128;
    can_data[2] = aimtheta + 128;
    can_data[3] = theta/2 +128;
    CANMessage msg(TARGET_CAN_ID,can_data,CAN_LENGTH);//CANプロトコルに変換
    bool result = can.write(msg);//CAN 送信
    if(result == true) {
        printf("send data");
    } else {
        printf("fail");
    }
}

void unzipControl()
{
    CANMessage msg;// 送られてきたデータを入れる箱

    if(can.read(msg)) {
        ;//CANデータの読み取り


        RX = msg.data[0] - 128; //<-128~127>に変換
        RY = msg.data[1] - 128;
        LX = msg.data[2] - 128;
        LY = msg.data[3] - 128;
        R2 = msg.data[4]; //トリガ値 <0~255>
        L2 = msg.data[5];
        up = (msg.data[6] & 0b10000000) >> 7;    //rx_buf[6]の値はボタン8個の値を合成したものだから、ビットシフトで1桁に解凍
        dwn = (msg.data[6] & 0b01000000) >> 6;
        lft = (msg.data[6] & 0b00100000) >> 5;
        rit = (msg.data[6] & 0b00010000) >> 4;
        sqr = (msg.data[6] & 0b00001000) >> 3;
        crss = (msg.data[6] & 0b00000100) >> 2;
        cicl = (msg.data[6] & 0b00000010) >> 1;
        tri = msg.data[6] & 0b00000001;
        R1 = (msg.data[7] & 0b10000000) >> 7;
        L1 = (msg.data[7] & 0b01000000) >> 6;
        R3 = (msg.data[7] & 0b00100000) >> 5;
        L3 = (msg.data[7] & 0b00010000) >> 4;
        shr = (msg.data[7] & 0b00001000) >> 3;
        opt = (msg.data[7] & 0b00000100) >> 2;
        PSb = (msg.data[7] & 0b00000010) >> 1;
        touc = msg.data[7] & 0b00000001;

        if(-10 < LX && LX <10) {
            LX = 0;
        }

        if(-10 < LY && LY < 10) {
            LY = 0;
        }

        f_vx = LX*4;
        f_vy = LY*4;
        ctrl_aimtheta = (R2 - L2)/3;



        /*
           printf("LX:%4d, ",LX);
           printf("LY:%4d, ",LY);
           printf("RX:%4d, ",RX);
           printf("RY:%4d, ",RY);
           printf("L2:%3d, ",L2);
           printf("R2:%3d, ",R2);
           printf("L1:%d, ",L1);
           printf("R1:%d, ",R1);
           printf("L3:%d, ",L3);
           printf("R3:%d, ",R3);
           printf("↑:%d, ",up);
           printf("↓:%d, ",dwn);
           printf("←:%d, ",lft);
           printf("→:%d, ",rit);
           printf("□:%d, ",sqr);
           printf("×:%d, ",crss);
           printf("〇:%d, ",cicl);
           printf("△:%d, ",tri);
           printf("shr:%d, ",shr);
           printf("opt:%d, ",opt);
           printf("PSb:%d, ",PSb);
           printf("touc:%d, ",touc);
           printf("\r\n");
        */

        //  hypot(x,y);
    }
    printf("LX:%4d, ",LX);
    printf("LY:%4d, ",LY);
    printf("ctrl_aimtheta:%11lf, ",ctrl_aimtheta);
}

double getGyroYaw()
{
    BNO055.setmode(OPERATION_MODE_NDOF);
    BNO055.get_calib();
    BNO055.get_angles();
    //BNO055(yaw 時計回りに0~360[degree])
    double Y_ = BNO055.euler.yaw;
    if(Y_ > 180) Y_ -= 360;
    return Y_; //-180~180[degree]
}