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@10:085ab7328054, 2017-10-23 (annotated)

Committer:
danstrider
Date:
Mon Oct 23 12:50:53 2017 +0000
Revision:
10:085ab7328054
Parent:
9:d5fcdcb3c89d
Child:
11:3b241ecb75ed
checked out on the hardware

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 9:d5fcdcb3c89d 127 }
mkelly10 9:d5fcdcb3c89d 128
danstrider 10:085ab7328054 129 void LinearActuator::pause() {
mkelly10 9:d5fcdcb3c89d 130 //this allows the controller to keep running while turning off the motor output
mkelly10 9:d5fcdcb3c89d 131 _motor.stop();
mkelly10 9:d5fcdcb3c89d 132 //paused flag causes controller output not to be piped to the motor
mkelly10 9:d5fcdcb3c89d 133 _paused = true;
mkelly10 9:d5fcdcb3c89d 134 }
mkelly10 9:d5fcdcb3c89d 135
danstrider 10:085ab7328054 136 void LinearActuator::unpause() {
mkelly10 9:d5fcdcb3c89d 137 //this resumes motor operation
mkelly10 9:d5fcdcb3c89d 138 _paused = false;
mkelly10 9:d5fcdcb3c89d 139 }
mkelly10 9:d5fcdcb3c89d 140
danstrider 10:085ab7328054 141 void LinearActuator::refreshPVState() {
mkelly10 9:d5fcdcb3c89d 142 _position = _filter.getPosition();
mkelly10 9:d5fcdcb3c89d 143 _velocity = _filter.getVelocity();
mkelly10 9:d5fcdcb3c89d 144
mkelly10 9:d5fcdcb3c89d 145 _position_mm = counts_to_dist(_position);
mkelly10 9:d5fcdcb3c89d 146 _velocity_mms = counts_to_velocity(_velocity);
mkelly10 9:d5fcdcb3c89d 147 }
mkelly10 9:d5fcdcb3c89d 148
mkelly10 9:d5fcdcb3c89d 149 // setting and getting variables
danstrider 10:085ab7328054 150 void LinearActuator::setPosition_mm(float dist) {
mkelly10 9:d5fcdcb3c89d 151 _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 152
mkelly10 9:d5fcdcb3c89d 153 _pid.writeSetPoint(_SetPoint_mm);
mkelly10 9:d5fcdcb3c89d 154 }
mkelly10 9:d5fcdcb3c89d 155
danstrider 10:085ab7328054 156 float LinearActuator::getPosition_mm() {
mkelly10 9:d5fcdcb3c89d 157 return _position_mm;
mkelly10 9:d5fcdcb3c89d 158 }
mkelly10 9:d5fcdcb3c89d 159
danstrider 10:085ab7328054 160 //float LinearActuator::getPosition_counts() {
danstrider 10:085ab7328054 161 // return _position; // returns raw adc counts (useful for zeroing)
danstrider 10:085ab7328054 162 //}
mkelly10 9:d5fcdcb3c89d 163
danstrider 10:085ab7328054 164 float LinearActuator::getVelocity_mms() {
mkelly10 9:d5fcdcb3c89d 165 return _velocity_mms;
mkelly10 9:d5fcdcb3c89d 166 }
mkelly10 9:d5fcdcb3c89d 167
danstrider 10:085ab7328054 168 void LinearActuator::setControllerP(float P) {
mkelly10 9:d5fcdcb3c89d 169 _Pgain = P;
mkelly10 9:d5fcdcb3c89d 170 _pid.setPgain(_Pgain);
mkelly10 9:d5fcdcb3c89d 171 return;
mkelly10 9:d5fcdcb3c89d 172 }
mkelly10 9:d5fcdcb3c89d 173
danstrider 10:085ab7328054 174 float LinearActuator::getControllerP() {
mkelly10 9:d5fcdcb3c89d 175 return _Pgain;
mkelly10 9:d5fcdcb3c89d 176 }
mkelly10 9:d5fcdcb3c89d 177
danstrider 10:085ab7328054 178 void LinearActuator::setControllerI(float I) {
mkelly10 9:d5fcdcb3c89d 179 _Igain = I;
mkelly10 9:d5fcdcb3c89d 180 _pid.setIgain(_Igain);
mkelly10 9:d5fcdcb3c89d 181 return;
mkelly10 9:d5fcdcb3c89d 182 }
mkelly10 9:d5fcdcb3c89d 183
danstrider 10:085ab7328054 184 float LinearActuator::getControllerI() {
mkelly10 9:d5fcdcb3c89d 185 return _Igain;
mkelly10 9:d5fcdcb3c89d 186 }
mkelly10 9:d5fcdcb3c89d 187
danstrider 10:085ab7328054 188 void LinearActuator::setControllerD(float D) {
mkelly10 9:d5fcdcb3c89d 189 _Dgain = D;
mkelly10 9:d5fcdcb3c89d 190 _pid.setDgain(_Dgain);
mkelly10 9:d5fcdcb3c89d 191 return;
mkelly10 9:d5fcdcb3c89d 192 }
mkelly10 9:d5fcdcb3c89d 193
danstrider 10:085ab7328054 194 float LinearActuator::getControllerD() {
mkelly10 9:d5fcdcb3c89d 195 return _Dgain;
mkelly10 9:d5fcdcb3c89d 196 }
mkelly10 9:d5fcdcb3c89d 197
danstrider 10:085ab7328054 198 float LinearActuator::getOutput() {
mkelly10 9:d5fcdcb3c89d 199 return _pid.getOutput();
mkelly10 9:d5fcdcb3c89d 200 }
danstrider 10:085ab7328054 201
danstrider 10:085ab7328054 202 void LinearActuator::setZeroCounts(int zero) {
mkelly10 9:d5fcdcb3c89d 203 _zeroCounts = clamp<int>(zero, 0, 4096);
mkelly10 9:d5fcdcb3c89d 204 return;
mkelly10 9:d5fcdcb3c89d 205 }
mkelly10 9:d5fcdcb3c89d 206
danstrider 10:085ab7328054 207 int LinearActuator::getZeroCounts() {
mkelly10 9:d5fcdcb3c89d 208 return _zeroCounts;
mkelly10 9:d5fcdcb3c89d 209 }
mkelly10 9:d5fcdcb3c89d 210
danstrider 10:085ab7328054 211 void LinearActuator::setTravelLimit(float limit) {
mkelly10 9:d5fcdcb3c89d 212 _extendLimit = limit;
mkelly10 9:d5fcdcb3c89d 213 return;
mkelly10 9:d5fcdcb3c89d 214 }
mkelly10 9:d5fcdcb3c89d 215
danstrider 10:085ab7328054 216 float LinearActuator::getTravelLimit() {
mkelly10 9:d5fcdcb3c89d 217 return _extendLimit;
mkelly10 9:d5fcdcb3c89d 218 }
mkelly10 9:d5fcdcb3c89d 219
danstrider 10:085ab7328054 220 void LinearActuator::setPotSlope(float slope) {
mkelly10 9:d5fcdcb3c89d 221 _slope = slope;
mkelly10 9:d5fcdcb3c89d 222 return;
mkelly10 9:d5fcdcb3c89d 223 }
mkelly10 9:d5fcdcb3c89d 224
danstrider 10:085ab7328054 225 float LinearActuator::getPotSlope() {
mkelly10 9:d5fcdcb3c89d 226 return _slope;
mkelly10 9:d5fcdcb3c89d 227 }
mkelly10 9:d5fcdcb3c89d 228
danstrider 10:085ab7328054 229 float LinearActuator::counts_to_dist(int count) {
mkelly10 9:d5fcdcb3c89d 230 float conv = _slope*(count-_zeroCounts);
mkelly10 9:d5fcdcb3c89d 231 return conv;
mkelly10 9:d5fcdcb3c89d 232 }
mkelly10 9:d5fcdcb3c89d 233
danstrider 10:085ab7328054 234 void LinearActuator::setFilterFrequency(float frequency) {
mkelly10 9:d5fcdcb3c89d 235 _filterFrequency = frequency;
mkelly10 9:d5fcdcb3c89d 236 _filter.writeWn(frequency);
mkelly10 9:d5fcdcb3c89d 237 }
mkelly10 9:d5fcdcb3c89d 238
danstrider 10:085ab7328054 239 int LinearActuator::dist_to_counts(float dist) {
mkelly10 9:d5fcdcb3c89d 240 float conv = (dist/_slope)+_zeroCounts;
mkelly10 9:d5fcdcb3c89d 241 return (int) conv;
mkelly10 9:d5fcdcb3c89d 242 }
mkelly10 9:d5fcdcb3c89d 243
danstrider 10:085ab7328054 244 float LinearActuator::counts_to_velocity(int count) {
mkelly10 9:d5fcdcb3c89d 245 float conv = count*_slope;
mkelly10 9:d5fcdcb3c89d 246 return conv;
mkelly10 9:d5fcdcb3c89d 247 }
mkelly10 9:d5fcdcb3c89d 248
danstrider 10:085ab7328054 249 void LinearActuator::_switchPressed() {
mkelly10 9:d5fcdcb3c89d 250 //first thing to do is stop the motor
mkelly10 9:d5fcdcb3c89d 251 _motor.stop();
mkelly10 9:d5fcdcb3c89d 252 _limit = true;
mkelly10 9:d5fcdcb3c89d 253 }
mkelly10 9:d5fcdcb3c89d 254
danstrider 10:085ab7328054 255 void LinearActuator::homePiston() {
mkelly10 9:d5fcdcb3c89d 256 //start calling the update for the Linear Actuator
mkelly10 9:d5fcdcb3c89d 257 //start the controller update and immediatley pause the motor output
mkelly10 9:d5fcdcb3c89d 258 start();
mkelly10 9:d5fcdcb3c89d 259 pause();
mkelly10 9:d5fcdcb3c89d 260
mkelly10 9:d5fcdcb3c89d 261 //trap program execution here until the filter is converged
mkelly10 9:d5fcdcb3c89d 262 while(_init){
mkelly10 9:d5fcdcb3c89d 263 //just wait here until the things calm down
mkelly10 9:d5fcdcb3c89d 264 }
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
mkelly10 9:d5fcdcb3c89d 270 _motor.run(-1.0);
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
mkelly10 9:d5fcdcb3c89d 283 //pc().printf("\n\rzero_counts: %4i \n\r" , _zeroCounts);
mkelly10 9:d5fcdcb3c89d 284 //stop the update loop
mkelly10 9:d5fcdcb3c89d 285 stop();
mkelly10 9:d5fcdcb3c89d 286 return;
mkelly10 9:d5fcdcb3c89d 287 }
mkelly10 9:d5fcdcb3c89d 288 }
mkelly10 9:d5fcdcb3c89d 289 }
mkelly10 9:d5fcdcb3c89d 290 }
mkelly10 9:d5fcdcb3c89d 291
danstrider 10:085ab7328054 292 bool LinearActuator::getSwitch() {
mkelly10 9:d5fcdcb3c89d 293 return _limit;
mkelly10 9:d5fcdcb3c89d 294 }
mkelly10 9:d5fcdcb3c89d 295
danstrider 10:085ab7328054 296 void LinearActuator::setDeadband(float deadband) {
mkelly10 9:d5fcdcb3c89d 297 _deadband = deadband;
mkelly10 9:d5fcdcb3c89d 298 _pid.setDeadBand(_deadband);
mkelly10 9:d5fcdcb3c89d 299 return;
mkelly10 9:d5fcdcb3c89d 300 }
mkelly10 9:d5fcdcb3c89d 301
danstrider 10:085ab7328054 302 bool LinearActuator::toggleDeadband(bool toggle) {
mkelly10 9:d5fcdcb3c89d 303 _pid.toggleDeadBand(toggle);
mkelly10 9:d5fcdcb3c89d 304 return toggle;
danstrider 10:085ab7328054 305 }