lee sanghun / Mbed 2 deprecated 2020_07_28_10_41

test4

Dependencies:   mbed BufferedSerial LS7366LIB2 FastPWM

main.cpp@3:7b195612e26d, 2020-12-08 (annotated)

Committer:
lsh3146
Date:
Tue Dec 08 01:27:11 2020 +0000
Revision:
3:7b195612e26d
Parent:
2:14b52dee1c35 
fgdhgfhfghgfh

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gohgwaja 0:7cff999a7f5c 1 #include "mbed.h"
gohgwaja 0:7cff999a7f5c 2 #include "main.h"
gohgwaja 0:7cff999a7f5c 3
lsh3146 3:7b195612e26d 4 #define sleep_duty 7
lsh3146 3:7b195612e26d 5
lsh3146 3:7b195612e26d 6 int Analysis_limit=2000;// change motor sleep
lsh3146 3:7b195612e26d 7
gohgwaja 0:7cff999a7f5c 8 void initial_position();
gohgwaja 0:7cff999a7f5c 9 void begin_pid();
gohgwaja 0:7cff999a7f5c 10 void command_init();
gohgwaja 0:7cff999a7f5c 11 void pid_info();
gohgwaja 1:7b5469bf5994 12 void Menual_Position_PID();
gohgwaja 0:7cff999a7f5c 13 void test();
gohgwaja 0:7cff999a7f5c 14
gohgwaja 0:7cff999a7f5c 15 int cnt=0;
gohgwaja 0:7cff999a7f5c 16
gohgwaja 0:7cff999a7f5c 17 int m_cnt=0;
gohgwaja 0:7cff999a7f5c 18 int menual=false;
gohgwaja 1:7b5469bf5994 19
lsh3146 3:7b195612e26d 20 double speed_sleep[6]={0,0,0,0,0,0};
lsh3146 3:7b195612e26d 21
lsh3146 3:7b195612e26d 22
lsh3146 3:7b195612e26d 23 int Analysis_data[400]={0};
lsh3146 3:7b195612e26d 24 int Analysis_cnt=0;
lsh3146 3:7b195612e26d 25 int Analysis_total=0;
lsh3146 3:7b195612e26d 26
lsh3146 3:7b195612e26d 27 int Analysis_sleep_cnt=0;
lsh3146 3:7b195612e26d 28
lsh3146 3:7b195612e26d 29 int stay_cnt=0;
lsh3146 3:7b195612e26d 30
lsh3146 3:7b195612e26d 31 double speed_0p5sec[6]={0,};
lsh3146 3:7b195612e26d 32
lsh3146 3:7b195612e26d 33 void input_data_Analysis()
lsh3146 3:7b195612e26d 34 {
lsh3146 3:7b195612e26d 35 for(int i=0;i<6;i++)
lsh3146 3:7b195612e26d 36 {
lsh3146 3:7b195612e26d 37 speed_0p5sec[i]=speed_0p5sec[i]*0.99 + dif_encoder_data[i]*0.01;
lsh3146 3:7b195612e26d 38 }
lsh3146 3:7b195612e26d 39
lsh3146 3:7b195612e26d 40 if(boot_cnt>10000)
lsh3146 3:7b195612e26d 41 {
lsh3146 3:7b195612e26d 42 for(int i=0;i<6;i++)
lsh3146 3:7b195612e26d 43 {
lsh3146 3:7b195612e26d 44
lsh3146 3:7b195612e26d 45 if(filterd_dif_taget_speed[i]<0.01 && filterd_dif_taget_speed[i]>-0.01)
lsh3146 3:7b195612e26d 46 {
lsh3146 3:7b195612e26d 47 if(speed_0p5sec[i]<0.1 && speed_0p5sec[i]>-0.1)
lsh3146 3:7b195612e26d 48 {
lsh3146 3:7b195612e26d 49 speed_sleep[i]++;
lsh3146 3:7b195612e26d 50 if(speed_sleep[i]>(100-sleep_duty))
lsh3146 3:7b195612e26d 51 speed_sleep[i]=(100-sleep_duty);
lsh3146 3:7b195612e26d 52
lsh3146 3:7b195612e26d 53 motor_gain[i]=(100-speed_sleep[i])/100;
lsh3146 3:7b195612e26d 54 }
lsh3146 3:7b195612e26d 55 }else if(filterd_dif_taget_speed[i]>0.01 || filterd_dif_taget_speed[i]<-0.01)
lsh3146 3:7b195612e26d 56 {
lsh3146 3:7b195612e26d 57 speed_sleep[i]--;
lsh3146 3:7b195612e26d 58 if(speed_sleep[i]<0)
lsh3146 3:7b195612e26d 59 speed_sleep[i]=0;
lsh3146 3:7b195612e26d 60 motor_gain[i]=(100-speed_sleep[i])/100;
lsh3146 3:7b195612e26d 61 }
lsh3146 3:7b195612e26d 62
lsh3146 3:7b195612e26d 63 }
lsh3146 3:7b195612e26d 64
lsh3146 3:7b195612e26d 65 }
lsh3146 3:7b195612e26d 66
lsh3146 3:7b195612e26d 67 //pc.printf("(%0.3f,%0.3f,%0.3f)",filterd_dif_taget_speed[0],filterd_dif_taget_speed[1],filterd_dif_taget_speed[2]);
lsh3146 3:7b195612e26d 68 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]));
lsh3146 3:7b195612e26d 69 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));
lsh3146 3:7b195612e26d 70 pc.printf("0.2s (%0.3f,%0.3f,%0.3f)",speed_0p5sec[0],speed_0p5sec[1],speed_0p5sec[2]);
lsh3146 3:7b195612e26d 71 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]);
lsh3146 3:7b195612e26d 72
lsh3146 3:7b195612e26d 73
lsh3146 3:7b195612e26d 74 }
lsh3146 3:7b195612e26d 75
lsh3146 3:7b195612e26d 76
gohgwaja 0:7cff999a7f5c 77 int main()
gohgwaja 0:7cff999a7f5c 78 {
gohgwaja 0:7cff999a7f5c 79 board_init();
gohgwaja 0:7cff999a7f5c 80 MCP23S17_Init();
gohgwaja 0:7cff999a7f5c 81 Segment_Init();
gohgwaja 0:7cff999a7f5c 82 Button_Init();
gohgwaja 0:7cff999a7f5c 83
gohgwaja 0:7cff999a7f5c 84 printf("READ Gain pgain : %ld, input : %ld\r\n", Button_Read_Flash(0), Button_Read_Flash(4));
gohgwaja 0:7cff999a7f5c 85
gohgwaja 0:7cff999a7f5c 86
gohgwaja 0:7cff999a7f5c 87 encoder_check2();
gohgwaja 0:7cff999a7f5c 88
gohgwaja 0:7cff999a7f5c 89 find_limit();
gohgwaja 0:7cff999a7f5c 90
gohgwaja 0:7cff999a7f5c 91 command_init();
gohgwaja 0:7cff999a7f5c 92
gohgwaja 0:7cff999a7f5c 93 pc.printf("PID LOOP_START\r\n");
gohgwaja 0:7cff999a7f5c 94 pid_info();
lsh3146 3:7b195612e26d 95
lsh3146 3:7b195612e26d 96 int a_cnt=0;
gohgwaja 0:7cff999a7f5c 97
gohgwaja 1:7b5469bf5994 98 while(1)
lsh3146 3:7b195612e26d 99 {
gohgwaja 0:7cff999a7f5c 100 Button_Detection();
gohgwaja 0:7cff999a7f5c 101
gohgwaja 0:7cff999a7f5c 102 comunication(); // 모터의 위치 값을 받음
gohgwaja 0:7cff999a7f5c 103 taget_position_cal((double)cmd_roll,(double)cmd_pitch,(double)cmd_heave,(double)cmd_sway,(double)cmd_surge,(double)cmd_yaw);
gohgwaja 0:7cff999a7f5c 104 encoder_read();
gohgwaja 0:7cff999a7f5c 105
lsh3146 3:7b195612e26d 106
lsh3146 3:7b195612e26d 107 Position_PID();
lsh3146 3:7b195612e26d 108 Speed_PID();
lsh3146 2:14b52dee1c35 109
lsh3146 3:7b195612e26d 110 int encoder_error[6] = {-1,-1,-1,-1,1,1};
gohgwaja 1:7b5469bf5994 111 for(int i=0; i<6; i++)
lsh3146 3:7b195612e26d 112 {
lsh3146 3:7b195612e26d 113 if(encoder_data[i]==encoder_error[i])
lsh3146 3:7b195612e26d 114 motor_power(i,0);//motor_offset[i]);
lsh3146 3:7b195612e26d 115 else
lsh3146 3:7b195612e26d 116 {
lsh3146 3:7b195612e26d 117 if(i < 3 && position_err_zero[i])
lsh3146 3:7b195612e26d 118 {
lsh3146 3:7b195612e26d 119 motor_power(i, 0);
lsh3146 3:7b195612e26d 120 }
lsh3146 3:7b195612e26d 121 else
lsh3146 3:7b195612e26d 122 {
lsh3146 3:7b195612e26d 123 motor_power(i,Speed_PID_OUTPUT[i]);
lsh3146 3:7b195612e26d 124 }
lsh3146 3:7b195612e26d 125 }
lsh3146 3:7b195612e26d 126 }
lsh3146 3:7b195612e26d 127
lsh3146 3:7b195612e26d 128 if(filter_dif_encoder_data[4]<=2 && filter_dif_encoder_data[4]>=-2)
lsh3146 3:7b195612e26d 129 {
lsh3146 3:7b195612e26d 130 if(Speed_PID_OUTPUT[4]>motor_offset[4])
lsh3146 3:7b195612e26d 131 motor_offset[4]+=0.2;
lsh3146 3:7b195612e26d 132 else if(Speed_PID_OUTPUT[4]<motor_offset[4])
lsh3146 3:7b195612e26d 133 motor_offset[4]-=0.2;
lsh3146 3:7b195612e26d 134 }
lsh3146 3:7b195612e26d 135
lsh3146 3:7b195612e26d 136 if(filter_dif_encoder_data[5]<=2 && filter_dif_encoder_data[5]>=-2)
lsh3146 3:7b195612e26d 137 {
lsh3146 3:7b195612e26d 138 if(Speed_PID_OUTPUT[5]>motor_offset[5])
lsh3146 3:7b195612e26d 139 motor_offset[5]+=0.2;
lsh3146 3:7b195612e26d 140 else if(Speed_PID_OUTPUT[5]<motor_offset[5])
lsh3146 3:7b195612e26d 141 motor_offset[5]-=0.2;
lsh3146 3:7b195612e26d 142 }
lsh3146 3:7b195612e26d 143
lsh3146 3:7b195612e26d 144 //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]);
lsh3146 3:7b195612e26d 145
lsh3146 3:7b195612e26d 146 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)
lsh3146 3:7b195612e26d 147 {
lsh3146 3:7b195612e26d 148 while(1)
lsh3146 3:7b195612e26d 149 {
lsh3146 3:7b195612e26d 150 pc.printf("while(1)\r\n");
lsh3146 3:7b195612e26d 151 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]);
lsh3146 3:7b195612e26d 152
lsh3146 3:7b195612e26d 153 encoder_data[0] = encoder1.read();
lsh3146 3:7b195612e26d 154 if(encoder_data[0]!=-1)
lsh3146 3:7b195612e26d 155 {
lsh3146 3:7b195612e26d 156 NVIC_SystemReset();
lsh3146 3:7b195612e26d 157 }
lsh3146 3:7b195612e26d 158 wait_us(2500);
lsh3146 3:7b195612e26d 159 }
lsh3146 3:7b195612e26d 160 }
lsh3146 3:7b195612e26d 161
lsh3146 3:7b195612e26d 162 a_cnt++;
lsh3146 3:7b195612e26d 163 if(a_cnt>9)
lsh3146 3:7b195612e26d 164 {
lsh3146 3:7b195612e26d 165 a_cnt=0;
lsh3146 3:7b195612e26d 166 //input_data_Analysis();
lsh3146 3:7b195612e26d 167 }
lsh3146 2:14b52dee1c35 168
lsh3146 3:7b195612e26d 169 A_ex_cmd_roll=cmd_roll;
lsh3146 3:7b195612e26d 170 A_ex_cmd_pitch=cmd_pitch;
lsh3146 3:7b195612e26d 171 A_ex_cmd_heave=cmd_heave;
lsh3146 3:7b195612e26d 172 A_ex_cmd_sway=cmd_sway;
lsh3146 3:7b195612e26d 173 A_ex_cmd_surge=cmd_surge;
lsh3146 3:7b195612e26d 174 A_ex_cmd_yaw=cmd_yaw;
gohgwaja 0:7cff999a7f5c 175 }
gohgwaja 0:7cff999a7f5c 176 }
gohgwaja 0:7cff999a7f5c 177
lsh3146 3:7b195612e26d 178
gohgwaja 0:7cff999a7f5c 179 void command_init()
gohgwaja 0:7cff999a7f5c 180 {
gohgwaja 0:7cff999a7f5c 181 cmd_roll = 32768;
gohgwaja 0:7cff999a7f5c 182 cmd_pitch = 32768;
gohgwaja 0:7cff999a7f5c 183 cmd_heave = 32768;
gohgwaja 0:7cff999a7f5c 184 cmd_sway = 32768;
gohgwaja 0:7cff999a7f5c 185 cmd_surge = 32768;
gohgwaja 0:7cff999a7f5c 186 cmd_yaw = 32768;
gohgwaja 0:7cff999a7f5c 187 }
gohgwaja 0:7cff999a7f5c 188
gohgwaja 0:7cff999a7f5c 189
gohgwaja 0:7cff999a7f5c 190 void pid_info()
gohgwaja 0:7cff999a7f5c 191 {
gohgwaja 0:7cff999a7f5c 192 pc.printf(" Speed_Pgain Speed_Igain position_Pgain Position_input_filter Speed_I_input_filter offset\r\n");
gohgwaja 0:7cff999a7f5c 193 for(int i = 0; i < 6; i++)
gohgwaja 0:7cff999a7f5c 194 {
gohgwaja 0:7cff999a7f5c 195 pc.printf("%2d. %6.3f %6.3f %6.3f %6.3f %6.3f %4d\r\n", i + 1, Speed_Pgain[i], Speed_Igain[i], position_Pgain[i], Position_input_filter[i], Speed_I_input_filter[i], (int)offset[i]);
gohgwaja 0:7cff999a7f5c 196 }
gohgwaja 0:7cff999a7f5c 197 pc.printf("\n\n\n");
gohgwaja 0:7cff999a7f5c 198
gohgwaja 0:7cff999a7f5c 199 }
gohgwaja 0:7cff999a7f5c 200
gohgwaja 0:7cff999a7f5c 201 void Menual_Position_PID()
gohgwaja 0:7cff999a7f5c 202 {
gohgwaja 0:7cff999a7f5c 203 for(int i=0; i<6; i++)
gohgwaja 0:7cff999a7f5c 204 {
gohgwaja 0:7cff999a7f5c 205 taget_speed[i]=button_offset_posion[i]/5;
gohgwaja 0:7cff999a7f5c 206 }
gohgwaja 0:7cff999a7f5c 207 }
gohgwaja 0:7cff999a7f5c 208
gohgwaja 0:7cff999a7f5c 209