Luis Enriquez / CACHAN_HOMOG

Homologation Cachan

Fork of TPixy-Interface by Stephen Wilkins

Pixy.cpp@0:ef0e3c67dc5b, 2016-03-14 (annotated)

Committer:
swilkins8
Date:
Mon Mar 14 00:35:23 2016 +0000
Revision:
0:ef0e3c67dc5b
Completed rewrite of code to expand interfacing options

Who changed what in which revision?

UserRevisionLine numberNew contents of line
swilkins8 0:ef0e3c67dc5b 1 #include "Pixy.h"
swilkins8 0:ef0e3c67dc5b 2
swilkins8 0:ef0e3c67dc5b 3 ServoLoop::ServoLoop(int32_t pgain, int32_t dgain)
swilkins8 0:ef0e3c67dc5b 4 {
swilkins8 0:ef0e3c67dc5b 5 m_pos = PIXY_RCS_CENTER_POS;
swilkins8 0:ef0e3c67dc5b 6 m_pgain = pgain;
swilkins8 0:ef0e3c67dc5b 7 m_dgain = dgain;
swilkins8 0:ef0e3c67dc5b 8 m_prevError = 0x80000000L;
swilkins8 0:ef0e3c67dc5b 9 }
swilkins8 0:ef0e3c67dc5b 10
swilkins8 0:ef0e3c67dc5b 11 void ServoLoop::update(int32_t error)
swilkins8 0:ef0e3c67dc5b 12 {
swilkins8 0:ef0e3c67dc5b 13 long int vel;
swilkins8 0:ef0e3c67dc5b 14 if (m_prevError != 0x80000000) {
swilkins8 0:ef0e3c67dc5b 15 vel = (error*m_pgain + (error - m_prevError)*m_dgain) >> 10;
swilkins8 0:ef0e3c67dc5b 16 m_pos += vel;
swilkins8 0:ef0e3c67dc5b 17 if (m_pos > PIXY_RCS_MAX_POS) {
swilkins8 0:ef0e3c67dc5b 18 m_pos = PIXY_RCS_MAX_POS;
swilkins8 0:ef0e3c67dc5b 19 } else if (m_pos < PIXY_RCS_MIN_POS) {
swilkins8 0:ef0e3c67dc5b 20 m_pos = PIXY_RCS_MIN_POS;
swilkins8 0:ef0e3c67dc5b 21 }
swilkins8 0:ef0e3c67dc5b 22 }
swilkins8 0:ef0e3c67dc5b 23 m_prevError = error;
swilkins8 0:ef0e3c67dc5b 24 }
swilkins8 0:ef0e3c67dc5b 25