motor.cpp

00001 #include "mbed.h"
00002 #include "motor.h"
00003 
00004 Motor::Motor(PinName pwm_pin, PinName phase_pin):
00005 PWM_FREQ            (1023),
00006 MEASUREMENT_INTERVAL(0.01),
00007 SLIT                (12),
00008 KP                  (5e-5),
00009 KI                  (1e-8),
00010 KD                  (5e-5),
00011 pwm                 (pwm_pin),
00012 phase               (phase_pin),
00013 counter             (0),
00014 target              (0.0),
00015 target_pre          (0.0),
00016 rpm                 (0.0),
00017 error_pre1          (0.0),
00018 error_pre2          (0.0),
00019 duty                (0.0),
00020 MV                  (0.0)
00021 {
00022     pwm.period( (1/PWM_FREQ) );
00023     pwm = 0.0;
00024     phase = 1;
00025 }
00026 
00027 void Motor::Set_phase(const int _phase)
00028 {
00029     phase = _phase;
00030 }
00031 
00032 void Motor::Set_target(const float _target)
00033 {
00034     target = _target;
00035 }
00036 
00037 float Motor::Get_rpm(void)
00038 {
00039     return rpm;
00040 }
00041 
00042 void Motor::count(void)
00043 {
00044     counter++;
00045 }
00046 
00047 void Motor::calc_rpm(void)
00048 {
00049     rpm = counter /(float)(SLIT*2) / MEASUREMENT_INTERVAL * 60;
00050     counter=0;
00051 }
00052 
00053 void Motor::drive(void)
00054 {
00055     float error_current = 0.0;
00056     float dMV           = 0.0;
00057     float SV            = target;
00058     
00059     calc_rpm();
00060     
00061     error_current = SV - rpm;
00062     dMV = KP * (error_current - error_pre1)
00063         + KI * error_current
00064         + KD * ((error_current - error_pre1) - (error_pre1 - error_pre2));
00065     MV += dMV;
00066     
00067     duty += MV;
00068     if(duty>1.0) duty = 1.0;
00069     if(duty<0)   duty = 0.0;
00070     
00071     pwm = duty;
00072     
00073     error_pre1 = error_current;
00074     error_pre2 = error_pre1;
00075     target_pre = target;
00076     
00077     //printf("duty:%.3f MV:%+.3f PV:%4.0frpm \n", duty, MV, rpm);
00078 }