Diff: target_position_cal.h

Revision:

0:e12eb40b9fef

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/target_position_cal.h	Thu Apr 23 00:38:16 2020 +0000
@@ -0,0 +1,82 @@
+#ifndef _TARGET_POSITION_CAL_H_
+#define _TARGET_POSITION_CAL_H_
+
+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=76;    // 모터2 기준위치 지정
+double origin_degree5=50;    // 모터3 기준위치 지정
+double origin_degree6=50;    // 모터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;
+    sw  =   ((sw    - 32768) / 1000) * 0.2746582;
+    su  =   ((su    - 32768) / 1000) * 0.2746582;
+    y   =   ((y     - 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;
+    
+}
+
+#endif
\ No newline at end of file