File content as of revision 1:01d35aa5501f:

#include "motor_driver.h"

MotorDriver::MotorDriver(PinName _in,PinName _dir,PinName _pot ,float _pwm_freq):duty(_in),dir(_dir),pot(_pot){
    //pwm周期設定
    pwm_freq = _pwm_freq;
    float pwm_period = 1.0f/pwm_freq;
    duty.period(pwm_period);
    
    //角度初期化
    now_angular.pos = get_angle();
    now_angular.vel = 0;
    pre_angular.pos = get_angle();
    pre_angular.vel = 0;
    
    //積分要素初期化
    pos_int = 0;
    
}


int MotorDriver::update_target(float _pos){
    tgt_angular.pos = _pos;
    tgt_angular.vel = 0;
    
    //積分要素初期化
    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 = get_angle();
    now_angular.vel = (now_angular.pos - pre_angular.pos)*pwm_freq;
    
    //偏差を求める
    float en = tgt_angular.pos - now_angular.pos;
    float en_diff = (pre_angular.pos - now_angular.pos)*pwm_freq;
    
    //積分
    pos_int += en/pwm_freq;
    
    //出力計算
    float output = en * KP + en_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){
    
    //2019年度モーターの場合，-1
    control = control*-1;
    
    if(control < 0){
        dir = 0;
        duty = -1.0f*control;
    }else{
        dir = 1;
        duty = control;
    }
}

float MotorDriver::get_angle(){
    return (pot.read()*1000.0f-MV_OFFSET)/MV_PER_DEG;
}