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@14:85b64a4d08e8, 2017-10-31 (annotated)

Committer:
danstrider
Date:
Tue Oct 31 17:06:52 2017 +0000
Revision:
14:85b64a4d08e8
Parent:
13:84fcbe1dcd62
Child:
54:d4990fb68404
New depth sensor class with tare and oversampling.

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) {
danstrider 14:85b64a4d08e8 276 pc().printf("\r\nHit limit switch\r\n");
mkelly10 9:d5fcdcb3c89d 277 //the switch has been pressed
mkelly10 9:d5fcdcb3c89d 278 if (abs(_filter.getVelocity()) < 0.1) {
mkelly10 9:d5fcdcb3c89d 279 //this is here to make sure the adc filter is not jittering around
mkelly10 9:d5fcdcb3c89d 280 //we are probably stable enough to take a zero here
mkelly10 9:d5fcdcb3c89d 281 _zeroCounts = _filter.getPosition();
mkelly10 9:d5fcdcb3c89d 282
mkelly10 9:d5fcdcb3c89d 283 // This can be used for troubleshooting
danstrider 11:3b241ecb75ed 284 pc().printf("\n\rzero_counts: %4i \n\r" , _zeroCounts);
mkelly10 9:d5fcdcb3c89d 285 return;
mkelly10 9:d5fcdcb3c89d 286 }
mkelly10 9:d5fcdcb3c89d 287 }
mkelly10 9:d5fcdcb3c89d 288 }
mkelly10 9:d5fcdcb3c89d 289 }
mkelly10 9:d5fcdcb3c89d 290
danstrider 10:085ab7328054 291 bool LinearActuator::getSwitch() {
mkelly10 9:d5fcdcb3c89d 292 return _limit;
mkelly10 9:d5fcdcb3c89d 293 }
mkelly10 9:d5fcdcb3c89d 294
danstrider 10:085ab7328054 295 void LinearActuator::setDeadband(float deadband) {
mkelly10 9:d5fcdcb3c89d 296 _deadband = deadband;
mkelly10 9:d5fcdcb3c89d 297 _pid.setDeadBand(_deadband);
mkelly10 9:d5fcdcb3c89d 298 return;
mkelly10 9:d5fcdcb3c89d 299 }
danstrider 11:3b241ecb75ed 300
danstrider 11:3b241ecb75ed 301 float LinearActuator::getDeadband() {
danstrider 11:3b241ecb75ed 302 return _deadband;
danstrider 11:3b241ecb75ed 303 }
mkelly10 9:d5fcdcb3c89d 304
danstrider 10:085ab7328054 305 bool LinearActuator::toggleDeadband(bool toggle) {
mkelly10 9:d5fcdcb3c89d 306 _pid.toggleDeadBand(toggle);
mkelly10 9:d5fcdcb3c89d 307 return toggle;
danstrider 10:085ab7328054 308 }