9/19
Dependencies: mbed KondoServoLibrary CalPID AMT21_right_arm ros_lib_melodic MotorController_AbsEC_right_arm
main.cpp@4:e20532e09d42, 2021-09-19 (annotated)
- Committer:
- oshin1030
- Date:
- Sun Sep 19 13:33:57 2021 +0000
- Revision:
- 4:e20532e09d42
- Parent:
- 3:ab266b418d90
9/19
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 | 3:ab266b418d90 | 5 | #include "KondoServo.h" |
oshin1030 | 4:e20532e09d42 | 6 | #include <ros.h> |
oshin1030 | 4:e20532e09d42 | 7 | #include <geometry_msgs/Point.h> |
oshin1030 | 3:ab266b418d90 | 8 | |
yuki0108 | 0:cb9f2ec9d902 | 9 | |
yuki0108 | 0:cb9f2ec9d902 | 10 | #define DELTA_T 0.01 //制御周期 |
yuki0108 | 0:cb9f2ec9d902 | 11 | #define DUTY_MAX 0.8 //duty比の最大値 |
yuki0108 | 1:b74b31a7df07 | 12 | #define OMEGA_MAX 15 //速度制御を利用した角度制御での角速度の最大値 |
oshin1030 | 3:ab266b418d90 | 13 | #define L_1 0.891 |
oshin1030 | 2:aeae129daa37 | 14 | #define L_2 0.820 |
oshin1030 | 3:ab266b418d90 | 15 | #define L_3 0.71362 |
yuki0108 | 0:cb9f2ec9d902 | 16 | |
yuki0108 | 0:cb9f2ec9d902 | 17 | #ifndef M_PI |
yuki0108 | 0:cb9f2ec9d902 | 18 | #define M_PI 3.14159265359f |
yuki0108 | 0:cb9f2ec9d902 | 19 | #endif |
yuki0108 | 0:cb9f2ec9d902 | 20 | |
yuki0108 | 1:b74b31a7df07 | 21 | Timer timer_loop; //制御周期用 |
oshin1030 | 3:ab266b418d90 | 22 | DigitalOut myled_1(LED1); |
oshin1030 | 3:ab266b418d90 | 23 | DigitalOut myled_2(LED2); |
oshin1030 | 3:ab266b418d90 | 24 | DigitalOut myled_3(LED3); |
oshin1030 | 3:ab266b418d90 | 25 | DigitalOut myled_4(LED4); |
yuki0108 | 0:cb9f2ec9d902 | 26 | |
yuki0108 | 0:cb9f2ec9d902 | 27 | ///////////////// 宣言部分 ////////////////////// |
yuki0108 | 1:b74b31a7df07 | 28 | CalPID speed_pid(0.2,0,0.00455,DELTA_T,DUTY_MAX); //速度制御のPD計算 |
oshin1030 | 3:ab266b418d90 | 29 | CalPID angle_duty_pid_1(0.5,0,0.00056,DELTA_T,DUTY_MAX); //角度制御(duty式)のPD計算 |
oshin1030 | 3:ab266b418d90 | 30 | CalPID angle_duty_pid_2(0.5,0.01,0.00056,DELTA_T,DUTY_MAX); //角度制御(duty式)のPD計算 |
oshin1030 | 3:ab266b418d90 | 31 | |
oshin1030 | 3:ab266b418d90 | 32 | CalPID angle_omega_pid_1(4.3,0,0.01,DELTA_T,OMEGA_MAX);//角度制御(速度式)のPD計算 原点 |
oshin1030 | 3:ab266b418d90 | 33 | CalPID angle_omega_pid_2(2.5,0,0.1,DELTA_T,OMEGA_MAX);//角度制御(速度式)のPD計算 肘関節 |
oshin1030 | 3:ab266b418d90 | 34 | |
oshin1030 | 3:ab266b418d90 | 35 | KondoServo servo(p13,p14,1,115200); |
oshin1030 | 3:ab266b418d90 | 36 | //Amt21 ecB(p28,p27,p8);//肘関節 |
oshin1030 | 3:ab266b418d90 | 37 | Amt21 ecA(p28,p27,p8); //原点 |
oshin1030 | 3:ab266b418d90 | 38 | Amt21 ecB(p9,p10,p15);//肘関節 |
yuki0108 | 0:cb9f2ec9d902 | 39 | // 上で宣言したインスタンスをMotorControllerに与える // |
oshin1030 | 3:ab266b418d90 | 40 | MotorController motorA(p25,p26,DELTA_T,ecA,speed_pid,angle_duty_pid_1,angle_omega_pid_1);//原点 |
oshin1030 | 3:ab266b418d90 | 41 | MotorController motorB(p24,p23,DELTA_T,ecB,speed_pid,angle_duty_pid_2,angle_omega_pid_2);//肘関節 |
oshin1030 | 3:ab266b418d90 | 42 | PwmOut motorR(p22); //fan |
oshin1030 | 3:ab266b418d90 | 43 | PwmOut motorL(p21); |
yuki0108 | 0:cb9f2ec9d902 | 44 | ////////////////////////////////////////////////////////////////////////// |
yuki0108 | 0:cb9f2ec9d902 | 45 | |
oshin1030 | 3:ab266b418d90 | 46 | double deg_rad_convert(double deg) |
oshin1030 | 3:ab266b418d90 | 47 | { |
oshin1030 | 3:ab266b418d90 | 48 | return deg*M_PI/180; |
oshin1030 | 3:ab266b418d90 | 49 | } |
yuki0108 | 0:cb9f2ec9d902 | 50 | |
oshin1030 | 3:ab266b418d90 | 51 | double rad_deg_convert(double rad) |
yuki0108 | 0:cb9f2ec9d902 | 52 | { |
oshin1030 | 3:ab266b418d90 | 53 | return rad*180/M_PI; |
oshin1030 | 2:aeae129daa37 | 54 | } |
yuki0108 | 1:b74b31a7df07 | 55 | |
oshin1030 | 3:ab266b418d90 | 56 | double theta1(double x, double y, double theta_2)//原点角度計算 |
oshin1030 | 3:ab266b418d90 | 57 | { |
oshin1030 | 3:ab266b418d90 | 58 | //(x:目標座標 y:目標座標) |
oshin1030 | 3:ab266b418d90 | 59 | double theta_1; |
oshin1030 | 3:ab266b418d90 | 60 | theta_1 = atan(y/x)-atan(L_2*sin(theta_1)/(L_1+L_2*cos(theta_2))); |
oshin1030 | 3:ab266b418d90 | 61 | //printf("%f\r\n",theta_1); |
oshin1030 | 3:ab266b418d90 | 62 | return theta_1; |
oshin1030 | 3:ab266b418d90 | 63 | } |
oshin1030 | 3:ab266b418d90 | 64 | |
oshin1030 | 3:ab266b418d90 | 65 | double theta2(double x, double y)//肘関節角度計算 |
oshin1030 | 3:ab266b418d90 | 66 | { |
oshin1030 | 3:ab266b418d90 | 67 | double theta_2; |
oshin1030 | 3:ab266b418d90 | 68 | theta_2 = acos((x*x+y*y-L_1*L_1-L_2*L_2)/(2*L_1*L_2)); |
oshin1030 | 3:ab266b418d90 | 69 | //printf("%f ",theta_2); |
oshin1030 | 3:ab266b418d90 | 70 | return theta_2; |
oshin1030 | 2:aeae129daa37 | 71 | } |
oshin1030 | 2:aeae129daa37 | 72 | |
oshin1030 | 4:e20532e09d42 | 73 | double robot_x, robot_y,robot_theta; //ロボット座標 |
oshin1030 | 3:ab266b418d90 | 74 | double pot_x, pot_y; //ポット座標 |
oshin1030 | 4:e20532e09d42 | 75 | int fan_switch = -1, arm_switch =0; //扇風機on/off, アーム関節on/off |
oshin1030 | 4:e20532e09d42 | 76 | |
oshin1030 | 4:e20532e09d42 | 77 | void robotCallback(const geometry_msgs::Point &robot_pos) |
oshin1030 | 4:e20532e09d42 | 78 | { |
oshin1030 | 4:e20532e09d42 | 79 | robot_x = robot_pos.x; |
oshin1030 | 4:e20532e09d42 | 80 | robot_y = robot_pos.y; |
oshin1030 | 4:e20532e09d42 | 81 | robot_theta = robot_pos.z; |
oshin1030 | 3:ab266b418d90 | 82 | |
oshin1030 | 4:e20532e09d42 | 83 | } |
oshin1030 | 4:e20532e09d42 | 84 | void potCallback(const geometry_msgs::Point &pot_pos) |
oshin1030 | 4:e20532e09d42 | 85 | { |
oshin1030 | 4:e20532e09d42 | 86 | pot_x = pot_pos.x; |
oshin1030 | 4:e20532e09d42 | 87 | pot_y = pot_pos.y; |
oshin1030 | 4:e20532e09d42 | 88 | } |
oshin1030 | 4:e20532e09d42 | 89 | void switchCallback(const geometry_msgs::Point &button_fa) |
oshin1030 | 4:e20532e09d42 | 90 | { |
oshin1030 | 4:e20532e09d42 | 91 | fan_switch = button_fa.x; //fan |
oshin1030 | 4:e20532e09d42 | 92 | arm_switch = button_fa.y; //arm |
oshin1030 | 4:e20532e09d42 | 93 | } |
oshin1030 | 4:e20532e09d42 | 94 | |
yuki0108 | 0:cb9f2ec9d902 | 95 | |
yuki0108 | 1:b74b31a7df07 | 96 | ////////////////////////////////////////////////////////////////////////////// |
oshin1030 | 4:e20532e09d42 | 97 | ros::NodeHandle nh; |
oshin1030 | 4:e20532e09d42 | 98 | ros::Subscriber<geometry_msgs::Point> sub_robo_posi("/posi_dr",&robotCallback); |
oshin1030 | 4:e20532e09d42 | 99 | ros::Subscriber<geometry_msgs::Point> sub_pot_posi("/posi_pot",&potCallback); |
oshin1030 | 4:e20532e09d42 | 100 | ros::Subscriber<geometry_msgs::Point> sub_switch("/fan_switch",&switchCallback); |
yuki0108 | 0:cb9f2ec9d902 | 101 | |
oshin1030 | 4:e20532e09d42 | 102 | geometry_msgs::Point relative_posi_pot; |
oshin1030 | 4:e20532e09d42 | 103 | geometry_msgs::Point target_kakudo; |
oshin1030 | 4:e20532e09d42 | 104 | ros::Publisher pub_pot("/relative_pot",&relative_posi_pot); |
oshin1030 | 4:e20532e09d42 | 105 | ros::Publisher pub_theta("/target_theta",&target_kakudo); |
oshin1030 | 3:ab266b418d90 | 106 | |
oshin1030 | 3:ab266b418d90 | 107 | int main () |
yuki0108 | 1:b74b31a7df07 | 108 | { |
oshin1030 | 4:e20532e09d42 | 109 | |
oshin1030 | 4:e20532e09d42 | 110 | nh.getHardware()->setBaud(115200); |
oshin1030 | 4:e20532e09d42 | 111 | nh.initNode(); |
oshin1030 | 4:e20532e09d42 | 112 | nh.subscribe(sub_robo_posi); |
oshin1030 | 4:e20532e09d42 | 113 | nh.subscribe(sub_pot_posi); |
oshin1030 | 4:e20532e09d42 | 114 | nh.subscribe(sub_switch); |
oshin1030 | 4:e20532e09d42 | 115 | nh.advertise(pub_pot); |
oshin1030 | 4:e20532e09d42 | 116 | nh.advertise(pub_theta); |
oshin1030 | 4:e20532e09d42 | 117 | NVIC_SetPriority(TIMER3_IRQn, 5); |
oshin1030 | 3:ab266b418d90 | 118 | |
oshin1030 | 2:aeae129daa37 | 119 | motorA.setEquation(0.082047,0.020033,-0.08206,0.020087);//原点、速度制御のC値D値 |
oshin1030 | 2:aeae129daa37 | 120 | motorB.setEquation(0.096001,0.067528,-0.08753,0.043631);//肘関節、速度制御のC値D値 |
oshin1030 | 3:ab266b418d90 | 121 | motorR.period_us(50); |
oshin1030 | 3:ab266b418d90 | 122 | motorL.period_us(50); |
yuki0108 | 0:cb9f2ec9d902 | 123 | ////////////////////////////////////////////////////////////////////////////// |
oshin1030 | 3:ab266b418d90 | 124 | int mode_1 = 1; //arm_switch = 1 用 |
oshin1030 | 4:e20532e09d42 | 125 | int mode_0 = 4; //arm_switch = 0 用 |
oshin1030 | 3:ab266b418d90 | 126 | double DegA,DegB; |
oshin1030 | 3:ab266b418d90 | 127 | double angleA,angleB; |
oshin1030 | 3:ab266b418d90 | 128 | double radA,radB; |
oshin1030 | 3:ab266b418d90 | 129 | double start_theta_A, start_theta_B; //初期角度 |
oshin1030 | 4:e20532e09d42 | 130 | double target_theta_A=0,target_theta_B=0;//目標角度(robot_xy, pot_xyから計算) |
oshin1030 | 4:e20532e09d42 | 131 | double relative_pot_x,relative_pot_y;//アーム原点から見たポットの相対座標 |
oshin1030 | 3:ab266b418d90 | 132 | int servo_speed = 37; |
oshin1030 | 3:ab266b418d90 | 133 | int servo_degree; |
oshin1030 | 4:e20532e09d42 | 134 | target_kakudo.x=0; |
oshin1030 | 4:e20532e09d42 | 135 | target_kakudo.y=0; |
oshin1030 | 3:ab266b418d90 | 136 | |
oshin1030 | 3:ab266b418d90 | 137 | double target_x = 1.05; //ポット相対座標x |
oshin1030 | 3:ab266b418d90 | 138 | double target_y = 1.05; //ポット相対座標y |
oshin1030 | 3:ab266b418d90 | 139 | target_theta_B = theta2(target_x,target_y); //目標角度計算(第二関節) |
oshin1030 | 3:ab266b418d90 | 140 | target_theta_A = theta1(target_x,target_y,target_theta_B); //目標角度計算(第一関節) |
oshin1030 | 3:ab266b418d90 | 141 | servo_degree = 148 - (int)(0.7111*rad_deg_convert(target_theta_B)); //目標位置(サーボモータ) |
oshin1030 | 3:ab266b418d90 | 142 | target_theta_B = deg_rad_convert(5.0); //第二関節決め打ち(実験用) |
oshin1030 | 4:e20532e09d42 | 143 | target_theta_A = deg_rad_convert(45.0); //第一関節決め打ち(実験用) |
oshin1030 | 3:ab266b418d90 | 144 | //printf("%f\t%f\r\n",target_theta_A,target_theta_B); |
oshin1030 | 4:e20532e09d42 | 145 | start_theta_A = deg_rad_convert(85); //初期角度 |
oshin1030 | 4:e20532e09d42 | 146 | start_theta_B = deg_rad_convert(-140); //初期角度 |
oshin1030 | 3:ab266b418d90 | 147 | wait(3); |
oshin1030 | 3:ab266b418d90 | 148 | |
yuki0108 | 0:cb9f2ec9d902 | 149 | while(1) { |
oshin1030 | 4:e20532e09d42 | 150 | nh.spinOnce(); |
oshin1030 | 3:ab266b418d90 | 151 | wait_ms(1); |
oshin1030 | 3:ab266b418d90 | 152 | ecA.rewriteCount_1(); |
oshin1030 | 3:ab266b418d90 | 153 | ecB.rewriteCount_2(); |
oshin1030 | 3:ab266b418d90 | 154 | DegA = ecA.getDeg(); |
oshin1030 | 3:ab266b418d90 | 155 | DegB = ecB.getDeg(); |
oshin1030 | 3:ab266b418d90 | 156 | radA = ecA.getRad()/3 + deg_rad_convert(159.72); |
oshin1030 | 3:ab266b418d90 | 157 | if(radA >= 0) { |
oshin1030 | 4:e20532e09d42 | 158 | radB = ecB.getRad()*36/60 - deg_rad_convert(115 + angleA/3 + angleA*16.63/90); |
oshin1030 | 3:ab266b418d90 | 159 | } else if(radA < 0) { |
oshin1030 | 4:e20532e09d42 | 160 | radB = ecB.getRad()*36/60 - deg_rad_convert(115 + angleA/3 - angleA*16.63/90); |
oshin1030 | 3:ab266b418d90 | 161 | } |
oshin1030 | 3:ab266b418d90 | 162 | angleA = DegA/3 + 159.72; |
oshin1030 | 3:ab266b418d90 | 163 | if(angleA >= 0) { |
oshin1030 | 4:e20532e09d42 | 164 | angleB = DegB*36/60 - 115 - angleA/3 - angleA*16.63/90; |
oshin1030 | 3:ab266b418d90 | 165 | } else if(angleA < 0) { |
oshin1030 | 4:e20532e09d42 | 166 | angleB = DegB*36/60 - 115 - angleA/3 + angleA*16.63/90; |
oshin1030 | 3:ab266b418d90 | 167 | } |
oshin1030 | 2:aeae129daa37 | 168 | |
oshin1030 | 4:e20532e09d42 | 169 | ///////////////目標角度を常に計算する////////////////// |
oshin1030 | 4:e20532e09d42 | 170 | relative_pot_x = 6.0 - (robot_x + (0.185*cos(-robot_theta) + 0.29275*sin(-robot_theta))); //potの相対座標x(目標座標) |
oshin1030 | 4:e20532e09d42 | 171 | relative_pot_y = 8.0 - (robot_y + (0.185*sin(-robot_theta) - 0.29275*cos(-robot_theta))); //potの相対座標y(目標座標) |
oshin1030 | 4:e20532e09d42 | 172 | target_theta_B = theta2(relative_pot_x,relative_pot_y); //目標角度計算(第二関節) |
oshin1030 | 4:e20532e09d42 | 173 | target_theta_A = theta1(relative_pot_x,relative_pot_y,target_theta_B); //目標角度計算(第一関節) |
oshin1030 | 4:e20532e09d42 | 174 | |
oshin1030 | 4:e20532e09d42 | 175 | relative_posi_pot.x = relative_pot_x; |
oshin1030 | 4:e20532e09d42 | 176 | relative_posi_pot.y = relative_pot_y; |
oshin1030 | 4:e20532e09d42 | 177 | if(target_theta_A){ |
oshin1030 | 4:e20532e09d42 | 178 | |
oshin1030 | 4:e20532e09d42 | 179 | target_kakudo.x = double(target_theta_A); |
oshin1030 | 4:e20532e09d42 | 180 | target_kakudo.y = double(target_theta_B); |
oshin1030 | 4:e20532e09d42 | 181 | } |
oshin1030 | 4:e20532e09d42 | 182 | target_kakudo.z=0; |
oshin1030 | 4:e20532e09d42 | 183 | pub_pot.publish(&relative_posi_pot); |
oshin1030 | 4:e20532e09d42 | 184 | pub_theta.publish(&target_kakudo); |
oshin1030 | 2:aeae129daa37 | 185 | |
oshin1030 | 3:ab266b418d90 | 186 | if(arm_switch == 1) { |
oshin1030 | 4:e20532e09d42 | 187 | //printf("%f\t%f\t%f\t%f\t%f\t%f\r\n",angleA,angleB,radA,radB,target_theta_A,target_theta_B); |
oshin1030 | 3:ab266b418d90 | 188 | myled_2 = 1; |
oshin1030 | 3:ab266b418d90 | 189 | mode_0 = 1; |
oshin1030 | 3:ab266b418d90 | 190 | if(mode_1 == 1) { |
oshin1030 | 3:ab266b418d90 | 191 | motorA.AcOmega_1(target_theta_A,0.2); |
oshin1030 | 3:ab266b418d90 | 192 | motorB.AcOmega_2(target_theta_B,angleA,0.2); |
oshin1030 | 4:e20532e09d42 | 193 | if(fabs(target_theta_A - radA) < 0.01 && fabs(target_theta_B - radB) < 0.01) { //収束判定 |
oshin1030 | 3:ab266b418d90 | 194 | mode_1++; |
oshin1030 | 3:ab266b418d90 | 195 | } |
oshin1030 | 4:e20532e09d42 | 196 | //printf("1_1 "); |
oshin1030 | 4:e20532e09d42 | 197 | } else if(mode_1 == 2) {//最初にswitchをオンにした後のため、最大dutyを下げておく |
oshin1030 | 4:e20532e09d42 | 198 | //printf("1_2 "); |
oshin1030 | 4:e20532e09d42 | 199 | motorA.AcOmega_1(target_theta_A,0.1);//更新し続ける目標値に常に移動 |
oshin1030 | 4:e20532e09d42 | 200 | motorB.AcOmega_2(target_theta_B,angleA,0.1); //更新し続ける目標値に常に移動 |
yuki0108 | 1:b74b31a7df07 | 201 | } |
oshin1030 | 3:ab266b418d90 | 202 | } else if(arm_switch == 0) { |
oshin1030 | 4:e20532e09d42 | 203 | //printf("%f\t%f\t%f\t%f\t%f\t%f\r\n",angleA,angleB,radA,radB,start_theta_A,start_theta_B); |
oshin1030 | 4:e20532e09d42 | 204 | myled_2 = 0; |
oshin1030 | 3:ab266b418d90 | 205 | mode_1 = 1; |
oshin1030 | 3:ab266b418d90 | 206 | if(mode_0 == 1) { |
oshin1030 | 4:e20532e09d42 | 207 | motorA.AcOmega_1(start_theta_A,0.2); //最初の位置に移動 |
oshin1030 | 4:e20532e09d42 | 208 | motorB.AcOmega_2(start_theta_B,angleA,0.2);//最初の位置に移動 |
oshin1030 | 4:e20532e09d42 | 209 | if(fabs(start_theta_B - radB) < 0.01 && fabs(start_theta_A - radA) < 0.01) { //収束判定 |
oshin1030 | 3:ab266b418d90 | 210 | mode_0++; |
oshin1030 | 3:ab266b418d90 | 211 | } |
oshin1030 | 4:e20532e09d42 | 212 | //printf("0_1 "); |
oshin1030 | 4:e20532e09d42 | 213 | } else if(mode_0 == 2) { |
oshin1030 | 4:e20532e09d42 | 214 | motorA.AcOmega_1(start_theta_A,0.01); |
oshin1030 | 4:e20532e09d42 | 215 | motorB.AcOmega_2(start_theta_B,angleA,0.04); |
oshin1030 | 4:e20532e09d42 | 216 | //printf("0_2 "); |
oshin1030 | 4:e20532e09d42 | 217 | } else {} |
yuki0108 | 1:b74b31a7df07 | 218 | } |
oshin1030 | 3:ab266b418d90 | 219 | if(fan_switch == 0) { |
oshin1030 | 3:ab266b418d90 | 220 | myled_1 = 0; |
oshin1030 | 3:ab266b418d90 | 221 | motorR = 0; |
oshin1030 | 3:ab266b418d90 | 222 | } else if (fan_switch == 1) { |
oshin1030 | 3:ab266b418d90 | 223 | myled_1 = 1; |
oshin1030 | 4:e20532e09d42 | 224 | motorR = 0.5; //扇風機回す |
oshin1030 | 4:e20532e09d42 | 225 | //printf("fan_on "); |
oshin1030 | 4:e20532e09d42 | 226 | } else if (fan_switch == 2){ |
oshin1030 | 4:e20532e09d42 | 227 | myled_1 = !myled_1; //点滅 |
oshin1030 | 4:e20532e09d42 | 228 | motorL = 0.5; |
oshin1030 | 3:ab266b418d90 | 229 | } |
yuki0108 | 0:cb9f2ec9d902 | 230 | } |
oshin1030 | 3:ab266b418d90 | 231 | } |