Why does trajectory example code not move my robot on bricks?

Hey, I followed the trajectory tutorial and created my DriveSubsystem class, and identified my system. The problem is when I run autonomous, nothing happens. I also get this strange error, does this mean that the command gets ran once and then is never run again? We use Falcon 500s for our drivetrain and the ADXRS450 gyro.

DifferentialDrive... Output not updated often enough.
   from: edu.wpi.first.wpilibj.MotorSafety.check(MotorSafety.java:96)

This has really been tripping us out, any help is appreciated.

public static DifferentialDriveOdometry m_odometry;

    /** Creates a new DriveSubsystem. */
    public DriveSubsystem() {
        resetEncoders();
        m_odometry = new DifferentialDriveOdometry(RobotMap.gyro.getRotation2d());
    }

    @Override
    public void periodic() {
        // Update the odometry in the periodic block
        m_odometry.update(
                RobotMap.gyro.getRotation2d(),
                EncoderSetter.nativeUnitsToDistanceMeters(RobotMap.MainLeftMotorBack.getSelectedSensorPosition(0)),
                EncoderSetter.nativeUnitsToDistanceMeters(RobotMap.MainRightMotorBack.getSelectedSensorPosition(0)));
    }

    /**
     * Returns the currently-estimated pose of the robot.
     *
     * @return The pose.
     */
    public Pose2d getPose() {
        return m_odometry.getPoseMeters();
    }

    /**
     * Returns the current wheel speeds of the robot.
     *
     * @return The current wheel speeds.
     */
    public DifferentialDriveWheelSpeeds getWheelSpeeds() {
        return new DifferentialDriveWheelSpeeds(
                EncoderSetter.nativeUnitsToDistanceMeters(RobotMap.MainLeftMotorBack.getSelectedSensorVelocity(0)),
                EncoderSetter.nativeUnitsToDistanceMeters(RobotMap.MainRightMotorBack.getSelectedSensorVelocity(0)));

    }

    /**
     * Resets the odometry to the specified pose.
     *
     * @param pose The pose to which to set the odometry.
     */
    public void resetOdometry(Pose2d pose) {
        resetEncoders();
        m_odometry.resetPosition(pose, RobotMap.gyro.getRotation2d());
    }

    /**
     * Drives the robot using arcade controls.
     *
     * @param fwd the commanded forward movement
     * @param rot the commanded rotation
     */
    public void arcadeDrive(double fwd, double rot) {
        RobotMap.m_drive.arcadeDrive(fwd, rot);
    }

    /**
     * Controls the left and right sides of the drive directly with voltages.
     *
     * @param leftVolts  the commanded left output
     * @param rightVolts the commanded right output
     */

    /** Resets the drive encoders to currently read a position of 0. */
    public void resetEncoders() {
        RobotMap.MainLeftMotorBack.setSelectedSensorPosition(0);
        RobotMap.MainRightMotorBack.setSelectedSensorPosition(0);
    }

    /**
     * Gets the average distance of the two encoders.
     *
     * @return the average of the two encoder readings
     */
    public double getAverageEncoderDistance() {
        return (EncoderSetter.nativeUnitsToDistanceMeters(RobotMap.MainLeftMotorBack.getSelectedSensorPosition(0))
                + EncoderSetter.nativeUnitsToDistanceMeters(RobotMap.MainRightMotorBack.getSelectedSensorPosition(0)))
                / 2.0;
    }

    /*
     * public Encoder getLeftEncoder() {
     * return RobotMap.MainLeftMotorBack;
     * }
     * public Encoder getRightEncoder() {
     * // return RobotMap.MainRightMotorBack;
     * return null;
     * }
     */

    public DifferentialDrive getDifferentialDrive() {
        Robot.m_driveSim.update(0.001);
        return RobotMap.m_drive;
    }

    /**
     * Sets the max output of the drive. Useful for scaling the drive to drive more
     * slowly.
     *
     * @param maxOutput the maximum output to which the drive will be constrained
     */
    public void setMaxOutput(double maxOutput) {
        RobotMap.m_drive.setMaxOutput(maxOutput);
    }

    /** Zeroes the heading of the robot. */
    public void zeroHeading() {
        RobotMap.gyro.reset();
    }

    /**
     * Returns the heading of the robot.
     *
     * @return the robot's heading in degrees, from -180 to 180
     */
    public double getHeading() {
        return RobotMap.gyro.getRotation2d().getDegrees();
    }

    /**
     * Returns the turn rate of the robot.
     *
     * @return The turn rate of the robot, in degrees per second
     */
    public double getTurnRate() {
        return -RobotMap.gyro.getRate();
    }

    public void tankDriveVolts(double leftVolts, double rightVolts) {
        RobotMap.MainLeftMotorBack.setVoltage(leftVolts);
        RobotMap.MainRightMotorBack.setVoltage(rightVolts);
        RobotMap.m_drive.feed();
    }

And yes, I am scheduling the command in autonomousInit().

@Override
  public void autonomousInit() {
    SequentialCommandGroup m_autonomousCommand = getAutonomousCommand();
    m_autonomousCommand.schedule();
  }

Nothing is in autonomous periodic and I have made sure to enter my constants.
config.json (691 Bytes)
sysid_data20220314-172927.json (1.3 MB)

Can you post the rest of your code, preferably on GitHub? We’re missing some of the most important parts.

That error message means the DifferentialDrive MotorSafety is not being fed. I can see you’re feeding it in tankDriveVolts but I don’t know how you are calling that method.

My team won’t let me share the rest of the code but m_drive is a differential drive class.

public void tankDriveVolts(double leftVolts, double rightVolts) {
        RobotMap.MainLeftMotorBack.setVoltage(leftVolts);
        RobotMap.MainRightMotorBack.setVoltage(rightVolts);
        RobotMap.m_drive.feed();
    }

It’s extremely hard to help without code. Without the rest of the code, nobody can tell what is going wrong.

Make sure that you are calling the tankDriveVolts method every loop cycle.

That’s about all I can help with without seeing the rest of the code.

Wait, so I have to call that method in my autonomous periodic? It’s just the example trajectory code from here Step 4: Creating and Following a Trajectory — FIRST Robotics Competition documentation. Does that imply that perhaps the ramsete command is being canceled? Am I scheduling the sequential command group wrong?

It should be called by the RamseteCommand, but since you haven’t shared that code, we can’t tell if it is.

1 Like

Also it would be helpful to know where the Robot class is getting the command from and how.

1 Like

It’s getting it from a function within the robot.java file which is the function in the example

It wasn’t working because I was only running the scheduler in teleop.

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.