Majordhome/pixy
Servoloop.cpp@0:b12a5af91608, 2015-04-13 (annotated)
- Committer:
- johnylafleur
- Date:
- Mon Apr 13 12:16:51 2015 +0000
- Revision:
- 0:b12a5af91608
- Child:
- 1:7915442d0bd8
Majordhome/pixy;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
johnylafleur | 0:b12a5af91608 | 1 | #include"mbed.h" |
johnylafleur | 0:b12a5af91608 | 2 | #include"Servoloop.h" |
johnylafleur | 0:b12a5af91608 | 3 | |
johnylafleur | 0:b12a5af91608 | 4 | // ServoLoop Constructor |
johnylafleur | 0:b12a5af91608 | 5 | Servoloop::Servoloop(int32_t proportionalGain, int32_t derivativeGain) |
johnylafleur | 0:b12a5af91608 | 6 | { |
johnylafleur | 0:b12a5af91608 | 7 | m_pos = RCS_CENTER_POS; |
johnylafleur | 0:b12a5af91608 | 8 | m_proportionalGain = proportionalGain; |
johnylafleur | 0:b12a5af91608 | 9 | m_derivativeGain = derivativeGain; |
johnylafleur | 0:b12a5af91608 | 10 | m_prevError = 0x80000000L; |
johnylafleur | 0:b12a5af91608 | 11 | } |
johnylafleur | 0:b12a5af91608 | 12 | // ServoLoop Update |
johnylafleur | 0:b12a5af91608 | 13 | // Calculates new output based on the measured |
johnylafleur | 0:b12a5af91608 | 14 | // error and the current state. |
johnylafleur | 0:b12a5af91608 | 15 | |
johnylafleur | 0:b12a5af91608 | 16 | |
johnylafleur | 0:b12a5af91608 | 17 | void Servoloop::update(int32_t error){ |
johnylafleur | 0:b12a5af91608 | 18 | long int velocity; |
johnylafleur | 0:b12a5af91608 | 19 | char buf[32]; |
johnylafleur | 0:b12a5af91608 | 20 | if (m_prevError!=0x80000000){ |
johnylafleur | 0:b12a5af91608 | 21 | velocity = (error*m_proportionalGain + (error - m_prevError)*m_derivativeGain)>>10; |
johnylafleur | 0:b12a5af91608 | 22 | m_pos += velocity; |
johnylafleur | 0:b12a5af91608 | 23 | if (m_pos>RCS_MAX_POS){ |
johnylafleur | 0:b12a5af91608 | 24 | m_pos = RCS_MAX_POS;} |
johnylafleur | 0:b12a5af91608 | 25 | else if (m_pos<RCS_MIN_POS){ |
johnylafleur | 0:b12a5af91608 | 26 | m_pos = RCS_MIN_POS;} |
johnylafleur | 0:b12a5af91608 | 27 | } |
johnylafleur | 0:b12a5af91608 | 28 | m_prevError = error; |
johnylafleur | 0:b12a5af91608 | 29 | } |
johnylafleur | 0:b12a5af91608 | 30 | |
johnylafleur | 0:b12a5af91608 | 31 | |
johnylafleur | 0:b12a5af91608 | 32 |