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
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; }