シンイチ 田中 / MotorController_AbsEC_right_arm

9/10

Dependents:   Right_Arm_9_10 Right_Arm

MotorController.cpp@5:2e5f40e761bd, 2021-09-19 (annotated)

Committer:
oshin1030
Date:
Sun Sep 19 13:33:48 2021 +0000
Revision:
5:2e5f40e761bd
Parent:
4:6bff84284ecb 
9/19;

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 {
oshin1030 4:6bff84284ecb 100 ec_->rewriteCount_2();
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 {
oshin1030 4:6bff84284ecb 109 ec_->rewriteCount_2();
oshin1030 4:6bff84284ecb 110 //double angle = 62*3.14159265359f/180 - ec_->getRad()/3;
oshin1030 4:6bff84284ecb 111 double angle = ec_->getRad()*36/60 - 6.4*3.14159265359f/180;
yuki0108 0:6451d750607b 112 double devia = target_angle - angle;
yuki0108 0:6451d750607b 113 double duty_omega = ac_duty_pid->calP_D(devia,ec_->getOmega());
oshin1030 4:6bff84284ecb 114 printf("%f\t%f\t%f\r\n",duty_omega, target_angle, angle);
yuki0108 0:6451d750607b 115 turn(duty_omega);
yuki0108 0:6451d750607b 116 }
oshin1030 4:6bff84284ecb 117 void MotorController::AcOmega_1(double target_angle,double max_duty)
yuki0108 0:6451d750607b 118 {
oshin1030 4:6bff84284ecb 119 ec_->rewriteCount_1();
yuki0108 1:013870241971 120 ec_->calOmega();
oshin1030 4:6bff84284ecb 121 double angle = ec_->getRad()/3 + 159.72*3.14159265359f/180;
oshin1030 4:6bff84284ecb 122 double devia = target_angle - angle;
yuki0108 0:6451d750607b 123 double omega = ac_omega_pid->calP_D(devia,ec_->getOmega());
yuki0108 1:013870241971 124 double duty=calSc(omega);
yuki0108 1:013870241971 125 if(fabs(devia)<ANGLE_ERROR) {//収束幅の設定
yuki0108 1:013870241971 126 omega=0;
yuki0108 1:013870241971 127 }
oshin1030 5:2e5f40e761bd 128 //printf("duty_1:%f\t",duty);
oshin1030 4:6bff84284ecb 129 if(duty > max_duty) duty = max_duty;
oshin1030 4:6bff84284ecb 130 if(duty < -max_duty)duty = -max_duty;
oshin1030 4:6bff84284ecb 131 turn(duty);
oshin1030 3:8668176943ae 132 }
oshin1030 4:6bff84284ecb 133 void MotorController::AcOmega_2(double target_angle,double angleA,double max_duty)
oshin1030 3:8668176943ae 134 {
oshin1030 4:6bff84284ecb 135 ec_->rewriteCount_2();
oshin1030 3:8668176943ae 136 ec_->calOmega();
oshin1030 4:6bff84284ecb 137 double angle;
oshin1030 4:6bff84284ecb 138 if(angleA >= 0){
oshin1030 5:2e5f40e761bd 139 angle = ec_->getRad()*36/60 + (-115 - angleA/3 - angleA*16.63/90)*3.14159265359f/180;
oshin1030 4:6bff84284ecb 140 }else if(angleA < 0){
oshin1030 5:2e5f40e761bd 141 angle = ec_->getRad()*36/60 + (-115 - angleA/3 + angleA*16.63/90)*3.14159265359f/180;
oshin1030 5:2e5f40e761bd 142 }
oshin1030 4:6bff84284ecb 143 double devia = target_angle - angle;
oshin1030 3:8668176943ae 144 double omega = ac_omega_pid->calP_D(devia,ec_->getOmega());
oshin1030 3:8668176943ae 145 double duty=calSc(omega);
oshin1030 3:8668176943ae 146 if(fabs(devia)<ANGLE_ERROR) {//収束幅の設定
oshin1030 3:8668176943ae 147 omega=0;
oshin1030 3:8668176943ae 148 }
oshin1030 5:2e5f40e761bd 149 //printf("duty_2:%f ",duty);
oshin1030 4:6bff84284ecb 150 if(duty > max_duty) duty = max_duty;
oshin1030 4:6bff84284ecb 151 if(duty < -max_duty)duty = -max_duty;
oshin1030 4:6bff84284ecb 152 turn(duty);
oshin1030 4:6bff84284ecb 153 }
oshin1030 4:6bff84284ecb 154
oshin1030 4:6bff84284ecb 155 void MotorController::AcOmega(double target_angle)
oshin1030 4:6bff84284ecb 156 {
oshin1030 4:6bff84284ecb 157 ec_->rewriteCount_2();
oshin1030 4:6bff84284ecb 158 ec_->calOmega();
oshin1030 4:6bff84284ecb 159 double angle = ec_->getRad()*36/60 - 6.4*3.14159265359f/180;
oshin1030 4:6bff84284ecb 160 double devia = target_angle - angle;
oshin1030 4:6bff84284ecb 161 double omega = ac_omega_pid->calP_D(devia,ec_->getOmega());
oshin1030 4:6bff84284ecb 162 double duty=calSc(omega);
oshin1030 4:6bff84284ecb 163 if(fabs(devia)<ANGLE_ERROR) {//収束幅の設定
oshin1030 4:6bff84284ecb 164 omega=0;
oshin1030 4:6bff84284ecb 165 }
oshin1030 5:2e5f40e761bd 166 //printf("%f\t%f\t%f\t\r\n",duty, target_angle, angle);
oshin1030 4:6bff84284ecb 167 if(duty > 0.2) duty = 0.2;
oshin1030 4:6bff84284ecb 168 if(duty < -0.2)duty = -0.2;
yuki0108 1:013870241971 169 turn(duty);
yuki0108 0:6451d750607b 170 }
oshin1030 3:8668176943ae 171
yuki0108 0:6451d750607b 172 void MotorController::stop()
yuki0108 0:6451d750607b 173 {
yuki0108 0:6451d750607b 174 motor_p = 0;
yuki0108 0:6451d750607b 175 motor_n = 0;
yuki0108 0:6451d750607b 176 }
yuki0108 0:6451d750607b 177