Diff: main.cpp

Revision:

0:cb9f2ec9d902

Child:

1:b74b31a7df07

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Apr 12 06:08:54 2021 +0000
@@ -0,0 +1,60 @@
+#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);
+    }
+}
+