広田 勇斗 / MD_PID

MD_PID

Dependencies:   QEI

Dependents:   Omni_2017_z BETA_A ALPHA_A GAMMA_A ... more

MD_PID.cpp@0:fe6ec67653f4, 2016-08-18 (annotated)

Committer:
Auk
Date:
Thu Aug 18 06:36:14 2016 +0000
Revision:
0:fe6ec67653f4
Child:
2:5b2331251b3e
First Commit.

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 {
Auk 0:fe6ec67653f4 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_;
Auk 0:fe6ec67653f4 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_;
Auk 0:fe6ec67653f4 21
Auk 0:fe6ec67653f4 22 Reset();
Auk 0:fe6ec67653f4 23 }
Auk 0:fe6ec67653f4 24
Auk 0:fe6ec67653f4 25 double MD_PID::Drive(double ref)
Auk 0:fe6ec67653f4 26 {
Auk 0:fe6ec67653f4 27 /*
Auk 0:fe6ec67653f4 28 if(ref1 != ref)
Auk 0:fe6ec67653f4 29 {
Auk 0:fe6ec67653f4 30 Reset();
Auk 0:fe6ec67653f4 31 ref1 = ref;
Auk 0:fe6ec67653f4 32 }
Auk 0:fe6ec67653f4 33 */
Auk 0:fe6ec67653f4 34 if(ref == 0)
Auk 0:fe6ec67653f4 35 {
Auk 0:fe6ec67653f4 36 duty = 0;
Auk 0:fe6ec67653f4 37 md->rotate(duty);
Auk 0:fe6ec67653f4 38 Read_Speed();
Auk 0:fe6ec67653f4 39 }
Auk 0:fe6ec67653f4 40 else
Auk 0:fe6ec67653f4 41 {
Auk 0:fe6ec67653f4 42 if(kp >= 1.0){
Auk 0:fe6ec67653f4 43 duty = ref / kp;
Auk 0:fe6ec67653f4 44 Read_Speed();
Auk 0:fe6ec67653f4 45 }
Auk 0:fe6ec67653f4 46 else
Auk 0:fe6ec67653f4 47 duty -= PID(ref);
Auk 0:fe6ec67653f4 48
Auk 0:fe6ec67653f4 49 if(duty > 1.0)
Auk 0:fe6ec67653f4 50 duty = 1.0;
Auk 0:fe6ec67653f4 51 else if(duty < -1.0)
Auk 0:fe6ec67653f4 52 duty = -1.0;
Auk 0:fe6ec67653f4 53 }
Auk 0:fe6ec67653f4 54
Auk 0:fe6ec67653f4 55 if( ( (ref >= 0) ^ (Speed() >= 0) ) && fabs(Speed()) > 0.01)
Auk 0:fe6ec67653f4 56 duty = 0;
Auk 0:fe6ec67653f4 57
Auk 0:fe6ec67653f4 58 md->rotate(duty);
Auk 0:fe6ec67653f4 59 return duty;
Auk 0:fe6ec67653f4 60 }
Auk 0:fe6ec67653f4 61
Auk 0:fe6ec67653f4 62 double MD_PID::PID(double target)
Auk 0:fe6ec67653f4 63 {
Auk 0:fe6ec67653f4 64 diff[0] = diff[1];
Auk 0:fe6ec67653f4 65 //delta = timer.read();
Auk 0:fe6ec67653f4 66 //qs = (double)(qei->Read()) / delta / (double)pulse ;
Auk 0:fe6ec67653f4 67 //timer.reset();
Auk 0:fe6ec67653f4 68 //qei->Reset();
Auk 0:fe6ec67653f4 69 Read_Speed();
Auk 0:fe6ec67653f4 70 diff[1] = qs - target;
Auk 0:fe6ec67653f4 71 integral += ((diff[0] + diff[1]) / 2.0) * delta;
Auk 0:fe6ec67653f4 72 p = kp * diff[1];
Auk 0:fe6ec67653f4 73 i = ki * integral;
Auk 0:fe6ec67653f4 74 d = kd * (diff[1] - diff[0]);
Auk 0:fe6ec67653f4 75 return p + i + d;
Auk 0:fe6ec67653f4 76 }
Auk 0:fe6ec67653f4 77
Auk 0:fe6ec67653f4 78 void MD_PID::Read_Speed()
Auk 0:fe6ec67653f4 79 {
Auk 0:fe6ec67653f4 80 delta = timer.read();
Auk 0:fe6ec67653f4 81 qs = (double)(qei->Read()) / delta / (double)pulse ;
Auk 0:fe6ec67653f4 82 timer.reset();
Auk 0:fe6ec67653f4 83 qei->Reset();
Auk 0:fe6ec67653f4 84 }
Auk 0:fe6ec67653f4 85
Auk 0:fe6ec67653f4 86 double MD_PID::Speed()
Auk 0:fe6ec67653f4 87 {
Auk 0:fe6ec67653f4 88 return qs;
Auk 0:fe6ec67653f4 89 }
Auk 0:fe6ec67653f4 90
Auk 0:fe6ec67653f4 91 void MD_PID::Reset()
Auk 0:fe6ec67653f4 92 {
Auk 0:fe6ec67653f4 93 qs = 0;
Auk 0:fe6ec67653f4 94 diff[0] = 0;
Auk 0:fe6ec67653f4 95 diff[1] = 0.0;
Auk 0:fe6ec67653f4 96 integral = 0.0;
Auk 0:fe6ec67653f4 97 timer.reset();
Auk 0:fe6ec67653f4 98 timer.start();
Auk 0:fe6ec67653f4 99 qei->Reset();
Auk 0:fe6ec67653f4 100 }
Auk 0:fe6ec67653f4 101
Auk 0:fe6ec67653f4 102 void MD_PID::PID_Step(double interval)
Auk 0:fe6ec67653f4 103 {
Auk 0:fe6ec67653f4 104 double time[200],speed[200];
Auk 0:fe6ec67653f4 105 Timer ST;
Auk 0:fe6ec67653f4 106 ST.reset();
Auk 0:fe6ec67653f4 107 ST.start();
Auk 0:fe6ec67653f4 108 md->rotate(-1);
Auk 0:fe6ec67653f4 109 for(int i = 0; i < 200; i++){
Auk 0:fe6ec67653f4 110 PID(0);
Auk 0:fe6ec67653f4 111 time[i] = ST.read();
Auk 0:fe6ec67653f4 112 speed[i] = Speed();
Auk 0:fe6ec67653f4 113 wait(interval);
Auk 0:fe6ec67653f4 114 }
Auk 0:fe6ec67653f4 115 md->rotate(0);
Auk 0:fe6ec67653f4 116 for(int i = 0; i < 200; i++){
Auk 0:fe6ec67653f4 117 printf("%f\t%f\r\n",time[i],speed[i]);
Auk 0:fe6ec67653f4 118 }
Auk 0:fe6ec67653f4 119 }
Auk 0:fe6ec67653f4 120
Auk 0:fe6ec67653f4 121 MD_PID* Create_MD_PID( int number, int i2c_addr, I2C *i2c, // QEI
Auk 0:fe6ec67653f4 122 double kp_, double ki_, double kd_, int pulse_, // PID
Auk 0:fe6ec67653f4 123 PinName pwm, PinName dere) // MD
Auk 0:fe6ec67653f4 124 {
Auk 0:fe6ec67653f4 125 QEI *qei = new QEI(number, i2c_addr, i2c);
Auk 0:fe6ec67653f4 126 printf("qei\r\n");
Auk 0:fe6ec67653f4 127 MD *md = new MD(pwm, dere);
Auk 0:fe6ec67653f4 128 printf("md\r\n");
Auk 0:fe6ec67653f4 129 MD_PID *md_pid = new MD_PID(md, kp_, ki_, kd_, pulse_, qei);
Auk 0:fe6ec67653f4 130 printf("md_pid\r\n");
Auk 0:fe6ec67653f4 131 return md_pid;
Auk 0:fe6ec67653f4 132 }