Rob Griffith / Mbed 2 deprecated rat_code

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

src/pid_controller.cpp@1:6f18bb7a77a5, 2018-11-15 (annotated)

Committer:
dionigi
Date:
Thu Nov 15 17:19:20 2018 +0000
Revision:
1:6f18bb7a77a5
Child:
3:35deb5c21b33
Added assignment3 files

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dionigi 1:6f18bb7a77a5 1 #include "pid_controller.h"
dionigi 1:6f18bb7a77a5 2 #include "globals.h"
dionigi 1:6f18bb7a77a5 3
dionigi 1:6f18bb7a77a5 4 PIDController::PIDController() {
dionigi 1:6f18bb7a77a5 5 this->reset();
dionigi 1:6f18bb7a77a5 6 }
dionigi 1:6f18bb7a77a5 7
dionigi 1:6f18bb7a77a5 8 /***
dionigi 1:6f18bb7a77a5 9 * Assignment 3
dionigi 1:6f18bb7a77a5 10 *
dionigi 1:6f18bb7a77a5 11 * Implement PID!
dionigi 1:6f18bb7a77a5 12 ***/
dionigi 1:6f18bb7a77a5 13
dionigi 1:6f18bb7a77a5 14 void PIDController::reset() volatile {
dionigi 1:6f18bb7a77a5 15 /**
dionigi 1:6f18bb7a77a5 16 * You'll have a lot to keep track of. Make sure to reset all
dionigi 1:6f18bb7a77a5 17 * those variables here.
dionigi 1:6f18bb7a77a5 18 **/
dionigi 1:6f18bb7a77a5 19
dionigi 1:6f18bb7a77a5 20 m_goalW = 0;
dionigi 1:6f18bb7a77a5 21 m_goalX = 0;
dionigi 1:6f18bb7a77a5 22
dionigi 1:6f18bb7a77a5 23 m_pwmW = 0;
dionigi 1:6f18bb7a77a5 24 m_pwmX = 0;
dionigi 1:6f18bb7a77a5 25
dionigi 1:6f18bb7a77a5 26 m_errorW = 0;
dionigi 1:6f18bb7a77a5 27 m_errorX = 0;
dionigi 1:6f18bb7a77a5 28 m_errorW_old = 0;
dionigi 1:6f18bb7a77a5 29 m_errorX_old = 0;
dionigi 1:6f18bb7a77a5 30
dionigi 1:6f18bb7a77a5 31 m_countsW = 0;
dionigi 1:6f18bb7a77a5 32 m_countsX = 0;
dionigi 1:6f18bb7a77a5 33 }
dionigi 1:6f18bb7a77a5 34
dionigi 1:6f18bb7a77a5 35 void PIDController::update() volatile {
dionigi 1:6f18bb7a77a5 36 /**
dionigi 1:6f18bb7a77a5 37 * Update your PID controller. This is what should be called
dionigi 1:6f18bb7a77a5 38 * every systick, so you should call all your important
dionigi 1:6f18bb7a77a5 39 * functions here.
dionigi 1:6f18bb7a77a5 40 **/
dionigi 1:6f18bb7a77a5 41 getSensorFeedback();
dionigi 1:6f18bb7a77a5 42
dionigi 1:6f18bb7a77a5 43 x_controller();
dionigi 1:6f18bb7a77a5 44 w_controller();
dionigi 1:6f18bb7a77a5 45
dionigi 1:6f18bb7a77a5 46 updateMotorPwm();
dionigi 1:6f18bb7a77a5 47 }
dionigi 1:6f18bb7a77a5 48
dionigi 1:6f18bb7a77a5 49 void PIDController::setXGoal(int counts) {
dionigi 1:6f18bb7a77a5 50 /**
dionigi 1:6f18bb7a77a5 51 * Set goal for X controller.
dionigi 1:6f18bb7a77a5 52 **/
dionigi 1:6f18bb7a77a5 53 m_goalX = counts;
dionigi 1:6f18bb7a77a5 54 }
dionigi 1:6f18bb7a77a5 55
dionigi 1:6f18bb7a77a5 56 void PIDController::setWGoal(int counts) {
dionigi 1:6f18bb7a77a5 57 /**
dionigi 1:6f18bb7a77a5 58 * Set goal for W controller. Make sure to associate a turning
dionigi 1:6f18bb7a77a5 59 * direction with a sign, e.g., positive is clockwise, negative
dionigi 1:6f18bb7a77a5 60 * is counter-clockwise.
dionigi 1:6f18bb7a77a5 61 **/
dionigi 1:6f18bb7a77a5 62 m_goalW = counts;
dionigi 1:6f18bb7a77a5 63 }
dionigi 1:6f18bb7a77a5 64
dionigi 1:6f18bb7a77a5 65 bool PIDController::isDone() volatile {
dionigi 1:6f18bb7a77a5 66 /**
dionigi 1:6f18bb7a77a5 67 * When is the PID done? Well, probably when you've reached
dionigi 1:6f18bb7a77a5 68 * your goals...
dionigi 1:6f18bb7a77a5 69 **/
dionigi 1:6f18bb7a77a5 70 if ( (m_goalW == m_countsW) && (m_goalX == m_countsX) ) {
dionigi 1:6f18bb7a77a5 71 return true;
dionigi 1:6f18bb7a77a5 72 }
dionigi 1:6f18bb7a77a5 73 return false;
dionigi 1:6f18bb7a77a5 74 }
dionigi 1:6f18bb7a77a5 75
dionigi 1:6f18bb7a77a5 76 char* PIDController::getData() {
dionigi 1:6f18bb7a77a5 77 /**
dionigi 1:6f18bb7a77a5 78 * Use this function to print data about your PID,
dionigi 1:6f18bb7a77a5 79 * because you can't print directly in systick!
dionigi 1:6f18bb7a77a5 80 **/
dionigi 1:6f18bb7a77a5 81 sprintf(buf, "goalx: %d\tgoalw: %d\tpwmx: %.3f\tpwmw: %.3f\terrorx: %d\terrorw: %d\n",
dionigi 1:6f18bb7a77a5 82 m_goalX,
dionigi 1:6f18bb7a77a5 83 m_goalW,
dionigi 1:6f18bb7a77a5 84 m_pwmX,
dionigi 1:6f18bb7a77a5 85 m_pwmW,
dionigi 1:6f18bb7a77a5 86 m_errorX,
dionigi 1:6f18bb7a77a5 87 m_errorW);
dionigi 1:6f18bb7a77a5 88 return buf;
dionigi 1:6f18bb7a77a5 89 }
dionigi 1:6f18bb7a77a5 90
dionigi 1:6f18bb7a77a5 91 /**
dionigi 1:6f18bb7a77a5 92 * Private functions to do the internal work for PID.
dionigi 1:6f18bb7a77a5 93 **/
dionigi 1:6f18bb7a77a5 94 void PIDController::getSensorFeedback() volatile {
dionigi 1:6f18bb7a77a5 95 /**
dionigi 1:6f18bb7a77a5 96 * Update sensor values, from encoders
dionigi 1:6f18bb7a77a5 97 **/
dionigi 1:6f18bb7a77a5 98
dionigi 1:6f18bb7a77a5 99 int encL = encoders.getPulsesL();
dionigi 1:6f18bb7a77a5 100 int encR = encoders.getPulsesR();
dionigi 1:6f18bb7a77a5 101
dionigi 1:6f18bb7a77a5 102 m_countsX = (encL + encR) / 2;
dionigi 1:6f18bb7a77a5 103 m_countsW = encL - encR;
dionigi 1:6f18bb7a77a5 104
dionigi 1:6f18bb7a77a5 105 // update m_countsX, m_countsW
dionigi 1:6f18bb7a77a5 106 }
dionigi 1:6f18bb7a77a5 107
dionigi 1:6f18bb7a77a5 108 void PIDController::x_controller() volatile {
dionigi 1:6f18bb7a77a5 109 /**
dionigi 1:6f18bb7a77a5 110 * Your X PID controller, sets m_pwmX
dionigi 1:6f18bb7a77a5 111 **/
dionigi 1:6f18bb7a77a5 112
dionigi 1:6f18bb7a77a5 113 m_errorX_old = m_errorX;
dionigi 1:6f18bb7a77a5 114 m_errorX = m_goalX - m_countsX;
dionigi 1:6f18bb7a77a5 115 // m_pwmX_raw = (KpX * m_errorX) + (KdX * (m_errorX_old - m_errorX));
dionigi 1:6f18bb7a77a5 116 m_pwmX = 10;
dionigi 1:6f18bb7a77a5 117 }
dionigi 1:6f18bb7a77a5 118
dionigi 1:6f18bb7a77a5 119 void PIDController::w_controller() volatile {
dionigi 1:6f18bb7a77a5 120 /**
dionigi 1:6f18bb7a77a5 121 * Your W PID controller, sets m_pwmW
dionigi 1:6f18bb7a77a5 122 **/
dionigi 1:6f18bb7a77a5 123
dionigi 1:6f18bb7a77a5 124 m_errorW_old = m_errorW;
dionigi 1:6f18bb7a77a5 125 m_errorW = m_goalW - m_countsW;
dionigi 1:6f18bb7a77a5 126 m_pwmW = (KpW * m_errorW) + (KdW * (m_errorW_old - m_errorW));
dionigi 1:6f18bb7a77a5 127 }
dionigi 1:6f18bb7a77a5 128
dionigi 1:6f18bb7a77a5 129 void PIDController::updateMotorPwm() volatile {
dionigi 1:6f18bb7a77a5 130 /**
dionigi 1:6f18bb7a77a5 131 * Takes m_pwmX, m_pwmW, and adjusts the motors based on those values.
dionigi 1:6f18bb7a77a5 132 **/
dionigi 1:6f18bb7a77a5 133
dionigi 1:6f18bb7a77a5 134 // if m_goalW > 0, rat is turning to the right
dionigi 1:6f18bb7a77a5 135 // set pwm based off of X (amplitude) and W (angle)
dionigi 1:6f18bb7a77a5 136 motors.setRightPwm((m_pwmX - m_pwmW)/20.0);
dionigi 1:6f18bb7a77a5 137 motors.setLeftPwm((m_pwmX + m_pwmW)/20.0);
dionigi 1:6f18bb7a77a5 138 }