lee sanghun / Mbed 2 deprecated 202001131705

45

Dependencies:   mbed BufferedSerial LS7366LIB FastPWM

target_position_cal.h@0:c21936a3520a, 2020-03-23 (annotated)

Committer:
lsh2205
Date:
Mon Mar 23 08:38:40 2020 +0000
Revision:
0:c21936a3520a
ss

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lsh2205 0:c21936a3520a 1 double first_degree, second_degree, third_degree, four_degree, five_degree, six_degree;
lsh2205 0:c21936a3520a 2
lsh2205 0:c21936a3520a 3 double motor1_degree=0;
lsh2205 0:c21936a3520a 4 double motor2_degree=0;
lsh2205 0:c21936a3520a 5 double motor3_degree=0;
lsh2205 0:c21936a3520a 6 double motor4_degree=0;
lsh2205 0:c21936a3520a 7 double motor5_degree=0;
lsh2205 0:c21936a3520a 8 double motor6_degree=0;
lsh2205 0:c21936a3520a 9
lsh2205 0:c21936a3520a 10 #define roll_gain 15 // roll 게인 ( 부호를 바꾸면 방향이 바뀐다.)
lsh2205 0:c21936a3520a 11 #define pitch_gain 15 // pitch 게인 ( 부호를 바꾸면 방향이 바뀐다.)
lsh2205 0:c21936a3520a 12 #define heave_gain 15 // heave_gain 게인 ( 부호를 바꾸면 방향이 바뀐다.)
lsh2205 0:c21936a3520a 13
lsh2205 0:c21936a3520a 14 double origin_degree1=50; // 모터1 기준위치 지정
lsh2205 0:c21936a3520a 15 double origin_degree2=50; // 모터2 기준위치 지정
lsh2205 0:c21936a3520a 16 double origin_degree3=50; // 모터3 기준위치 지정
lsh2205 0:c21936a3520a 17 double origin_degree4=60; // 모터2 기준위치 지정
lsh2205 0:c21936a3520a 18 double origin_degree5=90; // 모터3 기준위치 지정
lsh2205 0:c21936a3520a 19 double origin_degree6=90; // 모터3 기준위치 지정
lsh2205 0:c21936a3520a 20 double cal_scale=0.7; // 전체 게인 조절
lsh2205 0:c21936a3520a 21
lsh2205 0:c21936a3520a 22 double cal_roll=0;
lsh2205 0:c21936a3520a 23 double cal_pitch=0;
lsh2205 0:c21936a3520a 24 double cal_heave=0;
lsh2205 0:c21936a3520a 25 double cal_surge=0;
lsh2205 0:c21936a3520a 26 double cal_yaw=0;
lsh2205 0:c21936a3520a 27 double cal_sway=0;
lsh2205 0:c21936a3520a 28 double cal_senter=0;
lsh2205 0:c21936a3520a 29
lsh2205 0:c21936a3520a 30 double pitch_motor1, pitch_motor2, pitch_motor3;
lsh2205 0:c21936a3520a 31 double roll_motor1, roll_motor2, roll_motor3;
lsh2205 0:c21936a3520a 32 double heave_motor1, heave_motor2, heave_motor3;
lsh2205 0:c21936a3520a 33
lsh2205 0:c21936a3520a 34
lsh2205 0:c21936a3520a 35 void taget_position_cal(double r,double p, double h,double sw,double su,double y) // before, cal(roll_gain*dou_roll,pitch_gain*dou_pitch,heave_gain*dou_heave,0,gain); / add, surge, yaw, sway
lsh2205 0:c21936a3520a 36 {
lsh2205 0:c21936a3520a 37
lsh2205 0:c21936a3520a 38 r = ((r - 32768) / 1000) * 0.2746582;
lsh2205 0:c21936a3520a 39 p = ((p - 32768) / 1000) * 0.2746582;
lsh2205 0:c21936a3520a 40 h = ((h - 32768) / 1000) * 0.2746582;
lsh2205 0:c21936a3520a 41 su = ((su - 32768) / 1000) * 0.2746582;
lsh2205 0:c21936a3520a 42 y = ((y - 32768) / 1000) * 0.2746582;
lsh2205 0:c21936a3520a 43 sw = ((sw - 32768) / 1000) * 0.2746582;
lsh2205 0:c21936a3520a 44
lsh2205 0:c21936a3520a 45
lsh2205 0:c21936a3520a 46 r=roll_gain*r;
lsh2205 0:c21936a3520a 47 p=pitch_gain*p;
lsh2205 0:c21936a3520a 48 h=heave_gain*h;
lsh2205 0:c21936a3520a 49
lsh2205 0:c21936a3520a 50 pitch_motor1 = cal_scale * (-p);
lsh2205 0:c21936a3520a 51 pitch_motor2 = cal_scale * (p);
lsh2205 0:c21936a3520a 52 pitch_motor3 = cal_scale * (p);
lsh2205 0:c21936a3520a 53
lsh2205 0:c21936a3520a 54 roll_motor1 = 0;
lsh2205 0:c21936a3520a 55 roll_motor2 = (cal_scale * r);
lsh2205 0:c21936a3520a 56 roll_motor3 = (cal_scale * -r);
lsh2205 0:c21936a3520a 57
lsh2205 0:c21936a3520a 58 heave_motor1 = (cal_scale * h);
lsh2205 0:c21936a3520a 59 heave_motor2 = (cal_scale * h);
lsh2205 0:c21936a3520a 60 heave_motor3 = (cal_scale * h);
lsh2205 0:c21936a3520a 61
lsh2205 0:c21936a3520a 62 motor1_degree = cal_senter + pitch_motor1 + roll_motor1 + heave_motor1;
lsh2205 0:c21936a3520a 63 motor2_degree = cal_senter + pitch_motor2 + roll_motor2 + heave_motor2;
lsh2205 0:c21936a3520a 64 motor3_degree = cal_senter + pitch_motor3 + roll_motor3 + heave_motor3;
lsh2205 0:c21936a3520a 65
lsh2205 0:c21936a3520a 66 motor4_degree = 18.66 * sw; //회전비율조정(심툴즈 아웃풋 테스트 스로틀 맞춤)
lsh2205 0:c21936a3520a 67 motor5_degree = 9.33 * su-9.33 * y; //회전비율조정(심툴즈 아웃풋 테스트 스로틀 맞춤)
lsh2205 0:c21936a3520a 68 motor6_degree = 9.33 * su+9.33 * y; //회전비율조정(심툴즈 아웃풋 테스트 스로틀 맞춤)
lsh2205 0:c21936a3520a 69
lsh2205 0:c21936a3520a 70 first_degree = motor1_degree+origin_degree1;
lsh2205 0:c21936a3520a 71 second_degree = motor2_degree+origin_degree2;
lsh2205 0:c21936a3520a 72 third_degree = motor3_degree+origin_degree3;
lsh2205 0:c21936a3520a 73 four_degree = motor4_degree+origin_degree4;
lsh2205 0:c21936a3520a 74 five_degree = motor5_degree+origin_degree5;
lsh2205 0:c21936a3520a 75 six_degree = motor6_degree+origin_degree6;
lsh2205 0:c21936a3520a 76
lsh2205 0:c21936a3520a 77 }