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@3:ab266b418d90, 2021-09-10 (annotated)

Committer:
oshin1030
Date:
Fri Sep 10 09:02:52 2021 +0000
Revision:
3:ab266b418d90
Parent:
2:aeae129daa37
Child:
4:e20532e09d42
9/10

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 3:ab266b418d90 6 //#include <ros.h>
oshin1030 3:ab266b418d90 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
yuki0108 0:cb9f2ec9d902 56 void angleControll()
yuki0108 0:cb9f2ec9d902 57 {
oshin1030 2:aeae129daa37 58 //motorA.AcOmega_1(target_rad); //duty式角度制御
oshin1030 2:aeae129daa37 59 //motorB.AcOmega_2(target_rad); //速度式角度制御
oshin1030 2:aeae129daa37 60 }
oshin1030 2:aeae129daa37 61
oshin1030 3:ab266b418d90 62
oshin1030 2:aeae129daa37 63 double function(double x)
oshin1030 2:aeae129daa37 64 {
oshin1030 2:aeae129daa37 65 double y;
oshin1030 3:ab266b418d90 66 y = 0.6 + sqrt(0.01 - (x-0.6)*(x-0.6));//円の軌道
oshin1030 2:aeae129daa37 67 return y;
oshin1030 2:aeae129daa37 68 }
yuki0108 1:b74b31a7df07 69
oshin1030 3:ab266b418d90 70 double theta1(double x, double y, double theta_2)//原点角度計算
oshin1030 3:ab266b418d90 71 {
oshin1030 3:ab266b418d90 72 //(x:目標座標 y:目標座標)
oshin1030 3:ab266b418d90 73 double theta_1;
oshin1030 3:ab266b418d90 74 theta_1 = atan(y/x)-atan(L_2*sin(theta_1)/(L_1+L_2*cos(theta_2)));
oshin1030 3:ab266b418d90 75 //printf("%f\r\n",theta_1);
oshin1030 3:ab266b418d90 76 return theta_1;
oshin1030 3:ab266b418d90 77 }
oshin1030 3:ab266b418d90 78
oshin1030 3:ab266b418d90 79 double theta2(double x, double y)//肘関節角度計算
oshin1030 3:ab266b418d90 80 {
oshin1030 3:ab266b418d90 81 double theta_2;
oshin1030 3:ab266b418d90 82 theta_2 = acos((x*x+y*y-L_1*L_1-L_2*L_2)/(2*L_1*L_2));
oshin1030 3:ab266b418d90 83 //printf("%f ",theta_2);
oshin1030 3:ab266b418d90 84 return theta_2;
oshin1030 2:aeae129daa37 85 }
oshin1030 2:aeae129daa37 86
oshin1030 3:ab266b418d90 87 double robot_x, robot_y; //ロボット座標
oshin1030 3:ab266b418d90 88 double pot_x, pot_y; //ポット座標
oshin1030 3:ab266b418d90 89 int fan_switch = -1, arm_switch = 0; //扇風機on/off, アーム関節on/off
oshin1030 3:ab266b418d90 90
oshin1030 3:ab266b418d90 91 //void robotCallback(const geometry_msgs::Point &robot_pos)
oshin1030 3:ab266b418d90 92 //{
oshin1030 3:ab266b418d90 93 // robot_x = robot_pos.x;
oshin1030 3:ab266b418d90 94 // robot_y = robot_pos.y;
oshin1030 3:ab266b418d90 95 //
oshin1030 3:ab266b418d90 96 //}
oshin1030 3:ab266b418d90 97 //void potCallback(const geometry_msgs::Point &pot_pos)
oshin1030 3:ab266b418d90 98 //{
oshin1030 3:ab266b418d90 99 // pot_x = pot_pos.x;
oshin1030 3:ab266b418d90 100 // pot_y = pot_pos.y;
oshin1030 3:ab266b418d90 101 //}
oshin1030 3:ab266b418d90 102 //void switchCallback(const geometry_msgs::Point &button_fa)
oshin1030 3:ab266b418d90 103 //{
oshin1030 3:ab266b418d90 104 // fan_switch = button_fa.x; //fan
oshin1030 3:ab266b418d90 105 // arm_switch = button_fa.y; //arm
oshin1030 3:ab266b418d90 106 //}
yuki0108 0:cb9f2ec9d902 107
yuki0108 1:b74b31a7df07 108 //////////////////////////////////////////////////////////////////////////////
yuki0108 0:cb9f2ec9d902 109
oshin1030 3:ab266b418d90 110 float plot[]= {0.6,0.1,0.6,0.1};
oshin1030 3:ab266b418d90 111 double x[20];
oshin1030 3:ab266b418d90 112 double target_radA[20];
oshin1030 3:ab266b418d90 113 double target_radB[20];
oshin1030 3:ab266b418d90 114
oshin1030 3:ab266b418d90 115 //ros::NodeHandle nh;
oshin1030 3:ab266b418d90 116 //ros::Subscriber<geometry_msgs::Point> sub_robo_posi("/robot",&robotCallback);
oshin1030 3:ab266b418d90 117 //ros::Subscriber<geometry_msgs::Point> sub_pot_posi("/posi_pot",&potCallback);
oshin1030 3:ab266b418d90 118 //ros::Subscriber<geometry_msgs::Point> sub_switch("/fan_switch",&switchCallback);
oshin1030 3:ab266b418d90 119
oshin1030 3:ab266b418d90 120 int main ()
yuki0108 1:b74b31a7df07 121 {
oshin1030 3:ab266b418d90 122
oshin1030 3:ab266b418d90 123 // nh.getHardware()->setBaud(115200);
oshin1030 3:ab266b418d90 124 // nh.initNode();
oshin1030 3:ab266b418d90 125 // nh.subscribe(sub_robo_posi);
oshin1030 3:ab266b418d90 126 // nh.subscribe(sub_pot_posi);
oshin1030 3:ab266b418d90 127 // nh.subscribe(sub_switch);
oshin1030 3:ab266b418d90 128 // NVIC_SetPriority(TIMER3_IRQn, 5);
oshin1030 3:ab266b418d90 129
oshin1030 2:aeae129daa37 130 motorA.setEquation(0.082047,0.020033,-0.08206,0.020087);//原点、速度制御のC値D値
oshin1030 2:aeae129daa37 131 motorB.setEquation(0.096001,0.067528,-0.08753,0.043631);//肘関節、速度制御のC値D値
oshin1030 3:ab266b418d90 132 motorR.period_us(50);
oshin1030 3:ab266b418d90 133 motorL.period_us(50);
yuki0108 0:cb9f2ec9d902 134 //////////////////////////////////////////////////////////////////////////////
oshin1030 3:ab266b418d90 135 int i = 0;
oshin1030 3:ab266b418d90 136 int mode_1 = 1; //arm_switch = 1 用
oshin1030 3:ab266b418d90 137 int mode_0 = 1; //arm_switch = 0 用
oshin1030 3:ab266b418d90 138 double Deg,angle,Count,rad;
oshin1030 3:ab266b418d90 139 double DegA,DegB;
oshin1030 3:ab266b418d90 140 double angleA,angleB;
oshin1030 3:ab266b418d90 141 double CountA,CountB;
oshin1030 3:ab266b418d90 142 double radA,radB;
oshin1030 3:ab266b418d90 143 double start_theta_A, start_theta_B; //初期角度
oshin1030 3:ab266b418d90 144 double target_theta_A,target_theta_B;//目標角度(robot_xy, pot_xyから計算)
oshin1030 3:ab266b418d90 145 int servo_speed = 37;
oshin1030 3:ab266b418d90 146 int servo_degree;
oshin1030 3:ab266b418d90 147 static bool IsFirst = true;
oshin1030 3:ab266b418d90 148 int M=0,N=0;
oshin1030 3:ab266b418d90 149 /*
oshin1030 3:ab266b418d90 150 for(int i = 0; i < 20; i++) {//軌道のプロット
oshin1030 3:ab266b418d90 151 x[i] = 0.5 + 0.01*i;
oshin1030 3:ab266b418d90 152 target_radA[i] = theta1(x[i],function(x[i]),20.0);
oshin1030 3:ab266b418d90 153 target_radB[i] = theta2(x[i],function(x[i]),target_radA[i]);
oshin1030 3:ab266b418d90 154 target_radA[i] = -target_radA[i];
oshin1030 3:ab266b418d90 155 target_radB[i] = target_radB[i] - 1;
oshin1030 3:ab266b418d90 156 printf("%f\t%f\t%f\r\n",x[i],target_radA[i],target_radB[i]);
oshin1030 3:ab266b418d90 157 }*/
oshin1030 3:ab266b418d90 158
oshin1030 3:ab266b418d90 159 double target_x = 1.05; //ポット相対座標x
oshin1030 3:ab266b418d90 160 double target_y = 1.05; //ポット相対座標y
oshin1030 3:ab266b418d90 161 target_theta_B = theta2(target_x,target_y); //目標角度計算(第二関節)
oshin1030 3:ab266b418d90 162 target_theta_A = theta1(target_x,target_y,target_theta_B); //目標角度計算(第一関節)
oshin1030 3:ab266b418d90 163 servo_degree = 148 - (int)(0.7111*rad_deg_convert(target_theta_B)); //目標位置(サーボモータ)
oshin1030 3:ab266b418d90 164 target_theta_B = deg_rad_convert(5.0); //第二関節決め打ち(実験用)
oshin1030 3:ab266b418d90 165 target_theta_A =deg_rad_convert(45.0); //第一関節決め打ち(実験用)
oshin1030 3:ab266b418d90 166 //printf("%f\t%f\r\n",target_theta_A,target_theta_B);
oshin1030 3:ab266b418d90 167 start_theta_A = deg_rad_convert(95);
oshin1030 3:ab266b418d90 168 start_theta_B = deg_rad_convert(-165);
oshin1030 3:ab266b418d90 169 wait(3);
oshin1030 3:ab266b418d90 170
yuki0108 0:cb9f2ec9d902 171 while(1) {
oshin1030 3:ab266b418d90 172 /*
oshin1030 3:ab266b418d90 173 ecA.rewriteCount_1();
oshin1030 3:ab266b418d90 174 ecB.rewriteCount_2();
oshin1030 2:aeae129daa37 175 DegA = ecA.getDeg();
oshin1030 2:aeae129daa37 176 DegB = ecB.getDeg();
oshin1030 3:ab266b418d90 177 radA = ecA.getRad()/3 + deg_rad_convert(159.72);
oshin1030 3:ab266b418d90 178 if(radA >= 0){
oshin1030 3:ab266b418d90 179 radB = ecB.getRad()*36/60 - deg_rad_convert(102 + angleA/3 + angleA*16.63/90);
oshin1030 3:ab266b418d90 180 }else if(radA < 0){
oshin1030 3:ab266b418d90 181 radB = ecB.getRad()*36/60 - deg_rad_convert(102 + angleA/3 - angleA*16.63/90);
oshin1030 3:ab266b418d90 182 }
oshin1030 3:ab266b418d90 183 angleA = DegA/3 + 159.72;
oshin1030 3:ab266b418d90 184 if(angleA >= 0){
oshin1030 3:ab266b418d90 185 angleB = DegB*36/60 - angleA/3-102-angleA*16.63/90;
oshin1030 3:ab266b418d90 186 }else if(angleA < 0){
oshin1030 3:ab266b418d90 187 angleB = DegB*36/60 - angleA/3-102+angleA*16.63/90;
oshin1030 3:ab266b418d90 188 }
oshin1030 3:ab266b418d90 189
oshin1030 3:ab266b418d90 190 CountA = ecA.getCount();
oshin1030 3:ab266b418d90 191 CountB = ecB.getCount();
oshin1030 3:ab266b418d90 192 printf("%f\t%f\t%f\t%f\t%f\t%f\r\n",DegA,DegB,CountA,CountB,angleA,angleB);
oshin1030 3:ab266b418d90 193 */
oshin1030 3:ab266b418d90 194 //nh.spinOnce();
oshin1030 3:ab266b418d90 195 wait_ms(1);
oshin1030 3:ab266b418d90 196 ecA.rewriteCount_1();
oshin1030 3:ab266b418d90 197 ecB.rewriteCount_2();
oshin1030 3:ab266b418d90 198 DegA = ecA.getDeg();
oshin1030 3:ab266b418d90 199 DegB = ecB.getDeg();
oshin1030 3:ab266b418d90 200 radA = ecA.getRad()/3 + deg_rad_convert(159.72);
oshin1030 3:ab266b418d90 201 if(radA >= 0) {
oshin1030 3:ab266b418d90 202 radB = ecB.getRad()*36/60 - deg_rad_convert(102 + angleA/3 + angleA*16.63/90);
oshin1030 3:ab266b418d90 203 } else if(radA < 0) {
oshin1030 3:ab266b418d90 204 radB = ecB.getRad()*36/60 - deg_rad_convert(102 + angleA/3 - angleA*16.63/90);
oshin1030 3:ab266b418d90 205 }
oshin1030 3:ab266b418d90 206 angleA = DegA/3 + 159.72;
oshin1030 3:ab266b418d90 207 if(angleA >= 0) {
oshin1030 3:ab266b418d90 208 angleB = DegB*36/60 - 102 - angleA/3 - angleA*16.63/90;
oshin1030 3:ab266b418d90 209 } else if(angleA < 0) {
oshin1030 3:ab266b418d90 210 angleB = DegB*36/60 - 102 - angleA/3 + angleA*16.63/90;
oshin1030 3:ab266b418d90 211 }
oshin1030 2:aeae129daa37 212
oshin1030 2:aeae129daa37 213
oshin1030 3:ab266b418d90 214 if(arm_switch == 1) {
oshin1030 3:ab266b418d90 215 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 216 myled_2 = 1;
oshin1030 3:ab266b418d90 217 mode_0 = 1;
oshin1030 3:ab266b418d90 218 if(mode_1 == 1) {
oshin1030 3:ab266b418d90 219 motorA.AcOmega_1(target_theta_A,0.2);
oshin1030 3:ab266b418d90 220 if(fabs(target_theta_A - radA) < 0.01) { //収束判定
oshin1030 3:ab266b418d90 221 mode_1++;
oshin1030 3:ab266b418d90 222 }
oshin1030 3:ab266b418d90 223 printf("1_1 ");
oshin1030 3:ab266b418d90 224 } else if(mode_1 == 2) {
oshin1030 3:ab266b418d90 225 motorB.AcOmega_2(target_theta_B,angleA,0.2);
oshin1030 3:ab266b418d90 226 if(fabs(target_theta_B - radB) < 0.01) { //収束判定
oshin1030 3:ab266b418d90 227 mode_1++;
oshin1030 3:ab266b418d90 228 }
oshin1030 3:ab266b418d90 229 printf("1_2 ");
oshin1030 3:ab266b418d90 230 } else if(mode_1 == 3) {
oshin1030 3:ab266b418d90 231 mode_1++;
oshin1030 3:ab266b418d90 232 printf("1_3 ");
oshin1030 3:ab266b418d90 233 } else if(mode_1 == 4) {
oshin1030 3:ab266b418d90 234 printf("1_4 ");
oshin1030 3:ab266b418d90 235 motorA.AcOmega_1(target_theta_A,0.05); //位置固定
oshin1030 3:ab266b418d90 236 motorB.AcOmega_2(target_theta_B,angleA,0.05); //位置固定
yuki0108 1:b74b31a7df07 237
oshin1030 3:ab266b418d90 238 if(fabs(target_theta_A - radA) < 0.001) { //収束判定
oshin1030 3:ab266b418d90 239 target_theta_B = target_theta_B + 0.01744;
oshin1030 3:ab266b418d90 240 M++;
oshin1030 3:ab266b418d90 241 if(M == 2) {
oshin1030 3:ab266b418d90 242 target_theta_B = target_theta_B - 0.01744*2;
oshin1030 3:ab266b418d90 243 M = 0;
oshin1030 3:ab266b418d90 244 }
oshin1030 3:ab266b418d90 245 }
oshin1030 3:ab266b418d90 246 if(fabs(target_theta_B - radB) < 0.001) { //収束判定
oshin1030 3:ab266b418d90 247 target_theta_B = target_theta_B + 0.01744;
oshin1030 3:ab266b418d90 248 N++;
oshin1030 3:ab266b418d90 249 if(N == 2) {
oshin1030 3:ab266b418d90 250 target_theta_B = target_theta_B - 0.01744*2;
oshin1030 3:ab266b418d90 251 N = 0;
oshin1030 3:ab266b418d90 252 }
oshin1030 3:ab266b418d90 253 }
yuki0108 1:b74b31a7df07 254 }
oshin1030 3:ab266b418d90 255 } else if(arm_switch == 0) {
oshin1030 3:ab266b418d90 256 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 3:ab266b418d90 257 myled_2 = !myled_2;
oshin1030 3:ab266b418d90 258 mode_1 = 1;
oshin1030 3:ab266b418d90 259 if(mode_0 == 1) {
oshin1030 3:ab266b418d90 260 motorA.AcOmega_1(start_theta_A,0.2);
oshin1030 3:ab266b418d90 261 if(fabs(start_theta_A - radA) < 0.01) { //収束判定
oshin1030 3:ab266b418d90 262 mode_0++;
oshin1030 3:ab266b418d90 263 }
oshin1030 3:ab266b418d90 264 printf("0_1 ");
oshin1030 3:ab266b418d90 265 } else if(mode_0 == 2) {
oshin1030 3:ab266b418d90 266 motorB.AcOmega_2(start_theta_B,angleA,0.2);
oshin1030 3:ab266b418d90 267 if(fabs(start_theta_B - radB) < 0.01) { //収束判定
oshin1030 3:ab266b418d90 268 mode_0++;
oshin1030 3:ab266b418d90 269 }
oshin1030 3:ab266b418d90 270 printf("0_2 ");
oshin1030 3:ab266b418d90 271
oshin1030 3:ab266b418d90 272
oshin1030 3:ab266b418d90 273
oshin1030 3:ab266b418d90 274 } else if(mode_0 == 3) {
oshin1030 3:ab266b418d90 275 mode_0++;
oshin1030 3:ab266b418d90 276 printf("0_3 ");
oshin1030 3:ab266b418d90 277 } else if(mode_0 == 4) {
oshin1030 3:ab266b418d90 278 printf("0_4 ");
yuki0108 1:b74b31a7df07 279 }
yuki0108 1:b74b31a7df07 280 }
oshin1030 3:ab266b418d90 281 if(fan_switch == 0) {
oshin1030 3:ab266b418d90 282 myled_1 = 0;
oshin1030 3:ab266b418d90 283 motorR = 0;
oshin1030 3:ab266b418d90 284 } else if (fan_switch == 1) {
oshin1030 3:ab266b418d90 285 myled_1 = 1;
oshin1030 3:ab266b418d90 286 motorR = 0.1; //扇風機回す
oshin1030 3:ab266b418d90 287 printf("fan_on ");
oshin1030 3:ab266b418d90 288 }
yuki0108 0:cb9f2ec9d902 289 }
oshin1030 3:ab266b418d90 290 }