9/10
Dependencies: mbed KondoServoLibrary CalPID AMT21_right_arm ros_lib_melodic MotorController_AbsEC_right_arm
main.cpp@2:aeae129daa37, 2021-06-22 (annotated)
- Committer:
- oshin1030
- Date:
- Tue Jun 22 08:35:12 2021 +0000
- Revision:
- 2:aeae129daa37
- Parent:
- 1:b74b31a7df07
- Child:
- 3:ab266b418d90
6/22
Who changed what in which revision?
User | Revision | Line number | New 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" |
oshin1030 | 2:aeae129daa37 | 5 | #include <stdlib.h> |
oshin1030 | 2:aeae129daa37 | 6 | #include <math.h> |
yuki0108 | 0:cb9f2ec9d902 | 7 | |
yuki0108 | 0:cb9f2ec9d902 | 8 | #define DELTA_T 0.01 //制御周期 |
yuki0108 | 0:cb9f2ec9d902 | 9 | #define DUTY_MAX 0.8 //duty比の最大値 |
yuki0108 | 1:b74b31a7df07 | 10 | #define OMEGA_MAX 15 //速度制御を利用した角度制御での角速度の最大値 |
yuki0108 | 1:b74b31a7df07 | 11 | #define NUM_DATA 150 //記録データ数 |
oshin1030 | 2:aeae129daa37 | 12 | #define L_1 0.416 |
oshin1030 | 2:aeae129daa37 | 13 | #define L_2 0.820 |
yuki0108 | 0:cb9f2ec9d902 | 14 | |
yuki0108 | 0:cb9f2ec9d902 | 15 | #ifndef M_PI |
yuki0108 | 0:cb9f2ec9d902 | 16 | #define M_PI 3.14159265359f |
yuki0108 | 0:cb9f2ec9d902 | 17 | #endif |
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計算 |
oshin1030 | 2:aeae129daa37 | 25 | CalPID angle_omega_pid_1(1.0,0,0,DELTA_T,OMEGA_MAX);//角度制御(速度式)のPD計算 原点 |
oshin1030 | 2:aeae129daa37 | 26 | CalPID angle_omega_pid_2(1.0,0,0,DELTA_T,OMEGA_MAX);//角度制御(速度式)のPD計算 肘関節 |
oshin1030 | 2:aeae129daa37 | 27 | Amt21 ecA(p27,p28,p8); //原点 |
oshin1030 | 2:aeae129daa37 | 28 | Amt21 ecB(p13,p14,p15);//肘関節 |
yuki0108 | 0:cb9f2ec9d902 | 29 | // 上で宣言したインスタンスをMotorControllerに与える // |
oshin1030 | 2:aeae129daa37 | 30 | MotorController motorA(p26,p25,DELTA_T,ecA,speed_pid,angle_duty_pid,angle_omega_pid_1);//原点 |
oshin1030 | 2:aeae129daa37 | 31 | MotorController motorB(p24,p23,DELTA_T,ecB,speed_pid,angle_duty_pid,angle_omega_pid_2);//肘関節 |
yuki0108 | 0:cb9f2ec9d902 | 32 | ////////////////////////////////////////////////////////////////////////// |
yuki0108 | 0:cb9f2ec9d902 | 33 | |
yuki0108 | 0:cb9f2ec9d902 | 34 | float target_rad=0; //目標角速度(速度制御) |
yuki0108 | 0:cb9f2ec9d902 | 35 | float target_speed=0;//目標角度(角度制御) |
yuki0108 | 0:cb9f2ec9d902 | 36 | |
yuki0108 | 0:cb9f2ec9d902 | 37 | void speedControll() |
yuki0108 | 0:cb9f2ec9d902 | 38 | { |
oshin1030 | 2:aeae129daa37 | 39 | motorA.Sc(target_speed); //速度制御関数] |
oshin1030 | 2:aeae129daa37 | 40 | motorB.Sc(target_speed); |
oshin1030 | 2:aeae129daa37 | 41 | } |
yuki0108 | 1:b74b31a7df07 | 42 | |
yuki0108 | 0:cb9f2ec9d902 | 43 | void angleControll() |
yuki0108 | 0:cb9f2ec9d902 | 44 | { |
oshin1030 | 2:aeae129daa37 | 45 | //motorA.AcOmega_1(target_rad); //duty式角度制御 |
oshin1030 | 2:aeae129daa37 | 46 | //motorB.AcOmega_2(target_rad); //速度式角度制御 |
oshin1030 | 2:aeae129daa37 | 47 | } |
oshin1030 | 2:aeae129daa37 | 48 | |
oshin1030 | 2:aeae129daa37 | 49 | double function(double x) |
oshin1030 | 2:aeae129daa37 | 50 | { |
oshin1030 | 2:aeae129daa37 | 51 | double y; |
oshin1030 | 2:aeae129daa37 | 52 | y = 0.5; |
oshin1030 | 2:aeae129daa37 | 53 | return y; |
oshin1030 | 2:aeae129daa37 | 54 | } |
yuki0108 | 1:b74b31a7df07 | 55 | |
oshin1030 | 2:aeae129daa37 | 56 | double theta1(double x, double y, double angle_1)//原点角度計算 |
oshin1030 | 2:aeae129daa37 | 57 | { //(x:目標座標 y:目標座標 angle_1:現在角度) |
oshin1030 | 2:aeae129daa37 | 58 | double theta_1_a, theta_1_b; |
oshin1030 | 2:aeae129daa37 | 59 | double angle_1_PI = angle_1 * M_PI / 180; |
oshin1030 | 2:aeae129daa37 | 60 | theta_1_a = -acos((pow(x, 2) + pow(y, 2) + pow(L_1, 2) - pow(L_2, 2)) / (2 * L_1 * hypot(x, y))) + atan(y / x); |
oshin1030 | 2:aeae129daa37 | 61 | theta_1_b = acos((pow(x, 2) + pow(y, 2) + pow(L_1, 2) - pow(L_2, 2)) / (2 * L_1 * hypot(x, y))) + atan(y / x); |
oshin1030 | 2:aeae129daa37 | 62 | if (fabs(angle_1_PI - theta_1_a) > fabs(angle_1_PI - theta_1_b)) |
oshin1030 | 2:aeae129daa37 | 63 | return theta_1_b; |
oshin1030 | 2:aeae129daa37 | 64 | else |
oshin1030 | 2:aeae129daa37 | 65 | { |
oshin1030 | 2:aeae129daa37 | 66 | return theta_1_a; |
oshin1030 | 2:aeae129daa37 | 67 | } |
oshin1030 | 2:aeae129daa37 | 68 | } |
oshin1030 | 2:aeae129daa37 | 69 | |
oshin1030 | 2:aeae129daa37 | 70 | double theta2(double x, double y, double theta_1)//肘関節角度計算 |
oshin1030 | 2:aeae129daa37 | 71 | { |
oshin1030 | 2:aeae129daa37 | 72 | double theta_2; |
oshin1030 | 2:aeae129daa37 | 73 | theta_2 = atan((y - L_1 * sin(theta_1)) / (x - L_1 * cos(theta_1))) - theta_1; |
oshin1030 | 2:aeae129daa37 | 74 | return theta_2; |
yuki0108 | 0:cb9f2ec9d902 | 75 | } |
yuki0108 | 0:cb9f2ec9d902 | 76 | |
yuki0108 | 1:b74b31a7df07 | 77 | ////////////////////////////////////////////////////////////////////////////// |
yuki0108 | 0:cb9f2ec9d902 | 78 | |
yuki0108 | 1:b74b31a7df07 | 79 | int main () //調整時を想定しているのでデータを取っている。 |
yuki0108 | 1:b74b31a7df07 | 80 | { |
oshin1030 | 2:aeae129daa37 | 81 | motorA.setEquation(0.082047,0.020033,-0.08206,0.020087);//原点、速度制御のC値D値 |
oshin1030 | 2:aeae129daa37 | 82 | motorB.setEquation(0.096001,0.067528,-0.08753,0.043631);//肘関節、速度制御のC値D値 |
yuki0108 | 0:cb9f2ec9d902 | 83 | ////////////////////////////////////////////////////////////////////////////// |
oshin1030 | 2:aeae129daa37 | 84 | //int state=0; |
oshin1030 | 2:aeae129daa37 | 85 | int x; |
oshin1030 | 2:aeae129daa37 | 86 | double DegA; |
oshin1030 | 2:aeae129daa37 | 87 | double DegB; |
oshin1030 | 2:aeae129daa37 | 88 | double angleA; |
oshin1030 | 2:aeae129daa37 | 89 | double angleB; |
yuki0108 | 0:cb9f2ec9d902 | 90 | while(1) { |
oshin1030 | 2:aeae129daa37 | 91 | x = 0.5; |
oshin1030 | 2:aeae129daa37 | 92 | ecA.rewriteCount(); |
oshin1030 | 2:aeae129daa37 | 93 | ecB.rewriteCount(); |
oshin1030 | 2:aeae129daa37 | 94 | DegA = ecA.getDeg(); |
oshin1030 | 2:aeae129daa37 | 95 | DegB = ecB.getDeg(); |
oshin1030 | 2:aeae129daa37 | 96 | angleA = DegA/3; |
oshin1030 | 2:aeae129daa37 | 97 | angleB = DegB*36/60; |
oshin1030 | 2:aeae129daa37 | 98 | //double target_thetaA = theta1(x,function(x),angleA); |
oshin1030 | 2:aeae129daa37 | 99 | //double target_thetaB = theta2(x,function(x),target_thetaA); |
oshin1030 | 2:aeae129daa37 | 100 | //printf("%f\t%f\t%f\t%f\t%f\r\n",ecA.read(),ecA.getCount(),ecA.getDeg(),ecA.getOmega(),angleA,target_thetaA,target_thetaB); |
oshin1030 | 2:aeae129daa37 | 101 | |
oshin1030 | 2:aeae129daa37 | 102 | |
oshin1030 | 2:aeae129daa37 | 103 | |
oshin1030 | 2:aeae129daa37 | 104 | |
oshin1030 | 2:aeae129daa37 | 105 | |
oshin1030 | 2:aeae129daa37 | 106 | /* |
yuki0108 | 1:b74b31a7df07 | 107 | if(state==0) { |
yuki0108 | 1:b74b31a7df07 | 108 | if(toggle) {//スタート用のトグルスイッチ。要らない場合はこのif部分の処理をwhileの直前に |
yuki0108 | 1:b74b31a7df07 | 109 | state++; |
yuki0108 | 1:b74b31a7df07 | 110 | target_speed=5; //目標角速度を5rad/sに |
yuki0108 | 1:b74b31a7df07 | 111 | target_rad=(M_PI/6); //目標角度をπ/6(rad)に(単位に注意) |
yuki0108 | 1:b74b31a7df07 | 112 | |
yuki0108 | 1:b74b31a7df07 | 113 | timer_loop.reset(); |
yuki0108 | 1:b74b31a7df07 | 114 | timer_loop.start(); |
yuki0108 | 1:b74b31a7df07 | 115 | } |
yuki0108 | 1:b74b31a7df07 | 116 | } else if(state==1) { |
yuki0108 | 1:b74b31a7df07 | 117 | if(timer_loop.read()>DELTA_T) {//Tickerだと通信と割り込みが干渉してしまうのでタイマーで |
yuki0108 | 1:b74b31a7df07 | 118 | // speedControll();//速度制御 |
yuki0108 | 1:b74b31a7df07 | 119 | angleControll();//角度制御 |
yuki0108 | 1:b74b31a7df07 | 120 | |
yuki0108 | 1:b74b31a7df07 | 121 | timer_loop.reset(); |
yuki0108 | 1:b74b31a7df07 | 122 | } |
yuki0108 | 1:b74b31a7df07 | 123 | if(omega_count==NUM_DATA) {//データが集まったら表示 |
yuki0108 | 1:b74b31a7df07 | 124 | motor.stop(); |
yuki0108 | 1:b74b31a7df07 | 125 | displayData(); |
yuki0108 | 1:b74b31a7df07 | 126 | timer_loop.stop(); |
yuki0108 | 1:b74b31a7df07 | 127 | state++; |
yuki0108 | 1:b74b31a7df07 | 128 | } |
yuki0108 | 1:b74b31a7df07 | 129 | } |
oshin1030 | 2:aeae129daa37 | 130 | wait_us(10);*/ |
yuki0108 | 0:cb9f2ec9d902 | 131 | } |
yuki0108 | 0:cb9f2ec9d902 | 132 | } |
yuki0108 | 0:cb9f2ec9d902 | 133 |