Revision 1:07e046bbcb84, committed 2014-08-06

Comitter:

sandwich

Date:

Wed Aug 06 20:33:01 2014 +0000

Parent:

0:df16f9bfc07b

Commit message:

accounted for servo not being a motor. Made class more self contained

Changed in this revision

diff -r df16f9bfc07b -r 07e046bbcb84 IMUDepthControl.cpp
--- 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

diff -r df16f9bfc07b -r 07e046bbcb84 IMUDepthControl.h
--- a/IMUDepthControl.h	Wed Aug 06 20:17:02 2014 +0000
+++ b/IMUDepthControl.h	Wed Aug 06 20:33:01 2014 +0000
@@ -1,4 +1,6 @@
 #pragma once
+#include "mbed.h"
+#include "Servo.h"
 #include "MS5803.h"
 
 class IMUDepthControl
@@ -9,8 +11,14 @@
     float m_Kp;
     float m_Ki;
     float m_errorsum;
+    float m_delta_t;
+    float m_last_pos;
+    Timer t;
+    Servo output;
+    
+    Ticker feedback;
 public:
     IMUDepthControl(PinName sda, PinName scl, float Kp, float Ki);
     void setPoint(float setpoint);
-    float iterate();
+    void iterate();
 };
\ No newline at end of file

diff -r df16f9bfc07b -r 07e046bbcb84 Servo.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Wed Aug 06 20:33:01 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/Servo/#36b69a7ced07

diff -r df16f9bfc07b -r 07e046bbcb84 main.cpp
--- a/main.cpp	Wed Aug 06 20:17:02 2014 +0000
+++ b/main.cpp	Wed Aug 06 20:33:01 2014 +0000
@@ -6,9 +6,6 @@
 
 int main() {
     unit.setPoint(120.0);
-    float result=0;
     while(1) {
-        result=unit.iterate();
-        printf("%f\n", &result);
     }
 }