lee sanghun / Mbed 2 deprecated 2020_04_21_11_20

test

Dependencies:   mbed BufferedSerial LS7366LIB2 FastPWM

Position.h@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 #ifndef _POSITION_H_
lsh2205 0:e12eb40b9fef 2 #define _POSITION_H_
lsh2205 0:e12eb40b9fef 3
lsh2205 0:e12eb40b9fef 4 double taget_position[6]={0,};
lsh2205 0:e12eb40b9fef 5 double oder_position[6]={0,};
lsh2205 0:e12eb40b9fef 6
lsh2205 0:e12eb40b9fef 7 double now_degree[6]={0,};
lsh2205 0:e12eb40b9fef 8 double error_degree[6]={0,};
lsh2205 0:e12eb40b9fef 9
lsh2205 0:e12eb40b9fef 10 double gear[6]={1.5,1.5,1.5,1.5,1.5,1.5};
lsh2205 0:e12eb40b9fef 11
lsh2205 0:e12eb40b9fef 12 double boot_cnt=0;
lsh2205 0:e12eb40b9fef 13 int boot_cnt_max=15000;
lsh2205 0:e12eb40b9fef 14
lsh2205 0:e12eb40b9fef 15 void taget_position_read()
lsh2205 0:e12eb40b9fef 16 {
lsh2205 0:e12eb40b9fef 17
lsh2205 0:e12eb40b9fef 18 taget_position[0]=first_degree;
lsh2205 0:e12eb40b9fef 19 taget_position[1]=second_degree;
lsh2205 0:e12eb40b9fef 20 taget_position[2]=third_degree;
lsh2205 0:e12eb40b9fef 21 taget_position[3]=-four_degree;
lsh2205 0:e12eb40b9fef 22 taget_position[4]=five_degree;
lsh2205 0:e12eb40b9fef 23 taget_position[5]=six_degree;
lsh2205 0:e12eb40b9fef 24
lsh2205 0:e12eb40b9fef 25
lsh2205 0:e12eb40b9fef 26 for(int i = 0; i < 3; i++)
lsh2205 0:e12eb40b9fef 27 {
lsh2205 0:e12eb40b9fef 28 if(taget_position[i] > 160)
lsh2205 0:e12eb40b9fef 29 taget_position[i] = 160;
lsh2205 0:e12eb40b9fef 30 if(taget_position[i] < 0)
lsh2205 0:e12eb40b9fef 31 taget_position[i] = 0;
lsh2205 0:e12eb40b9fef 32 }
lsh2205 0:e12eb40b9fef 33
lsh2205 0:e12eb40b9fef 34
lsh2205 0:e12eb40b9fef 35 if(taget_position[3] > 0)
lsh2205 0:e12eb40b9fef 36 taget_position[3] = 0;
lsh2205 0:e12eb40b9fef 37 if(taget_position[3] < -152)
lsh2205 0:e12eb40b9fef 38 taget_position[3] = -152;
lsh2205 0:e12eb40b9fef 39
lsh2205 0:e12eb40b9fef 40
lsh2205 0:e12eb40b9fef 41 for(int i = 4; i < 6; i++)
lsh2205 0:e12eb40b9fef 42 {
lsh2205 0:e12eb40b9fef 43 if(taget_position[i] > 110)
lsh2205 0:e12eb40b9fef 44 taget_position[i] = 110;
lsh2205 0:e12eb40b9fef 45 if(taget_position[i] < 5)
lsh2205 0:e12eb40b9fef 46 taget_position[i] = 5;
lsh2205 0:e12eb40b9fef 47 }
lsh2205 0:e12eb40b9fef 48
lsh2205 0:e12eb40b9fef 49 }
lsh2205 0:e12eb40b9fef 50
lsh2205 0:e12eb40b9fef 51 void input_filter()
lsh2205 0:e12eb40b9fef 52 {
lsh2205 0:e12eb40b9fef 53
lsh2205 0:e12eb40b9fef 54
lsh2205 0:e12eb40b9fef 55 for(int i=0;i<6;i++)
lsh2205 0:e12eb40b9fef 56 oder_position[i] = oder_position[i]*(1-Position_input_filter[i]*(boot_cnt/boot_cnt_max)) + taget_position[i]*Position_input_filter[i]*(boot_cnt/boot_cnt_max);
lsh2205 0:e12eb40b9fef 57 }
lsh2205 0:e12eb40b9fef 58
lsh2205 0:e12eb40b9fef 59 void cal_error()
lsh2205 0:e12eb40b9fef 60 {
lsh2205 0:e12eb40b9fef 61 for(int i=0;i<6;i++)
lsh2205 0:e12eb40b9fef 62 error_degree[i] = oder_position[i]-now_degree[i];
lsh2205 0:e12eb40b9fef 63 }
lsh2205 0:e12eb40b9fef 64
lsh2205 0:e12eb40b9fef 65 void cal_degree()
lsh2205 0:e12eb40b9fef 66 {
lsh2205 0:e12eb40b9fef 67 for(int i=0;i<6;i++)
lsh2205 0:e12eb40b9fef 68 now_degree[i] = -(((double)encoder_data[i]/100)/1024)*90 * gear[i];
lsh2205 0:e12eb40b9fef 69 }
lsh2205 0:e12eb40b9fef 70
lsh2205 0:e12eb40b9fef 71 void Position_P()
lsh2205 0:e12eb40b9fef 72 {
lsh2205 0:e12eb40b9fef 73 for(int i=0;i<6;i++)
lsh2205 0:e12eb40b9fef 74 taget_speed[i]=-error_degree[i]*position_Pgain[i];
lsh2205 0:e12eb40b9fef 75
lsh2205 0:e12eb40b9fef 76 }
lsh2205 0:e12eb40b9fef 77
lsh2205 0:e12eb40b9fef 78
lsh2205 0:e12eb40b9fef 79
lsh2205 0:e12eb40b9fef 80 void Position_PID()
lsh2205 0:e12eb40b9fef 81 {
lsh2205 0:e12eb40b9fef 82 boot_cnt++;
lsh2205 0:e12eb40b9fef 83 if(boot_cnt>boot_cnt_max)
lsh2205 0:e12eb40b9fef 84 boot_cnt=boot_cnt_max;
lsh2205 0:e12eb40b9fef 85
lsh2205 0:e12eb40b9fef 86 taget_position_read();
lsh2205 0:e12eb40b9fef 87
lsh2205 0:e12eb40b9fef 88 input_filter();
lsh2205 0:e12eb40b9fef 89
lsh2205 0:e12eb40b9fef 90 cal_degree();
lsh2205 0:e12eb40b9fef 91
lsh2205 0:e12eb40b9fef 92 cal_error();
lsh2205 0:e12eb40b9fef 93
lsh2205 0:e12eb40b9fef 94 Position_P();
lsh2205 0:e12eb40b9fef 95
lsh2205 0:e12eb40b9fef 96 }
lsh2205 0:e12eb40b9fef 97
lsh2205 0:e12eb40b9fef 98 #endif