LinearActuator.cpp

00001 #include "mbed.h"
00002 #include "LinearActuator.hpp"
00003 #include "StaticDefs.hpp"
00004 #include "ConfigFile.h"
00005  
00006 // this is where the variables that can be set are set when the object is created
00007 LinearActuator::LinearActuator(float interval, PinName pwm, PinName dir, PinName reset, PinName limit, int adc_ch):
00008     _motor(pwm, dir, reset),
00009     _filter(),
00010     _pid(),
00011     _pulse(),
00012     _limitSwitch(limit)
00013 {
00014     _limitSwitch.fall(callback(this, &LinearActuator::_switchPressed));
00015     
00016     _Pgain = 0.10;
00017     _Igain = 0.0;
00018     _Dgain = 0.0;
00019     
00020     _filterFrequency = 1.0;
00021  
00022     _adc_channel = adc_ch;
00023  
00024     _dt = interval;
00025  
00026     _init = true;
00027     _paused = false;
00028     _limit = false;
00029     _slope = 498.729/4096;  //this value should be correct for our current string pots using .625" diameter and 12 bit ADC (hardcoded in config as 0.12176)
00030     _deadband = 0.5;
00031  
00032     _pid_high_limit = 0.0;
00033     _pid_low_limit = 0.0;
00034 }
00035  
00036 void LinearActuator::init() {
00037     // initialize and start all of the member objects.
00038     // The pos-velocity filter for this item needs to be allowed to converge
00039     // Before turning over control to the motor
00040     // make sure that adc().init is called in mainline code before calling this function
00041  
00042     //load gains into pid controller this should eventually be in a config file
00043     setFilterFrequency(_filterFrequency);
00044     setControllerP(_Pgain);
00045     setControllerI(_Igain);
00046     setControllerD(_Dgain);
00047     
00048     //setup the controller object
00049     //set deadband and limits
00050     toggleDeadband(true);
00051     setDeadband(_deadband);
00052 }
00053  
00054 void LinearActuator::update() { 
00055     // update the position velocity filter
00056     if (_adc_channel == 0) {
00057         _filter.update(_dt, adc().readCh0());
00058     } else if (_adc_channel == 1) {
00059         _filter.update(_dt, adc().readCh1());
00060     } else {
00061         error("\n\r This ADC channel does not exist");
00062     }
00063  
00064     // refresh the filter results and load into class variables
00065     refreshPVState();
00066  
00067     // update the PID controller with latest data
00068     _pid.update(_position_mm, _velocity_mms, _filter.getDt());
00069  
00070     if (_init) {
00071         //The initialization phase is active
00072         //dont run the motors until the velocity stabilizes
00073         if (abs(_velocity_mms)<0.1) {
00074             //we must be converged and can now release the motor controls
00075             _init = false;
00076         }
00077     } else if (_limit) {
00078         //if you end up here, an interrupt was triggered by an edge detection
00079         //case on the limit switch. That function would have stopped the motor
00080         //This logic should allow it to be re-enabled if the output being asked
00081         //for by the controller is the correct direction.
00082  
00083         if (_pid.getOutput() < 0.0) { //a negative pid output results in motion toward the switch
00084             //dont let the motor run
00085             return;
00086         } else {
00087             //We are now trucking along in the correct direction, it is ok to
00088             //reset the limit variable and allow the motor to run
00089             _limit = false;
00090         }
00091     } else if (_paused) {
00092         //if you get here, the pause function has stopped the motor
00093         //the only way out is for a function call to unpause the motor
00094         //this case also keeps the controller disconnected in the event that
00095         //homing is happening
00096         return;
00097     }
00098  
00099     else {
00100         //pipe controller data into the motor driver
00101         //check that the limit switch is not pressed and if it is only let the
00102         //motor run if the command is a negative number
00103         //this catches the case where the piston is being moved off of the switch
00104         //and the motor command reverses for some reason
00105  
00106         if ((_limitSwitch.read() == 0) && (_pid.getOutput()< 0)) {
00107             //dont run
00108             return;
00109         }
00110         
00111         // clamp the output to the motor to -1.0 to 1.0
00112         if (_pid.getOutput() > 1.0)
00113             _motor.run(1.0);
00114         else if (_pid.getOutput() < -1.0)
00115             _motor.run(-1.0);
00116         else 
00117             _motor.run(_pid.getOutput());
00118     }
00119 }
00120 
00121 // using main loop ticker (attached at 0.01 intervals)
00122 void LinearActuator::runLinearActuator() {
00123     _init = true;
00124 }
00125  
00126 void LinearActuator::pause() {
00127     //this allows the controller to keep running while turning off the motor output
00128     _motor.stop();
00129     //paused flag causes controller output not to be piped to the motor
00130     _paused = true;
00131 }
00132  
00133 void LinearActuator::unpause() {
00134     //this resumes motor operation
00135     _paused = false;
00136 }
00137  
00138 void LinearActuator::refreshPVState() {
00139     _position = _filter.getPosition();
00140     _velocity = _filter.getVelocity();
00141  
00142     _position_mm = counts_to_dist(_position);
00143     _velocity_mms = counts_to_velocity(_velocity);
00144 }
00145  
00146 // setting and getting variables
00147 void LinearActuator::setPosition_mm(float dist) {
00148     _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
00149  
00150     _pid.writeSetPoint(_SetPoint_mm);
00151 }
00152 
00153 float LinearActuator::getSetPosition_mm() {
00154     return _SetPoint_mm;
00155 }
00156  
00157 float LinearActuator::getPosition_mm() {
00158     return _position_mm;
00159 }
00160  
00161 float LinearActuator::getPosition_counts() {
00162     return _position;
00163 }
00164  
00165 float LinearActuator::getVelocity_mms() {
00166     return _velocity_mms;
00167 }
00168  
00169 void LinearActuator::setControllerP(float P) {
00170     _Pgain = P;
00171     _pid.setPgain(_Pgain);
00172     return;
00173 }
00174  
00175 float LinearActuator::getControllerP() {
00176     return _Pgain;
00177 }
00178  
00179 void LinearActuator::setControllerI(float I) {
00180     _Igain = I;
00181     _pid.setIgain(_Igain);
00182     return;
00183 }
00184  
00185 float LinearActuator::getControllerI() {
00186     return _Igain;
00187 }
00188  
00189 void LinearActuator::setControllerD(float D) {
00190     _Dgain = D;
00191     _pid.setDgain(_Dgain);
00192     return;
00193 }
00194  
00195 float LinearActuator::getControllerD() {
00196     return _Dgain;
00197 }
00198  
00199 float LinearActuator::getOutput() {
00200     return _pid.getOutput();
00201 }
00202 
00203 void LinearActuator::setZeroCounts(int zero) {
00204     _zeroCounts = clamp<int>(zero, 0, 4096);
00205     return;
00206 }
00207  
00208 int LinearActuator::getZeroCounts() {
00209     return _zeroCounts;    
00210 }
00211  
00212 void LinearActuator::setTravelLimit(float limit) {
00213     _extendLimit = limit;
00214     return;
00215 }
00216  
00217 float LinearActuator::getTravelLimit() {
00218     return _extendLimit;   
00219 }
00220  
00221 void LinearActuator::setPotSlope(float slope) {
00222     _slope = slope;
00223     return;
00224 }
00225  
00226 float LinearActuator::getPotSlope() {
00227     return _slope;    
00228 }
00229  
00230 float LinearActuator::counts_to_dist(int count) {
00231     float conv = _slope*(count-_zeroCounts);
00232     return conv;
00233 }
00234  
00235 void LinearActuator::setFilterFrequency(float frequency) {
00236     _filterFrequency = frequency;
00237     _filter.writeWn(frequency);   
00238 }
00239 
00240 float LinearActuator::getFilterFrequency() {
00241     return _filterFrequency;               //new 7/11/18
00242 }
00243  
00244 int LinearActuator::dist_to_counts(float dist) {
00245     float conv = (dist/_slope)+_zeroCounts;
00246     return (int) conv;
00247 }
00248  
00249 float LinearActuator::counts_to_velocity(int count) {
00250     float conv = count*_slope;
00251     return conv;
00252 }
00253  
00254 void LinearActuator::_switchPressed() {
00255     //first thing to do is stop the motor
00256     _motor.stop();
00257     _limit = true;
00258 }
00259  
00260 void LinearActuator::homePiston() {
00261     //system is already active, input readings should be valid
00262     
00263     // This sends the motor on a kamakaze mission toward the limit switch
00264     // The interrupt should catch and stop it, and the piston is now at home
00265     // position
00266     
00267     //unpause the motor (activate it)
00268     unpause();
00269     
00270     _motor.run(-0.5);
00271     
00272     xbee().printf("HOMING SEQUENCE ENGAGED. Press \"X\" to exit!\n\r");
00273     
00274     while (1) {
00275         //trap the program here while we wait for the limit switch to be triggered
00276         //when it does, the limit interrupt will stop the motors
00277         if (_limit) {
00278             xbee().printf("\r\nHit limit switch\r\n");
00279             //the switch has been pressed
00280             if (abs(_filter.getVelocity()) < 0.1) {
00281                 //this is here to make sure the adc filter is not jittering around
00282                 //we are probably stable enough to take a zero here
00283                 
00284                 _zeroCounts = _filter.getPosition() + 50; //get position of motors
00285                 
00286                 //Added 50 counts for some margin of error
00287                 
00288                 // This can be used for troubleshooting
00289                 xbee().printf("\n\rzero_counts: %4i     \n\r" , _zeroCounts);
00290                 
00291                 //pause the motor (deactivate it)
00292                 pause();
00293                 
00294                 break;  //end while loop
00295             }
00296         } //end of limit switch if statement
00297         
00298         if (xbee().readable()) {
00299             char user_input = xbee().getc();
00300             
00301             if (user_input == 'x' or user_input == 'X') {
00302                 xbee().printf("EXIT! HOMING NOT COMPLETE!\n\r");
00303                 break;  //end while loop
00304             }
00305             
00306             else if (user_input == 'c' or user_input == 'C') {
00307                 xbee().printf("Current counts: %d\n\r", _filter.getPosition());
00308             }
00309         }
00310     }
00311 }
00312  
00313 bool LinearActuator::getSwitch() {
00314     return _limit;
00315 }
00316 
00317 bool LinearActuator::getHardwareSwitchStatus() {
00318     return _limitSwitch.read();
00319 }
00320  
00321  void LinearActuator::setDeadband(float deadband) {
00322     _deadband = deadband;
00323     _pid.setDeadBand(_deadband);
00324     return;    
00325 }
00326 
00327 float LinearActuator::getDeadband() {
00328     return _deadband;
00329 }
00330  
00331 bool LinearActuator::toggleDeadband(bool toggle) {
00332     _pid.toggleDeadBand(toggle);
00333     return toggle;
00334 }
00335 
00336 void LinearActuator::setPIDHighLimit(float high_limit) {
00337     _pid_high_limit = high_limit;
00338     _pid.setHiLimit(_pid_high_limit);
00339 }
00340 
00341 void LinearActuator::setPIDLowLimit(float low_limit) {
00342     _pid_low_limit = low_limit;
00343     _pid.setLoLimit(_pid_low_limit);                   //default at zero, or the switch retracted
00344 }