Diff: MotorController.cpp

Revision:

0:7f95508de23e

Child:

1:76da1a46652f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorController.cpp	Sat Jul 10 01:51:58 2021 +0000
@@ -0,0 +1,164 @@
+#include "mbed.h"
+#include "EC.h"
+#include "CalPID.h"
+#include "MotorController.h"
+
+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_)
+{
+    ec_ = &ec;
+    sc_pid = &sc_;
+    ac_duty_pid = &ac_duty;
+    ac_omega_pid = &ac_omega;
+    motor_p.period_us(50);
+    motor_n.period_us(50);
+    setDeltaTime(dt);
+    angle_offset=0;
+//    state_sc=ACCELERATION_MODE;
+
+    calibration(0);
+}
+double MotorController::getAngle()
+{
+    angle=ec_->getRad();
+    angle+=angle_offset;
+    return angle;
+}
+void MotorController::calibration(double angle_calibration)
+{
+    angle_offset=angle_calibration;
+    ec_->reset();
+}
+void MotorController::period(double s)
+{
+    motor_p.period(s);
+    motor_n.period(s);
+}
+void MotorController::period_ms(int ms)
+{
+    motor_p.period_ms(ms);
+    motor_n.period_ms(ms);
+}
+void MotorController::period_us(int us)
+{
+    motor_p.period_us(us);
+    motor_n.period_us(us);
+}
+void MotorController::setDutyLimit(double max)
+{
+    sc_pid->setMaxValue(max);
+    ac_duty_pid->setMaxValue(max);
+}
+void MotorController::setOmegaLimit(double max)
+{
+    ac_omega_pid->setMaxValue(max);
+}
+void MotorController::setPDParamSc(double kp, double kd)
+{
+    sc_pid->setParameter(kp, 0, kd);
+}
+void MotorController::setPDParamAcDuty(double kp, double kd)
+{
+    ac_duty_pid->setParameter(kp, 0, kd);
+}
+void MotorController::setPDParamAcOmega(double kp, double kd)
+{
+    ac_omega_pid->setParameter(kp, 0, kd);
+}
+void MotorController::setDeltaTime(double dt)
+{
+    delta_t=dt;
+    sc_pid->setDELTA_T(dt);
+    ac_duty_pid->setDELTA_T(dt);
+    ac_omega_pid->setDELTA_T(dt);
+
+}
+void MotorController::setEquation(double cf,double df,double cb,double db)
+{
+    Cf_=cf;
+    if(cb>0)Cb_=cb;
+    else Cb_=-cb;
+    initial_Df_=df;
+    initial_Db_=-db;
+    Df_=initial_Df_;
+    Db_=initial_Db_;
+}
+void MotorController::turn(double duty)
+{
+    if (duty<1.0&&duty >= 0) {
+        motor_p = duty;
+        motor_n = 0;
+    } else if (duty < 0&&duty>-1.0) {
+        motor_p = 0;
+        motor_n = -duty;
+    }
+}
+void MotorController::Sc(double target_omega)
+{
+    ec_->calOmega();
+    double omega = ec_->getOmega();
+    double devia = target_omega - omega;
+//    double pid = sc_pid->calPD(devia);
+    double pid = sc_pid->calP_D(devia,ec_->getAcceleration());
+//    double duty;
+
+    if (target_omega > 0.0) {
+        Df_ += Cf_ * pid;
+        duty = Cf_ * target_omega + Df_;
+    } else if (target_omega < -0.0) {
+        Db_ += Cb_ * pid;
+        duty = Cb_ * target_omega + Db_;
+    } else
+        duty = 0;
+    turn(duty);
+}
+double MotorController::calSc(double target_omega)
+{
+    ec_->calOmega();
+    double omega = ec_->getOmega();
+    double devia = target_omega - omega;
+//    double pid = sc_pid->calPD(devia);
+    double pid = sc_pid->calP_D(devia,ec_->getAcceleration());
+
+    double duty_=0;
+    if (target_omega > 0.0) {
+        Df_ += Cf_ * pid;
+        duty_ = Cf_ * target_omega + Df_;
+    } else if (target_omega < -0.0) {
+        Db_ += Cb_ * pid;
+        duty_ = Cb_ * target_omega + Db_;
+    } else
+        duty_ = 0;
+    return duty_;
+}
+void MotorController::AcDuty(double target_angle)
+{
+    getAngle();
+    double devia = target_angle - angle;
+    double duty_omega = ac_duty_pid->calP_D(devia,ec_->getOmega());
+    turn(duty_omega);
+}
+void MotorController::AcOmega(double target_angle)
+{
+    getAngle();
+    double devia = target_angle - angle;
+    double omega = ac_omega_pid->calP_D(devia,ec_->getOmega());
+    double duty_offset=DUTY_OFFSET*cos(angle);
+    duty=calSc(omega)+duty_offset;
+    turn(duty);
+}
+void MotorController::stop()
+{
+    motor_p = 0;
+    motor_n = 0;
+}
+void MotorController::reset()
+{
+    ec_->reset();
+    Df_=initial_Df_;
+    Db_=initial_Db_;
+}
+double MotorController::getDuty()
+{
+//    return duty;
+    return Df_;
+}