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?
User | Revision | Line number | New 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 | } |