lee sanghun / Mbed 2 deprecated 2020_07_28_10_41

test4

Dependencies:   mbed BufferedSerial LS7366LIB2 FastPWM

speed_pid.h@4:bf278ddb8504, 2020-12-08 (annotated)

Committer:
lsh3146
Date:
Tue Dec 08 01:25:06 2020 +0000
Revision:
4:bf278ddb8504
Parent:
0:7cff999a7f5c 
aaaaaqqqqq

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