jetfishteam / Mbed 2 deprecated mbed_PI_Depth

PI depth control test

Dependencies:   MS5803 mbed Servo

IMUDepthControl.cpp

Committer:
sandwich
Date:
2014-08-06
Revision:
0:df16f9bfc07b
Child:
1:07e046bbcb84

File content as of revision 0:df16f9bfc07b:

#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;
}