File content as of revision 11:3b241ecb75ed:

#include "mbed.h"
#include "LinearActuator.hpp"
#include "StaticDefs.hpp"
#include "ConfigFile.h"
 
// this is where the variables that can be set are set when the object is created
LinearActuator::LinearActuator(float interval, PinName pwm, PinName dir, PinName reset, PinName limit, int adc_ch):
    _motor(pwm, dir, reset),
    _filter(),
    _pid(),
    _pulse(),
    _limitSwitch(limit)
{
    _limitSwitch.fall(callback(this, &LinearActuator::_switchPressed));
    
    _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;
        }
        
        // clamp the output to the motor to -1.0 to 1.0
        if (_pid.getOutput() > 1.0)
            _motor.run(1.0);
        else if (_pid.getOutput() < -1.0)
            _motor.run(-1.0);
        else 
            _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);
}
 
// setting and getting variables
void LinearActuator::setPosition_mm(float 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::getSetPosition_mm() {
    return _SetPoint_mm;
}
 
float LinearActuator::getPosition_mm() {
    return _position_mm;
}
 
float LinearActuator::getPosition_counts() {
    return _position;
}
 
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(-0.5);
    
    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;    
}

float LinearActuator::getDeadband() {
    return _deadband;
}
 
bool LinearActuator::toggleDeadband(bool toggle) {
    _pid.toggleDeadBand(toggle);
    return toggle;
}