MD_PID
Dependents: Omni_2017_z BETA_A ALPHA_A GAMMA_A ... more
MD_PID.cpp@12:d930cbffd2c9, 2017-09-27 (annotated)
- Committer:
- hirotayamato
- Date:
- Wed Sep 27 06:49:24 2017 +0000
- Revision:
- 12:d930cbffd2c9
- Parent:
- 11:b621815834c1
MD_PID
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Auk | 0:fe6ec67653f4 | 1 | #include "mbed.h" |
Auk | 0:fe6ec67653f4 | 2 | #include "MD_PID.h" |
Auk | 0:fe6ec67653f4 | 3 | #include "QEI.h" |
Auk | 0:fe6ec67653f4 | 4 | #include "MD.h" |
Auk | 0:fe6ec67653f4 | 5 | |
Auk | 0:fe6ec67653f4 | 6 | MD_PID::MD_PID() |
Auk | 0:fe6ec67653f4 | 7 | { |
hirotayamato | 8:16a31803a60d | 8 | ref1 = 0; |
Auk | 0:fe6ec67653f4 | 9 | } |
Auk | 0:fe6ec67653f4 | 10 | |
Auk | 0:fe6ec67653f4 | 11 | MD_PID::MD_PID(MD *md_, double kp_, double ki_, double kd_, int pulse_, QEI *qei_) |
Auk | 0:fe6ec67653f4 | 12 | { |
Auk | 0:fe6ec67653f4 | 13 | md = md_; |
Auk | 0:fe6ec67653f4 | 14 | qei = qei_; |
hirotayamato | 8:16a31803a60d | 15 | |
Auk | 0:fe6ec67653f4 | 16 | ref1 = 0; |
Auk | 0:fe6ec67653f4 | 17 | kp = kp_; |
Auk | 0:fe6ec67653f4 | 18 | ki = ki_; |
Auk | 0:fe6ec67653f4 | 19 | kd = kd_; |
Auk | 0:fe6ec67653f4 | 20 | pulse = pulse_; |
hirotayamato | 8:16a31803a60d | 21 | |
Auk | 0:fe6ec67653f4 | 22 | Reset(); |
Auk | 0:fe6ec67653f4 | 23 | } |
Auk | 0:fe6ec67653f4 | 24 | |
hirotayamato | 12:d930cbffd2c9 | 25 | double MD_PID::Drive(int a, double ref, double stop) |
Auk | 0:fe6ec67653f4 | 26 | { |
hirotayamato | 12:d930cbffd2c9 | 27 | if(a) |
hirotayamato | 12:d930cbffd2c9 | 28 | ref *= 6; |
Auk | 0:fe6ec67653f4 | 29 | /* |
Auk | 0:fe6ec67653f4 | 30 | if(ref1 != ref) |
Auk | 0:fe6ec67653f4 | 31 | { |
Auk | 0:fe6ec67653f4 | 32 | Reset(); |
Auk | 0:fe6ec67653f4 | 33 | ref1 = ref; |
Auk | 0:fe6ec67653f4 | 34 | } |
Auk | 0:fe6ec67653f4 | 35 | */ |
Auk | 0:fe6ec67653f4 | 36 | if(ref == 0) |
Auk | 0:fe6ec67653f4 | 37 | { |
Auk | 0:fe6ec67653f4 | 38 | duty = 0; |
Auk | 0:fe6ec67653f4 | 39 | md->rotate(duty); |
Auk | 0:fe6ec67653f4 | 40 | Read_Speed(); |
Auk | 0:fe6ec67653f4 | 41 | } |
Auk | 0:fe6ec67653f4 | 42 | else |
Auk | 0:fe6ec67653f4 | 43 | { |
Auk | 0:fe6ec67653f4 | 44 | if(kp >= 1.0){ |
Auk | 0:fe6ec67653f4 | 45 | duty = ref / kp; |
Auk | 0:fe6ec67653f4 | 46 | Read_Speed(); |
Auk | 0:fe6ec67653f4 | 47 | } |
Auk | 0:fe6ec67653f4 | 48 | else |
hirotayamato | 12:d930cbffd2c9 | 49 | |
hirotayamato | 12:d930cbffd2c9 | 50 | if(a){ |
hirotayamato | 12:d930cbffd2c9 | 51 | duty -= PID(ref); |
hirotayamato | 12:d930cbffd2c9 | 52 | }else{ |
hirotayamato | 12:d930cbffd2c9 | 53 | duty = ref; |
hirotayamato | 12:d930cbffd2c9 | 54 | } |
hirotayamato | 8:16a31803a60d | 55 | |
Auk | 0:fe6ec67653f4 | 56 | if(duty > 1.0) |
Auk | 0:fe6ec67653f4 | 57 | duty = 1.0; |
Auk | 0:fe6ec67653f4 | 58 | else if(duty < -1.0) |
Auk | 0:fe6ec67653f4 | 59 | duty = -1.0; |
Auk | 0:fe6ec67653f4 | 60 | } |
hirotayamato | 8:16a31803a60d | 61 | |
Auk | 3:0157bb21ba04 | 62 | if( ( (ref >= 0) ^ (Speed() >= 0) ) && fabs(Speed()) > stop) |
Auk | 0:fe6ec67653f4 | 63 | duty = 0; |
hirotayamato | 8:16a31803a60d | 64 | |
hirotayamato | 11:b621815834c1 | 65 | //printf("duty:%.2f\r\n", duty); |
Auk | 0:fe6ec67653f4 | 66 | md->rotate(duty); |
Auk | 0:fe6ec67653f4 | 67 | return duty; |
Auk | 0:fe6ec67653f4 | 68 | } |
Auk | 0:fe6ec67653f4 | 69 | |
Auk | 0:fe6ec67653f4 | 70 | double MD_PID::PID(double target) |
Auk | 0:fe6ec67653f4 | 71 | { |
Auk | 0:fe6ec67653f4 | 72 | diff[0] = diff[1]; |
Auk | 0:fe6ec67653f4 | 73 | //delta = timer.read(); |
Auk | 0:fe6ec67653f4 | 74 | //qs = (double)(qei->Read()) / delta / (double)pulse ; |
Auk | 0:fe6ec67653f4 | 75 | //timer.reset(); |
Auk | 0:fe6ec67653f4 | 76 | //qei->Reset(); |
Auk | 0:fe6ec67653f4 | 77 | Read_Speed(); |
Auk | 0:fe6ec67653f4 | 78 | diff[1] = qs - target; |
Auk | 0:fe6ec67653f4 | 79 | integral += ((diff[0] + diff[1]) / 2.0) * delta; |
Auk | 0:fe6ec67653f4 | 80 | p = kp * diff[1]; |
Auk | 0:fe6ec67653f4 | 81 | i = ki * integral; |
Auk | 0:fe6ec67653f4 | 82 | d = kd * (diff[1] - diff[0]); |
Auk | 0:fe6ec67653f4 | 83 | return p + i + d; |
Auk | 0:fe6ec67653f4 | 84 | } |
Auk | 0:fe6ec67653f4 | 85 | |
Auk | 0:fe6ec67653f4 | 86 | void MD_PID::Read_Speed() |
Auk | 0:fe6ec67653f4 | 87 | { |
Auk | 0:fe6ec67653f4 | 88 | delta = timer.read(); |
hirotayamato | 4:374ca057d8d5 | 89 | qs = (double)(qei->getPulses()) / delta / (double)pulse ; |
Auk | 0:fe6ec67653f4 | 90 | timer.reset(); |
hirotayamato | 4:374ca057d8d5 | 91 | qei->reset(); |
Auk | 0:fe6ec67653f4 | 92 | } |
Auk | 0:fe6ec67653f4 | 93 | |
Auk | 0:fe6ec67653f4 | 94 | double MD_PID::Speed() |
Auk | 0:fe6ec67653f4 | 95 | { |
Auk | 0:fe6ec67653f4 | 96 | return qs; |
Auk | 0:fe6ec67653f4 | 97 | } |
Auk | 0:fe6ec67653f4 | 98 | |
Auk | 0:fe6ec67653f4 | 99 | void MD_PID::Reset() |
Auk | 0:fe6ec67653f4 | 100 | { |
Auk | 0:fe6ec67653f4 | 101 | qs = 0; |
Auk | 0:fe6ec67653f4 | 102 | diff[0] = 0; |
Auk | 0:fe6ec67653f4 | 103 | diff[1] = 0.0; |
Auk | 0:fe6ec67653f4 | 104 | integral = 0.0; |
Auk | 0:fe6ec67653f4 | 105 | timer.reset(); |
Auk | 0:fe6ec67653f4 | 106 | timer.start(); |
hirotayamato | 4:374ca057d8d5 | 107 | qei->reset(); |
Auk | 0:fe6ec67653f4 | 108 | } |
Auk | 0:fe6ec67653f4 | 109 | |
Auk | 0:fe6ec67653f4 | 110 | void MD_PID::PID_Step(double interval) |
Auk | 0:fe6ec67653f4 | 111 | { |
Auk | 0:fe6ec67653f4 | 112 | double time[200],speed[200]; |
Auk | 0:fe6ec67653f4 | 113 | Timer ST; |
Auk | 0:fe6ec67653f4 | 114 | ST.reset(); |
Auk | 0:fe6ec67653f4 | 115 | ST.start(); |
Auk | 0:fe6ec67653f4 | 116 | md->rotate(-1); |
Auk | 0:fe6ec67653f4 | 117 | for(int i = 0; i < 200; i++){ |
Auk | 0:fe6ec67653f4 | 118 | PID(0); |
hirotayamato | 8:16a31803a60d | 119 | time[i] = ST.read(); |
Auk | 0:fe6ec67653f4 | 120 | speed[i] = Speed(); |
Auk | 0:fe6ec67653f4 | 121 | wait(interval); |
Auk | 0:fe6ec67653f4 | 122 | } |
Auk | 0:fe6ec67653f4 | 123 | md->rotate(0); |
Auk | 0:fe6ec67653f4 | 124 | for(int i = 0; i < 200; i++){ |
Auk | 0:fe6ec67653f4 | 125 | printf("%f\t%f\r\n",time[i],speed[i]); |
Auk | 0:fe6ec67653f4 | 126 | } |
Auk | 0:fe6ec67653f4 | 127 | } |
Auk | 0:fe6ec67653f4 | 128 | |
hirotayamato | 8:16a31803a60d | 129 | MD_PID* Create_MD_PID( PinName channelA, PinName channelB, PinName index, int pulsesPerRev,QEI::Encoding encoding, |
hirotayamato | 8:16a31803a60d | 130 | double kp_, double ki_, double kd_, int pulse_, |
hirotayamato | 4:374ca057d8d5 | 131 | PinName pwm, PinName dere) |
Auk | 0:fe6ec67653f4 | 132 | { |
hirotayamato | 4:374ca057d8d5 | 133 | QEI *qei = new QEI (channelA, channelB, index, pulsesPerRev, encoding); |
Auk | 0:fe6ec67653f4 | 134 | printf("qei\r\n"); |
hirotayamato | 4:374ca057d8d5 | 135 | MD *md = new MD (pwm, dere); |
Auk | 0:fe6ec67653f4 | 136 | printf("md\r\n"); |
hirotayamato | 4:374ca057d8d5 | 137 | MD_PID *md_pid = new MD_PID (md, kp_, ki_, kd_, pulse_, qei); |
Auk | 0:fe6ec67653f4 | 138 | printf("md_pid\r\n"); |
Auk | 0:fe6ec67653f4 | 139 | return md_pid; |
Auk | 0:fe6ec67653f4 | 140 | } |