Diff: LinearActuator/LinearActuator.cpp

Revision:

9:d5fcdcb3c89d

Child:

10:085ab7328054

diff -r 70412939a506 -r d5fcdcb3c89d LinearActuator/LinearActuator.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/LinearActuator/LinearActuator.cpp	Fri Oct 20 11:41:22 2017 +0000
@@ -0,0 +1,343 @@
+#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;
+}   
+            
\ No newline at end of file