Jonathan Goret / Servoloop

Majordhome/pixy

Dependents:   pixyMajordhome

Servoloop.cpp

Committer:
johnylafleur
Date:
2015-04-14
Revision:
1:7915442d0bd8
Parent:
0:b12a5af91608

File content as of revision 1:7915442d0bd8:

#include"mbed.h"
#include"Servoloop.h"

// ServoLoop Constructor
Servoloop::Servoloop(int32_t proportionalGain, int32_t derivativeGain)
{
m_pos = RCS_CENTER_POS;
m_proportionalGain = proportionalGain;
m_derivativeGain = derivativeGain;
m_prevError = 0x80000000L;
}
// ServoLoop Update
// Calculates new output based on the measured
// error and the current state.
void Servoloop::update(int32_t error){
long int velocity;
char buf[32];
if (m_prevError!=0x80000000){
velocity = (error*m_proportionalGain + (error - m_prevError)*m_derivativeGain)>>10;
m_pos += velocity;
if (m_pos>RCS_MAX_POS){
m_pos = RCS_MAX_POS;}
else if (m_pos<RCS_MIN_POS){
m_pos = RCS_MIN_POS;}
}
m_prevError = error;

}