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?
User | Revision | Line number | New 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 |