File content as of revision 4:bf278ddb8504:

#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=60;    // 모터1 기준위치 지정
double origin_degree2=60;    // 모터2 기준위치 지정
double origin_degree3=60;    // 모터3 기준위치 지정
double origin_degree4=76;    // 모터2 기준위치 지정
double origin_degree5=60;    // 모터3 기준위치 지정
double origin_degree6=60;    // 모터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