lee sanghun / Mbed 2 deprecated 2020_04_21_11_20

test

Dependencies:   mbed BufferedSerial LS7366LIB2 FastPWM

target_position_cal.h@0:e12eb40b9fef, 2020-04-23 (annotated)

Committer:
lsh2205
Date:
Thu Apr 23 00:38:16 2020 +0000
Revision:
0:e12eb40b9fef
test;

Who changed what in which revision?

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