Diff: Servoloop.cpp

Revision:

0:b12a5af91608

Child:

1:7915442d0bd8

diff -r 000000000000 -r b12a5af91608 Servoloop.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servoloop.cpp	Mon Apr 13 12:16:51 2015 +0000
@@ -0,0 +1,32 @@
+#include"mbed.h"
+#include"Servoloop.h"
+
+// ServoLoop Constructor
+Servoloop::Servoloop(int32_t proportionalGain, int32_t derivativeGain)
+{
+m_pos = RCS_CENTER_POS;
+m_proportionalGain = proportionalGain;
+m_derivativeGain = derivativeGain;
+m_prevError = 0x80000000L;
+}
+// ServoLoop Update
+// Calculates new output based on the measured
+// error and the current state.
+
+    
+void Servoloop::update(int32_t error){
+long int velocity;
+char buf[32];
+if (m_prevError!=0x80000000){
+velocity = (error*m_proportionalGain + (error - m_prevError)*m_derivativeGain)>>10;
+m_pos += velocity;
+if (m_pos>RCS_MAX_POS){
+m_pos = RCS_MAX_POS;}
+else if (m_pos<RCS_MIN_POS){
+m_pos = RCS_MIN_POS;}
+}
+m_prevError = error;
+}
+
+
+