Shao Rui
/
AGV
1
PositionSensor/PositionSensor.h@0:571a1835428e, 2021-01-25 (annotated)
- Committer:
- shaorui
- Date:
- Mon Jan 25 08:36:48 2021 +0000
- Revision:
- 0:571a1835428e
1
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shaorui | 0:571a1835428e | 1 | #ifndef POSITIONSENSOR_H |
shaorui | 0:571a1835428e | 2 | #define POSITIONSENSOR_H |
shaorui | 0:571a1835428e | 3 | |
shaorui | 0:571a1835428e | 4 | #define PI 3.14159265359f |
shaorui | 0:571a1835428e | 5 | class PositionSensor { |
shaorui | 0:571a1835428e | 6 | public: |
shaorui | 0:571a1835428e | 7 | virtual void Sample(float dt) = 0; |
shaorui | 0:571a1835428e | 8 | virtual float GetMechPosition() {return 0.0f;} |
shaorui | 0:571a1835428e | 9 | virtual float GetMechPositionFixed() {return 0.0f;} |
shaorui | 0:571a1835428e | 10 | virtual float GetElecPosition() {return 0.0f;} |
shaorui | 0:571a1835428e | 11 | virtual float GetMechVelocity() {return 0.0f;} |
shaorui | 0:571a1835428e | 12 | virtual float GetElecVelocity() {return 0.0f;} |
shaorui | 0:571a1835428e | 13 | virtual void ZeroPosition(void) = 0; |
shaorui | 0:571a1835428e | 14 | virtual int GetRawPosition(void) = 0; |
shaorui | 0:571a1835428e | 15 | virtual void SetElecOffset(float offset) = 0; |
shaorui | 0:571a1835428e | 16 | virtual int GetCPR(void) = 0; |
shaorui | 0:571a1835428e | 17 | virtual void WriteLUT(int new_lut[128]) = 0; |
shaorui | 0:571a1835428e | 18 | }; |
shaorui | 0:571a1835428e | 19 | |
shaorui | 0:571a1835428e | 20 | |
shaorui | 0:571a1835428e | 21 | class PositionSensorEncoder: public PositionSensor { |
shaorui | 0:571a1835428e | 22 | public: |
shaorui | 0:571a1835428e | 23 | PositionSensorEncoder(int CPR, float offset, int ppairs); |
shaorui | 0:571a1835428e | 24 | virtual void Sample(float dt); |
shaorui | 0:571a1835428e | 25 | virtual float GetMechPosition(); |
shaorui | 0:571a1835428e | 26 | virtual float GetElecPosition(); |
shaorui | 0:571a1835428e | 27 | virtual float GetMechVelocity(); |
shaorui | 0:571a1835428e | 28 | virtual float GetElecVelocity(); |
shaorui | 0:571a1835428e | 29 | virtual void ZeroPosition(void); |
shaorui | 0:571a1835428e | 30 | virtual void SetElecOffset(float offset); |
shaorui | 0:571a1835428e | 31 | virtual int GetRawPosition(void); |
shaorui | 0:571a1835428e | 32 | virtual int GetCPR(void); |
shaorui | 0:571a1835428e | 33 | virtual void WriteLUT(int new_lut[128]); |
shaorui | 0:571a1835428e | 34 | private: |
shaorui | 0:571a1835428e | 35 | InterruptIn *ZPulse; |
shaorui | 0:571a1835428e | 36 | DigitalIn *ZSense; |
shaorui | 0:571a1835428e | 37 | //DigitalOut *ZTest; |
shaorui | 0:571a1835428e | 38 | virtual void ZeroEncoderCount(void); |
shaorui | 0:571a1835428e | 39 | virtual void ZeroEncoderCountDown(void); |
shaorui | 0:571a1835428e | 40 | int _CPR, flag, rotations, _ppairs, raw; |
shaorui | 0:571a1835428e | 41 | //int state; |
shaorui | 0:571a1835428e | 42 | float _offset, MechPosition, MechOffset, dir, test_pos, oldVel, out_old, velVec[40]; |
shaorui | 0:571a1835428e | 43 | int offset_lut[128]; |
shaorui | 0:571a1835428e | 44 | }; |
shaorui | 0:571a1835428e | 45 | |
shaorui | 0:571a1835428e | 46 | class PositionSensorAM5147: public PositionSensor{ |
shaorui | 0:571a1835428e | 47 | public: |
shaorui | 0:571a1835428e | 48 | PositionSensorAM5147(int CPR, float offset, int ppairs); |
shaorui | 0:571a1835428e | 49 | virtual void Sample(float dt); |
shaorui | 0:571a1835428e | 50 | virtual float GetMechPosition(); |
shaorui | 0:571a1835428e | 51 | virtual float GetMechPositionFixed(); |
shaorui | 0:571a1835428e | 52 | virtual float GetElecPosition(); |
shaorui | 0:571a1835428e | 53 | virtual float GetMechVelocity(); |
shaorui | 0:571a1835428e | 54 | virtual float GetElecVelocity(); |
shaorui | 0:571a1835428e | 55 | virtual int GetRawPosition(); |
shaorui | 0:571a1835428e | 56 | virtual void ZeroPosition(); |
shaorui | 0:571a1835428e | 57 | virtual void SetElecOffset(float offset); |
shaorui | 0:571a1835428e | 58 | virtual void SetMechOffset(float offset); |
shaorui | 0:571a1835428e | 59 | virtual int GetCPR(void); |
shaorui | 0:571a1835428e | 60 | virtual void WriteLUT(int new_lut[128]); |
shaorui | 0:571a1835428e | 61 | private: |
shaorui | 0:571a1835428e | 62 | float position, ElecPosition, ElecOffset, MechPosition, MechOffset, modPosition, oldModPosition, oldVel, velVec[40], MechVelocity, ElecVelocity, ElecVelocityFilt; |
shaorui | 0:571a1835428e | 63 | int raw, _CPR, rotations, old_counts, _ppairs; |
shaorui | 0:571a1835428e | 64 | SPI *spi; |
shaorui | 0:571a1835428e | 65 | DigitalOut *cs; |
shaorui | 0:571a1835428e | 66 | int readAngleCmd; |
shaorui | 0:571a1835428e | 67 | int offset_lut[128]; |
shaorui | 0:571a1835428e | 68 | |
shaorui | 0:571a1835428e | 69 | }; |
shaorui | 0:571a1835428e | 70 | |
shaorui | 0:571a1835428e | 71 | #endif |