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

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers servoloop.cpp Source File

servoloop.cpp

00001 #include "servoloop.h"
00002 ServoLoop::ServoLoop(int32_t pgain, int32_t dgain)
00003 {
00004   m_pos = RCS_CENTER_POS;
00005   m_pgain = pgain;
00006   m_dgain = dgain;
00007   m_prevError = 0x80000000L;
00008 }
00009 
00010 void ServoLoop::update(int32_t error)
00011 {
00012   long int vel;
00013   //char buf[32];
00014   if (m_prevError!=0x80000000)
00015   { 
00016     vel = (error*m_pgain + (error - m_prevError)*m_dgain)>>10;
00017     //vel = (error*m_pgain + (error - m_prevError)*m_dgain);
00018     //sprintf(buf, "%ld\n", vel);
00019     //Serial.print(buf);
00020     m_pos += vel;
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     //cprintf("%d %d %d\n", m_axis, m_pos, vel);
00027   }
00028   m_prevError = error;
00029 }
Generated on Sun Jul 17 2022 22:28:23 by  doxygen 1.7.2