test4
Dependencies: mbed BufferedSerial LS7366LIB2 FastPWM
Diff: main.cpp
- Revision:
- 3:7b195612e26d
- Parent:
- 2:14b52dee1c35
--- a/main.cpp Tue Jul 28 01:42:16 2020 +0000 +++ b/main.cpp Tue Dec 08 01:27:11 2020 +0000 @@ -1,6 +1,10 @@ #include "mbed.h" #include "main.h" +#define sleep_duty 7 + +int Analysis_limit=2000;// change motor sleep + void initial_position(); void begin_pid(); void command_init(); @@ -13,9 +17,65 @@ int m_cnt=0; int menual=false; +double speed_sleep[6]={0,0,0,0,0,0}; + + +int Analysis_data[400]={0}; +int Analysis_cnt=0; +int Analysis_total=0; + +int Analysis_sleep_cnt=0; + +int stay_cnt=0; + +double speed_0p5sec[6]={0,}; + +void input_data_Analysis() +{ + for(int i=0;i<6;i++) + { + speed_0p5sec[i]=speed_0p5sec[i]*0.99 + dif_encoder_data[i]*0.01; + } + + if(boot_cnt>10000) + { + for(int i=0;i<6;i++) + { + + if(filterd_dif_taget_speed[i]<0.01 && filterd_dif_taget_speed[i]>-0.01) + { + if(speed_0p5sec[i]<0.1 && speed_0p5sec[i]>-0.1) + { + speed_sleep[i]++; + if(speed_sleep[i]>(100-sleep_duty)) + speed_sleep[i]=(100-sleep_duty); + + motor_gain[i]=(100-speed_sleep[i])/100; + } + }else if(filterd_dif_taget_speed[i]>0.01 || filterd_dif_taget_speed[i]<-0.01) + { + speed_sleep[i]--; + if(speed_sleep[i]<0) + speed_sleep[i]=0; + motor_gain[i]=(100-speed_sleep[i])/100; + } + + } + + } + + //pc.printf("(%0.3f,%0.3f,%0.3f)",filterd_dif_taget_speed[0],filterd_dif_taget_speed[1],filterd_dif_taget_speed[2]); + pc.printf("speed_sleep ( %d, %d, %d %d, %d, %d )",(int)(speed_sleep[0]),(int)(speed_sleep[1]),(int)(speed_sleep[2]),(int)(speed_sleep[3]),(int)(speed_sleep[4]),(int)(speed_sleep[5])); + pc.printf("gain ( %d, %d, %d %d, %d, %d )",(int)(motor_gain[0]*100),(int)(motor_gain[1]*100),(int)(motor_gain[2]*100),(int)(motor_gain[3]*100),(int)(motor_gain[4]*100),(int)(motor_gain[5]*100)); + pc.printf("0.2s (%0.3f,%0.3f,%0.3f)",speed_0p5sec[0],speed_0p5sec[1],speed_0p5sec[2]); + pc.printf("duty(%d,%d,%d,%d,%d,%d)\r\n",(int)last_percent[0],(int)last_percent[1],(int)last_percent[2],(int)last_percent[3],(int)last_percent[4],(int)last_percent[5]); + + + } + + int main() { - board_init(); MCP23S17_Init(); Segment_Init(); @@ -32,53 +92,90 @@ pc.printf("PID LOOP_START\r\n"); pid_info(); - + + int a_cnt=0; while(1) - { - reset_check(); - - m_cnt++; - if(m_cnt>1000) - { - m_cnt=0; - } - + { Button_Detection(); comunication(); // 모터의 위치 값을 받음 taget_position_cal((double)cmd_roll,(double)cmd_pitch,(double)cmd_heave,(double)cmd_sway,(double)cmd_surge,(double)cmd_yaw); encoder_read(); + + Position_PID(); + Speed_PID(); - if(Idle_Check() == false) - { - for(int i=0;i<6;i++) - { - Speed_Igain[i]=0.01; - } - } - else - { - for(int i=0;i<6;i++) - { - Speed_Igain[i]=0; - } - } - - if(!menual_mode) - Position_PID(); - else - Menual_Position_PID(); - - Speed_PID(); - + int encoder_error[6] = {-1,-1,-1,-1,1,1}; for(int i=0; i<6; i++) - motor_power(i,Speed_PID_OUTPUT[i]); + { + if(encoder_data[i]==encoder_error[i]) + motor_power(i,0);//motor_offset[i]); + else + { + if(i < 3 && position_err_zero[i]) + { + motor_power(i, 0); + } + else + { + motor_power(i,Speed_PID_OUTPUT[i]); + } + } + } + + if(filter_dif_encoder_data[4]<=2 && filter_dif_encoder_data[4]>=-2) + { + if(Speed_PID_OUTPUT[4]>motor_offset[4]) + motor_offset[4]+=0.2; + else if(Speed_PID_OUTPUT[4]<motor_offset[4]) + motor_offset[4]-=0.2; + } + + if(filter_dif_encoder_data[5]<=2 && filter_dif_encoder_data[5]>=-2) + { + if(Speed_PID_OUTPUT[5]>motor_offset[5]) + motor_offset[5]+=0.2; + else if(Speed_PID_OUTPUT[5]<motor_offset[5]) + motor_offset[5]-=0.2; + } + + //pc.printf("%d,%d,%d,%d,%d,%d\r\n",(int)filter_dif_encoder_data[4],(int)filter_dif_encoder_data[5],(int)motor_offset[4],(int)motor_offset[5],(int)Speed_PID_OUTPUT[4],(int)Speed_PID_OUTPUT[5]); + + if(encoder_data[0]==-1 && encoder_data[1]==-1 && encoder_data[2]==-1 && encoder_data[3]==-1 && encoder_data[4]==1 && encoder_data[5]==1) + { + while(1) + { + pc.printf("while(1)\r\n"); + pc.printf("%d,%d,%d,%d\r\n",(int)motor_offset[4],(int)motor_offset[5],(int)Speed_PID_OUTPUT[4],(int)Speed_PID_OUTPUT[5]); + + encoder_data[0] = encoder1.read(); + if(encoder_data[0]!=-1) + { + NVIC_SystemReset(); + } + wait_us(2500); + } + } + + a_cnt++; + if(a_cnt>9) + { + a_cnt=0; + //input_data_Analysis(); + } + A_ex_cmd_roll=cmd_roll; + A_ex_cmd_pitch=cmd_pitch; + A_ex_cmd_heave=cmd_heave; + A_ex_cmd_sway=cmd_sway; + A_ex_cmd_surge=cmd_surge; + A_ex_cmd_yaw=cmd_yaw; } } + void command_init() { cmd_roll = 32768;