Jonathan Goret / Servoloop

Majordhome/pixy

Dependents:   pixyMajordhome

Servoloop.cpp@1:7915442d0bd8, 2015-04-14 (annotated)

Committer:
johnylafleur
Date:
Tue Apr 14 11:00:17 2015 +0000
Revision:
1:7915442d0bd8
Parent:
0:b12a5af91608 
Pixy+MD25 Motor Track

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 void Servoloop::update(int32_t error){
johnylafleur 0:b12a5af91608 16 long int velocity;
johnylafleur 0:b12a5af91608 17 char buf[32];
johnylafleur 0:b12a5af91608 18 if (m_prevError!=0x80000000){
johnylafleur 0:b12a5af91608 19 velocity = (error*m_proportionalGain + (error - m_prevError)*m_derivativeGain)>>10;
johnylafleur 0:b12a5af91608 20 m_pos += velocity;
johnylafleur 0:b12a5af91608 21 if (m_pos>RCS_MAX_POS){
johnylafleur 0:b12a5af91608 22 m_pos = RCS_MAX_POS;}
johnylafleur 0:b12a5af91608 23 else if (m_pos<RCS_MIN_POS){
johnylafleur 0:b12a5af91608 24 m_pos = RCS_MIN_POS;}
johnylafleur 0:b12a5af91608 25 }
johnylafleur 0:b12a5af91608 26 m_prevError = error;
johnylafleur 1:7915442d0bd8 27
johnylafleur 0:b12a5af91608 28 }
johnylafleur 0:b12a5af91608 29
johnylafleur 0:b12a5af91608 30
johnylafleur 0:b12a5af91608 31