File content as of revision 9:d5fcdcb3c89d:

#include "mbed.h"
#include "LinearActuator.hpp"
#include "StaticDefs.hpp"
#include "ConfigFile.h"
 
LinearActuator::LinearActuator(float interval, PinName pwm, PinName dir, PinName reset, PinName limit, int adc_ch):
    _motor(pwm, dir, reset),
    _filter(),
    _pid(),
    //_time(),
    _pulse(),
    _limitSwitch(limit)
{
    _limitSwitch.fall(callback(this, &LinearActuator::_switchPressed));
    // this is where the variables that can be set are set when the object is created
    _Pgain = 0.10;
    _Igain = 0.0;
    _Dgain = 0.0;
    
    _filterFrequency = 1.0;
 
    _adc_channel = adc_ch;
 
    _dt = interval;
 
    _init = true;
    _paused = false;
    _limit = false;
    _slope = 498.729/4096;  //this value should be correct for our current string pots using .625" diameter and 12 bit ADC
    _deadband = 0.5;
 
}
 
void LinearActuator::init()
{
    // initialize and start all of the member objects.
    // The pos-velocity filter for this item needs to be allowed to converge
    // Before turning over control to the motor
    // make sure that adc().init is called in mainline code before calling this function
 
    //load gains into pid controller this should eventually be in a config file
    setFilterFrequency(_filterFrequency);
    setControllerP(_Pgain);
    setControllerI(_Igain);
    setControllerD(_Dgain);
    
    //setup the controller object
    //set deadband and limits
    toggleDeadband(true);
    setDeadband(_deadband);
 
}
 
void LinearActuator::update()
{
 
 
    //update the position velocity filter
    if (_adc_channel == 0) {
        _filter.update(_dt, adc().readCh0());
    } else if (_adc_channel == 1) {
        _filter.update(_dt, adc().readCh1());
    } else {
        error("\n\r This ADC channel does not exist");
    }
 
    //refresh the filter results and load into class variables
    refreshPVState();
 
    // update the PID controller with latest data
    _pid.update(_position_mm, _velocity_mms, _filter.getDt());
 
    if (_init) {
        //The initialization phase is active
        //dont run the motors until the velocity stabilizes
        if (abs(_velocity_mms)<0.1) {
            //we must be converged and can now release the motor controls
            _init = false;
        }
    } else if (_limit) {
        //if you end up here, an interrupt was triggered by an edge detection
        //case on the limit switch. That function would have stopped the motor
        //This logic should allow it to be re-enabled if the output being asked
        //for by the controller is the correct direction.
 
        if (_pid.getOutput() < 0.0) { //a negative pid output results in motion toward the switch
            //dont let the motor run
            return;
        } else {
            //We are now trucking along in the correct direction, it is ok to
            //reset the limit variable and allow the motor to run
            _limit = false;
        }
    } else if (_paused) {
        //if you get here, the pause function has stopped the motor
        //the only way out is for a function call to unpause the motor
        //this case also keeps the controller disconnected in the event that
        //homing is happening
        return;
    }
 
    else {
        //pipe controller data into the motor driver
        //check that the limit switch is not pressed and if it is only let the
        //motor run if the command is a negative number
        //this catches the case where the piston is being moved off of the switch
        //and the motor command reverses for some reason
 
        if ((_limitSwitch.read() == 0) && (_pid.getOutput()< 0)) {
            //dont run
            return;
        }
        _motor.run(_pid.getOutput());
    }
}
 
void LinearActuator::start()
{
    _init = true;
    _pulse.attach(this,&LinearActuator::update, _dt);
    
}
 
void LinearActuator::stop()
{
    _motor.stop();
    _pulse.detach();
}
 
void LinearActuator::pause()
{
    //this allows the controller to keep running while turning off the motor output
    _motor.stop();
    //paused flag causes controller output not to be piped to the motor
    _paused = true;
}
 
void LinearActuator::unpause()
{
    //this resumes motor operation
    _paused = false;
}
 
void LinearActuator::refreshPVState()
{
    _position = _filter.getPosition();
    _velocity = _filter.getVelocity();
 
    _position_mm = counts_to_dist(_position);
    _velocity_mms = counts_to_velocity(_velocity);
 
//    _position_mL = _mm_to_mL(_position_mm);
//    _velocity_mLs = _mm_to_mL(_velocity_mms);
 
}
 
// setting and getting variables
void LinearActuator::setPosition_mm(float dist)
{
    // convert mm representation to volume in mL
//    _SetPoint_mL = _mm_to_mL(dist);
    _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
 
    _pid.writeSetPoint(_SetPoint_mm);
 
}
 
float LinearActuator::getPosition_mm()
{
    return _position_mm;
}
 
float LinearActuator::getPosition_counts()
{
    return _position; // returns raw adc counts (useful for zeroing)
}
 
float LinearActuator::getVelocity_mms()
{
    return _velocity_mms;
}
 
void LinearActuator::setControllerP(float P)
{
    _Pgain = P;
    _pid.setPgain(_Pgain);
    return;
}
 
float LinearActuator::getControllerP()
{
    return _Pgain;
}
 
void LinearActuator::setControllerI(float I)
{
    _Igain = I;
    _pid.setIgain(_Igain);
    return;
}
 
float LinearActuator::getControllerI()
{
    return _Igain;
}
 
void LinearActuator::setControllerD(float D)
{
    _Dgain = D;
    _pid.setDgain(_Dgain);
    return;
}
 
float LinearActuator::getControllerD()
{
    return _Dgain;
}
 
float LinearActuator::getOutput()
{
    return _pid.getOutput();
}
 
void LinearActuator::setZeroCounts(int zero)
{
    _zeroCounts = clamp<int>(zero, 0, 4096);
    return;
}
 
int LinearActuator::getZeroCounts()
{
    return _zeroCounts;    
}
 
void LinearActuator::setTravelLimit(float limit)
{
    _extendLimit = limit;
    return;
}
 
float LinearActuator::getTravelLimit()
{
    return _extendLimit;   
}
 
void LinearActuator::setPotSlope(float slope)
{
    _slope = slope;
    return;
}
 
float LinearActuator::getPotSlope()
{
    return _slope;    
}
 
float LinearActuator::counts_to_dist(int count)
{
    float conv = _slope*(count-_zeroCounts);
    return conv;
}
 
void LinearActuator::setFilterFrequency(float frequency)
{
    _filterFrequency = frequency;
    _filter.writeWn(frequency);   
}
 
int LinearActuator::dist_to_counts(float dist)
{
    float conv = (dist/_slope)+_zeroCounts;
    return (int) conv;
}
 
float LinearActuator::counts_to_velocity(int count)
{
    float conv = count*_slope;
    return conv;
}
 
void LinearActuator::_switchPressed()
{
    //first thing to do is stop the motor
    _motor.stop();
    _limit = true;
}
 
void LinearActuator::homePiston()
{
    //start calling the update for the Linear Actuator
    //start the controller update and immediatley pause the motor output
    start();
    pause();
    
    //trap program execution here until the filter is converged
    while(_init){
        //just wait here until the things calm down   
    }
    
    //Now that the readings are stabilized
    // This sends the motor on a kamakaze mission toward the limit switch
    // The interrupt should catch and stop it and the piston is now at home
    //position
    _motor.run(-1.0);
    
    while (1) {
        //trap the program here while we wait for the limit switch to be triggered
        //when it does, the limit interrupt will stop the motors
        if (_limit) {
            //the switch has been pressed
            if (abs(_filter.getVelocity()) < 0.1) {
                //this is here to make sure the adc filter is not jittering around
                //we are probably stable enough to take a zero here
                _zeroCounts = _filter.getPosition();
                
                // This can be used for troubleshooting
                //pc().printf("\n\rzero_counts: %4i     \n\r" , _zeroCounts);
                //stop the update loop
                stop();
                return;
            }
        }
    }
}
 
bool LinearActuator::getSwitch()
{
    return _limit;
}
 
 void LinearActuator::setDeadband(float deadband)
{
    _deadband = deadband;
    _pid.setDeadBand(_deadband);
    return;    
}
 
bool LinearActuator::toggleDeadband(bool toggle)
{
    _pid.toggleDeadBand(toggle);
    return toggle;
}