jetfishteam / Mbed 2 deprecated mbed_PI_Depth

PI depth control test

Dependencies:   MS5803 mbed Servo

IMUDepthControl.cpp@0:df16f9bfc07b, 2014-08-06 (annotated)

Committer:
sandwich
Date:
Wed Aug 06 20:17:02 2014 +0000
Revision:
0:df16f9bfc07b
Child:
1:07e046bbcb84
first commit

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 0:df16f9bfc07b 6 m_Ki(Ki)
sandwich 0:df16f9bfc07b 7 {
sandwich 0:df16f9bfc07b 8 IMU.MS5803Init();
sandwich 0:df16f9bfc07b 9 m_errorsum=0;
sandwich 0:df16f9bfc07b 10 }
sandwich 0:df16f9bfc07b 11
sandwich 0:df16f9bfc07b 12 void IMUDepthControl::setPoint(float setpoint)
sandwich 0:df16f9bfc07b 13 {
sandwich 0:df16f9bfc07b 14 m_set_point=setpoint;
sandwich 0:df16f9bfc07b 15 }
sandwich 0:df16f9bfc07b 16
sandwich 0:df16f9bfc07b 17 float IMUDepthControl::iterate()
sandwich 0:df16f9bfc07b 18 {
sandwich 0:df16f9bfc07b 19 IMU.Barometer_MS5803();
sandwich 0:df16f9bfc07b 20 float error=m_set_point-IMU.MS5803_Pressure(); //get the error
sandwich 0:df16f9bfc07b 21
sandwich 0:df16f9bfc07b 22 m_errorsum+=error; //integrate it
sandwich 0:df16f9bfc07b 23 float I=m_errorsum*m_Ki; //multiply it with the integral gain
sandwich 0:df16f9bfc07b 24
sandwich 0:df16f9bfc07b 25 float P=error*m_Kp; //proportional control
sandwich 0:df16f9bfc07b 26
sandwich 0:df16f9bfc07b 27 return P+I;
sandwich 0:df16f9bfc07b 28 }