9/10
Dependencies: mbed KondoServoLibrary CalPID AMT21_right_arm ros_lib_melodic MotorController_AbsEC_right_arm
main.cpp
- Committer:
- yuki0108
- Date:
- 2021-04-12
- Revision:
- 0:cb9f2ec9d902
- Child:
- 1:b74b31a7df07
File content as of revision 0:cb9f2ec9d902:
#include "mbed.h" #include "AMT21.h" #include "CalPID.h" #include "MotorController.h" #define DELTA_T 0.01 //制御周期 #define DUTY_MAX 0.8 //duty比の最大値 #define OMEGA_MAX 18 //速度制御を利用した角度制御での角速度の最大値 #ifndef M_PI #define M_PI 3.14159265359f #endif DigitalIn toggle(p6,PullUp); //Timer timer_loop; Ticker ticker; ///////////////// 宣言部分 ////////////////////// //CalPIDの引数は(kp,pi,pd(各PIDのパラメータ),周期,最大値)// CalPID speed_pid(0.2,0,0.0033642,DELTA_T,DUTY_MAX); //速度制御のPD計算 CalPID angle_duty_pid(0.5,0,0.00056,DELTA_T,DUTY_MAX); //角度制御(duty式)のPD計算 CalPID angle_omega_pid(8.926,0,0.04963,DELTA_T,OMEGA_MAX);//角度制御(速度式)のPD計算 Amt21 ec(p9,p10,p8); //エンコーダー // 上で宣言したインスタンスをMotorControllerに与える // MotorController motor(p22,p21,DELTA_T,ec,speed_pid,angle_duty_pid,angle_omega_pid); ////////////////////////////////////////////////////////////////////////// float target_rad=0; //目標角速度(速度制御) float target_speed=0;//目標角度(角度制御) void speedControll() { motor.Sc(target_speed); //速度制御関数 } void angleControll() { motor.AcDuty(target_rad); //duty式角度制御 // motor.AcOmega(target_rad); //速度式角度制御 } int main () { NVIC_SetPriority(TIMER3_IRQn, 1);//tickerの割り込み優先順位を下げエンコーダが飛ばないようにしている(青mbed仕様) motor.setEquation(0.0031,0.0047,-0.0030,0.0078);//速度制御のC値D値 ////////////////////////////////////////////////////////////////////////////// // target_speed=5; //目標角速度を5rad/sに // ticker.attach(&speedControll,DELTA_T); target_rad=(M_PI/6);; //目標角度をπ/6(rad)に(単位に注意) ticker.attach(&angleControll,DELTA_T); while(1) { wait_us(10); } }