jetfishteam / Mbed 2 deprecated robotic_fish_ver_4_9_pixy

My modifications/additions to the code

Dependencies:   ADXL345 ADXL345_I2C IMUfilter ITG3200 Servo fishgait mbed-rtos mbed pixy_cam

Fork of robotic_fish_ver_4_8 by jetfishteam

servoloop.cpp

Committer:
sandwich
Date:
2014-07-11
Revision:
25:4f2f441eceec

File content as of revision 25:4f2f441eceec:

#include "servoloop.h"
ServoLoop::ServoLoop(int32_t pgain, int32_t dgain)
{
  m_pos = RCS_CENTER_POS;
  m_pgain = pgain;
  m_dgain = dgain;
  m_prevError = 0x80000000L;
}

void ServoLoop::update(int32_t error)
{
  long int vel;
  //char buf[32];
  if (m_prevError!=0x80000000)
  { 
    vel = (error*m_pgain + (error - m_prevError)*m_dgain)>>10;
    //vel = (error*m_pgain + (error - m_prevError)*m_dgain);
    //sprintf(buf, "%ld\n", vel);
    //Serial.print(buf);
    m_pos += vel;
    if (m_pos>RCS_MAX_POS) 
      m_pos = RCS_MAX_POS; 
    else if (m_pos<RCS_MIN_POS) 
      m_pos = RCS_MIN_POS;

    //cprintf("%d %d %d\n", m_axis, m_pos, vel);
  }
  m_prevError = error;
}