File content as of revision 0:e4cee11fca3b:

#include "mbed.h"
#include "EC.h"
#include "number.h"

#define pi 3.1415926535
#define CALC_INTERVAL 0.01
#define TRY_MODE '0'
#define ROTATION_MODE '1'
#define PID_MODE '2'
#define SOLUTION 500

SpeedControl em(PA_6,PA_7,NC,SOLUTION,CALC_INTERVAL,PB_5,PB_4);
Serial pc(USBTX,USBRX);
void setup();
void print_scan();

int loop_kai=0;
char mode='0';
double duty=0;
float data[500][2];
int i=0;

double target_rotation=0,now_rotation=0,old_rotation=0;
double diff=0,diff_old=0,integral=0;
double Kp=0,Kd=0,Ki=0;

Ticker omega_tick;
Timer timer;
Num num(USBTX,USBRX);

void CalOmega()
{
    em.CalPreOmega();
    if(mode==ROTATION_MODE&&i<500){
        data[i][0]=(float)timer.read();
        data[i][1]=(float)em.getOmega()*60/(2*pi);
        i++;
    } else if(mode==PID_MODE){
        now_rotation=em.getCount()/SOLUTION;
    }
}

int main(void){
    setup();
    
    while(1){
        if(mode==ROTATION_MODE){
            if(i==500){
                for(int j=0;j<500;j++){
                    pc.printf("%f,%f\r\n",data[j][0],data[j][1]);
                }
                i++;
            }
        } else if(mode==PID_MODE){
            diff=target_rotation-now_rotation;
            integral+=diff;
            duty=Kp*diff+Kd*(diff-diff_old)+Ki*integral;
            diff_old=diff;
        }
        if(duty>0){
            em.turnF(duty);
        } else {
            em.turnB(-1*duty);
        }
        if(loop_kai%=100000){
            if(mode==TRY_MODE||mode==PID_MODE)print_scan();
            if(loop_kai==300000)loop_kai=0;
        }
        loop_kai++;
    }
}

void setup(){
    pc.printf("Mode 0 : otameshi\r\n");
    pc.printf("Mode 1 : jikkenn1\r\n");
    pc.printf("Mode 2 : jikkenn2\r\n");
    pc.printf("Input mode : ");
    while(1){
        if(pc.readable()) {
            mode=pc.getc();
            pc.printf("%c\r\n",mode);
            break;
        }
    }
    if(mode==ROTATION_MODE){
        pc.printf("duty= ");
        duty=num.get_number();
    } else if(mode==PID_MODE){
        pc.printf("target_rotation= ");
        target_rotation=num.get_number();
    } 
    pc.printf("start !\r\n");
    timer.start();
    omega_tick.attach(CalOmega,CALC_INTERVAL);
}

void print_scan(){
    pc.printf("count=%f rotation=%f omega=%f\r\n",em.getPreCount(),em.getPreCount()/SOLUTION,em.getOmega());
    //pc.printf("F=%f B=%f\r\n",(double)em.pwm_F_,(double)em.pwm_B_);
    
    if(pc.readable()) {
        char sel=pc.getc();
        
        switch(sel) {
            case 'i':
                duty+=0.1;
                pc.printf("duty=%f \r\n",duty);
                break;
            case 'o':
                duty-=0.1;
                pc.printf("duty=%f \r\n",duty);
                break;
            case 'p':
                Kp+=0.01;
                pc.printf("Kp=%f \r\n",Kp);
                break;
        }
    }
}