9/10

Dependents:   Right_Arm_9_10 Right_Arm

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 #ifndef MOTORCONTROLLER_H
yuki0108 0:6451d750607b 2 #define MOTORCONTROLLER_H
yuki0108 1:013870241971 3 #define ANGLE_ERROR 0.01
yuki0108 0:6451d750607b 4
yuki0108 0:6451d750607b 5 class MotorController
yuki0108 0:6451d750607b 6 {
yuki0108 0:6451d750607b 7 private:
yuki0108 0:6451d750607b 8 double delta_t;
yuki0108 0:6451d750607b 9
yuki0108 0:6451d750607b 10 double Cf_;
yuki0108 0:6451d750607b 11 double Cb_;
yuki0108 0:6451d750607b 12 double Df_;
yuki0108 0:6451d750607b 13 double Db_;
yuki0108 0:6451d750607b 14 double initial_Df_;
yuki0108 0:6451d750607b 15 double initial_Db_;
yuki0108 0:6451d750607b 16
yuki0108 0:6451d750607b 17 double old_angle;
yuki0108 0:6451d750607b 18 double old_omega;
yuki0108 0:6451d750607b 19
yuki0108 0:6451d750607b 20 double duty_max;
yuki0108 0:6451d750607b 21 double omega_max;
yuki0108 0:6451d750607b 22
yuki0108 0:6451d750607b 23 Amt21 *ec_;
yuki0108 0:6451d750607b 24 CalPID *sc_pid;
yuki0108 0:6451d750607b 25 CalPID *ac_duty_pid;
yuki0108 0:6451d750607b 26 CalPID *ac_omega_pid;
yuki0108 0:6451d750607b 27
yuki0108 0:6451d750607b 28 protected:
yuki0108 0:6451d750607b 29 PwmOut motor_p;
yuki0108 0:6451d750607b 30 PwmOut motor_n;
yuki0108 0:6451d750607b 31
yuki0108 0:6451d750607b 32 public:
yuki0108 0:6451d750607b 33 MotorController(PinName motor_p_, PinName motor_n_,double dt, Amt21 &ec, CalPID &sc_, CalPID &ac_duty, CalPID &ac_omega);//引数は下行
yuki0108 0:6451d750607b 34 //モーター正転、逆転、周期[s]、エンコーダ、速度制御用のPID、角度制御のPID、速度制御利用の角度制御のPID
yuki0108 0:6451d750607b 35
yuki0108 0:6451d750607b 36 //////////////////////////////////////////各クラスのコンストラクタ引数で設定はされているので呼び出しは不要。変更したい場合などに
yuki0108 0:6451d750607b 37 void period(double s);//pwm周期設定用関数
yuki0108 0:6451d750607b 38 void period_ms(int ms);//pwm周期設定用関数
yuki0108 0:6451d750607b 39 void period_us(int us);//pwm周期設定用関数
yuki0108 0:6451d750607b 40 void setDutyLimit(double max);//CalPIDによるduty比の最大値設定
yuki0108 0:6451d750607b 41 void setOmegaLimit(double max);//CalPIDによる角速度[rad/s]の最大値設定
yuki0108 0:6451d750607b 42 void setPDParam_sc(double kp, double kd);//速度制御のPDゲイン設定用関数
yuki0108 0:6451d750607b 43 void setPDParam_ac_duty(double kp, double kd);//(duty比による)角度制御のPDゲイン設定用関数
yuki0108 0:6451d750607b 44 void setPDParam_ac_omega(double kp, double kd);//(速度制御による)角度制御のPDゲイン設定用関数
yuki0108 0:6451d750607b 45 void setDeltaTime(double dt);//制御周期の設定用関数
yuki0108 0:6451d750607b 46
yuki0108 0:6451d750607b 47
yuki0108 0:6451d750607b 48 //////////////////////////////////////////実際に使うときに呼び出す関数
yuki0108 0:6451d750607b 49 void setEquation(double cf, double df, double cb, double db);//速度制御のC値、D値の設定用。設定が必要
yuki0108 0:6451d750607b 50 void turn(double duty);//モーターにduty比入力する
yuki0108 0:6451d750607b 51 void AcDuty(double target_angle);//PD算出されたdutyによる角度制御
oshin1030 4:6bff84284ecb 52 void AcOmega_1(double target_angle,double max_duty);//PD算出された角速度による角度制御。速度制御を利用している
oshin1030 4:6bff84284ecb 53 void AcOmega_2(double target_angle,double angleA,double max_duty);//PD算出された角速度による角度制御。速度制御を利用している
oshin1030 4:6bff84284ecb 54 void AcOmega(double target_angle);//PD算出された角速度による角度制御。速度制御を利用している
yuki0108 1:013870241971 55 double calSc(double target_omega);//速度制御
yuki0108 0:6451d750607b 56 void Sc(double target_omega);//速度制御
yuki0108 0:6451d750607b 57 void stop();//duty=0を入力
yuki0108 0:6451d750607b 58
yuki0108 0:6451d750607b 59 };
yuki0108 0:6451d750607b 60
yuki0108 0:6451d750607b 61 #endif