Jonathan Goret / Servoloop

Majordhome/pixy

Dependents:   pixyMajordhome

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Servoloop.cpp Source File

Servoloop.cpp

00001 #include"mbed.h"
00002 #include"Servoloop.h"
00003 
00004 // ServoLoop Constructor
00005 Servoloop::Servoloop(int32_t proportionalGain, int32_t derivativeGain)
00006 {
00007 m_pos = RCS_CENTER_POS;
00008 m_proportionalGain = proportionalGain;
00009 m_derivativeGain = derivativeGain;
00010 m_prevError = 0x80000000L;
00011 }
00012 // ServoLoop Update
00013 // Calculates new output based on the measured
00014 // error and the current state.
00015 void Servoloop::update(int32_t error){
00016 long int velocity;
00017 char buf[32];
00018 if (m_prevError!=0x80000000){
00019 velocity = (error*m_proportionalGain + (error - m_prevError)*m_derivativeGain)>>10;
00020 m_pos += velocity;
00021 if (m_pos>RCS_MAX_POS){
00022 m_pos = RCS_MAX_POS;}
00023 else if (m_pos<RCS_MIN_POS){
00024 m_pos = RCS_MIN_POS;}
00025 }
00026 m_prevError = error;
00027 
00028 }
00029 
00030 
00031
Generated on Tue Jul 12 2022 23:06:10 by  doxygen 1.7.2