Luis Enriquez / CACHAN_HOMOG

Homologation Cachan

Fork of TPixy-Interface by Stephen Wilkins

Pixy.cpp

Committer:
lenriquez389
Date:
2018-06-09
Revision:
4:ee635fc62b6e
Parent:
0:ef0e3c67dc5b

File content as of revision 4:ee635fc62b6e:

#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;
}