Flying Sea Glider / Mbed 2 deprecated 2019_10may_firstflight_jcw_nosd

most functionality to splashdwon, find neutral and start mission. short timeouts still in code for testing, will adjust to go directly to sit_idle after splashdown

Dependencies:   mbed MODSERIAL FATFileSystem

LinearActuator/LinearActuator.cpp@65:2ac186553959, 2018-06-19 (annotated)

Committer:
tnhnrl
Date:
Tue Jun 19 18:10:50 2018 +0000
Revision:
65:2ac186553959
Parent:
62:d502889e74f1
Child:
73:f6f378311c8d
Version with limit switches in output

Who changed what in which revision?

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