void
CheckDrive(
frc::DifferentialDrive& drive,
std::list<Packet*>& packets,
double magnitude,
double curve,
const MotorDrivePacket& expRightPacket,
const MotorDrivePacket& expLeftPacket,
bool squaredInputs
)
{
drive.ArcadeDrive(
magnitude,
curve,
squaredInputs
);
bool rightDrivePacketFound = false;
bool leftDrivePacketFound = false;
RedBotSpeedController& leftController = dynamic_cast<RedBotSpeedController&>(drive.getLeftController());
RedBotSpeedController& rightController = dynamic_cast<RedBotSpeedController&>(drive.getRightController());
packets.push_back(leftController.getNextPacket());
packets.push_back(rightController.getNextPacket());
std::list<Packet*>::const_reverse_iterator packetIter = packets.rbegin();
for (int i = 0; i < 2; i++)
{
Packet* packet = *(packetIter++);
MotorDrivePacket* mDrivePacket = dynamic_cast<MotorDrivePacket*>(packet);
CHECK(NULL != mDrivePacket);
if (mDrivePacket->getMotor() == MotorDrivePacket::MOTOR_RIGHT)
{
rightDrivePacketFound = true;
CHECK_EQUAL(expRightPacket.getSpeed(), mDrivePacket->getSpeed());
CHECK_EQUAL(expRightPacket.getDirection(), mDrivePacket->getDirection());
}
else
{
leftDrivePacketFound = true;
CHECK_EQUAL(expLeftPacket.getSpeed(), mDrivePacket->getSpeed());
CHECK_EQUAL(expLeftPacket.getDirection(), mDrivePacket->getDirection());
}
}
CHECK(rightDrivePacketFound);
CHECK(leftDrivePacketFound);
}
void TeleopPeriodic() {
// Drive with arcade style
m_robotDrive.ArcadeDrive(m_stick.GetY(), m_stick.GetX());
}