Diff: IMUDepthControl.cpp

Revision:

1:07e046bbcb84

Parent:

0:df16f9bfc07b

--- a/IMUDepthControl.cpp	Wed Aug 06 20:17:02 2014 +0000
+++ b/IMUDepthControl.cpp	Wed Aug 06 20:33:01 2014 +0000
@@ -3,10 +3,16 @@
 IMUDepthControl::IMUDepthControl(PinName sda, PinName scl, float Kp, float Ki) :
     IMU(sda,scl),
     m_Kp(Kp),
-    m_Ki(Ki)
+    m_Ki(Ki),
+    output(p25)
 {
     IMU.MS5803Init();
     m_errorsum=0;
+    m_set_point=0;
+    m_delta_t=0.01;
+    m_last_pos=0.0;
+    t.start();
+    feedback.attach(this, &IMUDepthControl::iterate, m_delta_t);
 }
 
 void IMUDepthControl::setPoint(float setpoint)
@@ -14,15 +20,22 @@
     m_set_point=setpoint;
 }
 
-float IMUDepthControl::iterate()
+void 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
+    m_errorsum+=(error*m_Ki); //integrate it
+    float I=m_errorsum; //multiply it with the integral gain
     
     float P=error*m_Kp; //proportional control
     
-    return P+I;
+    //account for the change in position because servos don't take velocities
+    float newpos=(P+I)*m_delta_t+m_last_pos;
+    if (newpos>1.0)
+        newpos=1.0;
+    else if (newpos<0.0)
+        newpos=0.0;
+    output.write(newpos);
+    m_last_pos=newpos;
 }
\ No newline at end of file