ryo shatari / MotorController_2021_1210shata

shata

MotorController.cpp@2:113b6a35d37c, 2021-12-10 (annotated)

Committer:
shatari
Date:
Fri Dec 10 14:04:16 2021 +0000
Revision:
2:113b6a35d37c
Parent:
1:76da1a46652f 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuki0108 0:7f95508de23e 1 #include "mbed.h"
yuki0108 0:7f95508de23e 2 #include "EC.h"
yuki0108 0:7f95508de23e 3 #include "CalPID.h"
yuki0108 0:7f95508de23e 4 #include "MotorController.h"
yuki0108 0:7f95508de23e 5
yuki0108 0:7f95508de23e 6 MotorController::MotorController(PinName motor_p_, PinName motor_n_,double dt, Ec &ec, CalPID &sc_, CalPID &ac_duty, CalPID &ac_omega) : motor_p(motor_p_), motor_n(motor_n_)
yuki0108 0:7f95508de23e 7 {
yuki0108 0:7f95508de23e 8 ec_ = &ec;
yuki0108 0:7f95508de23e 9 sc_pid = &sc_;
yuki0108 0:7f95508de23e 10 ac_duty_pid = &ac_duty;
yuki0108 0:7f95508de23e 11 ac_omega_pid = &ac_omega;
yuki0108 0:7f95508de23e 12 motor_p.period_us(50);
yuki0108 0:7f95508de23e 13 motor_n.period_us(50);
yuki0108 0:7f95508de23e 14 setDeltaTime(dt);
yuki0108 0:7f95508de23e 15 angle_offset=0;
yuki0108 0:7f95508de23e 16 calibration(0);
yuki0108 1:76da1a46652f 17 setAccelMax(200);
yuki0108 1:76da1a46652f 18 setDutyOffset(0);
yuki0108 1:76da1a46652f 19 pre_target_omega=0;
yuki0108 0:7f95508de23e 20 }
yuki0108 0:7f95508de23e 21 double MotorController::getAngle()
yuki0108 0:7f95508de23e 22 {
yuki0108 0:7f95508de23e 23 angle=ec_->getRad();
yuki0108 0:7f95508de23e 24 angle+=angle_offset;
yuki0108 0:7f95508de23e 25 return angle;
yuki0108 0:7f95508de23e 26 }
yuki0108 0:7f95508de23e 27 void MotorController::calibration(double angle_calibration)
yuki0108 0:7f95508de23e 28 {
yuki0108 0:7f95508de23e 29 angle_offset=angle_calibration;
yuki0108 0:7f95508de23e 30 ec_->reset();
yuki0108 0:7f95508de23e 31 }
yuki0108 0:7f95508de23e 32 void MotorController::period(double s)
yuki0108 0:7f95508de23e 33 {
yuki0108 0:7f95508de23e 34 motor_p.period(s);
yuki0108 0:7f95508de23e 35 motor_n.period(s);
yuki0108 0:7f95508de23e 36 }
yuki0108 0:7f95508de23e 37 void MotorController::period_ms(int ms)
yuki0108 0:7f95508de23e 38 {
yuki0108 0:7f95508de23e 39 motor_p.period_ms(ms);
yuki0108 0:7f95508de23e 40 motor_n.period_ms(ms);
yuki0108 0:7f95508de23e 41 }
yuki0108 0:7f95508de23e 42 void MotorController::period_us(int us)
yuki0108 0:7f95508de23e 43 {
yuki0108 0:7f95508de23e 44 motor_p.period_us(us);
yuki0108 0:7f95508de23e 45 motor_n.period_us(us);
yuki0108 0:7f95508de23e 46 }
yuki0108 0:7f95508de23e 47 void MotorController::setDutyLimit(double max)
yuki0108 0:7f95508de23e 48 {
yuki0108 0:7f95508de23e 49 sc_pid->setMaxValue(max);
yuki0108 0:7f95508de23e 50 ac_duty_pid->setMaxValue(max);
yuki0108 0:7f95508de23e 51 }
yuki0108 0:7f95508de23e 52 void MotorController::setOmegaLimit(double max)
yuki0108 0:7f95508de23e 53 {
yuki0108 0:7f95508de23e 54 ac_omega_pid->setMaxValue(max);
yuki0108 0:7f95508de23e 55 }
yuki0108 0:7f95508de23e 56 void MotorController::setPDParamSc(double kp, double kd)
yuki0108 0:7f95508de23e 57 {
yuki0108 0:7f95508de23e 58 sc_pid->setParameter(kp, 0, kd);
yuki0108 0:7f95508de23e 59 }
yuki0108 0:7f95508de23e 60 void MotorController::setPDParamAcDuty(double kp, double kd)
yuki0108 0:7f95508de23e 61 {
yuki0108 0:7f95508de23e 62 ac_duty_pid->setParameter(kp, 0, kd);
yuki0108 0:7f95508de23e 63 }
yuki0108 0:7f95508de23e 64 void MotorController::setPDParamAcOmega(double kp, double kd)
yuki0108 0:7f95508de23e 65 {
yuki0108 0:7f95508de23e 66 ac_omega_pid->setParameter(kp, 0, kd);
yuki0108 0:7f95508de23e 67 }
yuki0108 0:7f95508de23e 68 void MotorController::setDeltaTime(double dt)
yuki0108 0:7f95508de23e 69 {
yuki0108 0:7f95508de23e 70 delta_t=dt;
yuki0108 0:7f95508de23e 71 sc_pid->setDELTA_T(dt);
yuki0108 0:7f95508de23e 72 ac_duty_pid->setDELTA_T(dt);
yuki0108 0:7f95508de23e 73 ac_omega_pid->setDELTA_T(dt);
yuki0108 0:7f95508de23e 74
yuki0108 0:7f95508de23e 75 }
yuki0108 0:7f95508de23e 76 void MotorController::setEquation(double cf,double df,double cb,double db)
yuki0108 0:7f95508de23e 77 {
yuki0108 0:7f95508de23e 78 Cf_=cf;
yuki0108 0:7f95508de23e 79 if(cb>0)Cb_=cb;
yuki0108 0:7f95508de23e 80 else Cb_=-cb;
yuki0108 0:7f95508de23e 81 initial_Df_=df;
yuki0108 0:7f95508de23e 82 initial_Db_=-db;
yuki0108 0:7f95508de23e 83 Df_=initial_Df_;
yuki0108 0:7f95508de23e 84 Db_=initial_Db_;
yuki0108 0:7f95508de23e 85 }
yuki0108 1:76da1a46652f 86 void MotorController::setDutyOffset(double duty_off)
yuki0108 1:76da1a46652f 87 {
yuki0108 1:76da1a46652f 88 duty_offset=duty_off;
yuki0108 1:76da1a46652f 89 }
yuki0108 1:76da1a46652f 90 void MotorController::setAccelMax(double a_max)
yuki0108 1:76da1a46652f 91 {
yuki0108 1:76da1a46652f 92 acceleration_max=a_max*delta_t;
yuki0108 1:76da1a46652f 93 }
yuki0108 0:7f95508de23e 94 void MotorController::turn(double duty)
yuki0108 0:7f95508de23e 95 {
yuki0108 0:7f95508de23e 96 if (duty<1.0&&duty >= 0) {
yuki0108 0:7f95508de23e 97 motor_p = duty;
yuki0108 0:7f95508de23e 98 motor_n = 0;
yuki0108 0:7f95508de23e 99 } else if (duty < 0&&duty>-1.0) {
yuki0108 0:7f95508de23e 100 motor_p = 0;
yuki0108 0:7f95508de23e 101 motor_n = -duty;
yuki0108 0:7f95508de23e 102 }
yuki0108 0:7f95508de23e 103 }
yuki0108 1:76da1a46652f 104 void MotorController::Sc(double target_omega_input)
yuki0108 1:76da1a46652f 105 {
yuki0108 1:76da1a46652f 106 double duty=calSc(target_omega_input);
yuki0108 1:76da1a46652f 107 turn(duty);
yuki0108 1:76da1a46652f 108
yuki0108 1:76da1a46652f 109 }
yuki0108 1:76da1a46652f 110 double MotorController::calSc(double target_omega_input)
yuki0108 0:7f95508de23e 111 {
yuki0108 1:76da1a46652f 112 ///////////////////////////////////////////////加速度制限
yuki0108 1:76da1a46652f 113 double target_omega=target_omega_input;
shatari 2:113b6a35d37c 114 present_target_omega = target_omega;
yuki0108 1:76da1a46652f 115 if((target_omega_input-pre_target_omega)>acceleration_max) {
yuki0108 1:76da1a46652f 116 target_omega=pre_target_omega+acceleration_max;
yuki0108 1:76da1a46652f 117 } else if((target_omega_input-pre_target_omega)<-acceleration_max) {
yuki0108 1:76da1a46652f 118 target_omega=pre_target_omega-acceleration_max;
yuki0108 1:76da1a46652f 119 }
yuki0108 1:76da1a46652f 120 ///////////////////////////////////////////////PD計算
yuki0108 0:7f95508de23e 121 ec_->calOmega();
yuki0108 0:7f95508de23e 122 double omega = ec_->getOmega();
yuki0108 0:7f95508de23e 123 double devia = target_omega - omega;
shatari 2:113b6a35d37c 124 present_devia_omega = devia;
yuki0108 0:7f95508de23e 125 double pid = sc_pid->calP_D(devia,ec_->getAcceleration());
yuki0108 1:76da1a46652f 126 double duty_;
yuki0108 0:7f95508de23e 127 if (target_omega > 0.0) {
yuki0108 0:7f95508de23e 128 Df_ += Cf_ * pid;
yuki0108 0:7f95508de23e 129 duty_ = Cf_ * target_omega + Df_;
yuki0108 0:7f95508de23e 130 } else if (target_omega < -0.0) {
yuki0108 0:7f95508de23e 131 Db_ += Cb_ * pid;
yuki0108 0:7f95508de23e 132 duty_ = Cb_ * target_omega + Db_;
yuki0108 0:7f95508de23e 133 } else
yuki0108 0:7f95508de23e 134 duty_ = 0;
yuki0108 1:76da1a46652f 135
yuki0108 1:76da1a46652f 136 pre_target_omega=target_omega;
yuki0108 0:7f95508de23e 137 return duty_;
yuki0108 0:7f95508de23e 138 }
yuki0108 0:7f95508de23e 139 void MotorController::AcDuty(double target_angle)
yuki0108 0:7f95508de23e 140 {
yuki0108 0:7f95508de23e 141 getAngle();
yuki0108 0:7f95508de23e 142 double devia = target_angle - angle;
yuki0108 0:7f95508de23e 143 double duty_omega = ac_duty_pid->calP_D(devia,ec_->getOmega());
yuki0108 0:7f95508de23e 144 turn(duty_omega);
yuki0108 0:7f95508de23e 145 }
yuki0108 0:7f95508de23e 146 void MotorController::AcOmega(double target_angle)
yuki0108 0:7f95508de23e 147 {
yuki0108 0:7f95508de23e 148 getAngle();
yuki0108 0:7f95508de23e 149 double devia = target_angle - angle;
yuki0108 0:7f95508de23e 150 double omega = ac_omega_pid->calP_D(devia,ec_->getOmega());
yuki0108 1:76da1a46652f 151 double duty_offset_=duty_offset*cos(angle);
yuki0108 1:76da1a46652f 152
yuki0108 1:76da1a46652f 153 double duty=calSc(omega)+duty_offset_;
yuki0108 1:76da1a46652f 154 //duty=calSc(omega);
yuki0108 0:7f95508de23e 155 turn(duty);
yuki0108 0:7f95508de23e 156 }
yuki0108 0:7f95508de23e 157 void MotorController::stop()
yuki0108 0:7f95508de23e 158 {
yuki0108 0:7f95508de23e 159 motor_p = 0;
yuki0108 0:7f95508de23e 160 motor_n = 0;
yuki0108 0:7f95508de23e 161 }
yuki0108 0:7f95508de23e 162 void MotorController::reset()
yuki0108 0:7f95508de23e 163 {
yuki0108 0:7f95508de23e 164 ec_->reset();
yuki0108 0:7f95508de23e 165 Df_=initial_Df_;
yuki0108 0:7f95508de23e 166 Db_=initial_Db_;
yuki0108 0:7f95508de23e 167 }
yuki0108 1:76da1a46652f 168