File content as of revision 2:f1e26c8084f2:

//20221027試験用

#include "mbed.h"
#include "EC.h"
#define RESOLUTION 2048
#include "CalPID.h"
#include "MotorController.h"

//0.01秒ごとに角速度を取る 303k8ではこれ以上下げるとうまくいかなかった
#define DELTA_T 0.01
//dutyの絶対値の上限を決めて暴走を防ぐ
//モーター特性が極端に違うものは上限を変える
#define DUTY_MAX 0.40 //タイヤA
//角度の上限 今回は不要だけどCalPIDの引数なので消さずに放置してください
#define OMEGA_MAX 6
//角速度を格納する配列の要素数
#define NUM_DATA 500
Serial pc(USBTX,USBRX);

//速度制御のPID 前3つが係数なのでこれを調整 P→D→Iの順に調整
CalPID speed_pidA(0.01,0.0,0.000,DELTA_T,DUTY_MAX);
CalPID speed_pidB(0.01,0.0,0.000,DELTA_T,DUTY_MAX);
CalPID speed_pidC(0.01,0.0,0.000,DELTA_T,DUTY_MAX);
CalPID speed_pidD(0.01,0.0,0.000,DELTA_T,DUTY_MAX);
//角度制御のPID 今回は不要だけどMotorContorollerの引数なので消さずに放置してください
CalPID angle_omega_pid(0.0,0.0,0.0,DELTA_T,OMEGA_MAX);
//エンコーダ（A層,B層,分解能）
//モーター（正転,逆転,PWM周期,エンコーダ,CALPIDの角速度PID,角度PID)
/////////////R1(f446re)////////////////

/*
//タイヤA
Ec2multi ecA(PC_6,PC_8,RESOLUTION);
MotorController motorA(PA_8,PA_9,50,ecA,speed_pidA,angle_omega_pid);
//タイヤB
Ec2multi ecB(PB_14,PB_15,RESOLUTION); 
MotorController motorB(PB_1,PA_11,50,ecB,speed_pidB,angle_omega_pid);
//タイヤC
Ec2multi ecC(PC_12,PC_10,RESOLUTION);  
MotorController motorC(PA_0,PA_1,50,ecC,speed_pidC,angle_omega_pid);
//タイヤD
Ec2multi ecD(PC_2,PC_3,RESOLUTION);  
MotorController motorD(PB_4,PB_5,50,ecD,speed_pidC,angle_omega_pid);
*/


//タイヤA
Ec2multi ecA(PC_3,PC_2,RESOLUTION);
MotorController motorA(PA_8,PA_9,50,ecA,speed_pidA,angle_omega_pid);
//タイヤB
Ec2multi ecB(PC_12,PC_10,RESOLUTION); 
MotorController motorB(PB_1,PA_11,50,ecB,speed_pidB,angle_omega_pid);
//タイヤC
Ec2multi ecC(PC_6,PC_8,RESOLUTION);  
MotorController motorC(PA_0,PA_1,50,ecC,speed_pidC,angle_omega_pid);
//タイヤD
Ec2multi ecD(PB_14,PB_15,RESOLUTION);  
MotorController motorD(PB_4,PB_5,50,ecD,speed_pidC,angle_omega_pid);


////////////////R2(f303k8)///////////////////////
/*
//タイヤA
Ec2multi ecA(PB_4,PB_5,RESOLUTION);
MotorController motorA(PA_10,PA_9,50,ecA,speed_pidA,angle_omega_pid);
//タイヤB
Ec2multi ecB(PB_7,PB_6,RESOLUTION); 
MotorController motorB(PA_6,PA_7,50,ecB,speed_pidB,angle_omega_pid);
//タイヤC
Ec2multi ecC(PB_3,PF_1,RESOLUTION);  
MotorController motorC(PA_1,PA_3,50,ecC,speed_pidC,angle_omega_pid);
//タイヤD
Ec2multi ecD(PA_8,PF_0,RESOLUTION);  
MotorController motorD(PB_1,PB_0,50,ecD,speed_pidC,angle_omega_pid);
*/

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


//角速度を保存する変数と関数
float omega_savedA[NUM_DATA]= {};
float omega_savedB[NUM_DATA]= {};
float omega_savedC[NUM_DATA]= {};
float omega_savedD[NUM_DATA]= {};
int omega_count=0;
void saveOmega()
{
    if(omega_count<NUM_DATA) {
        omega_savedA[omega_count]=ecA.getOmega();
        omega_savedB[omega_count]=ecB.getOmega();
        omega_savedC[omega_count]=ecC.getOmega();
        omega_savedD[omega_count]=ecD.getOmega();
        omega_count++;
    }
}


float target_speed=0;
int save_count;
#define SAVE_COUNT_THRESHOLD 5 //0.05秒ごとに角速度を配列に格納
void motorOut()//モーターを目標角速度で動かそうとし、一定の間隔で角速度を配列に保存
{
    motorA.Sc(target_speed);
    motorB.Sc(target_speed);
    motorC.Sc(target_speed);
    motorD.Sc(target_speed);
    save_count++;
    if(save_count>=SAVE_COUNT_THRESHOLD) {
        saveOmega();
        save_count=0;
    }
}

void displayData()//保存したデータをprintf
{
    printf("A\r\n");
    for(int i=0; i<omega_count; i++) {
        printf("%f,",omega_savedA[i]);
        wait(0.05);//printf重いのでマイコンが落ちないように
    }
    printf("\r\nB\r\n");
    for(int i=0; i<omega_count; i++) {
        printf("%f,",omega_savedB[i]);
        wait(0.05);//printf重いのでマイコンが落ちないように
    }
    printf("\r\nC\r\n");
    for(int i=0; i<omega_count; i++) {
        printf("%f,",omega_savedC[i]);
        wait(0.05);//printf重いのでマイコンが落ちないように
    }
    printf("\r\nD\r\n");
    for(int i=0; i<omega_count; i++) {
        printf("%f,",omega_savedD[i]);
        wait(0.05);//printf重いのでマイコンが落ちないように
    }
    omega_count=0;
}

int main ()
{
    //モーター加速度の上限
    //無視するために大きくしてある
    //機体が動き始めたときに動きがずれるなら加速度の上限を下げる
    motorA.setAccelMax(10000000);
    motorB.setAccelMax(10000000);
    motorC.setAccelMax(10000000);
    motorD.setAccelMax(10000000);
    //ギア比　タイヤ:エンコーダ＝１:rとしたときのrの値
    //R1
    /*
    ecA.setGearRatio(0.6667);
    ecB.setGearRatio(0.6667);
    ecC.setGearRatio(0.6667);
    ecD.setGearRatio(0.6667);
    */
    //R2
    ecA.setGearRatio(0.50);
    ecB.setGearRatio(0.50);
    ecC.setGearRatio(0.50);
    ecD.setGearRatio(0.50);
    
    
    //角速度とduty比の関係式設定
    //横軸をタイヤの角速度[rad/s]とduty比とした時の傾きとy切片
    //正転時の傾き,y切片,逆転時の傾き,y切片
    /////////R1///////////
    /*motorA.setEquation(0.009911272,0.008627973,-0.009919857,-0.009919857); 
    motorB.setEquation(0.009657781,0.009657781,-0.010037358,-0.010037358);
    motorC.setEquation(0.010134584,0.010134584,-0.010120037,0.014440573);
    motorD.setEquation(0.010158297,0.010158297,-0.010374406,-0.010374406);  */
    //////////R2//////////
    motorA.setEquation(0.009436162,0.009436162,-0.00948947,-0.00948947); 
    motorB.setEquation(0.009421729,0.009421729,-0.009465773,-0.009465773);
    motorC.setEquation(0.008833138,0.008833138,-0.009762915,0.058112534);
    motorD.setEquation(0.010214862,0.010214862,-0.009606806,-0.009606806);

    //事前にプログラムをマイコンに読み込んでおく
    //緊急停止スイッチで切った状態で電源をつなぎ、マイコンをリセットする
    //改行をしたら緊急停止スイッチを解除する
    printf("\r\n");
    wait(4);
    //STARTが書かれた時点で開始する
    printf("START\r\n");

    target_speed=-18.0;//ここで目標角速度を設定 最初はduty2の角速度で上手くいきそうならduty4.5の角速度
    ticker.attach(&motorOut,DELTA_T);//割り込みタイマーをオンにする

    wait(3.0);//モーターが割り込みでまわり10秒分のデータを取る
    ticker.detach();//割り込みを止める（止めないとモーターを動かし続けてしまう）
    motorA.stop();//モーターを止める(dutyを0にしている)nn
    motorB.stop();
    motorC.stop();
    motorD.stop();
    displayData();//角速度のデータが表示されるのでエクセルにいれてグラフにする
    printf("\r\n");
}