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@11:3b241ecb75ed, 2017-10-27 (annotated)

Committer:
danstrider
Date:
Fri Oct 27 00:37:32 2017 +0000
Revision:
11:3b241ecb75ed
Parent:
10:085ab7328054
Child:
12:a0519d11d2b6
This version has been in the pool, working with all the hardware.  Had occasional string pot problems and got stuck in RISE, letting the battery stall out against the endcap.

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 }
danstrider 11:3b241ecb75ed 155
danstrider 11:3b241ecb75ed 156 float LinearActuator::getSetPosition_mm() {
danstrider 11:3b241ecb75ed 157 return _SetPoint_mm;
danstrider 11:3b241ecb75ed 158 }
mkelly10 9:d5fcdcb3c89d 159
danstrider 10:085ab7328054 160 float LinearActuator::getPosition_mm() {
mkelly10 9:d5fcdcb3c89d 161 return _position_mm;
mkelly10 9:d5fcdcb3c89d 162 }
mkelly10 9:d5fcdcb3c89d 163
danstrider 11:3b241ecb75ed 164 float LinearActuator::getPosition_counts() {
danstrider 11:3b241ecb75ed 165 return _position;
danstrider 11:3b241ecb75ed 166 }
mkelly10 9:d5fcdcb3c89d 167
danstrider 10:085ab7328054 168 float LinearActuator::getVelocity_mms() {
mkelly10 9:d5fcdcb3c89d 169 return _velocity_mms;
mkelly10 9:d5fcdcb3c89d 170 }
mkelly10 9:d5fcdcb3c89d 171
danstrider 10:085ab7328054 172 void LinearActuator::setControllerP(float P) {
mkelly10 9:d5fcdcb3c89d 173 _Pgain = P;
mkelly10 9:d5fcdcb3c89d 174 _pid.setPgain(_Pgain);
mkelly10 9:d5fcdcb3c89d 175 return;
mkelly10 9:d5fcdcb3c89d 176 }
mkelly10 9:d5fcdcb3c89d 177
danstrider 10:085ab7328054 178 float LinearActuator::getControllerP() {
mkelly10 9:d5fcdcb3c89d 179 return _Pgain;
mkelly10 9:d5fcdcb3c89d 180 }
mkelly10 9:d5fcdcb3c89d 181
danstrider 10:085ab7328054 182 void LinearActuator::setControllerI(float I) {
mkelly10 9:d5fcdcb3c89d 183 _Igain = I;
mkelly10 9:d5fcdcb3c89d 184 _pid.setIgain(_Igain);
mkelly10 9:d5fcdcb3c89d 185 return;
mkelly10 9:d5fcdcb3c89d 186 }
mkelly10 9:d5fcdcb3c89d 187
danstrider 10:085ab7328054 188 float LinearActuator::getControllerI() {
mkelly10 9:d5fcdcb3c89d 189 return _Igain;
mkelly10 9:d5fcdcb3c89d 190 }
mkelly10 9:d5fcdcb3c89d 191
danstrider 10:085ab7328054 192 void LinearActuator::setControllerD(float D) {
mkelly10 9:d5fcdcb3c89d 193 _Dgain = D;
mkelly10 9:d5fcdcb3c89d 194 _pid.setDgain(_Dgain);
mkelly10 9:d5fcdcb3c89d 195 return;
mkelly10 9:d5fcdcb3c89d 196 }
mkelly10 9:d5fcdcb3c89d 197
danstrider 10:085ab7328054 198 float LinearActuator::getControllerD() {
mkelly10 9:d5fcdcb3c89d 199 return _Dgain;
mkelly10 9:d5fcdcb3c89d 200 }
mkelly10 9:d5fcdcb3c89d 201
danstrider 10:085ab7328054 202 float LinearActuator::getOutput() {
mkelly10 9:d5fcdcb3c89d 203 return _pid.getOutput();
mkelly10 9:d5fcdcb3c89d 204 }
danstrider 10:085ab7328054 205
danstrider 10:085ab7328054 206 void LinearActuator::setZeroCounts(int zero) {
mkelly10 9:d5fcdcb3c89d 207 _zeroCounts = clamp<int>(zero, 0, 4096);
mkelly10 9:d5fcdcb3c89d 208 return;
mkelly10 9:d5fcdcb3c89d 209 }
mkelly10 9:d5fcdcb3c89d 210
danstrider 10:085ab7328054 211 int LinearActuator::getZeroCounts() {
mkelly10 9:d5fcdcb3c89d 212 return _zeroCounts;
mkelly10 9:d5fcdcb3c89d 213 }
mkelly10 9:d5fcdcb3c89d 214
danstrider 10:085ab7328054 215 void LinearActuator::setTravelLimit(float limit) {
mkelly10 9:d5fcdcb3c89d 216 _extendLimit = limit;
mkelly10 9:d5fcdcb3c89d 217 return;
mkelly10 9:d5fcdcb3c89d 218 }
mkelly10 9:d5fcdcb3c89d 219
danstrider 10:085ab7328054 220 float LinearActuator::getTravelLimit() {
mkelly10 9:d5fcdcb3c89d 221 return _extendLimit;
mkelly10 9:d5fcdcb3c89d 222 }
mkelly10 9:d5fcdcb3c89d 223
danstrider 10:085ab7328054 224 void LinearActuator::setPotSlope(float slope) {
mkelly10 9:d5fcdcb3c89d 225 _slope = slope;
mkelly10 9:d5fcdcb3c89d 226 return;
mkelly10 9:d5fcdcb3c89d 227 }
mkelly10 9:d5fcdcb3c89d 228
danstrider 10:085ab7328054 229 float LinearActuator::getPotSlope() {
mkelly10 9:d5fcdcb3c89d 230 return _slope;
mkelly10 9:d5fcdcb3c89d 231 }
mkelly10 9:d5fcdcb3c89d 232
danstrider 10:085ab7328054 233 float LinearActuator::counts_to_dist(int count) {
mkelly10 9:d5fcdcb3c89d 234 float conv = _slope*(count-_zeroCounts);
mkelly10 9:d5fcdcb3c89d 235 return conv;
mkelly10 9:d5fcdcb3c89d 236 }
mkelly10 9:d5fcdcb3c89d 237
danstrider 10:085ab7328054 238 void LinearActuator::setFilterFrequency(float frequency) {
mkelly10 9:d5fcdcb3c89d 239 _filterFrequency = frequency;
mkelly10 9:d5fcdcb3c89d 240 _filter.writeWn(frequency);
mkelly10 9:d5fcdcb3c89d 241 }
mkelly10 9:d5fcdcb3c89d 242
danstrider 10:085ab7328054 243 int LinearActuator::dist_to_counts(float dist) {
mkelly10 9:d5fcdcb3c89d 244 float conv = (dist/_slope)+_zeroCounts;
mkelly10 9:d5fcdcb3c89d 245 return (int) conv;
mkelly10 9:d5fcdcb3c89d 246 }
mkelly10 9:d5fcdcb3c89d 247
danstrider 10:085ab7328054 248 float LinearActuator::counts_to_velocity(int count) {
mkelly10 9:d5fcdcb3c89d 249 float conv = count*_slope;
mkelly10 9:d5fcdcb3c89d 250 return conv;
mkelly10 9:d5fcdcb3c89d 251 }
mkelly10 9:d5fcdcb3c89d 252
danstrider 10:085ab7328054 253 void LinearActuator::_switchPressed() {
mkelly10 9:d5fcdcb3c89d 254 //first thing to do is stop the motor
mkelly10 9:d5fcdcb3c89d 255 _motor.stop();
mkelly10 9:d5fcdcb3c89d 256 _limit = true;
mkelly10 9:d5fcdcb3c89d 257 }
mkelly10 9:d5fcdcb3c89d 258
danstrider 10:085ab7328054 259 void LinearActuator::homePiston() {
mkelly10 9:d5fcdcb3c89d 260 //start calling the update for the Linear Actuator
mkelly10 9:d5fcdcb3c89d 261 //start the controller update and immediatley pause the motor output
mkelly10 9:d5fcdcb3c89d 262 start();
mkelly10 9:d5fcdcb3c89d 263 pause();
mkelly10 9:d5fcdcb3c89d 264
danstrider 11:3b241ecb75ed 265 // //trap program execution here until the filter is converged
danstrider 11:3b241ecb75ed 266 // while(_init){
danstrider 11:3b241ecb75ed 267 // //just wait here until the things calm down
danstrider 11:3b241ecb75ed 268 // }
mkelly10 9:d5fcdcb3c89d 269
mkelly10 9:d5fcdcb3c89d 270 //Now that the readings are stabilized
mkelly10 9:d5fcdcb3c89d 271 // This sends the motor on a kamakaze mission toward the limit switch
danstrider 10:085ab7328054 272 // The interrupt should catch and stop it, and the piston is now at home
danstrider 10:085ab7328054 273 // position
danstrider 11:3b241ecb75ed 274 _motor.run(-0.5);
mkelly10 9:d5fcdcb3c89d 275
mkelly10 9:d5fcdcb3c89d 276 while (1) {
mkelly10 9:d5fcdcb3c89d 277 //trap the program here while we wait for the limit switch to be triggered
mkelly10 9:d5fcdcb3c89d 278 //when it does, the limit interrupt will stop the motors
mkelly10 9:d5fcdcb3c89d 279 if (_limit) {
mkelly10 9:d5fcdcb3c89d 280 //the switch has been pressed
mkelly10 9:d5fcdcb3c89d 281 if (abs(_filter.getVelocity()) < 0.1) {
mkelly10 9:d5fcdcb3c89d 282 //this is here to make sure the adc filter is not jittering around
mkelly10 9:d5fcdcb3c89d 283 //we are probably stable enough to take a zero here
mkelly10 9:d5fcdcb3c89d 284 _zeroCounts = _filter.getPosition();
mkelly10 9:d5fcdcb3c89d 285
mkelly10 9:d5fcdcb3c89d 286 // This can be used for troubleshooting
danstrider 11:3b241ecb75ed 287 pc().printf("\n\rzero_counts: %4i \n\r" , _zeroCounts);
mkelly10 9:d5fcdcb3c89d 288 //stop the update loop
mkelly10 9:d5fcdcb3c89d 289 stop();
mkelly10 9:d5fcdcb3c89d 290 return;
mkelly10 9:d5fcdcb3c89d 291 }
mkelly10 9:d5fcdcb3c89d 292 }
mkelly10 9:d5fcdcb3c89d 293 }
mkelly10 9:d5fcdcb3c89d 294 }
mkelly10 9:d5fcdcb3c89d 295
danstrider 10:085ab7328054 296 bool LinearActuator::getSwitch() {
mkelly10 9:d5fcdcb3c89d 297 return _limit;
mkelly10 9:d5fcdcb3c89d 298 }
mkelly10 9:d5fcdcb3c89d 299
danstrider 10:085ab7328054 300 void LinearActuator::setDeadband(float deadband) {
mkelly10 9:d5fcdcb3c89d 301 _deadband = deadband;
mkelly10 9:d5fcdcb3c89d 302 _pid.setDeadBand(_deadband);
mkelly10 9:d5fcdcb3c89d 303 return;
mkelly10 9:d5fcdcb3c89d 304 }
danstrider 11:3b241ecb75ed 305
danstrider 11:3b241ecb75ed 306 float LinearActuator::getDeadband() {
danstrider 11:3b241ecb75ed 307 return _deadband;
danstrider 11:3b241ecb75ed 308 }
mkelly10 9:d5fcdcb3c89d 309
danstrider 10:085ab7328054 310 bool LinearActuator::toggleDeadband(bool toggle) {
mkelly10 9:d5fcdcb3c89d 311 _pid.toggleDeadBand(toggle);
mkelly10 9:d5fcdcb3c89d 312 return toggle;
danstrider 10:085ab7328054 313 }