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.hpp@73:f6f378311c8d, 2018-07-30 (annotated)
- Committer:
- tnhnrl
- Date:
- Mon Jul 30 16:48:48 2018 +0000
- Revision:
- 73:f6f378311c8d
- Parent:
- 65:2ac186553959
- Child:
- 74:d281aaef9766
work in progress 7/30 12:48 pm
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
tnhnrl | 62:d502889e74f1 | 1 | #ifndef LINEARACTUATOR_HPP |
tnhnrl | 62:d502889e74f1 | 2 | #define LINEARACTUATOR_HPP |
tnhnrl | 62:d502889e74f1 | 3 | |
tnhnrl | 62:d502889e74f1 | 4 | #include "mbed.h" |
tnhnrl | 62:d502889e74f1 | 5 | #include "PololuHBridge.hpp" |
tnhnrl | 62:d502889e74f1 | 6 | #include "PidController.hpp" |
tnhnrl | 62:d502889e74f1 | 7 | #include "ltc1298.hpp" |
tnhnrl | 62:d502889e74f1 | 8 | #include "PosVelFilter.hpp" |
tnhnrl | 62:d502889e74f1 | 9 | |
tnhnrl | 62:d502889e74f1 | 10 | //Dependencies |
tnhnrl | 62:d502889e74f1 | 11 | //This Class requires adc readings to sense the position of the piston |
tnhnrl | 62:d502889e74f1 | 12 | //This is a resource that ends up being shared among other classes in the vehicle |
tnhnrl | 62:d502889e74f1 | 13 | //for this reason it makes sense for it to be its own entity that is started in |
tnhnrl | 62:d502889e74f1 | 14 | //the main line code |
tnhnrl | 62:d502889e74f1 | 15 | |
tnhnrl | 62:d502889e74f1 | 16 | class LinearActuator { |
tnhnrl | 62:d502889e74f1 | 17 | public: |
tnhnrl | 62:d502889e74f1 | 18 | LinearActuator(float interval, PinName pwm, PinName dir, PinName reset, PinName limit, int adc_ch); |
tnhnrl | 62:d502889e74f1 | 19 | |
tnhnrl | 62:d502889e74f1 | 20 | // functions for setting up |
tnhnrl | 62:d502889e74f1 | 21 | void init(); |
tnhnrl | 62:d502889e74f1 | 22 | void update(); |
tnhnrl | 62:d502889e74f1 | 23 | void start(); |
tnhnrl | 62:d502889e74f1 | 24 | void stop(); |
tnhnrl | 62:d502889e74f1 | 25 | void pause(); |
tnhnrl | 62:d502889e74f1 | 26 | void unpause(); |
tnhnrl | 62:d502889e74f1 | 27 | |
tnhnrl | 62:d502889e74f1 | 28 | void runLinearActuator(); //new 03/12/2018 |
tnhnrl | 62:d502889e74f1 | 29 | |
tnhnrl | 62:d502889e74f1 | 30 | void refreshPVState(); |
tnhnrl | 62:d502889e74f1 | 31 | |
tnhnrl | 62:d502889e74f1 | 32 | // setting and getting variables |
tnhnrl | 62:d502889e74f1 | 33 | void setPosition_mm(float dist); |
tnhnrl | 62:d502889e74f1 | 34 | float getSetPosition_mm(); |
tnhnrl | 62:d502889e74f1 | 35 | |
tnhnrl | 62:d502889e74f1 | 36 | float getPosition_mm(); |
tnhnrl | 62:d502889e74f1 | 37 | float getPosition_counts(); |
tnhnrl | 62:d502889e74f1 | 38 | float getVelocity_mms(); |
tnhnrl | 62:d502889e74f1 | 39 | |
tnhnrl | 62:d502889e74f1 | 40 | void setControllerP(float P); |
tnhnrl | 62:d502889e74f1 | 41 | float getControllerP(); |
tnhnrl | 62:d502889e74f1 | 42 | |
tnhnrl | 62:d502889e74f1 | 43 | void setControllerI(float I); |
tnhnrl | 62:d502889e74f1 | 44 | float getControllerI(); |
tnhnrl | 62:d502889e74f1 | 45 | |
tnhnrl | 62:d502889e74f1 | 46 | void setControllerD(float D); |
tnhnrl | 62:d502889e74f1 | 47 | float getControllerD(); |
tnhnrl | 62:d502889e74f1 | 48 | |
tnhnrl | 62:d502889e74f1 | 49 | void setZeroCounts(int zero); |
tnhnrl | 62:d502889e74f1 | 50 | int getZeroCounts(); |
tnhnrl | 62:d502889e74f1 | 51 | |
tnhnrl | 62:d502889e74f1 | 52 | void setTravelLimit(float limit); |
tnhnrl | 62:d502889e74f1 | 53 | float getTravelLimit(); |
tnhnrl | 62:d502889e74f1 | 54 | |
tnhnrl | 62:d502889e74f1 | 55 | void setPotSlope(float slope); |
tnhnrl | 62:d502889e74f1 | 56 | float getPotSlope(); |
tnhnrl | 62:d502889e74f1 | 57 | |
tnhnrl | 62:d502889e74f1 | 58 | void homePiston(); |
tnhnrl | 62:d502889e74f1 | 59 | bool getSwitch(); |
tnhnrl | 62:d502889e74f1 | 60 | |
tnhnrl | 62:d502889e74f1 | 61 | float getOutput(); |
tnhnrl | 62:d502889e74f1 | 62 | |
tnhnrl | 62:d502889e74f1 | 63 | void setFilterFrequency(float frequency); |
tnhnrl | 73:f6f378311c8d | 64 | float getFilterFrequency(); // 7/11/18 |
tnhnrl | 62:d502889e74f1 | 65 | |
tnhnrl | 62:d502889e74f1 | 66 | void setDeadband(float deadband); |
tnhnrl | 62:d502889e74f1 | 67 | float getDeadband(); |
tnhnrl | 62:d502889e74f1 | 68 | bool toggleDeadband(bool toggle); |
tnhnrl | 62:d502889e74f1 | 69 | |
tnhnrl | 62:d502889e74f1 | 70 | void setPIDHighLimit(float high_limit); |
tnhnrl | 62:d502889e74f1 | 71 | void setPIDLowLimit(float low_limit); |
tnhnrl | 62:d502889e74f1 | 72 | |
tnhnrl | 65:2ac186553959 | 73 | bool getHardwareSwitchStatus(); //new |
tnhnrl | 65:2ac186553959 | 74 | |
tnhnrl | 62:d502889e74f1 | 75 | protected: |
tnhnrl | 62:d502889e74f1 | 76 | PololuHBridge _motor; |
tnhnrl | 62:d502889e74f1 | 77 | PosVelFilter _filter; |
tnhnrl | 62:d502889e74f1 | 78 | PIDController _pid; |
tnhnrl | 62:d502889e74f1 | 79 | Ticker _pulse; |
tnhnrl | 62:d502889e74f1 | 80 | InterruptIn _limitSwitch; |
tnhnrl | 62:d502889e74f1 | 81 | |
tnhnrl | 62:d502889e74f1 | 82 | void _switchPressed(); |
tnhnrl | 62:d502889e74f1 | 83 | void _switchReleased(); |
tnhnrl | 62:d502889e74f1 | 84 | void _calculateSensorSlope(); |
tnhnrl | 62:d502889e74f1 | 85 | |
tnhnrl | 62:d502889e74f1 | 86 | bool _init; |
tnhnrl | 62:d502889e74f1 | 87 | bool _paused; |
tnhnrl | 62:d502889e74f1 | 88 | bool _limit; |
tnhnrl | 62:d502889e74f1 | 89 | |
tnhnrl | 62:d502889e74f1 | 90 | int _adc_channel; |
tnhnrl | 62:d502889e74f1 | 91 | |
tnhnrl | 62:d502889e74f1 | 92 | float _filterFrequency; |
tnhnrl | 62:d502889e74f1 | 93 | |
tnhnrl | 62:d502889e74f1 | 94 | float _dt; |
tnhnrl | 62:d502889e74f1 | 95 | |
tnhnrl | 62:d502889e74f1 | 96 | float _SetPoint_mm; |
tnhnrl | 62:d502889e74f1 | 97 | |
tnhnrl | 62:d502889e74f1 | 98 | // position and velocity in counts (PVF runs on counts) |
tnhnrl | 62:d502889e74f1 | 99 | float _position; |
tnhnrl | 62:d502889e74f1 | 100 | float _velocity; |
tnhnrl | 62:d502889e74f1 | 101 | |
tnhnrl | 62:d502889e74f1 | 102 | // position and velocity converted to mm and mm/s |
tnhnrl | 62:d502889e74f1 | 103 | float _position_mm; |
tnhnrl | 62:d502889e74f1 | 104 | float _velocity_mms; |
tnhnrl | 62:d502889e74f1 | 105 | |
tnhnrl | 62:d502889e74f1 | 106 | // linear actuator servo PID gains |
tnhnrl | 62:d502889e74f1 | 107 | float _Pgain; |
tnhnrl | 62:d502889e74f1 | 108 | float _Igain; |
tnhnrl | 62:d502889e74f1 | 109 | float _Dgain; |
tnhnrl | 62:d502889e74f1 | 110 | |
tnhnrl | 62:d502889e74f1 | 111 | float _deadband; |
tnhnrl | 62:d502889e74f1 | 112 | |
tnhnrl | 62:d502889e74f1 | 113 | int _zeroCounts; //gets assigned by homing function. can also be stored in config |
tnhnrl | 62:d502889e74f1 | 114 | float _extendLimit; //config variable, limits the extension of the piston, this is same as datum for normal operation, |
tnhnrl | 62:d502889e74f1 | 115 | |
tnhnrl | 62:d502889e74f1 | 116 | float _slope; |
tnhnrl | 62:d502889e74f1 | 117 | int dist_to_counts(float dist); |
tnhnrl | 62:d502889e74f1 | 118 | float counts_to_dist(int count); |
tnhnrl | 62:d502889e74f1 | 119 | float counts_to_velocity(int count); |
tnhnrl | 62:d502889e74f1 | 120 | |
tnhnrl | 62:d502889e74f1 | 121 | float _pid_high_limit; |
tnhnrl | 62:d502889e74f1 | 122 | float _pid_low_limit; |
tnhnrl | 62:d502889e74f1 | 123 | }; |
tnhnrl | 62:d502889e74f1 | 124 | |
tnhnrl | 62:d502889e74f1 | 125 | |
tnhnrl | 62:d502889e74f1 | 126 | template <typename T> |
tnhnrl | 62:d502889e74f1 | 127 | T clamp(T value, T min, T max) |
tnhnrl | 62:d502889e74f1 | 128 | { |
tnhnrl | 62:d502889e74f1 | 129 | if(value < min) { |
tnhnrl | 62:d502889e74f1 | 130 | return min; |
tnhnrl | 62:d502889e74f1 | 131 | } else if(value > max) { |
tnhnrl | 62:d502889e74f1 | 132 | return max; |
tnhnrl | 62:d502889e74f1 | 133 | } else { |
tnhnrl | 62:d502889e74f1 | 134 | return value; |
tnhnrl | 62:d502889e74f1 | 135 | } |
tnhnrl | 62:d502889e74f1 | 136 | }; |
tnhnrl | 62:d502889e74f1 | 137 | |
tnhnrl | 62:d502889e74f1 | 138 | #endif |