7th_DENSOU / Mbed 2 deprecated Right_Arm_9_10

9/10

Dependencies:   mbed KondoServoLibrary CalPID AMT21_right_arm ros_lib_melodic MotorController_AbsEC_right_arm

main.cpp@1:b74b31a7df07, 2021-04-14 (annotated)

Committer:
yuki0108
Date:
Wed Apr 14 12:19:14 2021 +0000
Revision:
1:b74b31a7df07
Parent:
0:cb9f2ec9d902
Child:
2:aeae129daa37
4.14

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 1:b74b31a7df07 8 #define OMEGA_MAX 15 //速度制御を利用した角度制御での角速度の最大値
yuki0108 1:b74b31a7df07 9 #define NUM_DATA 150 //記録データ数
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 1:b74b31a7df07 15 Serial pc(USBTX, USBRX); //printfも通シリアル通信なのでこれを書いて、pc.printfとしないとと最悪通信と干渉します。
yuki0108 0:cb9f2ec9d902 16 DigitalIn toggle(p6,PullUp);
yuki0108 1:b74b31a7df07 17 DigitalOut led1(LED1);
yuki0108 0:cb9f2ec9d902 18
yuki0108 1:b74b31a7df07 19 Timer timer_loop; //制御周期用
yuki0108 1:b74b31a7df07 20
yuki0108 0:cb9f2ec9d902 21
yuki0108 0:cb9f2ec9d902 22 ///////////////// 宣言部分 //////////////////////
yuki0108 1:b74b31a7df07 23 CalPID speed_pid(0.2,0,0.00455,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 1:b74b31a7df07 25 CalPID angle_omega_pid(25.0,0,0.00224,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 1:b74b31a7df07 30 ////データ記録まわり/////
yuki0108 1:b74b31a7df07 31 float angle_saved[NUM_DATA]= {};
yuki0108 1:b74b31a7df07 32 int angle_count=0;
yuki0108 1:b74b31a7df07 33 void saveAngle()
yuki0108 1:b74b31a7df07 34 {
yuki0108 1:b74b31a7df07 35 if(angle_count<NUM_DATA) {
yuki0108 1:b74b31a7df07 36 angle_saved[angle_count]=ec.getRad();
yuki0108 1:b74b31a7df07 37 angle_count++;
yuki0108 1:b74b31a7df07 38 }
yuki0108 1:b74b31a7df07 39 }
yuki0108 1:b74b31a7df07 40 float omega_saved[NUM_DATA]= {};
yuki0108 1:b74b31a7df07 41 int omega_count=0;
yuki0108 1:b74b31a7df07 42 void saveOmega()
yuki0108 1:b74b31a7df07 43 {
yuki0108 1:b74b31a7df07 44 if(omega_count<NUM_DATA) {
yuki0108 1:b74b31a7df07 45 omega_saved[omega_count]=ec.getOmega();
yuki0108 1:b74b31a7df07 46 omega_count++;
yuki0108 1:b74b31a7df07 47 }
yuki0108 1:b74b31a7df07 48 }
yuki0108 1:b74b31a7df07 49 void displayData()
yuki0108 1:b74b31a7df07 50 {
yuki0108 1:b74b31a7df07 51 for(int i=0; i<omega_count; i++) {
yuki0108 1:b74b31a7df07 52 pc.printf("%f\t%f\t%f\r\n",i*DELTA_T,angle_saved[i],omega_saved[i]);
yuki0108 1:b74b31a7df07 53 wait(0.001);
yuki0108 1:b74b31a7df07 54 }
yuki0108 1:b74b31a7df07 55 omega_count=0;
yuki0108 1:b74b31a7df07 56 angle_count=0;
yuki0108 1:b74b31a7df07 57 }
yuki0108 1:b74b31a7df07 58 ////////////////////////////////////////////////////////////////////////////
yuki0108 0:cb9f2ec9d902 59
yuki0108 0:cb9f2ec9d902 60 float target_rad=0; //目標角速度（速度制御）
yuki0108 0:cb9f2ec9d902 61 float target_speed=0;//目標角度（角度制御）
yuki0108 0:cb9f2ec9d902 62
yuki0108 0:cb9f2ec9d902 63 void speedControll()
yuki0108 0:cb9f2ec9d902 64 {
yuki0108 0:cb9f2ec9d902 65 motor.Sc(target_speed); //速度制御関数
yuki0108 1:b74b31a7df07 66
yuki0108 1:b74b31a7df07 67 saveAngle();//データ記録
yuki0108 1:b74b31a7df07 68 saveOmega();//データ記録
yuki0108 0:cb9f2ec9d902 69 }
yuki0108 0:cb9f2ec9d902 70 void angleControll()
yuki0108 0:cb9f2ec9d902 71 {
yuki0108 1:b74b31a7df07 72 // motor.AcDuty(target_rad); //duty式角度制御
yuki0108 1:b74b31a7df07 73 motor.AcOmega(target_rad); //速度式角度制御
yuki0108 1:b74b31a7df07 74
yuki0108 1:b74b31a7df07 75 saveAngle();//データ記録
yuki0108 1:b74b31a7df07 76 saveOmega();//データ記録
yuki0108 0:cb9f2ec9d902 77 }
yuki0108 0:cb9f2ec9d902 78
yuki0108 1:b74b31a7df07 79 //////////////////////////////////////////////////////////////////////////////
yuki0108 0:cb9f2ec9d902 80
yuki0108 1:b74b31a7df07 81 int main () //調整時を想定しているのでデータを取っている。
yuki0108 1:b74b31a7df07 82 {
yuki0108 1:b74b31a7df07 83 motor.setEquation(0.022639,0.022651,-0.022297,0.026744);//速度制御のC値D値
yuki0108 0:cb9f2ec9d902 84 //////////////////////////////////////////////////////////////////////////////
yuki0108 1:b74b31a7df07 85 int state=0;
yuki0108 0:cb9f2ec9d902 86
yuki0108 0:cb9f2ec9d902 87 while(1) {
yuki0108 1:b74b31a7df07 88 if(state==0) {
yuki0108 1:b74b31a7df07 89 if(toggle) {//スタート用のトグルスイッチ。要らない場合はこのif部分の処理をwhileの直前に
yuki0108 1:b74b31a7df07 90 state++;
yuki0108 1:b74b31a7df07 91 target_speed=5; //目標角速度を5rad/sに
yuki0108 1:b74b31a7df07 92 target_rad=(M_PI/6); //目標角度をπ/6（rad）に（単位に注意）
yuki0108 1:b74b31a7df07 93
yuki0108 1:b74b31a7df07 94 timer_loop.reset();
yuki0108 1:b74b31a7df07 95 timer_loop.start();
yuki0108 1:b74b31a7df07 96 }
yuki0108 1:b74b31a7df07 97 } else if(state==1) {
yuki0108 1:b74b31a7df07 98 if(timer_loop.read()>DELTA_T) {//Tickerだと通信と割り込みが干渉してしまうのでタイマーで
yuki0108 1:b74b31a7df07 99 // speedControll();//速度制御
yuki0108 1:b74b31a7df07 100 angleControll();//角度制御
yuki0108 1:b74b31a7df07 101
yuki0108 1:b74b31a7df07 102 timer_loop.reset();
yuki0108 1:b74b31a7df07 103 }
yuki0108 1:b74b31a7df07 104 if(omega_count==NUM_DATA) {//データが集まったら表示
yuki0108 1:b74b31a7df07 105 motor.stop();
yuki0108 1:b74b31a7df07 106 displayData();
yuki0108 1:b74b31a7df07 107 timer_loop.stop();
yuki0108 1:b74b31a7df07 108 state++;
yuki0108 1:b74b31a7df07 109 }
yuki0108 1:b74b31a7df07 110 }
yuki0108 0:cb9f2ec9d902 111 wait_us(10);
yuki0108 0:cb9f2ec9d902 112 }
yuki0108 0:cb9f2ec9d902 113 }
yuki0108 0:cb9f2ec9d902 114