test4
Dependencies: mbed BufferedSerial LS7366LIB2 FastPWM
Position.h
- Committer:
- lsh3146
- Date:
- 2020-12-08
- Revision:
- 3:7b195612e26d
- Parent:
- 0:7cff999a7f5c
File content as of revision 3:7b195612e26d:
#ifndef _POSITION_H_ #define _POSITION_H_ double taget_position[6]={0,}; double oder_position[6]={0,}; double now_degree[6]={0,}; double error_degree[6]={0,}; double gear[6]={1.5,1.5,1.5,1.5,1.5,1.5}; double boot_cnt=0; int boot_cnt_max=5000; // 서스 중량에 따라 조절 defalut : 15000 bool position_err_zero[6] = {false, false, false, false, false, false}; void taget_position_read() { taget_position[0]=first_degree; taget_position[1]=second_degree; taget_position[2]=third_degree; taget_position[3]=-four_degree; taget_position[4]=five_degree; taget_position[5]=six_degree; for(int i = 0; i < 3; i++) { if(taget_position[i] > 100) taget_position[i] = 100; if(taget_position[i] < 20) taget_position[i] = 20; } if(taget_position[3] > 0) taget_position[3] = 0; if(taget_position[3] < -152) taget_position[3] = -152; for(int i = 4; i < 6; i++) { if(taget_position[i] > 100) taget_position[i] = 100; if(taget_position[i] < 10) taget_position[i] = 10; } } void input_filter() { for(int i=0;i<6;i++) 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); } void cal_error() { for(int i=0;i<6;i++) { error_degree[i] = oder_position[i]-now_degree[i]; if(i < 3 && error_degree[i] > -0.5 && error_degree[i] < 0.5) { position_err_zero[i] = true; error_degree[i] = 0; result_i[i] = 0; } else { position_err_zero[i] = false; } } } void cal_degree() { for(int i=0;i<6;i++) now_degree[i] = (int32_t)(-(((double)encoder_data[i]/100)/1024)*90 * gear[i]); } void Position_P() { for(int i=0;i<6;i++) taget_speed[i]=-error_degree[i]*position_Pgain[i]; } void Position_PID() { boot_cnt++; if(boot_cnt>boot_cnt_max) boot_cnt=boot_cnt_max; taget_position_read(); input_filter(); cal_degree(); cal_error(); Position_P(); } #endif