7th_DENSOU / Mbed 2 deprecated Right_Arm

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?

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"
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 }