File content as of revision 1:0e8ec231cb2f:

//CAN通信で受け取った(double)rotate‗1～rotate‗3に対し、実際に回転数制御を行う
#include "mbed.h"
#include "EC.h" //Encoderライブラリをインクルード
#include "SpeedController.h"    //SpeedControlライブラリをインクルード
#define RESOLUTION 512  //分解能
//#define BASIC_SPEED 1   //動確用
//#define TEST_DUTY 0.3   //動確用


Ec4multi EC_1(p15,p16,RESOLUTION);
Ec4multi EC_2(p17,p18,RESOLUTION);

SpeedControl motor_1(p21,p22,50,EC_1);
SpeedControl motor_2(p23,p24,50,EC_2);

Ticker motor_tick;  //角速度計算用ticker
Serial pc(USBTX,USBRX);

Timer timer;


int j=0;
double rotate_1=0,rotate_2=0;
double motor1_omega[1000]={},motor2_omega[1000]={},time_when[1000];
int motor1_count[1000]={},motor2_count[1000]={};

void CalOmega()
{
    motor1_count[j]=EC_1.getCount();
    motor1_omega[j]=EC_1.getOmega();
    motor2_count[j]=EC_2.getCount();
    motor2_omega[j]=EC_2.getOmega();
//    time_when[j]=EC_1.timer_.read();
    if(rotate_1==0) motor_1.stop();
    else motor_1.Sc(rotate_1);
    if(rotate_2==0) motor_2.stop();
    else motor_2.Sc(rotate_2);
    j++;
}

int main()
{
    motor_1.period_us(50);
    motor_2.period_us(50);
    motor_1.setEquation(0.322,0.08,0,0.0100);    //求めたC,Dの値を設定
    motor_2.setEquation(0.0302,0.0755,-0.0241,0.1301);    //求めたC,Dの値を設定
    motor_1.setPDparam(0,0.0);  //PIDの係数を設定
    motor_2.setPDparam(0,0.0);  //PIDの係数を設定
    motor_tick.attach(&CalOmega,0.05);

    while(1) {

        if(pc.readable()) {
            char sel=pc.getc();
            if(sel=='q') {
                for(int i=0; i<1000; i++) {

//                    pc.printf("%f    ",time_when[i]);
                    pc.printf("%f,%f    ",motor1_omega[i],motor2_omega[i]);
                    pc.printf("%d,%d\r\n",motor1_count[i],motor2_count[i]);
                }
            }
        }
    }
}