Hello,
I have two trajectory questions.
-
Now that we are getting into commands, trajectories, and the like, we are wondering if there are any examples anyone can share or docs to help us figure out the syntax of creating a Generate Ramsete command as a stand-alone command rather than inline in Robot Container. I understand why the examples do this, but I feel we will have more success training the programmers on our team who have not been part of the online trainings we have done to get to this point if RobotContainer is as clean as possible.
When I go to create a new command in VSCode, Generate Ramsete is not a template. So, I am wondering, if we created a new Generate Ramsete command, how do we set it up. I tried copying the Ramsete Command into a new Instant command, but I am getting many errors. See my code and errors below. -
In setting up the trajectories (we are using pathweaver), how would one go about having the robot spin in place (or as it is moving from one waypoint to another. This is on a Romi,so it has a differential drive)?
// 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 java.io.IOException;
import java.nio.file.Path;
import java.util.List;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.controller.RamseteController;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.trajectory.Trajectory;
import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
import edu.wpi.first.wpilibj.trajectory.TrajectoryUtil;
import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.PrintCommand;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import frc.robot.Constants;
import frc.robot.Waypoints;
import frc.robot.subsystems.DriveSubsystem;
// 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
public class testRamsete extends InstantCommand {
DriveSubsystem m_drivetrain;
public testRamsete(DriveSubsystem drivetrain) {
m_drivetrain = drivetrain;
// Use addRequirements() here to declare subsystem dependencies.
addRequirements(m_drivetrain);
}
Trajectory pathtraj (){ //I know I need to move this into a class that triggers on startup; I put it here for simplicity's sake.
String trajectoryJSON = "paths/Unnamed.wpilib.json";
Trajectory trajectory = new Trajectory();
try {
Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectoryJSON);
trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
} catch (IOException ex) {
new PrintCommand("Unable to open trajectory: " ); //+ trajectoryJSON, ex.getStackTrace()
}
return trajectory;
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
RamseteCommand ramseteCommand =
new RamseteCommand( //Wants a different constructor.
pathtraj(),
m_drivetrain::getPose,
new RamseteController(
Constants.AutoConstants.kRamseteB, Constants.AutoConstants.kRamseteZeta),
new SimpleMotorFeedforward( //Wants a different constructor.
Constants.DriveConstants.ksVolts,
Constants.DriveConstants.kvVoltSecondsPerMeter,
Constants.DriveConstants.kaVoltSecondsSquaredPerMeter),
Constants.DriveConstants.kDriveKinematics,
m_drivetrain::getWheelSpeeds,//Wants this expression to be a functional interface; it seems as though it is.
new PIDController(Constants.DriveConstants.kPDriveVel, 0, 0),
new PIDController(Constants.DriveConstants.kPDriveVel, 0, 0),
// RamseteCommand passes volts to the callback
m_drivetrain::tankDriveVolts, //Wants this expression to be a functional interface; it seems as though it is.
m_drivetrain);
m_drivetrain.resetOdometry(pathtraj.getInitialPose() ) //Cannot find the trajectory
.andThen(ramseteCommand)
.andThen(() -> m_drivetrain.tankDriveVolts(0, 0));
}
}
