Reset Odometry and RamseteCommand issue

woodworker2001 · March 1, 2022, 1:33am

I am trying to get some more complex autos working with PathWeaver and Ramsete Command.

I created a parallel race group so I can run the intake while following a path and when it gets to the end of the path it will shut the intake off. I am passing in a Trajectory to this command group for use with the Ramsete Command. I am also passing in my drivetrain.

When I run drivetrain.resetOdometry(trajectory.getInitialPose(); it does not enter the drivetrain and reset the initial pose, but when I put it in getAutonomousCommand it does enter that and reset it. The issue with it there is I dont know how to get it to change to the correct trajectory based on which auto is selected.

I am working with TwoBallBlue2 command and the FollowPathAndIntake … Below is the github repository.

Thanks
Dan

github.com

mavericks2252/RobotCode/blob/main/src/main/java/frc/robot/commands/FollowPathAndIntake.java

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot.commands;

import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.RamseteController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import frc.robot.Constants;
import frc.robot.subsystems.BallRun;
import frc.robot.subsystems.DriveTrain;
import frc.robot.subsystems.Intake;

// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
// information, see:
// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html

This file has been truncated. show original

Joe_Ross · March 1, 2022, 3:05am

You’re putting the reset command in the constructor, so it runs when the command is constructed (at robot startup). One way to make sure that it’s called at runtime is to add it as an InstantCommand at the beginning of the group…

woodworker2001 · March 1, 2022, 3:20am

Ok that makes sense… but I have a dumb question… what does that look like?

new InstantCommand (drivetrain.resetOdometry(trajectory.getInitialPose())),

Didn’t think this would work since the method for resetting odometry isn’t a command?…. Very new to Java so staying to figure this all out

Isn’t the cosnstructor run everytime a new instance of the command is called and therefore running when calling the new command from getAutonomousCommand? (It is not apprently. Just trying to understand the issue)

Thanks!!!

Joe_Ross · March 1, 2022, 3:40am

new InstantCommand(()->drivetrain.resetOdometry(trajectory.getInitialPose()))

Since InstantCommand takes a Runnable, you can use a lambda. More explanation here: How to implement the Runnable interface using lambda expression in Java?

getAutonomousCommand doesn’t create a new command. It returns one of a list of commands from the SendableChooser that were constructed here, at startup: RobotCode/src/main/java/frc/robot/RobotContainer.java at 1787c3404e10f7c9dc5bff9b1a78d8acee4ee5fe · mavericks2252/RobotCode · GitHub

woodworker2001 · March 1, 2022, 10:21am

Ok great thanks. Will try this tonight

woodworker2001 · March 1, 2022, 1:55pm

That works great and seems to fix the problem I was having, but it has created another issue. I had the RamseteCommand and RunIntake commands in a ParallelRaceGroup. Problem is adding the InstantCommand ends immediately and terminates the race group.

I am trying to move this over to a ParallelDeadlineGroup so that it ends once the ramseteCommand ends, but I am doing something wrong when trying to add the ramseteCommand as a deadline command and I am not sure what that is. Any advice on using the ParallelDeadlineGroup?
It does not like how I am adding the RamseteCommand in line 45 of this command

github.com

mavericks2252/RobotCode/blob/main/src/main/java/frc/robot/commands/FollowPathAndIntakeNew.java

// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot.commands;

import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.RamseteController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import frc.robot.Constants;
import frc.robot.subsystems.BallRun;
import frc.robot.subsystems.DriveTrain;
import frc.robot.subsystems.Intake;

// NOTE:  Consider using this command inline, rather than writing a subclass.  For more

This file has been truncated. show original

woodworker2001 · March 1, 2022, 5:04pm

Would putting this into the parallelRaceGroup on my FollowPathAndIntake Command?

addCommands(
ramseteCommand.beforeStarting(() → drivetrain.resetOdometry(trajectory.getInitialPose())),
new RunIntake(intake, ballRun)
);

NewtonCrosby · March 1, 2022, 5:07pm

Make all of these things a sequential command group.

Then your sequence would be:

    addCommands(
        new InstantCommand.... <-- reset odometry,
        new ParallelCommandGroup ... <-- drop intake and run trajectory
    )

woodworker2001 · March 1, 2022, 5:19pm

Ok, that makes sense too… and the parallelCommandGroup could be a race or deadlineGroup whatever I needed to make it end and move on to the next one.

system · March 1, 2023, 5:19pm

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