Flying Sea Glider / Mbed 2 deprecated 2019_19feb19_jcw_noSD

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 MODSERIAL FATFileSystem

LinearActuator/LinearActuator.cpp@13:84fcbe1dcd62, 2017-10-30 (annotated)

Committer:
tnhnrl
Date:
Mon Oct 30 21:09:19 2017 +0000
Revision:
13:84fcbe1dcd62
Parent:
12:a0519d11d2b6
Child:
14:85b64a4d08e8
Tested hardware for bce and made several pause fixes.  Homing works. Added offset to pitch and depth.txt files, and the control loop works.  PID gain for depth had to be negative.  Next is to replace the motor and test pitch loop.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mkelly10 9:d5fcdcb3c89d 1 #include "mbed.h"
mkelly10 9:d5fcdcb3c89d 2 #include "LinearActuator.hpp"
mkelly10 9:d5fcdcb3c89d 3 #include "StaticDefs.hpp"
mkelly10 9:d5fcdcb3c89d 4 #include "ConfigFile.h"
mkelly10 9:d5fcdcb3c89d 5
danstrider 10:085ab7328054 6 // this is where the variables that can be set are set when the object is created
mkelly10 9:d5fcdcb3c89d 7 LinearActuator::LinearActuator(float interval, PinName pwm, PinName dir, PinName reset, PinName limit, int adc_ch):
mkelly10 9:d5fcdcb3c89d 8 _motor(pwm, dir, reset),
mkelly10 9:d5fcdcb3c89d 9 _filter(),
mkelly10 9:d5fcdcb3c89d 10 _pid(),
mkelly10 9:d5fcdcb3c89d 11 _pulse(),
mkelly10 9:d5fcdcb3c89d 12 _limitSwitch(limit)
mkelly10 9:d5fcdcb3c89d 13 {
mkelly10 9:d5fcdcb3c89d 14 _limitSwitch.fall(callback(this, &LinearActuator::_switchPressed));
danstrider 10:085ab7328054 15
mkelly10 9:d5fcdcb3c89d 16 _Pgain = 0.10;
mkelly10 9:d5fcdcb3c89d 17 _Igain = 0.0;
mkelly10 9:d5fcdcb3c89d 18 _Dgain = 0.0;
mkelly10 9:d5fcdcb3c89d 19
mkelly10 9:d5fcdcb3c89d 20 _filterFrequency = 1.0;
mkelly10 9:d5fcdcb3c89d 21
mkelly10 9:d5fcdcb3c89d 22 _adc_channel = adc_ch;
mkelly10 9:d5fcdcb3c89d 23
mkelly10 9:d5fcdcb3c89d 24 _dt = interval;
mkelly10 9:d5fcdcb3c89d 25
mkelly10 9:d5fcdcb3c89d 26 _init = true;
mkelly10 9:d5fcdcb3c89d 27 _paused = false;
mkelly10 9:d5fcdcb3c89d 28 _limit = false;
mkelly10 9:d5fcdcb3c89d 29 _slope = 498.729/4096; //this value should be correct for our current string pots using .625" diameter and 12 bit ADC
mkelly10 9:d5fcdcb3c89d 30 _deadband = 0.5;
mkelly10 9:d5fcdcb3c89d 31
mkelly10 9:d5fcdcb3c89d 32 }
mkelly10 9:d5fcdcb3c89d 33
danstrider 10:085ab7328054 34 void LinearActuator::init() {
mkelly10 9:d5fcdcb3c89d 35 // initialize and start all of the member objects.
mkelly10 9:d5fcdcb3c89d 36 // The pos-velocity filter for this item needs to be allowed to converge
mkelly10 9:d5fcdcb3c89d 37 // Before turning over control to the motor
mkelly10 9:d5fcdcb3c89d 38 // make sure that adc().init is called in mainline code before calling this function
mkelly10 9:d5fcdcb3c89d 39
mkelly10 9:d5fcdcb3c89d 40 //load gains into pid controller this should eventually be in a config file
mkelly10 9:d5fcdcb3c89d 41 setFilterFrequency(_filterFrequency);
mkelly10 9:d5fcdcb3c89d 42 setControllerP(_Pgain);
mkelly10 9:d5fcdcb3c89d 43 setControllerI(_Igain);
mkelly10 9:d5fcdcb3c89d 44 setControllerD(_Dgain);
mkelly10 9:d5fcdcb3c89d 45
mkelly10 9:d5fcdcb3c89d 46 //setup the controller object
mkelly10 9:d5fcdcb3c89d 47 //set deadband and limits
mkelly10 9:d5fcdcb3c89d 48 toggleDeadband(true);
mkelly10 9:d5fcdcb3c89d 49 setDeadband(_deadband);
mkelly10 9:d5fcdcb3c89d 50 }
mkelly10 9:d5fcdcb3c89d 51
danstrider 10:085ab7328054 52 void LinearActuator::update() {
danstrider 10:085ab7328054 53 // update the position velocity filter
mkelly10 9:d5fcdcb3c89d 54 if (_adc_channel == 0) {
mkelly10 9:d5fcdcb3c89d 55 _filter.update(_dt, adc().readCh0());
mkelly10 9:d5fcdcb3c89d 56 } else if (_adc_channel == 1) {
mkelly10 9:d5fcdcb3c89d 57 _filter.update(_dt, adc().readCh1());
mkelly10 9:d5fcdcb3c89d 58 } else {
mkelly10 9:d5fcdcb3c89d 59 error("\n\r This ADC channel does not exist");
mkelly10 9:d5fcdcb3c89d 60 }
mkelly10 9:d5fcdcb3c89d 61
danstrider 10:085ab7328054 62 // refresh the filter results and load into class variables
mkelly10 9:d5fcdcb3c89d 63 refreshPVState();
mkelly10 9:d5fcdcb3c89d 64
mkelly10 9:d5fcdcb3c89d 65 // update the PID controller with latest data
mkelly10 9:d5fcdcb3c89d 66 _pid.update(_position_mm, _velocity_mms, _filter.getDt());
mkelly10 9:d5fcdcb3c89d 67
mkelly10 9:d5fcdcb3c89d 68 if (_init) {
mkelly10 9:d5fcdcb3c89d 69 //The initialization phase is active
mkelly10 9:d5fcdcb3c89d 70 //dont run the motors until the velocity stabilizes
mkelly10 9:d5fcdcb3c89d 71 if (abs(_velocity_mms)<0.1) {
mkelly10 9:d5fcdcb3c89d 72 //we must be converged and can now release the motor controls
mkelly10 9:d5fcdcb3c89d 73 _init = false;
mkelly10 9:d5fcdcb3c89d 74 }
mkelly10 9:d5fcdcb3c89d 75 } else if (_limit) {
mkelly10 9:d5fcdcb3c89d 76 //if you end up here, an interrupt was triggered by an edge detection
mkelly10 9:d5fcdcb3c89d 77 //case on the limit switch. That function would have stopped the motor
mkelly10 9:d5fcdcb3c89d 78 //This logic should allow it to be re-enabled if the output being asked
mkelly10 9:d5fcdcb3c89d 79 //for by the controller is the correct direction.
mkelly10 9:d5fcdcb3c89d 80
mkelly10 9:d5fcdcb3c89d 81 if (_pid.getOutput() < 0.0) { //a negative pid output results in motion toward the switch
mkelly10 9:d5fcdcb3c89d 82 //dont let the motor run
mkelly10 9:d5fcdcb3c89d 83 return;
mkelly10 9:d5fcdcb3c89d 84 } else {
mkelly10 9:d5fcdcb3c89d 85 //We are now trucking along in the correct direction, it is ok to
mkelly10 9:d5fcdcb3c89d 86 //reset the limit variable and allow the motor to run
mkelly10 9:d5fcdcb3c89d 87 _limit = false;
mkelly10 9:d5fcdcb3c89d 88 }
mkelly10 9:d5fcdcb3c89d 89 } else if (_paused) {
mkelly10 9:d5fcdcb3c89d 90 //if you get here, the pause function has stopped the motor
mkelly10 9:d5fcdcb3c89d 91 //the only way out is for a function call to unpause the motor
mkelly10 9:d5fcdcb3c89d 92 //this case also keeps the controller disconnected in the event that
mkelly10 9:d5fcdcb3c89d 93 //homing is happening
mkelly10 9:d5fcdcb3c89d 94 return;
mkelly10 9:d5fcdcb3c89d 95 }
mkelly10 9:d5fcdcb3c89d 96
mkelly10 9:d5fcdcb3c89d 97 else {
mkelly10 9:d5fcdcb3c89d 98 //pipe controller data into the motor driver
mkelly10 9:d5fcdcb3c89d 99 //check that the limit switch is not pressed and if it is only let the
mkelly10 9:d5fcdcb3c89d 100 //motor run if the command is a negative number
mkelly10 9:d5fcdcb3c89d 101 //this catches the case where the piston is being moved off of the switch
mkelly10 9:d5fcdcb3c89d 102 //and the motor command reverses for some reason
mkelly10 9:d5fcdcb3c89d 103
mkelly10 9:d5fcdcb3c89d 104 if ((_limitSwitch.read() == 0) && (_pid.getOutput()< 0)) {
mkelly10 9:d5fcdcb3c89d 105 //dont run
mkelly10 9:d5fcdcb3c89d 106 return;
mkelly10 9:d5fcdcb3c89d 107 }
danstrider 10:085ab7328054 108
danstrider 10:085ab7328054 109 // clamp the output to the motor to -1.0 to 1.0
danstrider 10:085ab7328054 110 if (_pid.getOutput() > 1.0)
danstrider 10:085ab7328054 111 _motor.run(1.0);
danstrider 10:085ab7328054 112 else if (_pid.getOutput() < -1.0)
danstrider 10:085ab7328054 113 _motor.run(-1.0);
danstrider 10:085ab7328054 114 else
danstrider 10:085ab7328054 115 _motor.run(_pid.getOutput());
mkelly10 9:d5fcdcb3c89d 116 }
mkelly10 9:d5fcdcb3c89d 117 }
mkelly10 9:d5fcdcb3c89d 118
danstrider 10:085ab7328054 119 void LinearActuator::start() {
mkelly10 9:d5fcdcb3c89d 120 _init = true;
mkelly10 9:d5fcdcb3c89d 121 _pulse.attach(this,&LinearActuator::update, _dt);
mkelly10 9:d5fcdcb3c89d 122 }
mkelly10 9:d5fcdcb3c89d 123
danstrider 10:085ab7328054 124 void LinearActuator::stop() {
mkelly10 9:d5fcdcb3c89d 125 _motor.stop();
mkelly10 9:d5fcdcb3c89d 126 _pulse.detach();
mkelly10 12:a0519d11d2b6 127 _init = true;
mkelly10 9:d5fcdcb3c89d 128 }
mkelly10 9:d5fcdcb3c89d 129
danstrider 10:085ab7328054 130 void LinearActuator::pause() {
mkelly10 9:d5fcdcb3c89d 131 //this allows the controller to keep running while turning off the motor output
mkelly10 9:d5fcdcb3c89d 132 _motor.stop();
mkelly10 9:d5fcdcb3c89d 133 //paused flag causes controller output not to be piped to the motor
mkelly10 9:d5fcdcb3c89d 134 _paused = true;
mkelly10 9:d5fcdcb3c89d 135 }
mkelly10 9:d5fcdcb3c89d 136
danstrider 10:085ab7328054 137 void LinearActuator::unpause() {
mkelly10 9:d5fcdcb3c89d 138 //this resumes motor operation
mkelly10 9:d5fcdcb3c89d 139 _paused = false;
mkelly10 9:d5fcdcb3c89d 140 }
mkelly10 9:d5fcdcb3c89d 141
danstrider 10:085ab7328054 142 void LinearActuator::refreshPVState() {
mkelly10 9:d5fcdcb3c89d 143 _position = _filter.getPosition();
mkelly10 9:d5fcdcb3c89d 144 _velocity = _filter.getVelocity();
mkelly10 9:d5fcdcb3c89d 145
mkelly10 9:d5fcdcb3c89d 146 _position_mm = counts_to_dist(_position);
mkelly10 9:d5fcdcb3c89d 147 _velocity_mms = counts_to_velocity(_velocity);
mkelly10 9:d5fcdcb3c89d 148 }
mkelly10 9:d5fcdcb3c89d 149
mkelly10 9:d5fcdcb3c89d 150 // setting and getting variables
danstrider 10:085ab7328054 151 void LinearActuator::setPosition_mm(float dist) {
mkelly10 9:d5fcdcb3c89d 152 _SetPoint_mm = clamp<float>(dist, 0.0, _extendLimit); //this is another spot that prevents the requested set point from going out of range, this template function is defined in the controller header file fyi
mkelly10 9:d5fcdcb3c89d 153
mkelly10 9:d5fcdcb3c89d 154 _pid.writeSetPoint(_SetPoint_mm);
mkelly10 9:d5fcdcb3c89d 155 }
danstrider 11:3b241ecb75ed 156
danstrider 11:3b241ecb75ed 157 float LinearActuator::getSetPosition_mm() {
danstrider 11:3b241ecb75ed 158 return _SetPoint_mm;
danstrider 11:3b241ecb75ed 159 }
mkelly10 9:d5fcdcb3c89d 160
danstrider 10:085ab7328054 161 float LinearActuator::getPosition_mm() {
mkelly10 9:d5fcdcb3c89d 162 return _position_mm;
mkelly10 9:d5fcdcb3c89d 163 }
mkelly10 9:d5fcdcb3c89d 164
danstrider 11:3b241ecb75ed 165 float LinearActuator::getPosition_counts() {
danstrider 11:3b241ecb75ed 166 return _position;
danstrider 11:3b241ecb75ed 167 }
mkelly10 9:d5fcdcb3c89d 168
danstrider 10:085ab7328054 169 float LinearActuator::getVelocity_mms() {
mkelly10 9:d5fcdcb3c89d 170 return _velocity_mms;
mkelly10 9:d5fcdcb3c89d 171 }
mkelly10 9:d5fcdcb3c89d 172
danstrider 10:085ab7328054 173 void LinearActuator::setControllerP(float P) {
mkelly10 9:d5fcdcb3c89d 174 _Pgain = P;
mkelly10 9:d5fcdcb3c89d 175 _pid.setPgain(_Pgain);
mkelly10 9:d5fcdcb3c89d 176 return;
mkelly10 9:d5fcdcb3c89d 177 }
mkelly10 9:d5fcdcb3c89d 178
danstrider 10:085ab7328054 179 float LinearActuator::getControllerP() {
mkelly10 9:d5fcdcb3c89d 180 return _Pgain;
mkelly10 9:d5fcdcb3c89d 181 }
mkelly10 9:d5fcdcb3c89d 182
danstrider 10:085ab7328054 183 void LinearActuator::setControllerI(float I) {
mkelly10 9:d5fcdcb3c89d 184 _Igain = I;
mkelly10 9:d5fcdcb3c89d 185 _pid.setIgain(_Igain);
mkelly10 9:d5fcdcb3c89d 186 return;
mkelly10 9:d5fcdcb3c89d 187 }
mkelly10 9:d5fcdcb3c89d 188
danstrider 10:085ab7328054 189 float LinearActuator::getControllerI() {
mkelly10 9:d5fcdcb3c89d 190 return _Igain;
mkelly10 9:d5fcdcb3c89d 191 }
mkelly10 9:d5fcdcb3c89d 192
danstrider 10:085ab7328054 193 void LinearActuator::setControllerD(float D) {
mkelly10 9:d5fcdcb3c89d 194 _Dgain = D;
mkelly10 9:d5fcdcb3c89d 195 _pid.setDgain(_Dgain);
mkelly10 9:d5fcdcb3c89d 196 return;
mkelly10 9:d5fcdcb3c89d 197 }
mkelly10 9:d5fcdcb3c89d 198
danstrider 10:085ab7328054 199 float LinearActuator::getControllerD() {
mkelly10 9:d5fcdcb3c89d 200 return _Dgain;
mkelly10 9:d5fcdcb3c89d 201 }
mkelly10 9:d5fcdcb3c89d 202
danstrider 10:085ab7328054 203 float LinearActuator::getOutput() {
mkelly10 9:d5fcdcb3c89d 204 return _pid.getOutput();
mkelly10 9:d5fcdcb3c89d 205 }
danstrider 10:085ab7328054 206
danstrider 10:085ab7328054 207 void LinearActuator::setZeroCounts(int zero) {
mkelly10 9:d5fcdcb3c89d 208 _zeroCounts = clamp<int>(zero, 0, 4096);
mkelly10 9:d5fcdcb3c89d 209 return;
mkelly10 9:d5fcdcb3c89d 210 }
mkelly10 9:d5fcdcb3c89d 211
danstrider 10:085ab7328054 212 int LinearActuator::getZeroCounts() {
mkelly10 9:d5fcdcb3c89d 213 return _zeroCounts;
mkelly10 9:d5fcdcb3c89d 214 }
mkelly10 9:d5fcdcb3c89d 215
danstrider 10:085ab7328054 216 void LinearActuator::setTravelLimit(float limit) {
mkelly10 9:d5fcdcb3c89d 217 _extendLimit = limit;
mkelly10 9:d5fcdcb3c89d 218 return;
mkelly10 9:d5fcdcb3c89d 219 }
mkelly10 9:d5fcdcb3c89d 220
danstrider 10:085ab7328054 221 float LinearActuator::getTravelLimit() {
mkelly10 9:d5fcdcb3c89d 222 return _extendLimit;
mkelly10 9:d5fcdcb3c89d 223 }
mkelly10 9:d5fcdcb3c89d 224
danstrider 10:085ab7328054 225 void LinearActuator::setPotSlope(float slope) {
mkelly10 9:d5fcdcb3c89d 226 _slope = slope;
mkelly10 9:d5fcdcb3c89d 227 return;
mkelly10 9:d5fcdcb3c89d 228 }
mkelly10 9:d5fcdcb3c89d 229
danstrider 10:085ab7328054 230 float LinearActuator::getPotSlope() {
mkelly10 9:d5fcdcb3c89d 231 return _slope;
mkelly10 9:d5fcdcb3c89d 232 }
mkelly10 9:d5fcdcb3c89d 233
danstrider 10:085ab7328054 234 float LinearActuator::counts_to_dist(int count) {
mkelly10 9:d5fcdcb3c89d 235 float conv = _slope*(count-_zeroCounts);
mkelly10 9:d5fcdcb3c89d 236 return conv;
mkelly10 9:d5fcdcb3c89d 237 }
mkelly10 9:d5fcdcb3c89d 238
danstrider 10:085ab7328054 239 void LinearActuator::setFilterFrequency(float frequency) {
mkelly10 9:d5fcdcb3c89d 240 _filterFrequency = frequency;
mkelly10 9:d5fcdcb3c89d 241 _filter.writeWn(frequency);
mkelly10 9:d5fcdcb3c89d 242 }
mkelly10 9:d5fcdcb3c89d 243
danstrider 10:085ab7328054 244 int LinearActuator::dist_to_counts(float dist) {
mkelly10 9:d5fcdcb3c89d 245 float conv = (dist/_slope)+_zeroCounts;
mkelly10 9:d5fcdcb3c89d 246 return (int) conv;
mkelly10 9:d5fcdcb3c89d 247 }
mkelly10 9:d5fcdcb3c89d 248
danstrider 10:085ab7328054 249 float LinearActuator::counts_to_velocity(int count) {
mkelly10 9:d5fcdcb3c89d 250 float conv = count*_slope;
mkelly10 9:d5fcdcb3c89d 251 return conv;
mkelly10 9:d5fcdcb3c89d 252 }
mkelly10 9:d5fcdcb3c89d 253
danstrider 10:085ab7328054 254 void LinearActuator::_switchPressed() {
mkelly10 9:d5fcdcb3c89d 255 //first thing to do is stop the motor
mkelly10 9:d5fcdcb3c89d 256 _motor.stop();
mkelly10 9:d5fcdcb3c89d 257 _limit = true;
mkelly10 9:d5fcdcb3c89d 258 }
mkelly10 9:d5fcdcb3c89d 259
danstrider 10:085ab7328054 260 void LinearActuator::homePiston() {
mkelly10 9:d5fcdcb3c89d 261 //start calling the update for the Linear Actuator
mkelly10 9:d5fcdcb3c89d 262 //start the controller update and immediatley pause the motor output
mkelly10 9:d5fcdcb3c89d 263 start();
mkelly10 9:d5fcdcb3c89d 264 pause();
mkelly10 9:d5fcdcb3c89d 265
mkelly10 9:d5fcdcb3c89d 266 //Now that the readings are stabilized
mkelly10 9:d5fcdcb3c89d 267 // This sends the motor on a kamakaze mission toward the limit switch
danstrider 10:085ab7328054 268 // The interrupt should catch and stop it, and the piston is now at home
danstrider 10:085ab7328054 269 // position
danstrider 11:3b241ecb75ed 270 _motor.run(-0.5);
mkelly10 9:d5fcdcb3c89d 271
mkelly10 9:d5fcdcb3c89d 272 while (1) {
mkelly10 9:d5fcdcb3c89d 273 //trap the program here while we wait for the limit switch to be triggered
mkelly10 9:d5fcdcb3c89d 274 //when it does, the limit interrupt will stop the motors
mkelly10 9:d5fcdcb3c89d 275 if (_limit) {
mkelly10 9:d5fcdcb3c89d 276 //the switch has been pressed
mkelly10 9:d5fcdcb3c89d 277 if (abs(_filter.getVelocity()) < 0.1) {
mkelly10 9:d5fcdcb3c89d 278 //this is here to make sure the adc filter is not jittering around
mkelly10 9:d5fcdcb3c89d 279 //we are probably stable enough to take a zero here
mkelly10 9:d5fcdcb3c89d 280 _zeroCounts = _filter.getPosition();
mkelly10 9:d5fcdcb3c89d 281
mkelly10 9:d5fcdcb3c89d 282 // This can be used for troubleshooting
danstrider 11:3b241ecb75ed 283 pc().printf("\n\rzero_counts: %4i \n\r" , _zeroCounts);
mkelly10 9:d5fcdcb3c89d 284 return;
mkelly10 9:d5fcdcb3c89d 285 }
mkelly10 9:d5fcdcb3c89d 286 }
mkelly10 9:d5fcdcb3c89d 287 }
mkelly10 9:d5fcdcb3c89d 288 }
mkelly10 9:d5fcdcb3c89d 289
danstrider 10:085ab7328054 290 bool LinearActuator::getSwitch() {
mkelly10 9:d5fcdcb3c89d 291 return _limit;
mkelly10 9:d5fcdcb3c89d 292 }
mkelly10 9:d5fcdcb3c89d 293
danstrider 10:085ab7328054 294 void LinearActuator::setDeadband(float deadband) {
mkelly10 9:d5fcdcb3c89d 295 _deadband = deadband;
mkelly10 9:d5fcdcb3c89d 296 _pid.setDeadBand(_deadband);
mkelly10 9:d5fcdcb3c89d 297 return;
mkelly10 9:d5fcdcb3c89d 298 }
danstrider 11:3b241ecb75ed 299
danstrider 11:3b241ecb75ed 300 float LinearActuator::getDeadband() {
danstrider 11:3b241ecb75ed 301 return _deadband;
danstrider 11:3b241ecb75ed 302 }
mkelly10 9:d5fcdcb3c89d 303
danstrider 10:085ab7328054 304 bool LinearActuator::toggleDeadband(bool toggle) {
mkelly10 9:d5fcdcb3c89d 305 _pid.toggleDeadBand(toggle);
mkelly10 9:d5fcdcb3c89d 306 return toggle;
danstrider 10:085ab7328054 307 }