File content as of revision 4:b44ae8a3fa1b:

#include "motor_driver.h"
#include "math.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;
    
    //角度移動平均初期化
    for(int i=0;i<MOV_AVE_NUM;i++){
        angle_buf[i] = (pot.read()*1000.0f-MV_OFFSET)/MV_PER_DEG;
    }
    
}


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;
    
    //積分
    if(fabsf(en)<I_THRE) pos_int += en/pwm_freq;
    
    //出力計算
    float output = 0;
    if(fabsf(en)>D_THRE){
        output = en*KP + en_diff*KD + pos_int*KI;
    }else{
        output = en*KP  + 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(){
    
    for(int i=0;i<MOV_AVE_NUM-1;i++){
        angle_buf[i] = angle_buf[i+1];
    }
    
    angle_buf[MOV_AVE_NUM-1] = (pot.read()*1000.0f-MV_OFFSET)/MV_PER_DEG;
    
    float angle = 0;
    for(int i=0;i<MOV_AVE_NUM;i++) angle += angle_buf[i];
    angle = angle/(float)MOV_AVE_NUM;
    
    return angle;
}