Permalink
Browse files

Print debugging messages when setting pid goal pos

  • Loading branch information...
1 parent 32bf72d commit ffa486cda6d0bcb7bf534713c30b2d9973e36c8b @bmcdorman bmcdorman committed Feb 23, 2013
Showing with 9 additions and 7 deletions.
  1. +9 −7 src/motors_p.cpp
View
@@ -86,11 +86,11 @@ void Private::Motor::setPidGains(port_t port, const short &p, const short &i, co
if(port > 3) return;
Private::Kovan *kovan = Private::Kovan::instance();
- kovan->enqueueCommand(createWriteCommand(PID_PN_0 + port, p));
- kovan->enqueueCommand(createWriteCommand(PID_IN_0 + port, i));
- kovan->enqueueCommand(createWriteCommand(PID_DN_0 + port, d));
- kovan->enqueueCommand(createWriteCommand(PID_PD_0 + port, pd));
- kovan->enqueueCommand(createWriteCommand(PID_ID_0 + port, id));
+ kovan->enqueueCommand(createWriteCommand(PID_PN_0 + port, p), false);
+ kovan->enqueueCommand(createWriteCommand(PID_IN_0 + port, i), false);
+ kovan->enqueueCommand(createWriteCommand(PID_DN_0 + port, d), false);
+ kovan->enqueueCommand(createWriteCommand(PID_PD_0 + port, pd), false);
+ kovan->enqueueCommand(createWriteCommand(PID_ID_0 + port, id), false);
kovan->enqueueCommand(createWriteCommand(PID_DD_0 + port, dd));
}
@@ -146,7 +146,7 @@ void Private::Motor::setPidVelocity(port_t port, const int &ticks)
{
Private::Kovan *kovan = Private::Kovan::instance();
port = fixPort(port);
- kovan->enqueueCommand(createWriteCommand(goalSpeedLowRegisters[port], ticks & 0x0000FFFF));
+ kovan->enqueueCommand(createWriteCommand(goalSpeedLowRegisters[port], ticks & 0x0000FFFF), false);
kovan->enqueueCommand(createWriteCommand(goalSpeedHighRegisters[port], (ticks & 0xFFFF0000) >> 16));
}
@@ -162,7 +162,9 @@ void Private::Motor::setPidGoalPos(port_t port, int pos)
Private::Kovan *kovan = Private::Kovan::instance();
port = fixPort(port);
pos += m_cleared[port];
- kovan->enqueueCommand(createWriteCommand(goalPosLowRegisters[port], pos & 0x0000FFFF));
+ std::cout << "setPidGoalPos: adjusted port = " << port << " adjusted pos = " << pos
+ << " (m_cleared[port] = " << m_cleared[port] << ")";
+ kovan->enqueueCommand(createWriteCommand(goalPosLowRegisters[port], pos & 0x0000FFFF), false);
kovan->enqueueCommand(createWriteCommand(goalPosHighRegisters[port], (pos & 0xFFFF0000) >> 16));
}

0 comments on commit ffa486c

Please sign in to comment.