Homologation Cachan
Fork of TPixy-Interface by
Diff: Pixy.cpp
- Revision:
- 0:ef0e3c67dc5b
diff -r 000000000000 -r ef0e3c67dc5b Pixy.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Pixy.cpp Mon Mar 14 00:35:23 2016 +0000 @@ -0,0 +1,25 @@ +#include "Pixy.h" + +ServoLoop::ServoLoop(int32_t pgain, int32_t dgain) +{ + m_pos = PIXY_RCS_CENTER_POS; + m_pgain = pgain; + m_dgain = dgain; + m_prevError = 0x80000000L; +} + +void ServoLoop::update(int32_t error) +{ + long int vel; + if (m_prevError != 0x80000000) { + vel = (error*m_pgain + (error - m_prevError)*m_dgain) >> 10; + m_pos += vel; + if (m_pos > PIXY_RCS_MAX_POS) { + m_pos = PIXY_RCS_MAX_POS; + } else if (m_pos < PIXY_RCS_MIN_POS) { + m_pos = PIXY_RCS_MIN_POS; + } + } + m_prevError = error; +} +