ryo shatari / MotorController_2021_1210shata

shata

MotorController.cpp@0:7f95508de23e, 2021-07-10 (annotated)

Committer:
yuki0108
Date:
Sat Jul 10 01:51:58 2021 +0000
Revision:
0:7f95508de23e
Child:
1:76da1a46652f
7.10

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 // state_sc=ACCELERATION_MODE;
yuki0108 0:7f95508de23e 17
yuki0108 0:7f95508de23e 18 calibration(0);
yuki0108 0:7f95508de23e 19 }
yuki0108 0:7f95508de23e 20 double MotorController::getAngle()
yuki0108 0:7f95508de23e 21 {
yuki0108 0:7f95508de23e 22 angle=ec_->getRad();
yuki0108 0:7f95508de23e 23 angle+=angle_offset;
yuki0108 0:7f95508de23e 24 return angle;
yuki0108 0:7f95508de23e 25 }
yuki0108 0:7f95508de23e 26 void MotorController::calibration(double angle_calibration)
yuki0108 0:7f95508de23e 27 {
yuki0108 0:7f95508de23e 28 angle_offset=angle_calibration;
yuki0108 0:7f95508de23e 29 ec_->reset();
yuki0108 0:7f95508de23e 30 }
yuki0108 0:7f95508de23e 31 void MotorController::period(double s)
yuki0108 0:7f95508de23e 32 {
yuki0108 0:7f95508de23e 33 motor_p.period(s);
yuki0108 0:7f95508de23e 34 motor_n.period(s);
yuki0108 0:7f95508de23e 35 }
yuki0108 0:7f95508de23e 36 void MotorController::period_ms(int ms)
yuki0108 0:7f95508de23e 37 {
yuki0108 0:7f95508de23e 38 motor_p.period_ms(ms);
yuki0108 0:7f95508de23e 39 motor_n.period_ms(ms);
yuki0108 0:7f95508de23e 40 }
yuki0108 0:7f95508de23e 41 void MotorController::period_us(int us)
yuki0108 0:7f95508de23e 42 {
yuki0108 0:7f95508de23e 43 motor_p.period_us(us);
yuki0108 0:7f95508de23e 44 motor_n.period_us(us);
yuki0108 0:7f95508de23e 45 }
yuki0108 0:7f95508de23e 46 void MotorController::setDutyLimit(double max)
yuki0108 0:7f95508de23e 47 {
yuki0108 0:7f95508de23e 48 sc_pid->setMaxValue(max);
yuki0108 0:7f95508de23e 49 ac_duty_pid->setMaxValue(max);
yuki0108 0:7f95508de23e 50 }
yuki0108 0:7f95508de23e 51 void MotorController::setOmegaLimit(double max)
yuki0108 0:7f95508de23e 52 {
yuki0108 0:7f95508de23e 53 ac_omega_pid->setMaxValue(max);
yuki0108 0:7f95508de23e 54 }
yuki0108 0:7f95508de23e 55 void MotorController::setPDParamSc(double kp, double kd)
yuki0108 0:7f95508de23e 56 {
yuki0108 0:7f95508de23e 57 sc_pid->setParameter(kp, 0, kd);
yuki0108 0:7f95508de23e 58 }
yuki0108 0:7f95508de23e 59 void MotorController::setPDParamAcDuty(double kp, double kd)
yuki0108 0:7f95508de23e 60 {
yuki0108 0:7f95508de23e 61 ac_duty_pid->setParameter(kp, 0, kd);
yuki0108 0:7f95508de23e 62 }
yuki0108 0:7f95508de23e 63 void MotorController::setPDParamAcOmega(double kp, double kd)
yuki0108 0:7f95508de23e 64 {
yuki0108 0:7f95508de23e 65 ac_omega_pid->setParameter(kp, 0, kd);
yuki0108 0:7f95508de23e 66 }
yuki0108 0:7f95508de23e 67 void MotorController::setDeltaTime(double dt)
yuki0108 0:7f95508de23e 68 {
yuki0108 0:7f95508de23e 69 delta_t=dt;
yuki0108 0:7f95508de23e 70 sc_pid->setDELTA_T(dt);
yuki0108 0:7f95508de23e 71 ac_duty_pid->setDELTA_T(dt);
yuki0108 0:7f95508de23e 72 ac_omega_pid->setDELTA_T(dt);
yuki0108 0:7f95508de23e 73
yuki0108 0:7f95508de23e 74 }
yuki0108 0:7f95508de23e 75 void MotorController::setEquation(double cf,double df,double cb,double db)
yuki0108 0:7f95508de23e 76 {
yuki0108 0:7f95508de23e 77 Cf_=cf;
yuki0108 0:7f95508de23e 78 if(cb>0)Cb_=cb;
yuki0108 0:7f95508de23e 79 else Cb_=-cb;
yuki0108 0:7f95508de23e 80 initial_Df_=df;
yuki0108 0:7f95508de23e 81 initial_Db_=-db;
yuki0108 0:7f95508de23e 82 Df_=initial_Df_;
yuki0108 0:7f95508de23e 83 Db_=initial_Db_;
yuki0108 0:7f95508de23e 84 }
yuki0108 0:7f95508de23e 85 void MotorController::turn(double duty)
yuki0108 0:7f95508de23e 86 {
yuki0108 0:7f95508de23e 87 if (duty<1.0&&duty >= 0) {
yuki0108 0:7f95508de23e 88 motor_p = duty;
yuki0108 0:7f95508de23e 89 motor_n = 0;
yuki0108 0:7f95508de23e 90 } else if (duty < 0&&duty>-1.0) {
yuki0108 0:7f95508de23e 91 motor_p = 0;
yuki0108 0:7f95508de23e 92 motor_n = -duty;
yuki0108 0:7f95508de23e 93 }
yuki0108 0:7f95508de23e 94 }
yuki0108 0:7f95508de23e 95 void MotorController::Sc(double target_omega)
yuki0108 0:7f95508de23e 96 {
yuki0108 0:7f95508de23e 97 ec_->calOmega();
yuki0108 0:7f95508de23e 98 double omega = ec_->getOmega();
yuki0108 0:7f95508de23e 99 double devia = target_omega - omega;
yuki0108 0:7f95508de23e 100 // double pid = sc_pid->calPD(devia);
yuki0108 0:7f95508de23e 101 double pid = sc_pid->calP_D(devia,ec_->getAcceleration());
yuki0108 0:7f95508de23e 102 // double duty;
yuki0108 0:7f95508de23e 103
yuki0108 0:7f95508de23e 104 if (target_omega > 0.0) {
yuki0108 0:7f95508de23e 105 Df_ += Cf_ * pid;
yuki0108 0:7f95508de23e 106 duty = Cf_ * target_omega + Df_;
yuki0108 0:7f95508de23e 107 } else if (target_omega < -0.0) {
yuki0108 0:7f95508de23e 108 Db_ += Cb_ * pid;
yuki0108 0:7f95508de23e 109 duty = Cb_ * target_omega + Db_;
yuki0108 0:7f95508de23e 110 } else
yuki0108 0:7f95508de23e 111 duty = 0;
yuki0108 0:7f95508de23e 112 turn(duty);
yuki0108 0:7f95508de23e 113 }
yuki0108 0:7f95508de23e 114 double MotorController::calSc(double target_omega)
yuki0108 0:7f95508de23e 115 {
yuki0108 0:7f95508de23e 116 ec_->calOmega();
yuki0108 0:7f95508de23e 117 double omega = ec_->getOmega();
yuki0108 0:7f95508de23e 118 double devia = target_omega - omega;
yuki0108 0:7f95508de23e 119 // double pid = sc_pid->calPD(devia);
yuki0108 0:7f95508de23e 120 double pid = sc_pid->calP_D(devia,ec_->getAcceleration());
yuki0108 0:7f95508de23e 121
yuki0108 0:7f95508de23e 122 double duty_=0;
yuki0108 0:7f95508de23e 123 if (target_omega > 0.0) {
yuki0108 0:7f95508de23e 124 Df_ += Cf_ * pid;
yuki0108 0:7f95508de23e 125 duty_ = Cf_ * target_omega + Df_;
yuki0108 0:7f95508de23e 126 } else if (target_omega < -0.0) {
yuki0108 0:7f95508de23e 127 Db_ += Cb_ * pid;
yuki0108 0:7f95508de23e 128 duty_ = Cb_ * target_omega + Db_;
yuki0108 0:7f95508de23e 129 } else
yuki0108 0:7f95508de23e 130 duty_ = 0;
yuki0108 0:7f95508de23e 131 return duty_;
yuki0108 0:7f95508de23e 132 }
yuki0108 0:7f95508de23e 133 void MotorController::AcDuty(double target_angle)
yuki0108 0:7f95508de23e 134 {
yuki0108 0:7f95508de23e 135 getAngle();
yuki0108 0:7f95508de23e 136 double devia = target_angle - angle;
yuki0108 0:7f95508de23e 137 double duty_omega = ac_duty_pid->calP_D(devia,ec_->getOmega());
yuki0108 0:7f95508de23e 138 turn(duty_omega);
yuki0108 0:7f95508de23e 139 }
yuki0108 0:7f95508de23e 140 void MotorController::AcOmega(double target_angle)
yuki0108 0:7f95508de23e 141 {
yuki0108 0:7f95508de23e 142 getAngle();
yuki0108 0:7f95508de23e 143 double devia = target_angle - angle;
yuki0108 0:7f95508de23e 144 double omega = ac_omega_pid->calP_D(devia,ec_->getOmega());
yuki0108 0:7f95508de23e 145 double duty_offset=DUTY_OFFSET*cos(angle);
yuki0108 0:7f95508de23e 146 duty=calSc(omega)+duty_offset;
yuki0108 0:7f95508de23e 147 turn(duty);
yuki0108 0:7f95508de23e 148 }
yuki0108 0:7f95508de23e 149 void MotorController::stop()
yuki0108 0:7f95508de23e 150 {
yuki0108 0:7f95508de23e 151 motor_p = 0;
yuki0108 0:7f95508de23e 152 motor_n = 0;
yuki0108 0:7f95508de23e 153 }
yuki0108 0:7f95508de23e 154 void MotorController::reset()
yuki0108 0:7f95508de23e 155 {
yuki0108 0:7f95508de23e 156 ec_->reset();
yuki0108 0:7f95508de23e 157 Df_=initial_Df_;
yuki0108 0:7f95508de23e 158 Db_=initial_Db_;
yuki0108 0:7f95508de23e 159 }
yuki0108 0:7f95508de23e 160 double MotorController::getDuty()
yuki0108 0:7f95508de23e 161 {
yuki0108 0:7f95508de23e 162 // return duty;
yuki0108 0:7f95508de23e 163 return Df_;
yuki0108 0:7f95508de23e 164 }