lee sanghun / Mbed 2 deprecated 2020_07_28_10_41

test4

Dependencies:   mbed BufferedSerial LS7366LIB2 FastPWM

speed_pid.h@0:7cff999a7f5c, 2020-05-11 (annotated)

Committer:
gohgwaja
Date:
Mon May 11 08:47:18 2020 +0000
Revision:
0:7cff999a7f5c
Child:
3:7b195612e26d
Child:
4:bf278ddb8504
hahaha

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,};
gohgwaja 0:7cff999a7f5c 5 double error_speed[6]={0,};
gohgwaja 0:7cff999a7f5c 6 double Speed_PID_OUTPUT[7]={0,};
gohgwaja 0:7cff999a7f5c 7
gohgwaja 0:7cff999a7f5c 8 void cal_speed_error()
gohgwaja 0:7cff999a7f5c 9 {
gohgwaja 0:7cff999a7f5c 10 for(int i=0;i<6;i++)
gohgwaja 0:7cff999a7f5c 11 error_speed[i]=filter_dif_encoder_data[i]-taget_speed[i];
gohgwaja 0:7cff999a7f5c 12
gohgwaja 0:7cff999a7f5c 13 }
gohgwaja 0:7cff999a7f5c 14
gohgwaja 0:7cff999a7f5c 15 double result_i[6]={0,};
gohgwaja 0:7cff999a7f5c 16 double filter_result_i[6]={0,};
gohgwaja 0:7cff999a7f5c 17 double output_i[6]={0,};
gohgwaja 0:7cff999a7f5c 18
gohgwaja 0:7cff999a7f5c 19 void Speed_I()
gohgwaja 0:7cff999a7f5c 20 {
gohgwaja 0:7cff999a7f5c 21 for(int i=0;i<6;i++)
gohgwaja 0:7cff999a7f5c 22 {
gohgwaja 0:7cff999a7f5c 23 if(filter_dif_encoder_data[i]<taget_speed[i])
gohgwaja 0:7cff999a7f5c 24 result_i[i]+=1*Speed_Igain[i];
gohgwaja 0:7cff999a7f5c 25 else if(filter_dif_encoder_data[i]>taget_speed[i])
gohgwaja 0:7cff999a7f5c 26 result_i[i]-=1*Speed_Igain[i];
gohgwaja 0:7cff999a7f5c 27
gohgwaja 0:7cff999a7f5c 28 filter_result_i[i] = filter_result_i[i]*(1-Speed_I_input_filter[i]) + result_i[i]*Speed_I_input_filter[i];
gohgwaja 0:7cff999a7f5c 29 output_i[i] = - filter_result_i[i];
gohgwaja 0:7cff999a7f5c 30 }
gohgwaja 0:7cff999a7f5c 31 }
gohgwaja 0:7cff999a7f5c 32 double output_p[6]={0,};
gohgwaja 0:7cff999a7f5c 33 void Speed_P()
gohgwaja 0:7cff999a7f5c 34 {
gohgwaja 0:7cff999a7f5c 35 for(int i=0;i<6;i++)
gohgwaja 0:7cff999a7f5c 36 output_p[i] = error_speed[i] * Speed_Pgain[i];
gohgwaja 0:7cff999a7f5c 37 }
gohgwaja 0:7cff999a7f5c 38
gohgwaja 0:7cff999a7f5c 39 void Speed_PID()
gohgwaja 0:7cff999a7f5c 40 {
gohgwaja 0:7cff999a7f5c 41 cal_speed_error();
gohgwaja 0:7cff999a7f5c 42 Speed_I();
gohgwaja 0:7cff999a7f5c 43 Speed_P();
gohgwaja 0:7cff999a7f5c 44
gohgwaja 0:7cff999a7f5c 45 for(int i=0;i<6;i++)
gohgwaja 0:7cff999a7f5c 46 {
gohgwaja 0:7cff999a7f5c 47 Speed_PID_OUTPUT[i] = output_i[i] + output_p[i];
gohgwaja 0:7cff999a7f5c 48 if(i > 2)
gohgwaja 0:7cff999a7f5c 49 {
gohgwaja 0:7cff999a7f5c 50 Speed_PID_OUTPUT[i] *= -1;
gohgwaja 0:7cff999a7f5c 51 }
gohgwaja 0:7cff999a7f5c 52 }
gohgwaja 0:7cff999a7f5c 53
gohgwaja 0:7cff999a7f5c 54 //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 55 //pc.printf("\n");
gohgwaja 0:7cff999a7f5c 56
gohgwaja 0:7cff999a7f5c 57 }
gohgwaja 0:7cff999a7f5c 58
gohgwaja 0:7cff999a7f5c 59 #endif