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@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?
User | Revision | Line number | New 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 | } |