Ruprecht Altenburger
/
TPixy-Interface
SPI read Pixi Cam V1
Pixy.cpp
- Committer:
- altb2
- Date:
- 2021-08-23
- Revision:
- 4:478d4d9193a1
- Parent:
- 0:ef0e3c67dc5b
File content as of revision 4:478d4d9193a1:
#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; }