Diff: servoloop.cpp

Revision:

25:4f2f441eceec

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/servoloop.cpp	Fri Jul 11 14:30:36 2014 +0000
@@ -0,0 +1,29 @@
+#include "servoloop.h"
+ServoLoop::ServoLoop(int32_t pgain, int32_t dgain)
+{
+  m_pos = RCS_CENTER_POS;
+  m_pgain = pgain;
+  m_dgain = dgain;
+  m_prevError = 0x80000000L;
+}
+
+void ServoLoop::update(int32_t error)
+{
+  long int vel;
+  //char buf[32];
+  if (m_prevError!=0x80000000)
+  { 
+    vel = (error*m_pgain + (error - m_prevError)*m_dgain)>>10;
+    //vel = (error*m_pgain + (error - m_prevError)*m_dgain);
+    //sprintf(buf, "%ld\n", vel);
+    //Serial.print(buf);
+    m_pos += vel;
+    if (m_pos>RCS_MAX_POS) 
+      m_pos = RCS_MAX_POS; 
+    else if (m_pos<RCS_MIN_POS) 
+      m_pos = RCS_MIN_POS;
+
+    //cprintf("%d %d %d\n", m_axis, m_pos, vel);
+  }
+  m_prevError = error;
+}
\ No newline at end of file