![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
45
Dependencies: mbed BufferedSerial LS7366LIB FastPWM
target_position_cal.h
- Committer:
- lsh2205
- Date:
- 2020-03-23
- Revision:
- 0:c21936a3520a
File content as of revision 0:c21936a3520a:
double first_degree, second_degree, third_degree, four_degree, five_degree, six_degree; double motor1_degree=0; double motor2_degree=0; double motor3_degree=0; double motor4_degree=0; double motor5_degree=0; double motor6_degree=0; #define roll_gain 15 // roll 게인 ( 부호를 바꾸면 방향이 바뀐다.) #define pitch_gain 15 // pitch 게인 ( 부호를 바꾸면 방향이 바뀐다.) #define heave_gain 15 // heave_gain 게인 ( 부호를 바꾸면 방향이 바뀐다.) double origin_degree1=50; // 모터1 기준위치 지정 double origin_degree2=50; // 모터2 기준위치 지정 double origin_degree3=50; // 모터3 기준위치 지정 double origin_degree4=60; // 모터2 기준위치 지정 double origin_degree5=90; // 모터3 기준위치 지정 double origin_degree6=90; // 모터3 기준위치 지정 double cal_scale=0.7; // 전체 게인 조절 double cal_roll=0; double cal_pitch=0; double cal_heave=0; double cal_surge=0; double cal_yaw=0; double cal_sway=0; double cal_senter=0; double pitch_motor1, pitch_motor2, pitch_motor3; double roll_motor1, roll_motor2, roll_motor3; double heave_motor1, heave_motor2, heave_motor3; 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 { r = ((r - 32768) / 1000) * 0.2746582; p = ((p - 32768) / 1000) * 0.2746582; h = ((h - 32768) / 1000) * 0.2746582; su = ((su - 32768) / 1000) * 0.2746582; y = ((y - 32768) / 1000) * 0.2746582; sw = ((sw - 32768) / 1000) * 0.2746582; r=roll_gain*r; p=pitch_gain*p; h=heave_gain*h; pitch_motor1 = cal_scale * (-p); pitch_motor2 = cal_scale * (p); pitch_motor3 = cal_scale * (p); roll_motor1 = 0; roll_motor2 = (cal_scale * r); roll_motor3 = (cal_scale * -r); heave_motor1 = (cal_scale * h); heave_motor2 = (cal_scale * h); heave_motor3 = (cal_scale * h); motor1_degree = cal_senter + pitch_motor1 + roll_motor1 + heave_motor1; motor2_degree = cal_senter + pitch_motor2 + roll_motor2 + heave_motor2; motor3_degree = cal_senter + pitch_motor3 + roll_motor3 + heave_motor3; motor4_degree = 18.66 * sw; //회전비율조정(심툴즈 아웃풋 테스트 스로틀 맞춤) motor5_degree = 9.33 * su-9.33 * y; //회전비율조정(심툴즈 아웃풋 테스트 스로틀 맞춤) motor6_degree = 9.33 * su+9.33 * y; //회전비율조정(심툴즈 아웃풋 테스트 스로틀 맞춤) first_degree = motor1_degree+origin_degree1; second_degree = motor2_degree+origin_degree2; third_degree = motor3_degree+origin_degree3; four_degree = motor4_degree+origin_degree4; five_degree = motor5_degree+origin_degree5; six_degree = motor6_degree+origin_degree6; }