File content as of revision 0:079a31fc71bb:

#include <mbed.h>
#include <DCmotor.hpp>
#include <rotary_inc.hpp>

DCmotor MOTOR(PA_0,PA_1, 0.5,0.0, 1);
RotaryInc Rotary(PC_3, PC_2, 256, 1, 4);
Timer TIME;

int main()
{
    TIME.start();
    double e1 = 0;
    double e2 = 0;
    double e3 = 0;
    double Kp = 0.00040;//0.00000264;
    double Ki = 0.000021;
    double Kd = 0.000003;
    double target = 360;
    double now = 0;
    double now_ang_v = 0.0;
    double P = 0;
    double I = 0;
    double D = 0;
    double dt = 0.01;
    double time = 0;
    double pwm = 0.0;
    double last = 0.0;
    while(1)
    {
        MOTOR.drive(pwm);
        last = now;
        now = Rotary.degree_now();//角度
        //now_ang_v = (now - last) / dt;
        //now_ang_v = (now - last);
        
        e3 = e2;
        e2 = e1;
        e1 = target - now;
        P = Kp * (e1 - e2);
        I = Ki * e1 * dt;
        D = Kd * (e1 - 2 * e2 + e3) / dt;
        pwm += P + I + D;
        printf("%lf,%lf\n",now,target);
        //printf("ang_vel %lf\tpwm %lf\te1 %lf\te2 %lf\te3 %lf\n",now,pwm, e1, e2, e3);
        while(TIME.read_ms() - time <= 10);
        time = TIME.read_ms();
        /*if(target * 0.99 <= now_ang_v && now_ang_v <= target * 1.01){
            printf("Finish\n");
            MOTOR.drive(0);
            break;
        }*/
    }
}
        //sum = Rotary.degree_diff();
        //printf("P : %lf I : %lf D : %lf pwm : %d\n",P, I, D, pwm);
        //MOTOR.out(pwm);
        //e2 = e1;
        //e1 = now = Rotary.degree_now();
        //e3 = (e1 - e2) / dt;
        //now = Rotary.getSpeed();
        //printf("%lf %lf\n",pwm,now);
        //MOTOR.drive(pwm += 0.001);
        //pwm += P + I;