lee sanghun / Mbed 2 deprecated 2020_07_28_10_41

test4

Dependencies:   mbed BufferedSerial LS7366LIB2 FastPWM

target_position_cal.h@4:bf278ddb8504, 2020-12-08 (annotated)

Committer:
lsh3146
Date:
Tue Dec 08 01:25:06 2020 +0000
Revision:
4:bf278ddb8504
Parent:
0:7cff999a7f5c 
aaaaaqqqqq

Who changed what in which revision?

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