lee sanghun / Mbed 2 deprecated 2020_07_28_10_41

test4

Dependencies:   mbed BufferedSerial LS7366LIB2 FastPWM

Position.h@3:7b195612e26d, 2020-12-08 (annotated)

Committer:
lsh3146
Date:
Tue Dec 08 01:27:11 2020 +0000
Revision:
3:7b195612e26d
Parent:
0:7cff999a7f5c 
fgdhgfhfghgfh

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