ryo shatari / MotorController_2021_1210shata

shata

MotorController.h@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 #ifndef MOTORCONTROLLER_H
yuki0108 0:7f95508de23e 2 #define MOTORCONTROLLER_H
yuki0108 0:7f95508de23e 3
yuki0108 0:7f95508de23e 4 //#define ACCELERATION_MODE 0
yuki0108 0:7f95508de23e 5 //#define BRAKE_MODE 1
yuki0108 0:7f95508de23e 6 //#define CONVERGENCE_MODE 2
yuki0108 0:7f95508de23e 7 //#define THREASHOLD_SC 1.0
yuki0108 0:7f95508de23e 8 //#define THREASHOLD_SC_TIME 0.005
yuki0108 0:7f95508de23e 9 //#define THREASHOLD_AC 0.1
yuki0108 0:7f95508de23e 10
yuki0108 0:7f95508de23e 11 #define DUTY_OFFSET 0.022
yuki0108 0:7f95508de23e 12 #define THREASHOLD_DUTY_OFFSET 0.41
yuki0108 0:7f95508de23e 13
yuki0108 0:7f95508de23e 14 class MotorController
yuki0108 0:7f95508de23e 15 {
yuki0108 0:7f95508de23e 16 private:
yuki0108 0:7f95508de23e 17 double duty;
yuki0108 0:7f95508de23e 18
yuki0108 0:7f95508de23e 19 double delta_t;
yuki0108 0:7f95508de23e 20 double angle;
yuki0108 0:7f95508de23e 21 double angle_offset;
yuki0108 0:7f95508de23e 22
yuki0108 0:7f95508de23e 23 double Cf_;
yuki0108 0:7f95508de23e 24 double Cb_;
yuki0108 0:7f95508de23e 25 double Df_;
yuki0108 0:7f95508de23e 26 double Db_;
yuki0108 0:7f95508de23e 27 double initial_Df_;
yuki0108 0:7f95508de23e 28 double initial_Db_;
yuki0108 0:7f95508de23e 29
yuki0108 0:7f95508de23e 30 double duty_max;
yuki0108 0:7f95508de23e 31 double omega_max;
yuki0108 0:7f95508de23e 32
yuki0108 0:7f95508de23e 33 int state_sc;
yuki0108 0:7f95508de23e 34 double acc_time;
yuki0108 0:7f95508de23e 35
yuki0108 0:7f95508de23e 36 Ec *ec_;
yuki0108 0:7f95508de23e 37 CalPID *sc_pid;
yuki0108 0:7f95508de23e 38 CalPID *ac_duty_pid;
yuki0108 0:7f95508de23e 39 CalPID *ac_omega_pid;
yuki0108 0:7f95508de23e 40
yuki0108 0:7f95508de23e 41 protected:
yuki0108 0:7f95508de23e 42 PwmOut motor_p;
yuki0108 0:7f95508de23e 43 PwmOut motor_n;
yuki0108 0:7f95508de23e 44
yuki0108 0:7f95508de23e 45 public:
yuki0108 0:7f95508de23e 46 MotorController(PinName motor_p_, PinName motor_n_,double dt, Ec &ec, CalPID &sc_, CalPID &ac_duty, CalPID &ac_omega);//引数は下行
yuki0108 0:7f95508de23e 47 //モーター正転、逆転、周期[s]、エンコーダ、速度制御用のPID、角度制御のPID、速度制御利用の角度制御のPID
yuki0108 0:7f95508de23e 48
yuki0108 0:7f95508de23e 49 //////////////////////////////////////////各クラスのコンストラクタ引数で設定はされているので呼び出しは不要。変更したい場合などに
yuki0108 0:7f95508de23e 50 void period(double s);//pwm周期設定用関数
yuki0108 0:7f95508de23e 51 void period_ms(int ms);//pwm周期設定用関数
yuki0108 0:7f95508de23e 52 void period_us(int us);//pwm周期設定用関数
yuki0108 0:7f95508de23e 53 void setDutyLimit(double max);//CalPIDによるduty比の最大値設定
yuki0108 0:7f95508de23e 54 void setOmegaLimit(double max);//CalPIDによる角速度[rad/s]の最大値設定
yuki0108 0:7f95508de23e 55 void setPDParamSc(double kp, double kd);//速度制御のPDゲイン設定用関数
yuki0108 0:7f95508de23e 56 void setPDParamAcDuty(double kp, double kd);//（duty比による）角度制御のPDゲイン設定用関数
yuki0108 0:7f95508de23e 57 void setPDParamAcOmega(double kp, double kd);//（速度制御による）角度制御のPDゲイン設定用関数
yuki0108 0:7f95508de23e 58 void setDeltaTime(double dt);//制御周期の設定用関数
yuki0108 0:7f95508de23e 59
yuki0108 0:7f95508de23e 60
yuki0108 0:7f95508de23e 61 //////////////////////////////////////////実際に使うときに呼び出す関数
yuki0108 0:7f95508de23e 62 double getAngle();
yuki0108 0:7f95508de23e 63 void calibration(double angle_calibration);
yuki0108 0:7f95508de23e 64 void setEquation(double cf, double df, double cb, double db);//速度制御のC値、D値の設定用。設定が必要
yuki0108 0:7f95508de23e 65 void turn(double duty);//モーターにduty比入力する
yuki0108 0:7f95508de23e 66 void AcDuty(double target_angle);//PD算出されたdutyによる角度制御
yuki0108 0:7f95508de23e 67 void AcOmega(double target_angle);//PD算出された角速度による角度制御。速度制御を利用している
yuki0108 0:7f95508de23e 68 void Sc(double target_omega);//速度制御
yuki0108 0:7f95508de23e 69 double calSc(double target_omega);//速度制御
yuki0108 0:7f95508de23e 70 void stop();//duty=0を入力
yuki0108 0:7f95508de23e 71 void reset();
yuki0108 0:7f95508de23e 72
yuki0108 0:7f95508de23e 73 double getDuty();
yuki0108 0:7f95508de23e 74
yuki0108 0:7f95508de23e 75 };
yuki0108 0:7f95508de23e 76
yuki0108 0:7f95508de23e 77 #endif