Sally Brown / Mbed 2 deprecated RenBuggy_PID

Control for RenBuggy

Dependencies:   PID PinDetect mbed

RenBuggy_PID.cpp@0:f414c64e674f, 2014-03-04 (annotated)

Committer:
salatron
Date:
Tue Mar 04 13:31:17 2014 +0000
Revision:
0:f414c64e674f
Child:
1:8a2a7adb3c5d
Version1

; RenBuggy with PID controll

Who changed what in which revision?

UserRevisionLine numberNew contents of line
salatron 0:f414c64e674f 1 /*******************************************************************************
salatron 0:f414c64e674f 2 * RenBED PID Motor Control for RenBuggy *
salatron 0:f414c64e674f 3 * Copyright (c) 2014 Sally Brown *
salatron 0:f414c64e674f 4 * *
salatron 0:f414c64e674f 5 * Permission is hereby granted, free of charge, to any person obtaining a copy *
salatron 0:f414c64e674f 6 * of this software and associated documentation files (the "Software"), to deal*
salatron 0:f414c64e674f 7 * in the Software without restriction, including without limitation the rights *
salatron 0:f414c64e674f 8 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
salatron 0:f414c64e674f 9 * copies of the Software, and to permit persons to whom the Software is *
salatron 0:f414c64e674f 10 * furnished to do so, subject to the following conditions: *
salatron 0:f414c64e674f 11 * *
salatron 0:f414c64e674f 12 * The above copyright notice and this permission notice shall be included in *
salatron 0:f414c64e674f 13 * all copies or substantial portions of the Software. *
salatron 0:f414c64e674f 14 * *
salatron 0:f414c64e674f 15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
salatron 0:f414c64e674f 16 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
salatron 0:f414c64e674f 17 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
salatron 0:f414c64e674f 18 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
salatron 0:f414c64e674f 19 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,*
salatron 0:f414c64e674f 20 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
salatron 0:f414c64e674f 21 * THE SOFTWARE. *
salatron 0:f414c64e674f 22 * *
salatron 0:f414c64e674f 23 * PID_Controller.cpp *
salatron 0:f414c64e674f 24 * *
salatron 0:f414c64e674f 25 *******************************************************************************/
salatron 0:f414c64e674f 26
salatron 0:f414c64e674f 27 #ifndef _PIDCONTROLLER_C
salatron 0:f414c64e674f 28 #define _PIDCONTROLLER_C
salatron 0:f414c64e674f 29
salatron 0:f414c64e674f 30 #include "RenBuggy_PID.h"
salatron 0:f414c64e674f 31
salatron 0:f414c64e674f 32 PID_Controller::PID_Controller
salatron 0:f414c64e674f 33 (
salatron 0:f414c64e674f 34 PinName motorL,
salatron 0:f414c64e674f 35 PinName motorR,
salatron 0:f414c64e674f 36 PinName brakeL,
salatron 0:f414c64e674f 37 PinName brakeR,
salatron 0:f414c64e674f 38 PinName sensorL,
salatron 0:f414c64e674f 39 PinName sensorR
salatron 0:f414c64e674f 40 ) :
salatron 0:f414c64e674f 41 m_controllerL(1.0, 0.0, 0.0, RATE), //Kc, Ti, Td, interval
salatron 0:f414c64e674f 42 m_controllerR(1.0, 0.0, 0.0, RATE),
salatron 0:f414c64e674f 43 m_motorL(motorL),
salatron 0:f414c64e674f 44 m_motorR(motorR),
salatron 0:f414c64e674f 45 m_brakeL(brakeL),
salatron 0:f414c64e674f 46 m_brakeR(brakeR),
salatron 0:f414c64e674f 47 m_senseL(sensorL),
salatron 0:f414c64e674f 48 m_senseR(sensorR),
salatron 0:f414c64e674f 49 m_numberStrips(16), //Default to 16 stripes
salatron 0:f414c64e674f 50 m_wheelCircumference(16.96), //Default to 16.96
salatron 0:f414c64e674f 51 m_stripesL(0), //Initialise the number of stripes to 0.
salatron 0:f414c64e674f 52 m_stripesR(0),
salatron 0:f414c64e674f 53 m_turnLeft(false),
salatron 0:f414c64e674f 54 m_turnRight(false),
salatron 0:f414c64e674f 55 m_fProportionLeft(0.0),
salatron 0:f414c64e674f 56 m_fProportionRight(0.0),
salatron 0:f414c64e674f 57 m_iProportionLeft(0),
salatron 0:f414c64e674f 58 m_iProportionRight(0)
salatron 0:f414c64e674f 59 {
salatron 0:f414c64e674f 60 m_senseL.setSampleFrequency(1000);
salatron 0:f414c64e674f 61 m_senseR.setSampleFrequency(1000); //If this is playing up, consider changing this to 1001?
salatron 0:f414c64e674f 62
salatron 0:f414c64e674f 63 //It's 5 samples before it recognises it's held on.
salatron 0:f414c64e674f 64 m_senseL.setSamplesTillHeld(5);
salatron 0:f414c64e674f 65 m_senseR.setSamplesTillHeld(5);
salatron 0:f414c64e674f 66
salatron 0:f414c64e674f 67 //Only when it's been held high and then goes low will it increment the number of counts.
salatron 0:f414c64e674f 68 m_senseL.attach_deasserted_held(this, &PID_Controller::countL);
salatron 0:f414c64e674f 69 m_senseR.attach_deasserted_held(this, &PID_Controller::countR);
salatron 0:f414c64e674f 70
salatron 0:f414c64e674f 71 setUpControllers();
salatron 0:f414c64e674f 72 }
salatron 0:f414c64e674f 73
salatron 0:f414c64e674f 74 void PID_Controller::SetUpConstants(int numberStripes, float wheelCircumference)
salatron 0:f414c64e674f 75 {
salatron 0:f414c64e674f 76 m_numberStrips = numberStripes;
salatron 0:f414c64e674f 77 m_wheelCircumference = wheelCircumference;
salatron 0:f414c64e674f 78 }
salatron 0:f414c64e674f 79
salatron 0:f414c64e674f 80 void PID_Controller::Forwards(int CountsForward)
salatron 0:f414c64e674f 81 {
salatron 0:f414c64e674f 82 m_turnRight = false;
salatron 0:f414c64e674f 83 m_turnLeft = false;
salatron 0:f414c64e674f 84
salatron 0:f414c64e674f 85 CountsForward = CountsForward * (m_numberStrips/m_wheelCircumference);
salatron 0:f414c64e674f 86
salatron 0:f414c64e674f 87 m_rate.attach(this, &PID_Controller::doSomePID, RATE); //Attach the counter if it hasn't gone too far. Then hopefully just sit in a loop.
salatron 0:f414c64e674f 88
salatron 0:f414c64e674f 89 m_stripesL = m_stripesR = 0;
salatron 0:f414c64e674f 90 m_brakeR = m_brakeL = 0;
salatron 0:f414c64e674f 91
salatron 0:f414c64e674f 92 m_fProportionLeft = m_fProportionRight = 1.0;
salatron 0:f414c64e674f 93
salatron 0:f414c64e674f 94 while ((m_brakeR == 0) || (m_brakeL == 0))
salatron 0:f414c64e674f 95 {
salatron 0:f414c64e674f 96 if (CountsForward < m_stripesL)
salatron 0:f414c64e674f 97 {
salatron 0:f414c64e674f 98 m_motorL = 0.0;
salatron 0:f414c64e674f 99 m_brakeL = 1;
salatron 0:f414c64e674f 100 }
salatron 0:f414c64e674f 101 if (CountsForward < m_stripesR)
salatron 0:f414c64e674f 102 {
salatron 0:f414c64e674f 103 m_motorR = 0.0;
salatron 0:f414c64e674f 104 m_brakeR = 1;
salatron 0:f414c64e674f 105 if (CountsForward < m_stripesL)
salatron 0:f414c64e674f 106 {
salatron 0:f414c64e674f 107 m_rate.detach();
salatron 0:f414c64e674f 108 }
salatron 0:f414c64e674f 109 }
salatron 0:f414c64e674f 110 }
salatron 0:f414c64e674f 111 return;
salatron 0:f414c64e674f 112 }
salatron 0:f414c64e674f 113
salatron 0:f414c64e674f 114 void PID_Controller::Left(int AngleLeft, int RadiusLeft)
salatron 0:f414c64e674f 115 {
salatron 0:f414c64e674f 116 m_turnRight = false; //Turning left, NOT turning right
salatron 0:f414c64e674f 117 m_turnLeft = true;
salatron 0:f414c64e674f 118
salatron 0:f414c64e674f 119 m_rate.attach(this, &PID_Controller::doSomePID, RATE);
salatron 0:f414c64e674f 120
salatron 0:f414c64e674f 121 m_brakeR = m_brakeL = 0; //Turning off the brakes is often quite fun.
salatron 0:f414c64e674f 122 m_stripesL = m_stripesR = 0;
salatron 0:f414c64e674f 123
salatron 0:f414c64e674f 124 float m_fDistanceL = (2*pi*(RadiusLeft - 6.5))*(m_numberStrips/m_wheelCircumference)/(360/AngleLeft);
salatron 0:f414c64e674f 125 float m_fDistanceR = (2*pi*(RadiusLeft + 6.5))*(m_numberStrips/m_wheelCircumference)/(360/AngleLeft); //gives the length of the arc over which the wheel will travel, and translates that into a number of wheel stripes
salatron 0:f414c64e674f 126
salatron 0:f414c64e674f 127 int iDistanceL = (int) m_fDistanceL; //Cast the distance into an int
salatron 0:f414c64e674f 128 int iDistanceR = (int) m_fDistanceR;
salatron 0:f414c64e674f 129
salatron 0:f414c64e674f 130 int LeftWheelDist = iDistanceL; //Set the distance the left wheel travels
salatron 0:f414c64e674f 131 int RightWheelDist = iDistanceR; //Set the distance the right wheel travels
salatron 0:f414c64e674f 132
salatron 0:f414c64e674f 133 float myfloatL = (float)LeftWheelDist/(float)RightWheelDist;
salatron 0:f414c64e674f 134 m_iProportionLeft = floor(myfloatL);
salatron 0:f414c64e674f 135 m_fProportionLeft = myfloatL;
salatron 0:f414c64e674f 136
salatron 0:f414c64e674f 137 float myfloatR = (float)RightWheelDist/LeftWheelDist; //When turning right, you only use the left wheel's proportion
salatron 0:f414c64e674f 138 m_iProportionRight = ceil(myfloatR);
salatron 0:f414c64e674f 139 m_fProportionRight = myfloatR;
salatron 0:f414c64e674f 140
salatron 0:f414c64e674f 141 while ((m_brakeR == 0))
salatron 0:f414c64e674f 142 {
salatron 0:f414c64e674f 143 if (LeftWheelDist <= m_stripesL) //If the left motor has gone far enough
salatron 0:f414c64e674f 144 {
salatron 0:f414c64e674f 145 m_motorL = 0.0; //Stop the motor
salatron 0:f414c64e674f 146 m_brakeL = 1; //Apply the brakes
salatron 0:f414c64e674f 147 }
salatron 0:f414c64e674f 148 if (RightWheelDist <= m_stripesR)
salatron 0:f414c64e674f 149 {
salatron 0:f414c64e674f 150 m_motorR = 0.0;
salatron 0:f414c64e674f 151 m_brakeR = 1;
salatron 0:f414c64e674f 152 if (LeftWheelDist <= m_stripesL)
salatron 0:f414c64e674f 153 {
salatron 0:f414c64e674f 154 m_rate.detach();
salatron 0:f414c64e674f 155 }
salatron 0:f414c64e674f 156 }
salatron 0:f414c64e674f 157 }
salatron 0:f414c64e674f 158 }
salatron 0:f414c64e674f 159
salatron 0:f414c64e674f 160 void PID_Controller::Right(int AngleRight, int RadiusRight)
salatron 0:f414c64e674f 161 {
salatron 0:f414c64e674f 162 m_turnRight = true;
salatron 0:f414c64e674f 163 m_turnLeft = false;
salatron 0:f414c64e674f 164
salatron 0:f414c64e674f 165 m_rate.attach(this, &PID_Controller::doSomePID, RATE);
salatron 0:f414c64e674f 166
salatron 0:f414c64e674f 167 m_brakeR = m_brakeL = 0; //Turning off the brakes is often quite fun.
salatron 0:f414c64e674f 168 m_stripesL = m_stripesR = 0;
salatron 0:f414c64e674f 169
salatron 0:f414c64e674f 170 float m_fDistanceL = (2*pi*(RadiusRight + 6.5))*(m_numberStrips/m_wheelCircumference)/(360/AngleRight); //Forcing it to an int beforehand didn't work. It twitches instead of going argh no, but it still doesn't really work.
salatron 0:f414c64e674f 171 float m_fDistanceR = (2*pi*(RadiusRight - 6.5))*(m_numberStrips/m_wheelCircumference)/(360/AngleRight);
salatron 0:f414c64e674f 172
salatron 0:f414c64e674f 173 int iDistanceL = (int) m_fDistanceL;
salatron 0:f414c64e674f 174 int iDistanceR = (int) m_fDistanceR;
salatron 0:f414c64e674f 175
salatron 0:f414c64e674f 176 int LeftWheelDist = iDistanceL; //Formula for the length of an arc, divided by circumference, multiplied by 16 stripes.
salatron 0:f414c64e674f 177 int RightWheelDist = iDistanceR; //This give a distance, after which it will stop moving. These lines are causing problems...
salatron 0:f414c64e674f 178
salatron 0:f414c64e674f 179 float myfloatL = (float)LeftWheelDist/(float)RightWheelDist;
salatron 0:f414c64e674f 180 m_iProportionLeft = (int) ceil(myfloatL);
salatron 0:f414c64e674f 181 m_fProportionLeft = myfloatL;
salatron 0:f414c64e674f 182
salatron 0:f414c64e674f 183 float myfloatR = (float)RightWheelDist/(float)LeftWheelDist;
salatron 0:f414c64e674f 184 m_iProportionRight = (int) floor(myfloatR);
salatron 0:f414c64e674f 185
salatron 0:f414c64e674f 186 while ((m_brakeL == 0))
salatron 0:f414c64e674f 187 {
salatron 0:f414c64e674f 188 if (LeftWheelDist <= m_stripesL) //If the left motor has gone far enough
salatron 0:f414c64e674f 189 {
salatron 0:f414c64e674f 190 m_motorL = 0.0; //Stop the motor
salatron 0:f414c64e674f 191 m_brakeL = 1; //Apply the brakes
salatron 0:f414c64e674f 192 }
salatron 0:f414c64e674f 193 if (RightWheelDist <= m_stripesR)
salatron 0:f414c64e674f 194 {
salatron 0:f414c64e674f 195 m_motorR = 0.0;
salatron 0:f414c64e674f 196 m_brakeR = 1;
salatron 0:f414c64e674f 197 if (LeftWheelDist <= m_stripesL)
salatron 0:f414c64e674f 198 {
salatron 0:f414c64e674f 199 m_rate.detach();
salatron 0:f414c64e674f 200 }
salatron 0:f414c64e674f 201 }
salatron 0:f414c64e674f 202 }
salatron 0:f414c64e674f 203 }
salatron 0:f414c64e674f 204
salatron 0:f414c64e674f 205 void PID_Controller::doSomePID()
salatron 0:f414c64e674f 206 {
salatron 0:f414c64e674f 207 PIDLeft();
salatron 0:f414c64e674f 208 PIDRight();
salatron 0:f414c64e674f 209 }
salatron 0:f414c64e674f 210
salatron 0:f414c64e674f 211 void PID_Controller::PIDLeft()
salatron 0:f414c64e674f 212 {
salatron 0:f414c64e674f 213 float fSPL = 0.0;
salatron 0:f414c64e674f 214 int SPL = 0;
salatron 0:f414c64e674f 215
salatron 0:f414c64e674f 216 if(m_turnLeft)
salatron 0:f414c64e674f 217 {
salatron 0:f414c64e674f 218 fSPL = m_stripesR / m_fProportionRight;
salatron 0:f414c64e674f 219 SPL = (int) fSPL;
salatron 0:f414c64e674f 220 }
salatron 0:f414c64e674f 221 else if(m_turnRight)
salatron 0:f414c64e674f 222 {
salatron 0:f414c64e674f 223 fSPL = m_stripesR * m_fProportionLeft;
salatron 0:f414c64e674f 224 SPL = (int) fSPL;
salatron 0:f414c64e674f 225 }
salatron 0:f414c64e674f 226 else
salatron 0:f414c64e674f 227 {
salatron 0:f414c64e674f 228 SPL = m_stripesR;
salatron 0:f414c64e674f 229 }
salatron 0:f414c64e674f 230
salatron 0:f414c64e674f 231 m_controllerL.setProcessValue(m_stripesL);
salatron 0:f414c64e674f 232
salatron 0:f414c64e674f 233 m_motorL = (m_controllerL.compute()); //PWM output * some speed proportion. May slow it down or speed it up.*/
salatron 0:f414c64e674f 234
salatron 0:f414c64e674f 235 m_controllerL.setSetPoint(SPL);
salatron 0:f414c64e674f 236 }
salatron 0:f414c64e674f 237
salatron 0:f414c64e674f 238 void PID_Controller::PIDRight()
salatron 0:f414c64e674f 239 {
salatron 0:f414c64e674f 240 float fSPR = 0.0;
salatron 0:f414c64e674f 241 int SPR = 0;
salatron 0:f414c64e674f 242
salatron 0:f414c64e674f 243 if(m_turnRight) //If you're turning right, the left wheel goes further, so use xProportionLeft
salatron 0:f414c64e674f 244 {
salatron 0:f414c64e674f 245 fSPR = m_stripesL / m_fProportionLeft;
salatron 0:f414c64e674f 246 SPR = (int) fSPR;
salatron 0:f414c64e674f 247 }
salatron 0:f414c64e674f 248 else if(m_turnLeft) //If you're turning left, the right wheel goes further, so use xProportionRight
salatron 0:f414c64e674f 249 {
salatron 0:f414c64e674f 250 fSPR = m_stripesL * m_fProportionRight;
salatron 0:f414c64e674f 251 SPR = (int) fSPR;
salatron 0:f414c64e674f 252 }
salatron 0:f414c64e674f 253 else
salatron 0:f414c64e674f 254 {
salatron 0:f414c64e674f 255 SPR = m_stripesL;
salatron 0:f414c64e674f 256 }
salatron 0:f414c64e674f 257
salatron 0:f414c64e674f 258 m_controllerR.setProcessValue(m_stripesR); //Set the process value (what it IS).
salatron 0:f414c64e674f 259
salatron 0:f414c64e674f 260 m_motorR = (m_controllerR.compute()); //Calculate the PWM duty cycle
salatron 0:f414c64e674f 261
salatron 0:f414c64e674f 262 m_controllerR.setSetPoint(SPR); //SPR = set point right. this sets the set point (what it SHOULD BE).
salatron 0:f414c64e674f 263 }
salatron 0:f414c64e674f 264
salatron 0:f414c64e674f 265 void PID_Controller::countL()
salatron 0:f414c64e674f 266 {
salatron 0:f414c64e674f 267 m_stripesL++;
salatron 0:f414c64e674f 268 }
salatron 0:f414c64e674f 269
salatron 0:f414c64e674f 270 void PID_Controller::countR()
salatron 0:f414c64e674f 271 {
salatron 0:f414c64e674f 272 m_stripesR++;
salatron 0:f414c64e674f 273 }
salatron 0:f414c64e674f 274
salatron 0:f414c64e674f 275 void PID_Controller::setUpControllers()
salatron 0:f414c64e674f 276 {
salatron 0:f414c64e674f 277 //m_controllerL = PID(1.0, 0.0, 0.0, RATE); //Kc, Ti, Td, interval
salatron 0:f414c64e674f 278 //m_controllerR = PID(1.0, 0.0, 0.0, RATE);
salatron 0:f414c64e674f 279
salatron 0:f414c64e674f 280 m_controllerL.setInputLimits(0.0, 200);
salatron 0:f414c64e674f 281 m_controllerR.setInputLimits(0.0, 200);
salatron 0:f414c64e674f 282 //Pwm output from 0.0 to 1.0 (PWM duty cycle %)
salatron 0:f414c64e674f 283 m_controllerL.setOutputLimits(0.0, 1.0);
salatron 0:f414c64e674f 284 m_controllerR.setOutputLimits(0.0, 1.0);
salatron 0:f414c64e674f 285 //If there's a bias. FULL SPEED AHEAD. I don't know why this works but it does.
salatron 0:f414c64e674f 286 m_controllerL.setBias(1.0);
salatron 0:f414c64e674f 287 m_controllerR.setBias(1.0);
salatron 0:f414c64e674f 288 //Set it to auto mode.
salatron 0:f414c64e674f 289 m_controllerL.setMode(AUTO_MODE);
salatron 0:f414c64e674f 290 m_controllerR.setMode(AUTO_MODE);
salatron 0:f414c64e674f 291 }
salatron 0:f414c64e674f 292
salatron 0:f414c64e674f 293 #endif