lee sanghun / Mbed 2 deprecated 2020_04_21_11_20

test

Dependencies:   mbed BufferedSerial LS7366LIB2 FastPWM

noustf.txt@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 /*
lsh2205 0:e12eb40b9fef 2 void initial_position()
lsh2205 0:e12eb40b9fef 3 {
lsh2205 0:e12eb40b9fef 4 cmd_roll = 32768;
lsh2205 0:e12eb40b9fef 5 cmd_pitch = 32768;
lsh2205 0:e12eb40b9fef 6 cmd_heave = 32768;
lsh2205 0:e12eb40b9fef 7 cmd_sway = 32768;
lsh2205 0:e12eb40b9fef 8 cmd_surge = 32768;
lsh2205 0:e12eb40b9fef 9 cmd_yaw = 32768;
lsh2205 0:e12eb40b9fef 10
lsh2205 0:e12eb40b9fef 11 double base_position[6]= {0,0,0,0,0,0};
lsh2205 0:e12eb40b9fef 12 double motor_duty=200;
lsh2205 0:e12eb40b9fef 13
lsh2205 0:e12eb40b9fef 14 base_position[0]=-(origin_degree1/(1.5*90))*1024*100;
lsh2205 0:e12eb40b9fef 15 base_position[1]=-(origin_degree2/(1.5*90))*1024*100;
lsh2205 0:e12eb40b9fef 16 base_position[2]=-(origin_degree3/(1.5*90))*1024*100;
lsh2205 0:e12eb40b9fef 17 base_position[3]=(origin_degree4/(1.5*90))*1024*100;
lsh2205 0:e12eb40b9fef 18 base_position[4]=-(origin_degree5/(1.5*90))*1024*100;
lsh2205 0:e12eb40b9fef 19 base_position[5]=-(origin_degree6/(1.5*90))*1024*100;
lsh2205 0:e12eb40b9fef 20
lsh2205 0:e12eb40b9fef 21
lsh2205 0:e12eb40b9fef 22 bool base_position_check[6]= {false,false,false,false,false,false};
lsh2205 0:e12eb40b9fef 23
lsh2205 0:e12eb40b9fef 24 int b_cnt=0;
lsh2205 0:e12eb40b9fef 25 for(int i=0; i<4001; i++) {
lsh2205 0:e12eb40b9fef 26 encoder_read();
lsh2205 0:e12eb40b9fef 27
lsh2205 0:e12eb40b9fef 28 b_cnt++;
lsh2205 0:e12eb40b9fef 29 if(b_cnt>499) {
lsh2205 0:e12eb40b9fef 30 b_cnt=0;
lsh2205 0:e12eb40b9fef 31 pc.printf("Base_positioning : %d \r\n",i+1);
lsh2205 0:e12eb40b9fef 32
lsh2205 0:e12eb40b9fef 33 for(int k=0; k<6; k++) {
lsh2205 0:e12eb40b9fef 34 pc.printf("%8.0f", base_position[k]);
lsh2205 0:e12eb40b9fef 35 }
lsh2205 0:e12eb40b9fef 36 pc.printf("\r\n");
lsh2205 0:e12eb40b9fef 37 for(int k=0; k<6; k++) {
lsh2205 0:e12eb40b9fef 38 pc.printf("%8d", encoder_data[k]);
lsh2205 0:e12eb40b9fef 39 }
lsh2205 0:e12eb40b9fef 40 pc.printf("\r\n");
lsh2205 0:e12eb40b9fef 41 }
lsh2205 0:e12eb40b9fef 42
lsh2205 0:e12eb40b9fef 43
lsh2205 0:e12eb40b9fef 44
lsh2205 0:e12eb40b9fef 45 for(int u=0; u<3; u++) {
lsh2205 0:e12eb40b9fef 46 if(encoder_data[u]<base_position[u]) {
lsh2205 0:e12eb40b9fef 47 motor_power(u, 0);
lsh2205 0:e12eb40b9fef 48 base_position_check[u]=true;
lsh2205 0:e12eb40b9fef 49 } else if(encoder_data[u]<base_position[u]*0.9)
lsh2205 0:e12eb40b9fef 50 motor_power(u, motor_duty*0.5);
lsh2205 0:e12eb40b9fef 51 else if(encoder_data[u]<base_position[u]*0.8)
lsh2205 0:e12eb40b9fef 52 motor_power(u, motor_duty*0.7);
lsh2205 0:e12eb40b9fef 53 else if(encoder_data[u]<base_position[u]*0.7)
lsh2205 0:e12eb40b9fef 54 motor_power(u, motor_duty*0.9);
lsh2205 0:e12eb40b9fef 55 else
lsh2205 0:e12eb40b9fef 56 motor_power(u, motor_duty);
lsh2205 0:e12eb40b9fef 57 }
lsh2205 0:e12eb40b9fef 58
lsh2205 0:e12eb40b9fef 59 for(int u=4; u<6; u++) {
lsh2205 0:e12eb40b9fef 60 if(encoder_data[u]<base_position[u]) {
lsh2205 0:e12eb40b9fef 61 motor_power(u, 0);
lsh2205 0:e12eb40b9fef 62 base_position_check[u]=true;
lsh2205 0:e12eb40b9fef 63 } else if(encoder_data[u]<base_position[u]*0.9)
lsh2205 0:e12eb40b9fef 64 motor_power(u, -motor_duty*0.5);
lsh2205 0:e12eb40b9fef 65 else if(encoder_data[u]<base_position[u]*0.8)
lsh2205 0:e12eb40b9fef 66 motor_power(u, -motor_duty*0.7);
lsh2205 0:e12eb40b9fef 67 else if(encoder_data[u]<base_position[u]*0.7)
lsh2205 0:e12eb40b9fef 68 motor_power(u, -motor_duty*0.9);
lsh2205 0:e12eb40b9fef 69 else
lsh2205 0:e12eb40b9fef 70 motor_power(u, -motor_duty);
lsh2205 0:e12eb40b9fef 71 }
lsh2205 0:e12eb40b9fef 72
lsh2205 0:e12eb40b9fef 73 for(int u=3; u<4; u++) {
lsh2205 0:e12eb40b9fef 74 if(encoder_data[u]>base_position[u]) {
lsh2205 0:e12eb40b9fef 75 motor_power(u, 0);
lsh2205 0:e12eb40b9fef 76 base_position_check[u]=true;
lsh2205 0:e12eb40b9fef 77 } else if(encoder_data[u]>base_position[u]*0.9)
lsh2205 0:e12eb40b9fef 78 motor_power(u, motor_duty*0.5);
lsh2205 0:e12eb40b9fef 79 else if(encoder_data[u]>base_position[u]*0.8)
lsh2205 0:e12eb40b9fef 80 motor_power(u, motor_duty*0.7);
lsh2205 0:e12eb40b9fef 81 else if(encoder_data[u]>base_position[u]*0.7)
lsh2205 0:e12eb40b9fef 82 motor_power(u, motor_duty*0.9);
lsh2205 0:e12eb40b9fef 83 else
lsh2205 0:e12eb40b9fef 84 motor_power(u, motor_duty);
lsh2205 0:e12eb40b9fef 85 }
lsh2205 0:e12eb40b9fef 86
lsh2205 0:e12eb40b9fef 87 }
lsh2205 0:e12eb40b9fef 88
lsh2205 0:e12eb40b9fef 89 motor_power(0, 0);
lsh2205 0:e12eb40b9fef 90 motor_power(1, 0);
lsh2205 0:e12eb40b9fef 91 motor_power(2, 0);
lsh2205 0:e12eb40b9fef 92 motor_power(3, 0);
lsh2205 0:e12eb40b9fef 93 motor_power(4, 0);
lsh2205 0:e12eb40b9fef 94 motor_power(5, 0);
lsh2205 0:e12eb40b9fef 95 }
lsh2205 0:e12eb40b9fef 96 */
lsh2205 0:e12eb40b9fef 97
lsh2205 0:e12eb40b9fef 98 /*
lsh2205 0:e12eb40b9fef 99 m_cnt++;
lsh2205 0:e12eb40b9fef 100 if(m_cnt>3000)
lsh2205 0:e12eb40b9fef 101 {
lsh2205 0:e12eb40b9fef 102 m_cnt=0;
lsh2205 0:e12eb40b9fef 103 pc.printf(" Speed_Pgain Speed_Igain position_Pgain Position_input_filter Speed_I_input_filter offset\r\n");
lsh2205 0:e12eb40b9fef 104 for(int i = 0; i < 6; i++)
lsh2205 0:e12eb40b9fef 105 {
lsh2205 0:e12eb40b9fef 106 pc.printf("%2d. %6.3f %6.3f %6.3f %6.3f %6.3f %4d\r\n", i + 1, Speed_Pgain[i], Speed_Igain[i], position_Pgain[i], Position_input_filter[i], Speed_I_input_filter[i], (int)offset[i]);
lsh2205 0:e12eb40b9fef 107 }
lsh2205 0:e12eb40b9fef 108 pc.printf("\n\n\n");
lsh2205 0:e12eb40b9fef 109 }
lsh2205 0:e12eb40b9fef 110 */