Trajectory builder with PhotonVision not working

Technical Programming
,
1

link to the github in question: https://github.com/FRC4418/2023_ChargedUp/tree/Gonk’sVisionShtuff

I’m trying to build a trajectory using the PhotonVision Pose Estimation.
Basic overview of what I’ve got so far:

  1. Vision subsystem that gets the Pose2D to the target AprilTag
  2. Drivesubsystem with all the typical methods as well as a DifferentialDrivePoseEstimator with an UpdateOdometry that updates the estimator and runs the .addVisionMeasurement method.
  3. I put all the trajectory stuff in 1 command
    here is the initialize part of that command
    photonTrak = TrajectoryGenerator.generateTrajectory(
        DriveSubsystem.estimator.getEstimatedPosition(),
        List.of(), 
        vision.getCameraToTarget(),
        driveTrain.getTrajConfig()
      );
     SmartDashboard.putNumber("Length of Traj (Seconds)", photonTrak.getTotalTimeSeconds());

There are no errors while deploy for this code. However, when I enable, I just get a bunch of “Output not updated enough” errors and the wheels don’t spin at all. Could someone please look at the code in the github and let me know any logic problems you see. This is my first year of FRC and I might have overlooked something obvious Thank you in advance.

Edit to add: All the checks in the autonomous period method work, but the statement to add the time to shuffleboard never gets put on it.

1 Like
2

I’ll ask @Dresden to comment here

3

I’m probably not the most knowledgeable on this subject, but those errors are used for safety, to make sure the robot is getting updated values to the wheels, so it for whatever reason the code breaks the robot doesn’t just run away and keep the wheels at the last updated speed.

I know we used to get this using differentialDrive last year, and we fixed it by making sure the differentialDrive is getting fed 0 whenever it isn’t being used, instead of just not feeding it any values period. My guess is it has to do with the Ramsette.

One big issue is that you have the Ramsette Command in execute. With WPIlib Command Based, you create a command and then call .schedule() to run it. This means you need to move everything inside Execute over to Initialize, and then at the bottom of initialize you need to call ramsetteCommand.schedule(). That way you create the command, and then run it. Also, remove the .andThen() portion.

You also have to set the end condition of the command to be the same end condition as the ramsetteCommand, or else the trajBuilder command will never end. I think this would work:

// Returns true when the command should end.
  @Override
  public boolean isFinished() {
    return ramsetteCommand.isFinished();
  }

And in RobotContainer:

public Command getAutonomousCommand() {
    // An example command will be run in autonomous

    return new buildTraj(driveTrain, vision)
  }

Something we do that I would also recommend, you should also have an ArcadeDrive command that just takes in suppliers for fwd and rot, and then throws it into the .arcadeDrive() method in the drive subsystem:

public class ArcadeDrive extends CommandBase {
    private final DriveSubsystem driveSubsystem;
    private final DoubleSupplier fwd, rot;

    public ArcadeDrive(DoubleSupplier fwd, DoubleSupplier rot, DriveSubsystem driveSubsystem) {
        this.driveSubsystem = driveSubsystem;
        this.fwd = fwd;
        this.rot = rot;
        addRequirements(driveSubsystem);
    }

    @Override
    public void initialize() {
        super.initialize();
        driveSubsystem.tankDriveVolts(0, 0);
    }

    @Override
    public void execute() {
        super.execute();
        driveSubsystem.arcadeDrive(fwd, rot);
    }

    @Override
    public boolean isFinished() {
        return false;
    }
}

Make sure this command gets scheduled. The best way is to do driveSubsystem.setDefaultCommand(new ArcadeDrive()) so that whenever you don’t give the drive subsystem a command, it drives like normal.

See if this works. Also, if you get any more watchdog errors, make sure that you set motors to zero when you stop using them instead of just not giving them any updated values. For example, our auto command looked like this, where .disable() just set all motors to zero:

return autoChooser.getSelected().andThen(driveSubsystem::disable).andThen(shooterSubsystem::disable)
        .andThen(intakeSubsystem::disable).andThen(loaderSubsystem::disable);

I’m not sure if this is needed or not, I’m just looking back at our old code, and I’m pretty sure we used this to get around the watchdog error. It might work fine without it though. If this doesn’t fix your problem, I’ll call in another person from our team who might know more.

3 Likes