lee sanghun / Mbed 2 deprecated 2020_07_28_10_41

test4

Dependencies:   mbed BufferedSerial LS7366LIB2 FastPWM

main.cpp@4:bf278ddb8504, 2020-12-08 (annotated)

Committer:
lsh3146
Date:
Tue Dec 08 01:25:06 2020 +0000
Revision:
4:bf278ddb8504
Parent:
2:14b52dee1c35 
aaaaaqqqqq

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