Stephen Wilkins / TPixy-Interface

A library for interfacing with the Pixy color recognition camera

Dependents:   PixyCamera MbedOS_Robot_Team ManualControlFinal PlayBack ... more

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Pixy.cpp Source File

Pixy.cpp

00001 #include "Pixy.h"
00002 
00003 ServoLoop::ServoLoop(int32_t pgain, int32_t dgain)
00004 {
00005     m_pos = PIXY_RCS_CENTER_POS;
00006     m_pgain = pgain;
00007     m_dgain = dgain;
00008     m_prevError = 0x80000000L;
00009 }
00010 
00011 void ServoLoop::update(int32_t error)
00012 {
00013     long int vel;
00014     if (m_prevError != 0x80000000) {
00015         vel = (error*m_pgain + (error - m_prevError)*m_dgain) >> 10;
00016         m_pos += vel;
00017         if (m_pos > PIXY_RCS_MAX_POS) {
00018             m_pos = PIXY_RCS_MAX_POS;
00019         } else if (m_pos < PIXY_RCS_MIN_POS) {
00020             m_pos = PIXY_RCS_MIN_POS;
00021         }
00022     }
00023     m_prevError = error;
00024 }
00025
Generated on Tue Jul 12 2022 18:15:33 by  doxygen 1.7.2