Sally Brown / Mbed 2 deprecated RenBuggy_PID

Control for RenBuggy

Dependencies:   PID PinDetect mbed

RenBuggy_PID.cpp@2:cd7543fdcb8c, 2014-03-06 (annotated)

Committer:
salatron
Date:
Thu Mar 06 09:56:11 2014 +0000
Revision:
2:cd7543fdcb8c
Parent:
1:8a2a7adb3c5d 
version 3

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 1:8a2a7adb3c5d 3 * Copyright (c) 2014 Sally Brown & Liz Lloyd *
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 1:8a2a7adb3c5d 32 PID_Stripes::PID_Stripes(PinName motorL, PinName motorR, PinName brakeL, PinName brakeR, PinName sensorL, PinName sensorR) :
salatron 1:8a2a7adb3c5d 33 PID_Controller(motorL, motorR, brakeL, brakeR),
salatron 1:8a2a7adb3c5d 34 m_senseL(sensorL),
salatron 1:8a2a7adb3c5d 35 m_senseR(sensorR)
salatron 1:8a2a7adb3c5d 36 {
salatron 1:8a2a7adb3c5d 37 m_senseL.setSampleFrequency(1000);
salatron 1:8a2a7adb3c5d 38 m_senseR.setSampleFrequency(1000); //If this is playing up, consider changing this to 1001?
salatron 1:8a2a7adb3c5d 39
salatron 1:8a2a7adb3c5d 40 //It's 5 samples before it recognises it's held on.
salatron 1:8a2a7adb3c5d 41 m_senseL.setSamplesTillHeld(5);
salatron 1:8a2a7adb3c5d 42 m_senseR.setSamplesTillHeld(5);
salatron 1:8a2a7adb3c5d 43
salatron 1:8a2a7adb3c5d 44 PID_Controller* basePointer = dynamic_cast<PID_Stripes*>(this);
salatron 1:8a2a7adb3c5d 45 //Only when it's been held high and then goes low will it increment the number of counts.
salatron 1:8a2a7adb3c5d 46 m_senseL.attach_deasserted_held(basePointer, &PID_Controller::countL);
salatron 1:8a2a7adb3c5d 47 m_senseR.attach_deasserted_held(basePointer, &PID_Controller::countR);
salatron 1:8a2a7adb3c5d 48 }
salatron 1:8a2a7adb3c5d 49
salatron 1:8a2a7adb3c5d 50 PID_Magnet::PID_Magnet(PinName motorL, PinName motorR, PinName brakeL, PinName brakeR, PinName sensorL, PinName sensorR) :
salatron 1:8a2a7adb3c5d 51 PID_Controller(motorL, motorR, brakeL, brakeR),
salatron 1:8a2a7adb3c5d 52 m_senseL(sensorL),
salatron 1:8a2a7adb3c5d 53 m_senseR(sensorR)
salatron 1:8a2a7adb3c5d 54 {
salatron 1:8a2a7adb3c5d 55 PID_Controller* basePointer = dynamic_cast<PID_Magnet*>(this);
salatron 1:8a2a7adb3c5d 56 m_senseL.fall(basePointer, &PID_Controller::countL);
salatron 1:8a2a7adb3c5d 57 m_senseR.fall(basePointer, &PID_Controller::countR);
salatron 1:8a2a7adb3c5d 58 }
salatron 1:8a2a7adb3c5d 59
salatron 0:f414c64e674f 60 PID_Controller::PID_Controller
salatron 0:f414c64e674f 61 (
salatron 0:f414c64e674f 62 PinName motorL,
salatron 0:f414c64e674f 63 PinName motorR,
salatron 0:f414c64e674f 64 PinName brakeL,
salatron 1:8a2a7adb3c5d 65 PinName brakeR
salatron 0:f414c64e674f 66 ) :
salatron 0:f414c64e674f 67 m_controllerL(1.0, 0.0, 0.0, RATE), //Kc, Ti, Td, interval
salatron 0:f414c64e674f 68 m_controllerR(1.0, 0.0, 0.0, RATE),
salatron 0:f414c64e674f 69 m_motorL(motorL),
salatron 0:f414c64e674f 70 m_motorR(motorR),
salatron 0:f414c64e674f 71 m_brakeL(brakeL),
salatron 0:f414c64e674f 72 m_brakeR(brakeR),
salatron 1:8a2a7adb3c5d 73 m_countsPerRev(16), //Default to 16 stripes
salatron 1:8a2a7adb3c5d 74 m_wheelCircumference(16.96), //Default to 16.96
salatron 1:8a2a7adb3c5d 75 m_axleLength(13),
salatron 1:8a2a7adb3c5d 76 m_stripesL(0), //Initialise the number of stripes to 0.
salatron 0:f414c64e674f 77 m_stripesR(0),
salatron 0:f414c64e674f 78 m_turnLeft(false),
salatron 0:f414c64e674f 79 m_turnRight(false),
salatron 0:f414c64e674f 80 m_fProportionLeft(0.0),
salatron 1:8a2a7adb3c5d 81 m_fProportionRight(0.0)
salatron 1:8a2a7adb3c5d 82 {
salatron 0:f414c64e674f 83 setUpControllers();
salatron 0:f414c64e674f 84 }
salatron 0:f414c64e674f 85
salatron 1:8a2a7adb3c5d 86 void PID_Controller::SetUpConstants(int countsPerRev, float wheelCircumference, float axleLength)
salatron 0:f414c64e674f 87 {
salatron 1:8a2a7adb3c5d 88 m_countsPerRev = countsPerRev;
salatron 0:f414c64e674f 89 m_wheelCircumference = wheelCircumference;
salatron 1:8a2a7adb3c5d 90 m_axleLength = axleLength;
salatron 0:f414c64e674f 91 }
salatron 0:f414c64e674f 92
salatron 1:8a2a7adb3c5d 93 void PID_Controller::Forwards(int distanceForward)
salatron 0:f414c64e674f 94 {
salatron 0:f414c64e674f 95 m_turnRight = false;
salatron 0:f414c64e674f 96 m_turnLeft = false;
salatron 1:8a2a7adb3c5d 97 bool moving = true;
salatron 0:f414c64e674f 98
salatron 1:8a2a7adb3c5d 99 int CountsForward = distanceForward * (m_countsPerRev/m_wheelCircumference);
salatron 0:f414c64e674f 100
salatron 0:f414c64e674f 101 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 102
salatron 0:f414c64e674f 103 m_stripesL = m_stripesR = 0;
salatron 0:f414c64e674f 104 m_brakeR = m_brakeL = 0;
salatron 0:f414c64e674f 105
salatron 0:f414c64e674f 106 m_fProportionLeft = m_fProportionRight = 1.0;
salatron 0:f414c64e674f 107
salatron 1:8a2a7adb3c5d 108 while(moving)
salatron 0:f414c64e674f 109 {
salatron 0:f414c64e674f 110 if (CountsForward < m_stripesL)
salatron 0:f414c64e674f 111 {
salatron 0:f414c64e674f 112 m_motorL = 0.0;
salatron 0:f414c64e674f 113 m_brakeL = 1;
salatron 0:f414c64e674f 114 }
salatron 0:f414c64e674f 115 if (CountsForward < m_stripesR)
salatron 0:f414c64e674f 116 {
salatron 0:f414c64e674f 117 m_motorR = 0.0;
salatron 0:f414c64e674f 118 m_brakeR = 1;
salatron 1:8a2a7adb3c5d 119 }
salatron 1:8a2a7adb3c5d 120 if(CountsForward < m_stripesR &&
salatron 1:8a2a7adb3c5d 121 CountsForward < m_stripesL)
salatron 1:8a2a7adb3c5d 122 {
salatron 1:8a2a7adb3c5d 123 m_rate.detach();
salatron 1:8a2a7adb3c5d 124 moving = false;
salatron 0:f414c64e674f 125 }
salatron 0:f414c64e674f 126 }
salatron 1:8a2a7adb3c5d 127
salatron 1:8a2a7adb3c5d 128 Stop();
salatron 1:8a2a7adb3c5d 129
salatron 0:f414c64e674f 130 return;
salatron 0:f414c64e674f 131 }
salatron 0:f414c64e674f 132
salatron 0:f414c64e674f 133 void PID_Controller::Left(int AngleLeft, int RadiusLeft)
salatron 0:f414c64e674f 134 {
salatron 0:f414c64e674f 135 m_turnRight = false; //Turning left, NOT turning right
salatron 0:f414c64e674f 136 m_turnLeft = true;
salatron 1:8a2a7adb3c5d 137 bool turning = true;
salatron 0:f414c64e674f 138
salatron 0:f414c64e674f 139 m_rate.attach(this, &PID_Controller::doSomePID, RATE);
salatron 0:f414c64e674f 140
salatron 0:f414c64e674f 141 m_brakeR = m_brakeL = 0; //Turning off the brakes is often quite fun.
salatron 0:f414c64e674f 142 m_stripesL = m_stripesR = 0;
salatron 0:f414c64e674f 143
salatron 1:8a2a7adb3c5d 144 int halfAxleLength = (m_axleLength + 1)/2;
salatron 1:8a2a7adb3c5d 145 RadiusLeft = RadiusLeft < halfAxleLength ? halfAxleLength : RadiusLeft;
salatron 2:cd7543fdcb8c 146 AngleLeft = AngleLeft > 90 ? 90 : AngleLeft;
salatron 0:f414c64e674f 147
salatron 1:8a2a7adb3c5d 148 float m_fDistanceL = (2*pi*(RadiusLeft - halfAxleLength))*(m_countsPerRev/m_wheelCircumference)/(360/AngleLeft);
salatron 1:8a2a7adb3c5d 149 float m_fDistanceR = (2*pi*(RadiusLeft + halfAxleLength))*(m_countsPerRev/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 150
salatron 1:8a2a7adb3c5d 151 int LeftWheelDist = (int) m_fDistanceL; //Cast the distance into an int
salatron 1:8a2a7adb3c5d 152 int RightWheelDist = (int) m_fDistanceR;
salatron 0:f414c64e674f 153
salatron 1:8a2a7adb3c5d 154 m_fProportionLeft = (float)LeftWheelDist/(float)RightWheelDist;
salatron 1:8a2a7adb3c5d 155 m_fProportionRight = (float)RightWheelDist/(float)LeftWheelDist; //When turning right, you only use the left wheel's proportion
salatron 0:f414c64e674f 156
salatron 1:8a2a7adb3c5d 157 while (turning)
salatron 0:f414c64e674f 158 {
salatron 0:f414c64e674f 159 if (LeftWheelDist <= m_stripesL) //If the left motor has gone far enough
salatron 0:f414c64e674f 160 {
salatron 0:f414c64e674f 161 m_motorL = 0.0; //Stop the motor
salatron 0:f414c64e674f 162 m_brakeL = 1; //Apply the brakes
salatron 0:f414c64e674f 163 }
salatron 0:f414c64e674f 164 if (RightWheelDist <= m_stripesR)
salatron 0:f414c64e674f 165 {
salatron 0:f414c64e674f 166 m_motorR = 0.0;
salatron 0:f414c64e674f 167 m_brakeR = 1;
salatron 1:8a2a7adb3c5d 168 }
salatron 1:8a2a7adb3c5d 169 if(RightWheelDist <= m_stripesR &&
salatron 1:8a2a7adb3c5d 170 LeftWheelDist <= m_stripesL)
salatron 1:8a2a7adb3c5d 171 {
salatron 1:8a2a7adb3c5d 172 m_rate.detach();
salatron 1:8a2a7adb3c5d 173 turning = false;
salatron 1:8a2a7adb3c5d 174 break;
salatron 0:f414c64e674f 175 }
salatron 0:f414c64e674f 176 }
salatron 1:8a2a7adb3c5d 177
salatron 1:8a2a7adb3c5d 178 Stop();
salatron 0:f414c64e674f 179 }
salatron 0:f414c64e674f 180
salatron 0:f414c64e674f 181 void PID_Controller::Right(int AngleRight, int RadiusRight)
salatron 0:f414c64e674f 182 {
salatron 0:f414c64e674f 183 m_turnRight = true;
salatron 0:f414c64e674f 184 m_turnLeft = false;
salatron 1:8a2a7adb3c5d 185 bool turning = true;
salatron 0:f414c64e674f 186
salatron 0:f414c64e674f 187 m_rate.attach(this, &PID_Controller::doSomePID, RATE);
salatron 1:8a2a7adb3c5d 188
salatron 0:f414c64e674f 189 m_brakeR = m_brakeL = 0; //Turning off the brakes is often quite fun.
salatron 0:f414c64e674f 190 m_stripesL = m_stripesR = 0;
salatron 0:f414c64e674f 191
salatron 1:8a2a7adb3c5d 192 int halfAxleLength = (m_axleLength + 1)/2;
salatron 1:8a2a7adb3c5d 193 RadiusRight = RadiusRight < halfAxleLength ? halfAxleLength : RadiusRight;
salatron 2:cd7543fdcb8c 194 AngleRight = AngleRight > 90 ? 90 : AngleRight;
salatron 0:f414c64e674f 195
salatron 1:8a2a7adb3c5d 196 float m_fDistanceL = (2*pi*(RadiusRight + halfAxleLength))*(m_countsPerRev/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 1:8a2a7adb3c5d 197 float m_fDistanceR = (2*pi*(RadiusRight - halfAxleLength))*(m_countsPerRev/m_wheelCircumference)/(360/AngleRight);
salatron 0:f414c64e674f 198
salatron 1:8a2a7adb3c5d 199 int LeftWheelDist = (int) m_fDistanceL;
salatron 1:8a2a7adb3c5d 200 int RightWheelDist = (int) m_fDistanceR;
salatron 0:f414c64e674f 201
salatron 1:8a2a7adb3c5d 202 m_fProportionLeft = (float)LeftWheelDist/(float)RightWheelDist;
salatron 1:8a2a7adb3c5d 203 m_fProportionRight = (float)RightWheelDist/(float)LeftWheelDist;
salatron 0:f414c64e674f 204
salatron 1:8a2a7adb3c5d 205 while (turning)
salatron 0:f414c64e674f 206 {
salatron 0:f414c64e674f 207 if (LeftWheelDist <= m_stripesL) //If the left motor has gone far enough
salatron 0:f414c64e674f 208 {
salatron 0:f414c64e674f 209 m_motorL = 0.0; //Stop the motor
salatron 0:f414c64e674f 210 m_brakeL = 1; //Apply the brakes
salatron 0:f414c64e674f 211 }
salatron 0:f414c64e674f 212 if (RightWheelDist <= m_stripesR)
salatron 0:f414c64e674f 213 {
salatron 0:f414c64e674f 214 m_motorR = 0.0;
salatron 0:f414c64e674f 215 m_brakeR = 1;
salatron 1:8a2a7adb3c5d 216 }
salatron 1:8a2a7adb3c5d 217 if(RightWheelDist <= m_stripesR &&
salatron 1:8a2a7adb3c5d 218 LeftWheelDist <= m_stripesL)
salatron 1:8a2a7adb3c5d 219 {
salatron 1:8a2a7adb3c5d 220 m_rate.detach();
salatron 1:8a2a7adb3c5d 221 turning = false;
salatron 1:8a2a7adb3c5d 222 break;
salatron 0:f414c64e674f 223 }
salatron 0:f414c64e674f 224 }
salatron 1:8a2a7adb3c5d 225
salatron 1:8a2a7adb3c5d 226 Stop();
salatron 0:f414c64e674f 227 }
salatron 0:f414c64e674f 228
salatron 1:8a2a7adb3c5d 229 void PID_Controller::Stop()
salatron 1:8a2a7adb3c5d 230 {
salatron 1:8a2a7adb3c5d 231 m_stripesL = 0;
salatron 1:8a2a7adb3c5d 232 m_stripesR = 0;
salatron 1:8a2a7adb3c5d 233 m_motorL = 0.0;
salatron 1:8a2a7adb3c5d 234 m_motorR = 0.0;
salatron 1:8a2a7adb3c5d 235 m_brakeL = 1;
salatron 1:8a2a7adb3c5d 236 m_brakeR = 1;
salatron 1:8a2a7adb3c5d 237
salatron 1:8a2a7adb3c5d 238 m_rate.detach();
salatron 1:8a2a7adb3c5d 239 }
salatron 1:8a2a7adb3c5d 240
salatron 0:f414c64e674f 241 void PID_Controller::doSomePID()
salatron 0:f414c64e674f 242 {
salatron 0:f414c64e674f 243 PIDLeft();
salatron 0:f414c64e674f 244 PIDRight();
salatron 0:f414c64e674f 245 }
salatron 0:f414c64e674f 246
salatron 0:f414c64e674f 247 void PID_Controller::PIDLeft()
salatron 0:f414c64e674f 248 {
salatron 0:f414c64e674f 249 float fSPL = 0.0;
salatron 0:f414c64e674f 250 int SPL = 0;
salatron 0:f414c64e674f 251
salatron 0:f414c64e674f 252 if(m_turnLeft)
salatron 0:f414c64e674f 253 {
salatron 0:f414c64e674f 254 fSPL = m_stripesR / m_fProportionRight;
salatron 0:f414c64e674f 255 SPL = (int) fSPL;
salatron 0:f414c64e674f 256 }
salatron 0:f414c64e674f 257 else if(m_turnRight)
salatron 0:f414c64e674f 258 {
salatron 0:f414c64e674f 259 fSPL = m_stripesR * m_fProportionLeft;
salatron 0:f414c64e674f 260 SPL = (int) fSPL;
salatron 0:f414c64e674f 261 }
salatron 0:f414c64e674f 262 else
salatron 0:f414c64e674f 263 {
salatron 0:f414c64e674f 264 SPL = m_stripesR;
salatron 0:f414c64e674f 265 }
salatron 0:f414c64e674f 266
salatron 0:f414c64e674f 267 m_controllerL.setProcessValue(m_stripesL);
salatron 0:f414c64e674f 268
salatron 0:f414c64e674f 269 m_motorL = (m_controllerL.compute()); //PWM output * some speed proportion. May slow it down or speed it up.*/
salatron 0:f414c64e674f 270
salatron 0:f414c64e674f 271 m_controllerL.setSetPoint(SPL);
salatron 0:f414c64e674f 272 }
salatron 0:f414c64e674f 273
salatron 0:f414c64e674f 274 void PID_Controller::PIDRight()
salatron 0:f414c64e674f 275 {
salatron 0:f414c64e674f 276 float fSPR = 0.0;
salatron 0:f414c64e674f 277 int SPR = 0;
salatron 0:f414c64e674f 278
salatron 0:f414c64e674f 279 if(m_turnRight) //If you're turning right, the left wheel goes further, so use xProportionLeft
salatron 0:f414c64e674f 280 {
salatron 0:f414c64e674f 281 fSPR = m_stripesL / m_fProportionLeft;
salatron 0:f414c64e674f 282 SPR = (int) fSPR;
salatron 0:f414c64e674f 283 }
salatron 0:f414c64e674f 284 else if(m_turnLeft) //If you're turning left, the right wheel goes further, so use xProportionRight
salatron 0:f414c64e674f 285 {
salatron 0:f414c64e674f 286 fSPR = m_stripesL * m_fProportionRight;
salatron 0:f414c64e674f 287 SPR = (int) fSPR;
salatron 0:f414c64e674f 288 }
salatron 0:f414c64e674f 289 else
salatron 0:f414c64e674f 290 {
salatron 0:f414c64e674f 291 SPR = m_stripesL;
salatron 0:f414c64e674f 292 }
salatron 0:f414c64e674f 293
salatron 0:f414c64e674f 294 m_controllerR.setProcessValue(m_stripesR); //Set the process value (what it IS).
salatron 0:f414c64e674f 295
salatron 0:f414c64e674f 296 m_motorR = (m_controllerR.compute()); //Calculate the PWM duty cycle
salatron 0:f414c64e674f 297
salatron 0:f414c64e674f 298 m_controllerR.setSetPoint(SPR); //SPR = set point right. this sets the set point (what it SHOULD BE).
salatron 0:f414c64e674f 299 }
salatron 0:f414c64e674f 300
salatron 0:f414c64e674f 301 void PID_Controller::countL()
salatron 0:f414c64e674f 302 {
salatron 0:f414c64e674f 303 m_stripesL++;
salatron 0:f414c64e674f 304 }
salatron 0:f414c64e674f 305
salatron 0:f414c64e674f 306 void PID_Controller::countR()
salatron 0:f414c64e674f 307 {
salatron 0:f414c64e674f 308 m_stripesR++;
salatron 0:f414c64e674f 309 }
salatron 0:f414c64e674f 310
salatron 0:f414c64e674f 311 void PID_Controller::setUpControllers()
salatron 0:f414c64e674f 312 {
salatron 0:f414c64e674f 313 m_controllerL.setInputLimits(0.0, 200);
salatron 0:f414c64e674f 314 m_controllerR.setInputLimits(0.0, 200);
salatron 0:f414c64e674f 315 //Pwm output from 0.0 to 1.0 (PWM duty cycle %)
salatron 0:f414c64e674f 316 m_controllerL.setOutputLimits(0.0, 1.0);
salatron 0:f414c64e674f 317 m_controllerR.setOutputLimits(0.0, 1.0);
salatron 0:f414c64e674f 318 //If there's a bias. FULL SPEED AHEAD. I don't know why this works but it does.
salatron 0:f414c64e674f 319 m_controllerL.setBias(1.0);
salatron 0:f414c64e674f 320 m_controllerR.setBias(1.0);
salatron 0:f414c64e674f 321 //Set it to auto mode.
salatron 0:f414c64e674f 322 m_controllerL.setMode(AUTO_MODE);
salatron 1:8a2a7adb3c5d 323 m_controllerR.setMode(AUTO_MODE);
salatron 0:f414c64e674f 324 }
salatron 0:f414c64e674f 325
salatron 1:8a2a7adb3c5d 326 void PID_Controller::ConfigurePID(float p, float i, float d)
salatron 1:8a2a7adb3c5d 327 {
salatron 1:8a2a7adb3c5d 328 m_controllerL.setTunings(p, i, d);
salatron 1:8a2a7adb3c5d 329 m_controllerR.setTunings(p, i, d);
salatron 1:8a2a7adb3c5d 330 }
salatron 1:8a2a7adb3c5d 331
salatron 2:cd7543fdcb8c 332 #endif