For autonomous, we currently have 2 types of commands, one that runs a TrapezoidalProfile to drive a set distance, and one that rotates the robot.
In autonomous, we are attempting to schedule it like this :
m_autonomousCommand = new RotateCommand(90).andThen(new TrapezoidDriveCommand(-1, 0));
Theoretically, this should rotate 90 degrees and then drive one meter. We have ensured that each command works independently and that the command’s isFinished works properly. We have also reset the encoders between the commands (in the TrapezoidDriveCommand Constructor).
However, the issue is that once the RotationCommand finishes, the Robot rotates about 20 degrees before going 1 meter forward. Using SmartDashboard, we determined that it is the TrapezoidalDriveCommand that is running when this phenomenon occurs. Here is the code used to run the profile.
public void setDriveStates(TrapezoidProfile.State left, TrapezoidProfile.State right) {
leftController.setReference(left.position, ControlType.kPosition, 0,
m_feedforward.calculate(left.velocity));
rightController.setReference(right.position, ControlType.kPosition, 0,
m_feedforward.calculate(right.velocity));
m_drive.feed();
}
For debugging purposes, we changed the command to go a distance of zero (below), and the problem persisted.
m_autonomousCommand = new RotateCommand(90).andThen(new TrapezoidDriveCommand(0, 0));
Furthermore, after graphing the setpoints from the TrapezoidalProfile, we saw that the setpoints for position and for velocity were both zero (as expected), yet the motors still received a command to rotate.
Since the encoders are reset to 0 before the trapezoidal command, and the SparkMaxPIDController is given a kPosition reference setpoint of 0, there should be no movement, right?
UPDATE: It looks like the SparkMax encoder reset method is never called. The TrapezoidalProfileCommand constructor is below, and as you can see, it calls the resetEncoders() method. It works when we call the method elsewhere like in the RotationCommand end() method, but not in this constructor.
public TrapezoidDriveCommand(double meters, double finalVelocity) {
super(
new TrapezoidProfile(
// Limit the max acceleration and velocity
new TrapezoidProfile.Constraints(
AutoConstants.kMaxSpeedMetersPerSecond,
AutoConstants.kMaxAccelerationMetersPerSecondSquared),
// End at desired position in meters; implicitly starts at 0
new TrapezoidProfile.State(meters, finalVelocity)),
// Pipe the profile state to the drive
setpointState -> Subsystems.driveSubsystem.setDriveStates(setpointState, setpointState),
// Require the drive
Subsystems.driveSubsystem);
// Reset drive encoders since we're starting at 0
Subsystems.driveSubsystem.resetEncoders();
}
}