7th_DENSOU / Mbed 2 deprecated Right_Arm

9/19

Dependencies:   mbed KondoServoLibrary CalPID AMT21_right_arm ros_lib_melodic MotorController_AbsEC_right_arm

main.cpp@0:cb9f2ec9d902, 2021-04-12 (annotated)

Committer:
yuki0108
Date:
Mon Apr 12 06:08:54 2021 +0000
Revision:
0:cb9f2ec9d902
Child:
1:b74b31a7df07
4.19

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yuki0108 0:cb9f2ec9d902 1 #include "mbed.h"
yuki0108 0:cb9f2ec9d902 2 #include "AMT21.h"
yuki0108 0:cb9f2ec9d902 3 #include "CalPID.h"
yuki0108 0:cb9f2ec9d902 4 #include "MotorController.h"
yuki0108 0:cb9f2ec9d902 5
yuki0108 0:cb9f2ec9d902 6 #define DELTA_T 0.01 //制御周期
yuki0108 0:cb9f2ec9d902 7 #define DUTY_MAX 0.8 //duty比の最大値
yuki0108 0:cb9f2ec9d902 8 #define OMEGA_MAX 18 //速度制御を利用した角度制御での角速度の最大値
yuki0108 0:cb9f2ec9d902 9
yuki0108 0:cb9f2ec9d902 10
yuki0108 0:cb9f2ec9d902 11 #ifndef M_PI
yuki0108 0:cb9f2ec9d902 12 #define M_PI 3.14159265359f
yuki0108 0:cb9f2ec9d902 13 #endif
yuki0108 0:cb9f2ec9d902 14
yuki0108 0:cb9f2ec9d902 15
yuki0108 0:cb9f2ec9d902 16 DigitalIn toggle(p6,PullUp);
yuki0108 0:cb9f2ec9d902 17
yuki0108 0:cb9f2ec9d902 18 //Timer timer_loop;
yuki0108 0:cb9f2ec9d902 19 Ticker ticker;
yuki0108 0:cb9f2ec9d902 20
yuki0108 0:cb9f2ec9d902 21 ///////////////// 宣言部分 //////////////////////
yuki0108 0:cb9f2ec9d902 22 //CalPIDの引数は（kp,pi,pd(各PIDのパラメータ),周期,最大値)//
yuki0108 0:cb9f2ec9d902 23 CalPID speed_pid(0.2,0,0.0033642,DELTA_T,DUTY_MAX); //速度制御のPD計算
yuki0108 0:cb9f2ec9d902 24 CalPID angle_duty_pid(0.5,0,0.00056,DELTA_T,DUTY_MAX); //角度制御（duty式）のPD計算
yuki0108 0:cb9f2ec9d902 25 CalPID angle_omega_pid(8.926,0,0.04963,DELTA_T,OMEGA_MAX);//角度制御（速度式）のPD計算
yuki0108 0:cb9f2ec9d902 26 Amt21 ec(p9,p10,p8); //エンコーダー
yuki0108 0:cb9f2ec9d902 27 //　上で宣言したインスタンスをMotorControllerに与える　//
yuki0108 0:cb9f2ec9d902 28 MotorController motor(p22,p21,DELTA_T,ec,speed_pid,angle_duty_pid,angle_omega_pid);
yuki0108 0:cb9f2ec9d902 29 //////////////////////////////////////////////////////////////////////////
yuki0108 0:cb9f2ec9d902 30
yuki0108 0:cb9f2ec9d902 31
yuki0108 0:cb9f2ec9d902 32 float target_rad=0; //目標角速度（速度制御）
yuki0108 0:cb9f2ec9d902 33 float target_speed=0;//目標角度（角度制御）
yuki0108 0:cb9f2ec9d902 34
yuki0108 0:cb9f2ec9d902 35 void speedControll()
yuki0108 0:cb9f2ec9d902 36 {
yuki0108 0:cb9f2ec9d902 37 motor.Sc(target_speed); //速度制御関数
yuki0108 0:cb9f2ec9d902 38 }
yuki0108 0:cb9f2ec9d902 39 void angleControll()
yuki0108 0:cb9f2ec9d902 40 {
yuki0108 0:cb9f2ec9d902 41 motor.AcDuty(target_rad); //duty式角度制御
yuki0108 0:cb9f2ec9d902 42 // motor.AcOmega(target_rad); //速度式角度制御
yuki0108 0:cb9f2ec9d902 43 }
yuki0108 0:cb9f2ec9d902 44
yuki0108 0:cb9f2ec9d902 45 int main ()
yuki0108 0:cb9f2ec9d902 46 {
yuki0108 0:cb9f2ec9d902 47 NVIC_SetPriority(TIMER3_IRQn, 1);//tickerの割り込み優先順位を下げエンコーダが飛ばないようにしている(青mbed仕様)
yuki0108 0:cb9f2ec9d902 48
yuki0108 0:cb9f2ec9d902 49 motor.setEquation(0.0031,0.0047,-0.0030,0.0078);//速度制御のC値D値
yuki0108 0:cb9f2ec9d902 50 //////////////////////////////////////////////////////////////////////////////
yuki0108 0:cb9f2ec9d902 51 // target_speed=5; //目標角速度を5rad/sに
yuki0108 0:cb9f2ec9d902 52 // ticker.attach(&speedControll,DELTA_T);
yuki0108 0:cb9f2ec9d902 53 target_rad=(M_PI/6);; //目標角度をπ/6（rad）に（単位に注意）
yuki0108 0:cb9f2ec9d902 54 ticker.attach(&angleControll,DELTA_T);
yuki0108 0:cb9f2ec9d902 55
yuki0108 0:cb9f2ec9d902 56 while(1) {
yuki0108 0:cb9f2ec9d902 57 wait_us(10);
yuki0108 0:cb9f2ec9d902 58 }
yuki0108 0:cb9f2ec9d902 59 }
yuki0108 0:cb9f2ec9d902 60