yuji fujita / pid

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   backdrive backdrive_3

pid.cpp@0:40cfee72b03a, 2019-07-01 (annotated)

Committer:
fuji_
Date:
Mon Jul 01 06:44:11 2019 +0000
Revision:
0:40cfee72b03a
20190701backdrive

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fuji_ 0:40cfee72b03a 1 #include "pid.h"
fuji_ 0:40cfee72b03a 2
fuji_ 0:40cfee72b03a 3 //position
fuji_ 0:40cfee72b03a 4 void PositionPid::period(float control_cycle){
fuji_ 0:40cfee72b03a 5 dt = control_cycle;
fuji_ 0:40cfee72b03a 6 }
fuji_ 0:40cfee72b03a 7
fuji_ 0:40cfee72b03a 8 void PositionPid::gain(float kp_, float ki_, float kd_) {
fuji_ 0:40cfee72b03a 9 kp = kp_;
fuji_ 0:40cfee72b03a 10 ki = ki_;
fuji_ 0:40cfee72b03a 11 kd = kd_;
fuji_ 0:40cfee72b03a 12 }
fuji_ 0:40cfee72b03a 13
fuji_ 0:40cfee72b03a 14 void PositionPid::reset(){
fuji_ 0:40cfee72b03a 15 for(int i=NOW;i<=OLD;i++){
fuji_ 0:40cfee72b03a 16 distance[i] = 0;
fuji_ 0:40cfee72b03a 17 difference[i] = 0;
fuji_ 0:40cfee72b03a 18 }
fuji_ 0:40cfee72b03a 19
fuji_ 0:40cfee72b03a 20 for(int i=P; i<=D; i++){
fuji_ 0:40cfee72b03a 21 result[i] = 0;
fuji_ 0:40cfee72b03a 22 }
fuji_ 0:40cfee72b03a 23 }
fuji_ 0:40cfee72b03a 24
fuji_ 0:40cfee72b03a 25 void PositionPid::cal(float current,float target){
fuji_ 0:40cfee72b03a 26 distance[NOW] = current;
fuji_ 0:40cfee72b03a 27
fuji_ 0:40cfee72b03a 28 difference[OLD] = difference[NOW];
fuji_ 0:40cfee72b03a 29 difference[NOW] = target - distance[NOW];
fuji_ 0:40cfee72b03a 30
fuji_ 0:40cfee72b03a 31 result[P] = difference[NOW];
fuji_ 0:40cfee72b03a 32 result[I] += (difference[NOW]+difference[OLD])/2 * dt;
fuji_ 0:40cfee72b03a 33 result[D] = (distance[NOW] - distance[OLD])/dt;
fuji_ 0:40cfee72b03a 34
fuji_ 0:40cfee72b03a 35 result[OUTPUT] = kp*result[P] + ki*result[I] + kd*result[D];
fuji_ 0:40cfee72b03a 36
fuji_ 0:40cfee72b03a 37 if(result[OUTPUT] > pid_constant::UPPER_LIMIT){
fuji_ 0:40cfee72b03a 38 result[OUTPUT] = pid_constant::UPPER_LIMIT;
fuji_ 0:40cfee72b03a 39 } else if(result[OUTPUT] < pid_constant::LOWER_LIMIT){
fuji_ 0:40cfee72b03a 40 result[OUTPUT] = pid_constant::LOWER_LIMIT;
fuji_ 0:40cfee72b03a 41 }
fuji_ 0:40cfee72b03a 42
fuji_ 0:40cfee72b03a 43 distance[OLD] = distance[NOW];
fuji_ 0:40cfee72b03a 44 }
fuji_ 0:40cfee72b03a 45
fuji_ 0:40cfee72b03a 46 float PositionPid::getState(int channel){
fuji_ 0:40cfee72b03a 47 float output;
fuji_ 0:40cfee72b03a 48 switch(channel){
fuji_ 0:40cfee72b03a 49 case P:
fuji_ 0:40cfee72b03a 50 output = result[P];
fuji_ 0:40cfee72b03a 51 case I:
fuji_ 0:40cfee72b03a 52 output = result[I];
fuji_ 0:40cfee72b03a 53 case D:
fuji_ 0:40cfee72b03a 54 output = result[D];
fuji_ 0:40cfee72b03a 55 case OUTPUT:
fuji_ 0:40cfee72b03a 56 output = result[OUTPUT];
fuji_ 0:40cfee72b03a 57 }
fuji_ 0:40cfee72b03a 58 return output;
fuji_ 0:40cfee72b03a 59 }
fuji_ 0:40cfee72b03a 60
fuji_ 0:40cfee72b03a 61
fuji_ 0:40cfee72b03a 62 //speed
fuji_ 0:40cfee72b03a 63 void SpeedPid::cal(float target_speed, float now_speed){
fuji_ 0:40cfee72b03a 64 diffelence[TWO_OLD] = diffelence[OLD];
fuji_ 0:40cfee72b03a 65 diffelence[OLD] = diffelence[NOW];
fuji_ 0:40cfee72b03a 66 diffelence[NOW] = target_speed - now_speed;
fuji_ 0:40cfee72b03a 67
fuji_ 0:40cfee72b03a 68 result[P] = diffelence[NOW] - diffelence[OLD];
fuji_ 0:40cfee72b03a 69 result[I] = diffelence[NOW] * dt;
fuji_ 0:40cfee72b03a 70 result[D] = diffelence[NOW] - diffelence[OLD]*2 + diffelence[TWO_OLD];
fuji_ 0:40cfee72b03a 71
fuji_ 0:40cfee72b03a 72 result[OUTPUT] += (result[P]*gain[P] + result[I]*gain[I] + result[D]*gain[D]);
fuji_ 0:40cfee72b03a 73
fuji_ 0:40cfee72b03a 74 if(result[OUTPUT] > 1.0f){
fuji_ 0:40cfee72b03a 75 result[OUTPUT] = 1.0f;
fuji_ 0:40cfee72b03a 76 } else if(result[OUTPUT] < -1.0f){
fuji_ 0:40cfee72b03a 77 result[OUTPUT] = -1.0f;
fuji_ 0:40cfee72b03a 78 }
fuji_ 0:40cfee72b03a 79 }
fuji_ 0:40cfee72b03a 80
fuji_ 0:40cfee72b03a 81 float SpeedPid::getState(int channel){
fuji_ 0:40cfee72b03a 82 float output;
fuji_ 0:40cfee72b03a 83 switch(channel){
fuji_ 0:40cfee72b03a 84 case P:
fuji_ 0:40cfee72b03a 85 output = result[P];
fuji_ 0:40cfee72b03a 86 case I:
fuji_ 0:40cfee72b03a 87 output = result[I];
fuji_ 0:40cfee72b03a 88 case D:
fuji_ 0:40cfee72b03a 89 output = result[D];
fuji_ 0:40cfee72b03a 90 case OUTPUT:
fuji_ 0:40cfee72b03a 91 output = result[OUTPUT];
fuji_ 0:40cfee72b03a 92 }
fuji_ 0:40cfee72b03a 93 return output;
fuji_ 0:40cfee72b03a 94 }
fuji_ 0:40cfee72b03a 95
fuji_ 0:40cfee72b03a 96 void SpeedPid::init(float kp_, float ki_, float kd_, float dt_){
fuji_ 0:40cfee72b03a 97 gain[P] = kp_;
fuji_ 0:40cfee72b03a 98 gain[I] = ki_;
fuji_ 0:40cfee72b03a 99 gain[D] = kd_;
fuji_ 0:40cfee72b03a 100 dt = dt_;
fuji_ 0:40cfee72b03a 101 }