File content as of revision 0:5a017390fffe:

#include "motor_driver.h"

MotorDriver::MotorDriver(PinName _in,PinName _dir,PinName _pot,float _pwm_freq):duty(_in),dir(_dir),pot(_pot){
    //pwm周期設定
    pwm_period = 1.0f/_pwm_freq;
    duty.period(pwm_period);
    
    //パラメータを計算
    kp = KP/360.0f;
    kd = KD*_pwm_freq/360.0f;
    ki = KI/(360.0f*_pwm_freq);
    kp_vel = KP_VEL*_pwm_freq/360.0f;
    kd_vel = KD_VEL*_pwm_freq*_pwm_freq/360.0f;
    
    min_pos = MIN_POS/360.0f;
    max_pos = MAX_POS/360.0f;
    
    //角度初期化
    now_angular.pos = pot.read();
    now_angular.vel = 0;
    pre_angular.pos = pot.read();
    pre_angular.vel = 0;
    
    //積分要素初期化
    pos_int = 0;
    
}


int MotorDriver::update_target(float _pos,float _vel){
    tgt_angular.pos = _pos/360;
    tgt_angular.vel = _vel*pwm_period/360;
    
    //積分要素初期化
    pos_int = 0;
    
    if(tgt_angular.pos < min_pos){
        tgt_angular.pos = min_pos;
        tgt_angular.vel = 0;
        return -1;
    }else if(tgt_angular.pos > max_pos){
        tgt_angular.pos = max_pos;
        tgt_angular.vel = 0;
        return -1;
    }
    
    return 0;
}


void MotorDriver::update_pwm(){
    motor_output(control_output());
}

float MotorDriver::control_output(){
    //角度更新
    pre_angular = now_angular;
    now_angular.pos = pot.read();
    now_angular.vel = (now_angular.pos - pre_angular.pos);
    
    //偏差を求める
    float dev = tgt_angular.pos - now_angular.pos;
    float dev_diff = pre_angular.pos - now_angular.pos;
    
    //積分
    pos_int += now_angular.pos;
    
    //出力計算
    float output = dev * kp + dev_diff * kd + pos_int * ki;
    
    //ここに速度比例制御を入れる
    
    //リミット
    if(output > 1.0f) output = 1.0f;
    if(output < -1.0f) output = -1.0f;
    
    return output;
}


void MotorDriver::motor_output(float control){
    if(control < 0){
        dir = 0;
        duty = -1.0f*control;
    }else{
        dir = 1;
        duty = control;
    }
}