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@25:4f2f441eceec, 2014-07-11 (annotated)

Committer:
sandwich
Date:
Fri Jul 11 14:30:36 2014 +0000
Revision:
25:4f2f441eceec
latest revision

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sandwich 25:4f2f441eceec 1 #include "servoloop.h"
sandwich 25:4f2f441eceec 2 ServoLoop::ServoLoop(int32_t pgain, int32_t dgain)
sandwich 25:4f2f441eceec 3 {
sandwich 25:4f2f441eceec 4 m_pos = RCS_CENTER_POS;
sandwich 25:4f2f441eceec 5 m_pgain = pgain;
sandwich 25:4f2f441eceec 6 m_dgain = dgain;
sandwich 25:4f2f441eceec 7 m_prevError = 0x80000000L;
sandwich 25:4f2f441eceec 8 }
sandwich 25:4f2f441eceec 9
sandwich 25:4f2f441eceec 10 void ServoLoop::update(int32_t error)
sandwich 25:4f2f441eceec 11 {
sandwich 25:4f2f441eceec 12 long int vel;
sandwich 25:4f2f441eceec 13 //char buf[32];
sandwich 25:4f2f441eceec 14 if (m_prevError!=0x80000000)
sandwich 25:4f2f441eceec 15 {
sandwich 25:4f2f441eceec 16 vel = (error*m_pgain + (error - m_prevError)*m_dgain)>>10;
sandwich 25:4f2f441eceec 17 //vel = (error*m_pgain + (error - m_prevError)*m_dgain);
sandwich 25:4f2f441eceec 18 //sprintf(buf, "%ld\n", vel);
sandwich 25:4f2f441eceec 19 //Serial.print(buf);
sandwich 25:4f2f441eceec 20 m_pos += vel;
sandwich 25:4f2f441eceec 21 if (m_pos>RCS_MAX_POS)
sandwich 25:4f2f441eceec 22 m_pos = RCS_MAX_POS;
sandwich 25:4f2f441eceec 23 else if (m_pos<RCS_MIN_POS)
sandwich 25:4f2f441eceec 24 m_pos = RCS_MIN_POS;
sandwich 25:4f2f441eceec 25
sandwich 25:4f2f441eceec 26 //cprintf("%d %d %d\n", m_axis, m_pos, vel);
sandwich 25:4f2f441eceec 27 }
sandwich 25:4f2f441eceec 28 m_prevError = error;
sandwich 25:4f2f441eceec 29 }