シンイチ 田中 / MotorController_AbsEC_right_arm

9/10

Dependents:   Right_Arm_9_10 Right_Arm

MotorController.cpp@3:8668176943ae, 2021-06-22 (annotated)

Committer:
oshin1030
Date:
Tue Jun 22 08:35:02 2021 +0000
Revision:
3:8668176943ae
Parent:
1:013870241971
Child:
4:6bff84284ecb
6/22

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuki0108 0:6451d750607b 1 #include "mbed.h"
yuki0108 0:6451d750607b 2 #include "AMT21.h"
yuki0108 0:6451d750607b 3 #include "CalPID.h"
yuki0108 0:6451d750607b 4 #include "MotorController.h"
yuki0108 0:6451d750607b 5
yuki0108 0:6451d750607b 6 MotorController::MotorController(PinName motor_p_, PinName motor_n_,double dt, Amt21 &ec, CalPID &sc_, CalPID &ac_duty, CalPID &ac_omega) : motor_p(motor_p_), motor_n(motor_n_)
yuki0108 0:6451d750607b 7 {
yuki0108 0:6451d750607b 8 ec_ = &ec;
yuki0108 0:6451d750607b 9 sc_pid = &sc_;
yuki0108 0:6451d750607b 10 ac_duty_pid = &ac_duty;
yuki0108 0:6451d750607b 11 ac_omega_pid = &ac_omega;
yuki0108 0:6451d750607b 12 motor_p.period_us(50);
yuki0108 0:6451d750607b 13 motor_n.period_us(50);
yuki0108 0:6451d750607b 14 setDeltaTime(dt);
yuki0108 0:6451d750607b 15
yuki0108 0:6451d750607b 16 }
yuki0108 0:6451d750607b 17 void MotorController::period(double s)
yuki0108 0:6451d750607b 18 {
yuki0108 0:6451d750607b 19 motor_p.period(s);
yuki0108 0:6451d750607b 20 motor_n.period(s);
yuki0108 0:6451d750607b 21 }
yuki0108 0:6451d750607b 22 void MotorController::period_ms(int ms)
yuki0108 0:6451d750607b 23 {
yuki0108 0:6451d750607b 24 motor_p.period_ms(ms);
yuki0108 0:6451d750607b 25 motor_n.period_ms(ms);
yuki0108 0:6451d750607b 26 }
yuki0108 0:6451d750607b 27 void MotorController::period_us(int us)
yuki0108 0:6451d750607b 28 {
yuki0108 0:6451d750607b 29 motor_p.period_us(us);
yuki0108 0:6451d750607b 30 motor_n.period_us(us);
yuki0108 0:6451d750607b 31 }
yuki0108 0:6451d750607b 32 void MotorController::setDutyLimit(double max)
yuki0108 0:6451d750607b 33 {
yuki0108 0:6451d750607b 34 sc_pid->setMaxValue(max);
yuki0108 0:6451d750607b 35 ac_duty_pid->setMaxValue(max);
yuki0108 0:6451d750607b 36 }
yuki0108 0:6451d750607b 37 void MotorController::setOmegaLimit(double max)
yuki0108 0:6451d750607b 38 {
yuki0108 0:6451d750607b 39 ac_omega_pid->setMaxValue(max);
yuki0108 0:6451d750607b 40 }
yuki0108 0:6451d750607b 41 void MotorController::setPDParam_sc(double kp, double kd)
yuki0108 0:6451d750607b 42 {
yuki0108 0:6451d750607b 43 sc_pid->setParameter(kp, 0, kd);
yuki0108 0:6451d750607b 44 }
yuki0108 0:6451d750607b 45 void MotorController::setPDParam_ac_duty(double kp, double kd)
yuki0108 0:6451d750607b 46 {
yuki0108 0:6451d750607b 47 ac_duty_pid->setParameter(kp, 0, kd);
yuki0108 0:6451d750607b 48 }
yuki0108 0:6451d750607b 49 void MotorController::setPDParam_ac_omega(double kp, double kd)
yuki0108 0:6451d750607b 50 {
yuki0108 0:6451d750607b 51 ac_omega_pid->setParameter(kp, 0, kd);
yuki0108 0:6451d750607b 52 }
yuki0108 0:6451d750607b 53 void MotorController::setDeltaTime(double dt)
yuki0108 0:6451d750607b 54 {
yuki0108 0:6451d750607b 55 delta_t=dt;
yuki0108 0:6451d750607b 56 sc_pid->setDELTA_T(dt);
yuki0108 0:6451d750607b 57 ac_duty_pid->setDELTA_T(dt);
yuki0108 0:6451d750607b 58 ac_omega_pid->setDELTA_T(dt);
yuki0108 0:6451d750607b 59
yuki0108 0:6451d750607b 60 }
yuki0108 0:6451d750607b 61 void MotorController::setEquation(double cf,double df,double cb,double db)
yuki0108 0:6451d750607b 62 {
yuki0108 0:6451d750607b 63 Cf_=cf;
yuki0108 0:6451d750607b 64 if(cb>0)Cb_=cb;
yuki0108 0:6451d750607b 65 else Cb_=-cb;
yuki0108 0:6451d750607b 66 initial_Df_=df;
yuki0108 0:6451d750607b 67 initial_Db_=-db;
yuki0108 0:6451d750607b 68 Df_=initial_Df_;
yuki0108 0:6451d750607b 69 Db_=initial_Db_;
yuki0108 0:6451d750607b 70 }
yuki0108 0:6451d750607b 71 void MotorController::turn(double duty)
yuki0108 0:6451d750607b 72 {
yuki0108 0:6451d750607b 73 if (duty >= 0) {
yuki0108 0:6451d750607b 74 motor_p = duty;
yuki0108 0:6451d750607b 75 motor_n = 0;
yuki0108 0:6451d750607b 76 } else if (duty < 0) {
yuki0108 0:6451d750607b 77 motor_p = 0;
yuki0108 0:6451d750607b 78 motor_n = -duty;
yuki0108 0:6451d750607b 79 }
yuki0108 0:6451d750607b 80 }
yuki0108 1:013870241971 81 double MotorController::calSc(double target_omega)
yuki0108 0:6451d750607b 82 {
yuki0108 0:6451d750607b 83 double omega = ec_->getOmega();
yuki0108 0:6451d750607b 84 double devia = target_omega - omega;
yuki0108 0:6451d750607b 85 double pid = sc_pid->calPD(devia);
yuki0108 0:6451d750607b 86
yuki0108 0:6451d750607b 87 double duty;
yuki0108 0:6451d750607b 88 if (target_omega > 0) {
yuki0108 0:6451d750607b 89 Df_ += Cf_ * pid;
yuki0108 0:6451d750607b 90 duty = Cf_ * target_omega + Df_;
yuki0108 0:6451d750607b 91 } else if (target_omega < 0) {
yuki0108 0:6451d750607b 92 Db_ += Cb_ * pid;
yuki0108 0:6451d750607b 93 duty = Cb_ * target_omega + Db_;
yuki0108 0:6451d750607b 94 } else
yuki0108 0:6451d750607b 95 duty = 0;
yuki0108 1:013870241971 96 return duty;
yuki0108 1:013870241971 97 }
yuki0108 1:013870241971 98 void MotorController::Sc(double target_omega)
yuki0108 1:013870241971 99 {
yuki0108 1:013870241971 100 ec_->rewriteCount();
yuki0108 1:013870241971 101 ec_->calOmega();
yuki0108 1:013870241971 102
yuki0108 1:013870241971 103 double duty=calSc(target_omega);
yuki0108 1:013870241971 104
yuki0108 0:6451d750607b 105 turn(duty);
yuki0108 0:6451d750607b 106 }
yuki0108 0:6451d750607b 107 void MotorController::AcDuty(double target_angle)
yuki0108 0:6451d750607b 108 {
yuki0108 1:013870241971 109 ec_->rewriteCount();
yuki0108 0:6451d750607b 110 double angle = ec_->getRad();
yuki0108 0:6451d750607b 111 double devia = target_angle - angle;
yuki0108 0:6451d750607b 112 double duty_omega = ac_duty_pid->calP_D(devia,ec_->getOmega());
yuki0108 0:6451d750607b 113 turn(duty_omega);
yuki0108 0:6451d750607b 114 }
oshin1030 3:8668176943ae 115 void MotorController::AcOmega_1(double target_angle, double angle_)
yuki0108 0:6451d750607b 116 {
yuki0108 1:013870241971 117 ec_->rewriteCount();
yuki0108 1:013870241971 118 ec_->calOmega();
oshin1030 3:8668176943ae 119 //double angle = ec_->getRad();
oshin1030 3:8668176943ae 120 double devia = target_angle - angle_;
yuki0108 0:6451d750607b 121 double omega = ac_omega_pid->calP_D(devia,ec_->getOmega());
oshin1030 3:8668176943ae 122 //printf("%f ",omega);
yuki0108 1:013870241971 123 double duty=calSc(omega);
yuki0108 1:013870241971 124 if(fabs(devia)<ANGLE_ERROR) {//収束幅の設定
yuki0108 1:013870241971 125 omega=0;
yuki0108 1:013870241971 126 }
oshin1030 3:8668176943ae 127 printf("%f ",-duty);
oshin1030 3:8668176943ae 128 if(duty > 0.2) duty = 0.2;
oshin1030 3:8668176943ae 129 if(duty < -0.2)duty = -0.2;
oshin1030 3:8668176943ae 130 printf("%f\t%f\t%f\r\n",-duty, target_angle, angle_);
oshin1030 3:8668176943ae 131 turn(-duty);
oshin1030 3:8668176943ae 132 }
oshin1030 3:8668176943ae 133 void MotorController::AcOmega_2(double target_angle, double angle_)
oshin1030 3:8668176943ae 134 {
oshin1030 3:8668176943ae 135 ec_->rewriteCount();
oshin1030 3:8668176943ae 136 ec_->calOmega();
oshin1030 3:8668176943ae 137 //double angle = ec_->getRad();
oshin1030 3:8668176943ae 138 double devia = target_angle - angle_;
oshin1030 3:8668176943ae 139 double omega = ac_omega_pid->calP_D(devia,ec_->getOmega());
oshin1030 3:8668176943ae 140 printf("%f ",omega);
oshin1030 3:8668176943ae 141 double duty=calSc(omega);
oshin1030 3:8668176943ae 142 if(fabs(devia)<ANGLE_ERROR) {//収束幅の設定
oshin1030 3:8668176943ae 143 omega=0;
oshin1030 3:8668176943ae 144 }
oshin1030 3:8668176943ae 145 printf("%f ",duty);
oshin1030 3:8668176943ae 146 if(duty > 0.3) duty = 0.3;
oshin1030 3:8668176943ae 147 if(duty < -0.3)duty = -0.3;
yuki0108 1:013870241971 148 turn(duty);
yuki0108 0:6451d750607b 149 }
oshin1030 3:8668176943ae 150
yuki0108 0:6451d750607b 151 void MotorController::stop()
yuki0108 0:6451d750607b 152 {
yuki0108 0:6451d750607b 153 motor_p = 0;
yuki0108 0:6451d750607b 154 motor_n = 0;
yuki0108 0:6451d750607b 155 }
yuki0108 0:6451d750607b 156