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()); }