lee sanghun / Mbed 2 deprecated 2020_07_28_10_41

test4

Dependencies:   mbed BufferedSerial LS7366LIB2 FastPWM

speed_pid.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 _SPEED_PID_H_
gohgwaja 0:7cff999a7f5c 2 #define _SPEED_PID_H_
gohgwaja 0:7cff999a7f5c 3
gohgwaja 0:7cff999a7f5c 4 double taget_speed[6]={0,};
lsh3146 3:7b195612e26d 5 double ex_taget_speed[6]={0,};
lsh3146 3:7b195612e26d 6 double dif_taget_speed[6]={0,};
lsh3146 3:7b195612e26d 7 double filterd_dif_taget_speed[6]={0,0,0,0,0,0,};
lsh3146 3:7b195612e26d 8
gohgwaja 0:7cff999a7f5c 9 double error_speed[6]={0,};
gohgwaja 0:7cff999a7f5c 10 double Speed_PID_OUTPUT[7]={0,};
lsh3146 3:7b195612e26d 11 double ex_Speed_PID_OUTPUT[7]={0,};
gohgwaja 0:7cff999a7f5c 12
gohgwaja 0:7cff999a7f5c 13 double result_i[6]={0,};
gohgwaja 0:7cff999a7f5c 14 double filter_result_i[6]={0,};
gohgwaja 0:7cff999a7f5c 15 double output_i[6]={0,};
gohgwaja 0:7cff999a7f5c 16
lsh3146 3:7b195612e26d 17
lsh3146 3:7b195612e26d 18 void cal_speed_error()
gohgwaja 0:7cff999a7f5c 19 {
gohgwaja 0:7cff999a7f5c 20 for(int i=0;i<6;i++)
gohgwaja 0:7cff999a7f5c 21 {
lsh3146 3:7b195612e26d 22 if(i < 3 && taget_speed[i] == 0)
lsh3146 3:7b195612e26d 23 {
lsh3146 3:7b195612e26d 24 error_speed[i] = 0;
lsh3146 3:7b195612e26d 25 }
lsh3146 3:7b195612e26d 26 else
lsh3146 3:7b195612e26d 27 {
lsh3146 3:7b195612e26d 28 error_speed[i]=filter_dif_encoder_data[i]-taget_speed[i];
lsh3146 3:7b195612e26d 29 }
lsh3146 3:7b195612e26d 30 }
lsh3146 3:7b195612e26d 31 for(int i=0;i<6;i++)
lsh3146 3:7b195612e26d 32 {
lsh3146 3:7b195612e26d 33 dif_taget_speed[i] = ex_taget_speed[i] - taget_speed[i];
lsh3146 3:7b195612e26d 34 ex_taget_speed[i]=taget_speed[i];
lsh3146 3:7b195612e26d 35
lsh3146 3:7b195612e26d 36 filterd_dif_taget_speed[i] = filterd_dif_taget_speed[i]*0.99 + dif_taget_speed[i]*0.01;
lsh3146 3:7b195612e26d 37 }
lsh3146 3:7b195612e26d 38
lsh3146 3:7b195612e26d 39 }
lsh3146 3:7b195612e26d 40
lsh3146 3:7b195612e26d 41
lsh3146 3:7b195612e26d 42 void Speed_I()
lsh3146 3:7b195612e26d 43 {
lsh3146 3:7b195612e26d 44 for(int i=0;i<6;i++)
lsh3146 3:7b195612e26d 45 {
lsh3146 3:7b195612e26d 46 result_i[i]+=error_speed[i]*Speed_Igain[i];
lsh3146 3:7b195612e26d 47
lsh3146 3:7b195612e26d 48
lsh3146 3:7b195612e26d 49 if(result_i[i]>500)
lsh3146 3:7b195612e26d 50 result_i[i]=500;
lsh3146 3:7b195612e26d 51 if(result_i[i]<-500)
lsh3146 3:7b195612e26d 52 result_i[i]=-500;
gohgwaja 0:7cff999a7f5c 53
lsh3146 3:7b195612e26d 54 output_i[i] = result_i[i];
gohgwaja 0:7cff999a7f5c 55 }
gohgwaja 0:7cff999a7f5c 56 }
gohgwaja 0:7cff999a7f5c 57 double output_p[6]={0,};
gohgwaja 0:7cff999a7f5c 58 void Speed_P()
gohgwaja 0:7cff999a7f5c 59 {
gohgwaja 0:7cff999a7f5c 60 for(int i=0;i<6;i++)
lsh3146 3:7b195612e26d 61 {
gohgwaja 0:7cff999a7f5c 62 output_p[i] = error_speed[i] * Speed_Pgain[i];
lsh3146 3:7b195612e26d 63 }
lsh3146 3:7b195612e26d 64
lsh3146 3:7b195612e26d 65 for(int i=0;i<3;i++)
lsh3146 3:7b195612e26d 66 {
lsh3146 3:7b195612e26d 67 int p_range = motor_gain[i]*500;
lsh3146 3:7b195612e26d 68
lsh3146 3:7b195612e26d 69 if(output_p[i]>p_range)
lsh3146 3:7b195612e26d 70 output_p[i]=p_range;
lsh3146 3:7b195612e26d 71 if(output_p[i]<-p_range)
lsh3146 3:7b195612e26d 72 output_p[i]=-p_range;
lsh3146 3:7b195612e26d 73 }
lsh3146 3:7b195612e26d 74
lsh3146 3:7b195612e26d 75
gohgwaja 0:7cff999a7f5c 76 }
gohgwaja 0:7cff999a7f5c 77
gohgwaja 0:7cff999a7f5c 78 void Speed_PID()
gohgwaja 0:7cff999a7f5c 79 {
gohgwaja 0:7cff999a7f5c 80 cal_speed_error();
gohgwaja 0:7cff999a7f5c 81 Speed_I();
gohgwaja 0:7cff999a7f5c 82 Speed_P();
gohgwaja 0:7cff999a7f5c 83
gohgwaja 0:7cff999a7f5c 84 for(int i=0;i<6;i++)
gohgwaja 0:7cff999a7f5c 85 {
gohgwaja 0:7cff999a7f5c 86 Speed_PID_OUTPUT[i] = output_i[i] + output_p[i];
gohgwaja 0:7cff999a7f5c 87 if(i > 2)
gohgwaja 0:7cff999a7f5c 88 {
gohgwaja 0:7cff999a7f5c 89 Speed_PID_OUTPUT[i] *= -1;
gohgwaja 0:7cff999a7f5c 90 }
gohgwaja 0:7cff999a7f5c 91 }
gohgwaja 0:7cff999a7f5c 92
gohgwaja 0:7cff999a7f5c 93 //pc.printf("%5.2f,%5.2f,%5.2f,%5.2f,%5.2f,%5.2f",error_speed[0],filter_dif_encoder_data[0],taget_speed[0],Speed_PID_OUTPUT[0],output_i[0],output_p[0]);
gohgwaja 0:7cff999a7f5c 94 //pc.printf("\n");
gohgwaja 0:7cff999a7f5c 95
gohgwaja 0:7cff999a7f5c 96 }
gohgwaja 0:7cff999a7f5c 97
gohgwaja 0:7cff999a7f5c 98 #endif