Jonathan Goret / Servoloop

Majordhome/pixy

Dependents:   pixyMajordhome

Servoloop.cpp@0:b12a5af91608, 2015-04-13 (annotated)

Committer:
johnylafleur
Date:
Mon Apr 13 12:16:51 2015 +0000
Revision:
0:b12a5af91608
Child:
1:7915442d0bd8
Majordhome/pixy;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
johnylafleur 0:b12a5af91608 1 #include"mbed.h"
johnylafleur 0:b12a5af91608 2 #include"Servoloop.h"
johnylafleur 0:b12a5af91608 3
johnylafleur 0:b12a5af91608 4 // ServoLoop Constructor
johnylafleur 0:b12a5af91608 5 Servoloop::Servoloop(int32_t proportionalGain, int32_t derivativeGain)
johnylafleur 0:b12a5af91608 6 {
johnylafleur 0:b12a5af91608 7 m_pos = RCS_CENTER_POS;
johnylafleur 0:b12a5af91608 8 m_proportionalGain = proportionalGain;
johnylafleur 0:b12a5af91608 9 m_derivativeGain = derivativeGain;
johnylafleur 0:b12a5af91608 10 m_prevError = 0x80000000L;
johnylafleur 0:b12a5af91608 11 }
johnylafleur 0:b12a5af91608 12 // ServoLoop Update
johnylafleur 0:b12a5af91608 13 // Calculates new output based on the measured
johnylafleur 0:b12a5af91608 14 // error and the current state.
johnylafleur 0:b12a5af91608 15
johnylafleur 0:b12a5af91608 16
johnylafleur 0:b12a5af91608 17 void Servoloop::update(int32_t error){
johnylafleur 0:b12a5af91608 18 long int velocity;
johnylafleur 0:b12a5af91608 19 char buf[32];
johnylafleur 0:b12a5af91608 20 if (m_prevError!=0x80000000){
johnylafleur 0:b12a5af91608 21 velocity = (error*m_proportionalGain + (error - m_prevError)*m_derivativeGain)>>10;
johnylafleur 0:b12a5af91608 22 m_pos += velocity;
johnylafleur 0:b12a5af91608 23 if (m_pos>RCS_MAX_POS){
johnylafleur 0:b12a5af91608 24 m_pos = RCS_MAX_POS;}
johnylafleur 0:b12a5af91608 25 else if (m_pos<RCS_MIN_POS){
johnylafleur 0:b12a5af91608 26 m_pos = RCS_MIN_POS;}
johnylafleur 0:b12a5af91608 27 }
johnylafleur 0:b12a5af91608 28 m_prevError = error;
johnylafleur 0:b12a5af91608 29 }
johnylafleur 0:b12a5af91608 30
johnylafleur 0:b12a5af91608 31
johnylafleur 0:b12a5af91608 32