Majordhome/pixy
Servoloop.cpp@1:7915442d0bd8, 2015-04-14 (annotated)
- Committer:
- johnylafleur
- Date:
- Tue Apr 14 11:00:17 2015 +0000
- Revision:
- 1:7915442d0bd8
- Parent:
- 0:b12a5af91608
Pixy+MD25 Motor Track
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 | void Servoloop::update(int32_t error){ |
johnylafleur | 0:b12a5af91608 | 16 | long int velocity; |
johnylafleur | 0:b12a5af91608 | 17 | char buf[32]; |
johnylafleur | 0:b12a5af91608 | 18 | if (m_prevError!=0x80000000){ |
johnylafleur | 0:b12a5af91608 | 19 | velocity = (error*m_proportionalGain + (error - m_prevError)*m_derivativeGain)>>10; |
johnylafleur | 0:b12a5af91608 | 20 | m_pos += velocity; |
johnylafleur | 0:b12a5af91608 | 21 | if (m_pos>RCS_MAX_POS){ |
johnylafleur | 0:b12a5af91608 | 22 | m_pos = RCS_MAX_POS;} |
johnylafleur | 0:b12a5af91608 | 23 | else if (m_pos<RCS_MIN_POS){ |
johnylafleur | 0:b12a5af91608 | 24 | m_pos = RCS_MIN_POS;} |
johnylafleur | 0:b12a5af91608 | 25 | } |
johnylafleur | 0:b12a5af91608 | 26 | m_prevError = error; |
johnylafleur | 1:7915442d0bd8 | 27 | |
johnylafleur | 0:b12a5af91608 | 28 | } |
johnylafleur | 0:b12a5af91608 | 29 | |
johnylafleur | 0:b12a5af91608 | 30 | |
johnylafleur | 0:b12a5af91608 | 31 |