Robert Zalog / Mbed 2 deprecated hermes_copy

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.

Dependencies:   mbed QEI beep

pid_controller.cpp@0:7a97ebb833eb, 2019-05-05 (annotated)

Committer:
rzalog
Date:
Sun May 05 00:02:11 2019 +0000
Revision:
0:7a97ebb833eb
test commit;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rzalog 0:7a97ebb833eb 1 #include "pid_controller.h"
rzalog 0:7a97ebb833eb 2 #include "globals.h"
rzalog 0:7a97ebb833eb 3
rzalog 0:7a97ebb833eb 4 PIDController::PIDController() {
rzalog 0:7a97ebb833eb 5 this->reset();
rzalog 0:7a97ebb833eb 6 }
rzalog 0:7a97ebb833eb 7
rzalog 0:7a97ebb833eb 8 void PIDController::reset() volatile {
rzalog 0:7a97ebb833eb 9 m_goalW = 0;
rzalog 0:7a97ebb833eb 10 m_goalX = 0;
rzalog 0:7a97ebb833eb 11
rzalog 0:7a97ebb833eb 12 m_pwmW = 0;
rzalog 0:7a97ebb833eb 13 m_pwmX = 0;
rzalog 0:7a97ebb833eb 14
rzalog 0:7a97ebb833eb 15 m_errorW = 0;
rzalog 0:7a97ebb833eb 16 m_errorX = 0;
rzalog 0:7a97ebb833eb 17 m_errorW_old = 0;
rzalog 0:7a97ebb833eb 18 m_errorX_old = 0;
rzalog 0:7a97ebb833eb 19
rzalog 0:7a97ebb833eb 20 m_countsW = 0;
rzalog 0:7a97ebb833eb 21 m_countsX = 0;
rzalog 0:7a97ebb833eb 22
rzalog 0:7a97ebb833eb 23 m_doneCounter = 0;
rzalog 0:7a97ebb833eb 24
rzalog 0:7a97ebb833eb 25 encoders.reset();
rzalog 0:7a97ebb833eb 26 }
rzalog 0:7a97ebb833eb 27
rzalog 0:7a97ebb833eb 28 void PIDController::update() volatile {
rzalog 0:7a97ebb833eb 29 getSensorFeedback();
rzalog 0:7a97ebb833eb 30 x_controller();
rzalog 0:7a97ebb833eb 31 w_controller();
rzalog 0:7a97ebb833eb 32 updateMotorPwm();
rzalog 0:7a97ebb833eb 33 }
rzalog 0:7a97ebb833eb 34
rzalog 0:7a97ebb833eb 35 void PIDController::setXGoal(int counts) {
rzalog 0:7a97ebb833eb 36 m_goalX = counts;
rzalog 0:7a97ebb833eb 37 }
rzalog 0:7a97ebb833eb 38
rzalog 0:7a97ebb833eb 39 void PIDController::setWGoal(int counts) {
rzalog 0:7a97ebb833eb 40 m_goalW = counts;
rzalog 0:7a97ebb833eb 41 }
rzalog 0:7a97ebb833eb 42
rzalog 0:7a97ebb833eb 43 bool PIDController::isDone() volatile {
rzalog 0:7a97ebb833eb 44 return m_doneCounter > done_limit;
rzalog 0:7a97ebb833eb 45 }
rzalog 0:7a97ebb833eb 46
rzalog 0:7a97ebb833eb 47 char* PIDController::getData() {
rzalog 0:7a97ebb833eb 48 sprintf(buf, "goalx: %d\tgoalw: %d\tpwmr: %.3f\tpwml: %.3f\tpwmx: %.3f\tpwmw: %.3f\terrorx: %d\terrorw: %d\n",
rzalog 0:7a97ebb833eb 49 m_goalX,
rzalog 0:7a97ebb833eb 50 m_goalW,
rzalog 0:7a97ebb833eb 51 m_pwmX + m_pwmW,
rzalog 0:7a97ebb833eb 52 m_pwmX - m_pwmW,
rzalog 0:7a97ebb833eb 53 m_pwmX,
rzalog 0:7a97ebb833eb 54 m_pwmW,
rzalog 0:7a97ebb833eb 55 m_errorX,
rzalog 0:7a97ebb833eb 56 m_errorW);
rzalog 0:7a97ebb833eb 57 return buf;
rzalog 0:7a97ebb833eb 58 }
rzalog 0:7a97ebb833eb 59
rzalog 0:7a97ebb833eb 60 /**
rzalog 0:7a97ebb833eb 61 * Private functions to do the internal work for PID.
rzalog 0:7a97ebb833eb 62 **/
rzalog 0:7a97ebb833eb 63 void PIDController::getSensorFeedback() volatile {
rzalog 0:7a97ebb833eb 64 int right, left;
rzalog 0:7a97ebb833eb 65 right = encoders.right();
rzalog 0:7a97ebb833eb 66 left = encoders.left();
rzalog 0:7a97ebb833eb 67
rzalog 0:7a97ebb833eb 68 m_countsW = right - left;
rzalog 0:7a97ebb833eb 69 m_countsX = (right + left) / 2;
rzalog 0:7a97ebb833eb 70 }
rzalog 0:7a97ebb833eb 71
rzalog 0:7a97ebb833eb 72 void PIDController::x_controller() volatile {
rzalog 0:7a97ebb833eb 73 m_errorX = m_goalX - m_countsX;
rzalog 0:7a97ebb833eb 74 m_pwmX = KpX * m_errorX + KdX * (m_errorX - m_errorX_old);
rzalog 0:7a97ebb833eb 75 m_errorX_old = m_errorX;
rzalog 0:7a97ebb833eb 76 }
rzalog 0:7a97ebb833eb 77
rzalog 0:7a97ebb833eb 78 void PIDController::w_controller() volatile {
rzalog 0:7a97ebb833eb 79 m_errorW = m_goalW - m_countsW;
rzalog 0:7a97ebb833eb 80 m_pwmW = KpW * m_errorW + KdW * (m_errorW - m_errorW_old);
rzalog 0:7a97ebb833eb 81 m_errorW_old = m_errorW;
rzalog 0:7a97ebb833eb 82 }
rzalog 0:7a97ebb833eb 83
rzalog 0:7a97ebb833eb 84 float limit(float val, float min, float max) {
rzalog 0:7a97ebb833eb 85 if (abs(val) > max) {
rzalog 0:7a97ebb833eb 86 if (val > 0) val = max;
rzalog 0:7a97ebb833eb 87 else if (val < 0) val = -max;
rzalog 0:7a97ebb833eb 88 }
rzalog 0:7a97ebb833eb 89 else if (abs(val) < min) {
rzalog 0:7a97ebb833eb 90 if (val > 0) val = min;
rzalog 0:7a97ebb833eb 91 else if (val < 0) val = -min;
rzalog 0:7a97ebb833eb 92 }
rzalog 0:7a97ebb833eb 93
rzalog 0:7a97ebb833eb 94 return val;
rzalog 0:7a97ebb833eb 95 }
rzalog 0:7a97ebb833eb 96
rzalog 0:7a97ebb833eb 97 void PIDController::updateMotorPwm() volatile {
rzalog 0:7a97ebb833eb 98 m_pwmX = limit(m_pwmX, 0, max_speed);
rzalog 0:7a97ebb833eb 99 m_pwmW = limit(m_pwmW, 0, max_speed);
rzalog 0:7a97ebb833eb 100
rzalog 0:7a97ebb833eb 101 float right = m_pwmX + m_pwmW;
rzalog 0:7a97ebb833eb 102 float left = m_pwmX - m_pwmW;
rzalog 0:7a97ebb833eb 103
rzalog 0:7a97ebb833eb 104 if (abs(right) < min_speed && abs(left) < min_speed) {
rzalog 0:7a97ebb833eb 105 m_doneCounter += 1;
rzalog 0:7a97ebb833eb 106 }
rzalog 0:7a97ebb833eb 107 else {
rzalog 0:7a97ebb833eb 108 motors.setRightPwm(m_pwmX + m_pwmW);
rzalog 0:7a97ebb833eb 109 motors.setLeftPwm(m_pwmX - m_pwmW);
rzalog 0:7a97ebb833eb 110 }
rzalog 0:7a97ebb833eb 111 }
rzalog 0:7a97ebb833eb 112