Stephen Wilkins / TPixy-Interface

A library for interfacing with the Pixy color recognition camera

Dependents:   PixyCamera MbedOS_Robot_Team ManualControlFinal PlayBack ... more

Diff: Pixy.cpp

Revision:
0:ef0e3c67dc5b
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Pixy.cpp	Mon Mar 14 00:35:23 2016 +0000
@@ -0,0 +1,25 @@
+#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;
+}
+