広田 勇斗 / MD_PID

MD_PID

Dependencies:   QEI

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?

UserRevisionLine numberNew 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 }