Stephen Wilkins / TPixy-Interface

A library for interfacing with the Pixy color recognition camera

Dependents:   PixyCamera MbedOS_Robot_Team ManualControlFinal PlayBack ... more

Pixy.cpp

Committer:
swilkins8
Date:
2016-03-14
Revision:
0:ef0e3c67dc5b

File content as of revision 0:ef0e3c67dc5b:

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