jetfishteam / Mbed 2 deprecated mbed_PI_Depth

PI depth control test

Dependencies:   MS5803 mbed Servo

IMUDepthControl.cpp@1:07e046bbcb84, 2014-08-06 (annotated)

Committer:
sandwich
Date:
Wed Aug 06 20:33:01 2014 +0000
Revision:
1:07e046bbcb84
Parent:
0:df16f9bfc07b 
accounted for servo not being a motor. Made class more self contained

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sandwich 0:df16f9bfc07b 1 #include "IMUDepthControl.h"
sandwich 0:df16f9bfc07b 2
sandwich 0:df16f9bfc07b 3 IMUDepthControl::IMUDepthControl(PinName sda, PinName scl, float Kp, float Ki) :
sandwich 0:df16f9bfc07b 4 IMU(sda,scl),
sandwich 0:df16f9bfc07b 5 m_Kp(Kp),
sandwich 1:07e046bbcb84 6 m_Ki(Ki),
sandwich 1:07e046bbcb84 7 output(p25)
sandwich 0:df16f9bfc07b 8 {
sandwich 0:df16f9bfc07b 9 IMU.MS5803Init();
sandwich 0:df16f9bfc07b 10 m_errorsum=0;
sandwich 1:07e046bbcb84 11 m_set_point=0;
sandwich 1:07e046bbcb84 12 m_delta_t=0.01;
sandwich 1:07e046bbcb84 13 m_last_pos=0.0;
sandwich 1:07e046bbcb84 14 t.start();
sandwich 1:07e046bbcb84 15 feedback.attach(this, &IMUDepthControl::iterate, m_delta_t);
sandwich 0:df16f9bfc07b 16 }
sandwich 0:df16f9bfc07b 17
sandwich 0:df16f9bfc07b 18 void IMUDepthControl::setPoint(float setpoint)
sandwich 0:df16f9bfc07b 19 {
sandwich 0:df16f9bfc07b 20 m_set_point=setpoint;
sandwich 0:df16f9bfc07b 21 }
sandwich 0:df16f9bfc07b 22
sandwich 1:07e046bbcb84 23 void IMUDepthControl::iterate()
sandwich 0:df16f9bfc07b 24 {
sandwich 0:df16f9bfc07b 25 IMU.Barometer_MS5803();
sandwich 0:df16f9bfc07b 26 float error=m_set_point-IMU.MS5803_Pressure(); //get the error
sandwich 0:df16f9bfc07b 27
sandwich 1:07e046bbcb84 28 m_errorsum+=(error*m_Ki); //integrate it
sandwich 1:07e046bbcb84 29 float I=m_errorsum; //multiply it with the integral gain
sandwich 0:df16f9bfc07b 30
sandwich 0:df16f9bfc07b 31 float P=error*m_Kp; //proportional control
sandwich 0:df16f9bfc07b 32
sandwich 1:07e046bbcb84 33 //account for the change in position because servos don't take velocities
sandwich 1:07e046bbcb84 34 float newpos=(P+I)*m_delta_t+m_last_pos;
sandwich 1:07e046bbcb84 35 if (newpos>1.0)
sandwich 1:07e046bbcb84 36 newpos=1.0;
sandwich 1:07e046bbcb84 37 else if (newpos<0.0)
sandwich 1:07e046bbcb84 38 newpos=0.0;
sandwich 1:07e046bbcb84 39 output.write(newpos);
sandwich 1:07e046bbcb84 40 m_last_pos=newpos;
sandwich 0:df16f9bfc07b 41 }