9/10
Dependencies: mbed KondoServoLibrary CalPID AMT21_right_arm ros_lib_melodic MotorController_AbsEC_right_arm
main.cpp
- Committer:
- yuki0108
- Date:
- 2021-04-14
- Revision:
- 1:b74b31a7df07
- Parent:
- 0:cb9f2ec9d902
- Child:
- 2:aeae129daa37
File content as of revision 1:b74b31a7df07:
#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 15 //速度制御を利用した角度制御での角速度の最大値 #define NUM_DATA 150 //記録データ数 #ifndef M_PI #define M_PI 3.14159265359f #endif Serial pc(USBTX, USBRX); //printfも通シリアル通信なのでこれを書いて、pc.printfとしないとと最悪通信と干渉します。 DigitalIn toggle(p6,PullUp); DigitalOut led1(LED1); Timer timer_loop; //制御周期用 ///////////////// 宣言部分 ////////////////////// CalPID speed_pid(0.2,0,0.00455,DELTA_T,DUTY_MAX); //速度制御のPD計算 CalPID angle_duty_pid(0.5,0,0.00056,DELTA_T,DUTY_MAX); //角度制御(duty式)のPD計算 CalPID angle_omega_pid(25.0,0,0.00224,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 angle_saved[NUM_DATA]= {}; int angle_count=0; void saveAngle() { if(angle_count<NUM_DATA) { angle_saved[angle_count]=ec.getRad(); angle_count++; } } float omega_saved[NUM_DATA]= {}; int omega_count=0; void saveOmega() { if(omega_count<NUM_DATA) { omega_saved[omega_count]=ec.getOmega(); omega_count++; } } void displayData() { for(int i=0; i<omega_count; i++) { pc.printf("%f\t%f\t%f\r\n",i*DELTA_T,angle_saved[i],omega_saved[i]); wait(0.001); } omega_count=0; angle_count=0; } //////////////////////////////////////////////////////////////////////////// float target_rad=0; //目標角速度(速度制御) float target_speed=0;//目標角度(角度制御) void speedControll() { motor.Sc(target_speed); //速度制御関数 saveAngle();//データ記録 saveOmega();//データ記録 } void angleControll() { // motor.AcDuty(target_rad); //duty式角度制御 motor.AcOmega(target_rad); //速度式角度制御 saveAngle();//データ記録 saveOmega();//データ記録 } ////////////////////////////////////////////////////////////////////////////// int main () //調整時を想定しているのでデータを取っている。 { motor.setEquation(0.022639,0.022651,-0.022297,0.026744);//速度制御のC値D値 ////////////////////////////////////////////////////////////////////////////// int state=0; while(1) { if(state==0) { if(toggle) {//スタート用のトグルスイッチ。要らない場合はこのif部分の処理をwhileの直前に state++; target_speed=5; //目標角速度を5rad/sに target_rad=(M_PI/6); //目標角度をπ/6(rad)に(単位に注意) timer_loop.reset(); timer_loop.start(); } } else if(state==1) { if(timer_loop.read()>DELTA_T) {//Tickerだと通信と割り込みが干渉してしまうのでタイマーで // speedControll();//速度制御 angleControll();//角度制御 timer_loop.reset(); } if(omega_count==NUM_DATA) {//データが集まったら表示 motor.stop(); displayData(); timer_loop.stop(); state++; } } wait_us(10); } }