Well to sanity checks things I grabbed the stock example at ...
http://wpilib.screenstepslive.com/s/...-mecanum-drive
...and tweaked it a bit. It is just mechanum drive with CANTalons and no gyro. I...
-changed the Talons to CANTalons and used 1,3,4,5 for the device IDs to match your bot. Although I did NOT cross-reference your code to make sure the number-to-positions matched (LF,LR,RF,RR).
-I increased the motor safety timeout to 500ms. Seems like the default is not long enough to cover the time between enabling and getting to the first TeleopOpLoop.
-Changed the joystick index to 0 from 1.
To use this example just create a new IterativeRobot and paste this into Robot.cpp.
Code:
#include "WPILib.h"/**
* Simplest program to drive a robot with mecanum drive using a single Logitech
* Extreme 3D Pro joystick and 4 drive motors connected as follows:
* - Digital Sidecar 1:
* - PWM 1 - Connected to front left drive motor
* - PWM 2 - Connected to rear left drive motor
* - PWM 3 - Connected to front right drive motor
* - PWM 4 - Connected to rear right drive motor
*/
class MecanumDefaultCode : public IterativeRobot
{
CANTalon lf; /*left front */
CANTalon lr;/*left rear */
CANTalon rf; /*right front */
CANTalon rr; /*right rear */
public:
RobotDrive *m_robotDrive; // RobotDrive object using PWM 1-4 for drive motors
Joystick *m_driveStick; // Joystick object on USB port 1 (mecanum drive)public:
/**
* Constructor for this "MecanumDefaultCode" Class.
*/
MecanumDefaultCode(void) : lf(1), lr(3), rf(4), rr(5)
{
/* Set every Talon to reset the motor safety timeout. */
lf.Set(0);
lr.Set(0);
rf.Set(0);
rr.Set(0);
// Create a RobotDrive object using PWMS 1, 2, 3, and 4
m_robotDrive = new RobotDrive(lf, lr, rf, rr);
m_robotDrive->SetExpiration(0.5);
// Define joystick being used at USB port #0 on the Drivers Station
m_driveStick = new Joystick(0);
// Twist is on Axis 3 for the Extreme 3D Pro
m_driveStick->SetAxisChannel(Joystick::kTwistAxis, 3);
}
/**
* Gets called once for each new packet from the DS.
*/
void TeleopPeriodic(void)
{
m_robotDrive->MecanumDrive_Cartesian(m_driveStick->GetX(), m_driveStick->GetY(), m_driveStick->GetTwist());
}
};
START_ROBOT_CLASS(MecanumDefaultCode);