Yuki Kimura / MotorDriver

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

motor_driver.cpp@0:5a017390fffe, 2019-10-23 (annotated)

Committer:
kimuraYUKI
Date:
Wed Oct 23 05:22:54 2019 +0000
Revision:
0:5a017390fffe
Child:
1:01d35aa5501f
first commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kimuraYUKI 0:5a017390fffe 1 #include "motor_driver.h"
kimuraYUKI 0:5a017390fffe 2
kimuraYUKI 0:5a017390fffe 3 MotorDriver::MotorDriver(PinName _in,PinName _dir,PinName _pot,float _pwm_freq):duty(_in),dir(_dir),pot(_pot){
kimuraYUKI 0:5a017390fffe 4 //pwm周期設定
kimuraYUKI 0:5a017390fffe 5 pwm_period = 1.0f/_pwm_freq;
kimuraYUKI 0:5a017390fffe 6 duty.period(pwm_period);
kimuraYUKI 0:5a017390fffe 7
kimuraYUKI 0:5a017390fffe 8 //パラメータを計算
kimuraYUKI 0:5a017390fffe 9 kp = KP/360.0f;
kimuraYUKI 0:5a017390fffe 10 kd = KD*_pwm_freq/360.0f;
kimuraYUKI 0:5a017390fffe 11 ki = KI/(360.0f*_pwm_freq);
kimuraYUKI 0:5a017390fffe 12 kp_vel = KP_VEL*_pwm_freq/360.0f;
kimuraYUKI 0:5a017390fffe 13 kd_vel = KD_VEL*_pwm_freq*_pwm_freq/360.0f;
kimuraYUKI 0:5a017390fffe 14
kimuraYUKI 0:5a017390fffe 15 min_pos = MIN_POS/360.0f;
kimuraYUKI 0:5a017390fffe 16 max_pos = MAX_POS/360.0f;
kimuraYUKI 0:5a017390fffe 17
kimuraYUKI 0:5a017390fffe 18 //角度初期化
kimuraYUKI 0:5a017390fffe 19 now_angular.pos = pot.read();
kimuraYUKI 0:5a017390fffe 20 now_angular.vel = 0;
kimuraYUKI 0:5a017390fffe 21 pre_angular.pos = pot.read();
kimuraYUKI 0:5a017390fffe 22 pre_angular.vel = 0;
kimuraYUKI 0:5a017390fffe 23
kimuraYUKI 0:5a017390fffe 24 //積分要素初期化
kimuraYUKI 0:5a017390fffe 25 pos_int = 0;
kimuraYUKI 0:5a017390fffe 26
kimuraYUKI 0:5a017390fffe 27 }
kimuraYUKI 0:5a017390fffe 28
kimuraYUKI 0:5a017390fffe 29
kimuraYUKI 0:5a017390fffe 30 int MotorDriver::update_target(float _pos,float _vel){
kimuraYUKI 0:5a017390fffe 31 tgt_angular.pos = _pos/360;
kimuraYUKI 0:5a017390fffe 32 tgt_angular.vel = _vel*pwm_period/360;
kimuraYUKI 0:5a017390fffe 33
kimuraYUKI 0:5a017390fffe 34 //積分要素初期化
kimuraYUKI 0:5a017390fffe 35 pos_int = 0;
kimuraYUKI 0:5a017390fffe 36
kimuraYUKI 0:5a017390fffe 37 if(tgt_angular.pos < min_pos){
kimuraYUKI 0:5a017390fffe 38 tgt_angular.pos = min_pos;
kimuraYUKI 0:5a017390fffe 39 tgt_angular.vel = 0;
kimuraYUKI 0:5a017390fffe 40 return -1;
kimuraYUKI 0:5a017390fffe 41 }else if(tgt_angular.pos > max_pos){
kimuraYUKI 0:5a017390fffe 42 tgt_angular.pos = max_pos;
kimuraYUKI 0:5a017390fffe 43 tgt_angular.vel = 0;
kimuraYUKI 0:5a017390fffe 44 return -1;
kimuraYUKI 0:5a017390fffe 45 }
kimuraYUKI 0:5a017390fffe 46
kimuraYUKI 0:5a017390fffe 47 return 0;
kimuraYUKI 0:5a017390fffe 48 }
kimuraYUKI 0:5a017390fffe 49
kimuraYUKI 0:5a017390fffe 50
kimuraYUKI 0:5a017390fffe 51 void MotorDriver::update_pwm(){
kimuraYUKI 0:5a017390fffe 52 motor_output(control_output());
kimuraYUKI 0:5a017390fffe 53 }
kimuraYUKI 0:5a017390fffe 54
kimuraYUKI 0:5a017390fffe 55 float MotorDriver::control_output(){
kimuraYUKI 0:5a017390fffe 56 //角度更新
kimuraYUKI 0:5a017390fffe 57 pre_angular = now_angular;
kimuraYUKI 0:5a017390fffe 58 now_angular.pos = pot.read();
kimuraYUKI 0:5a017390fffe 59 now_angular.vel = (now_angular.pos - pre_angular.pos);
kimuraYUKI 0:5a017390fffe 60
kimuraYUKI 0:5a017390fffe 61 //偏差を求める
kimuraYUKI 0:5a017390fffe 62 float dev = tgt_angular.pos - now_angular.pos;
kimuraYUKI 0:5a017390fffe 63 float dev_diff = pre_angular.pos - now_angular.pos;
kimuraYUKI 0:5a017390fffe 64
kimuraYUKI 0:5a017390fffe 65 //積分
kimuraYUKI 0:5a017390fffe 66 pos_int += now_angular.pos;
kimuraYUKI 0:5a017390fffe 67
kimuraYUKI 0:5a017390fffe 68 //出力計算
kimuraYUKI 0:5a017390fffe 69 float output = dev * kp + dev_diff * kd + pos_int * ki;
kimuraYUKI 0:5a017390fffe 70
kimuraYUKI 0:5a017390fffe 71 //ここに速度比例制御を入れる
kimuraYUKI 0:5a017390fffe 72
kimuraYUKI 0:5a017390fffe 73 //リミット
kimuraYUKI 0:5a017390fffe 74 if(output > 1.0f) output = 1.0f;
kimuraYUKI 0:5a017390fffe 75 if(output < -1.0f) output = -1.0f;
kimuraYUKI 0:5a017390fffe 76
kimuraYUKI 0:5a017390fffe 77 return output;
kimuraYUKI 0:5a017390fffe 78 }
kimuraYUKI 0:5a017390fffe 79
kimuraYUKI 0:5a017390fffe 80
kimuraYUKI 0:5a017390fffe 81 void MotorDriver::motor_output(float control){
kimuraYUKI 0:5a017390fffe 82 if(control < 0){
kimuraYUKI 0:5a017390fffe 83 dir = 0;
kimuraYUKI 0:5a017390fffe 84 duty = -1.0f*control;
kimuraYUKI 0:5a017390fffe 85 }else{
kimuraYUKI 0:5a017390fffe 86 dir = 1;
kimuraYUKI 0:5a017390fffe 87 duty = control;
kimuraYUKI 0:5a017390fffe 88 }
kimuraYUKI 0:5a017390fffe 89 }