File content as of revision 1:b74b31a7df07:

#include "mbed.h"
#include "AMT21.h"
#include "CalPID.h"
#include "MotorController.h"

#define DELTA_T 0.01    //制御周期
#define DUTY_MAX 0.8     //duty比の最大値
#define OMEGA_MAX 15    //速度制御を利用した角度制御での角速度の最大値
#define NUM_DATA 150    //記録データ数

#ifndef M_PI
#define M_PI 3.14159265359f
#endif

Serial pc(USBTX, USBRX);   //printfも通シリアル通信なのでこれを書いて、pc.printfとしないとと最悪通信と干渉します。
DigitalIn toggle(p6,PullUp);
DigitalOut led1(LED1);

Timer timer_loop;  //制御周期用


/////////////////               宣言部分                  //////////////////////
CalPID speed_pid(0.2,0,0.00455,DELTA_T,DUTY_MAX);   //速度制御のPD計算
CalPID angle_duty_pid(0.5,0,0.00056,DELTA_T,DUTY_MAX);   //角度制御（duty式）のPD計算
CalPID angle_omega_pid(25.0,0,0.00224,DELTA_T,OMEGA_MAX);//角度制御（速度式）のPD計算
Amt21 ec(p9,p10,p8); //エンコーダー
//　上で宣言したインスタンスをMotorControllerに与える　//
MotorController motor(p22,p21,DELTA_T,ec,speed_pid,angle_duty_pid,angle_omega_pid);
//////////////////////////////////////////////////////////////////////////
////データ記録まわり/////
float angle_saved[NUM_DATA]= {};
int angle_count=0;
void saveAngle()
{
    if(angle_count<NUM_DATA) {
        angle_saved[angle_count]=ec.getRad();
        angle_count++;
    }
}
float omega_saved[NUM_DATA]= {};
int omega_count=0;
void saveOmega()
{
    if(omega_count<NUM_DATA) {
        omega_saved[omega_count]=ec.getOmega();
        omega_count++;
    }
}
void displayData()
{
    for(int i=0; i<omega_count; i++) {
        pc.printf("%f\t%f\t%f\r\n",i*DELTA_T,angle_saved[i],omega_saved[i]);
        wait(0.001);
    }
    omega_count=0;
    angle_count=0;
}
////////////////////////////////////////////////////////////////////////////

float target_rad=0;  //目標角速度（速度制御）
float target_speed=0;//目標角度（角度制御）

void speedControll()
{
    motor.Sc(target_speed);       //速度制御関数

    saveAngle();//データ記録
    saveOmega();//データ記録
}
void angleControll()
{
//    motor.AcDuty(target_rad);    //duty式角度制御
    motor.AcOmega(target_rad);   //速度式角度制御

    saveAngle();//データ記録
    saveOmega();//データ記録
}

//////////////////////////////////////////////////////////////////////////////

int main ()     //調整時を想定しているのでデータを取っている。
{
    motor.setEquation(0.022639,0.022651,-0.022297,0.026744);//速度制御のC値D値
//////////////////////////////////////////////////////////////////////////////
    int state=0;

    while(1) {
        if(state==0) {
            if(toggle) {//スタート用のトグルスイッチ。要らない場合はこのif部分の処理をwhileの直前に
                state++;
                target_speed=5; //目標角速度を5rad/sに
                target_rad=(M_PI/6); //目標角度をπ/6（rad）に（単位に注意）

                timer_loop.reset();
                timer_loop.start();
            }
        } else if(state==1) {
            if(timer_loop.read()>DELTA_T) {//Tickerだと通信と割り込みが干渉してしまうのでタイマーで
//                speedControll();//速度制御
                angleControll();//角度制御

                timer_loop.reset();
            }
            if(omega_count==NUM_DATA) {//データが集まったら表示
                motor.stop();
                displayData();
                timer_loop.stop();
                state++;
            }
        }
        wait_us(10);
    }
}