jetfishteam / Mbed 2 deprecated mbed_PI_Depth

PI depth control test

Dependencies:   MS5803 mbed Servo

Diff: IMUDepthControl.cpp

Revision:
0:df16f9bfc07b
Child:
1:07e046bbcb84
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMUDepthControl.cpp	Wed Aug 06 20:17:02 2014 +0000
@@ -0,0 +1,28 @@
+#include "IMUDepthControl.h"
+
+IMUDepthControl::IMUDepthControl(PinName sda, PinName scl, float Kp, float Ki) :
+    IMU(sda,scl),
+    m_Kp(Kp),
+    m_Ki(Ki)
+{
+    IMU.MS5803Init();
+    m_errorsum=0;
+}
+
+void IMUDepthControl::setPoint(float setpoint)
+{
+    m_set_point=setpoint;
+}
+
+float IMUDepthControl::iterate()
+{
+    IMU.Barometer_MS5803();
+    float error=m_set_point-IMU.MS5803_Pressure(); //get the error
+    
+    m_errorsum+=error; //integrate it
+    float I=m_errorsum*m_Ki; //multiply it with the integral gain
+    
+    float P=error*m_Kp; //proportional control
+    
+    return P+I;
+}
\ No newline at end of file