lee sanghun / Mbed 2 deprecated 2020_07_28_10_41

test4

Dependencies:   mbed BufferedSerial LS7366LIB2 FastPWM

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