Ramsete Autonomous MatchTime & Speed Command Issues

Hello,

Our team is trying to use the SpeedBase RamseteCommand for our autonomous instead of the voltbase ramsete.

RamseteCommand ramseteCommand = new RamseteCommand(driveTrain.trajectory, driveTrain::getPosition, new RamseteController(Constants.kRamseteB, Constants.kRamseteZeta), Constants.kDriveKinematics, driveTrain::move, driveTrain);

right now we have 2 issue:

  1. We don’t know how to get the time since autonomous started to feed into our getPosition command.(ATM we have no odometry, we are just simulating.)
  public Pose2d getPosition(){
    time += 0.002;
    System.out.println(trajectory.sample(time).poseMeters);
    return trajectory.sample(time).poseMeters;
  }

From this source the getmatch time decrements it’s value…hence we can’t use it above? is their a match timeout constant somewhere we can use to turn decrement to increment? Or something else?

  1. We are not sure what the ramsete command is feeding to the move method as a command. We tried both methods below with weird outputs.
   public void move(double LeftSpeed, double RightSpeed) {
    rightSide.set(-RightSpeed);
    leftSide.set(LeftSpeed);
    drive.feed();
  }
  public void moveBase(double LeftSpeed, double RightSpeed) {
    drive.tankDrive(LeftSpeed,-RightSpeed);
  }

We used this as a reference: Step 3: Creating a Drive Subsystem — FIRST Robotics Competition documentation and instead of using driveWithVolt we made a drivewithSpeed.

Any ideas?

When you sample a trajectory, the time is relative to the length of the trajectory and not correlated to the match time. (e.g. sampling a trajectory with a time of 2 seconds will tell you what state the robot should be at 2 seconds into the trajectory and not 2 seconds into the match). The RamseteCommand uses the Timer class like this, so you can implement something similar to sample your trajectories.

If you are using the velocity version of the Ramsete Command (Not providing PID controllers to the RamseteCommand’s constructor), it should output a velocity setpoint that you would feed into a smart motor controller (CTRE Talon/Falcon, Rev SparkMax). Based on the code you have above, it doesn’t look like you’re using smart motor controller s and are using the SpeedControllerGroups and the DifferentialDrive classes to drive your robot. These assume you are feeding a percent output value from -1 to 1. If you are feeding it velocities, then I’d assume that your robot is always going 100% backwards/forwards.

Is there a particular reason why you want to use the velocity version of the RamseteCommand? I would probably just recommend using the voltage version, and divide the left/right outputs of the RamseteCommand by 12 to get your values between +/-1.

1 Like

Because in our code DEV, we have a Characterization, Sensor and PID dependency to design a volt based PID ramsete command for trajectory control, and the speed based ramsete command seemed easier, less inputs in the constructor.
But the end goal is the better PID one, we just wanted to see something move as fast as possible.
(Btw we just changed from labview to java, hence we are re-learning a lot of stuff we did in the previous years)

Thx for advising to divide it by 12 and use the .setvoltage. It actually seem to work. We will debug with following a circle, straight line and other stuff.
What do you mean by smart motor controller? any wplib or CTRE class library link to recommend?

Sorry, I misunderstood,

Did you mean 1. feed in voltage value by dividing speed by 12 and using .setVoltage
or 2. you meant using using ramsetecommand for voltage and dividing it’s calculated command by 12 with .set or .tankDrive?

Oh I forgot that existed. You can use either the setVoltage() function in the SpeedControllerGroup and pass it +/-12 or you can divide the voltage value by 12 to get +/- 1 to pass it into the .set() functions of either your SpeedControllerGroups or your setVoltage classes. I would recommend using setVoltage(), as that function takes into account the current battery voltage when running, so it will put a little more power to the output as the voltage of your battery drops over time from running your robot.

So a smart motor controller would be one of these:
CTRE Talon:

CTRE Falcon:

Rev Spark Max:

All these motor controllers have their own processors within them that can run their own PID loops using their respective code libraries. Depending on your preferences, you can run your PID loops on the roboRIO or the motor controllers themselves, as each has their own benefits/drawbacks. In order to use the PID loops on these motor controllers, you still need to feed them sensor values.

For the Talon, you need to give it a sensor either through the roboRIO or using the output port on the Talon itself. The Spark Max also operates the same way, but if you use a brushless motor, you can use the encoder inside of the brushless motor as your sensor. The Falcon is a Brushless motor that is integrated with its own motor controller and encoder, but you can tell it to follow an external sensor if you wanted to.

Ok, I’ll ask the team if they want to tackle smart controllers or use a PID & Voltage based ramsete command as our next step. ^_^. More research! Yea!

But for the moment. Base upon what you said before we got this

   public void move(double LeftSpeed, double RightSpeed) {
    System.out.println(LeftSpeed);
    System.out.println(RightSpeed);
    rightSide.setVoltage(-RightSpeed/Constants.kvVoltSecondsPerMeter); //Or 12  or kvVoltSecondsPerMeter *WheelRatio
    leftSide.setVoltage(LeftSpeed/Constants.kvVoltSecondsPerMeter); //Or 12
    drive.feed();
  }

Is it valid to take the kV param to transform the speed setpoint command calculated by ramsete and convert it to volt? At the moment for a couple of path chosen It seems to give great results. (Values of volt between + and - 12 and talon output between 1 and -1)

Well, that way of setting the voltages is technically a valid solution. By not giving the RamseteCommand PIDControllers arguments for the left/right outputs, all the RamseteCommand will give you are the ideal speeds that the left and right sides of your robot should be running at to follow the path. This assumes you will be feeding the speeds as setpoints into a smart motor controller, that will then use PID to calculate what outputs your motors should be running while following a trajectory.

If you generated a kvVoltSecondsPerMeter from frc-characterization that is good, then I think the math would work out where dividing the speed outputs from the RamseteController by kvVoltSecondsPerMeter would just give you an ideal voltage input the motors should be at each timestep in order to follow the path correctly.

The problem with this solution is that this is all assuming an ideal world. If your robot deviated slightly due to the environment, imperfections on your robot, etc., then you would not be using any feedback to take that error into account. So you aren’t doing any PID correction to account for any potential error that could occur when following a trajectory.

To reiterate, the proper way of implementing the RamseteController given your setup, would be to use the constructor that includes the PID controllers. The PID values should be the values generated from the frc-characterization tool for the onboard controller. This will result in the output of the Ramsete controller to change to voltage outputs that have been corrected by PID Controllers that take into account external errors. By not passing the PID controllers, you just get the ideal velocity values you should run through the trajectories at without any corrections to error. This may be passable for short/simple paths, but as you get longer, more complex paths, you would start to drift and end up with less accurate trajectory following.

To be extra clear, I would implement this like the example provided in WPILib:

// Ramsete Comand from wpiLib docs example
RamseteCommand ramseteCommand = new RamseteCommand(
        exampleTrajectory,
        m_robotDrive::getPose,
        new RamseteController(AutoConstants.kRamseteB, AutoConstants.kRamseteZeta),
        new SimpleMotorFeedforward(DriveConstants.ksVolts,
                                   DriveConstants.kvVoltSecondsPerMeter,
                                   DriveConstants.kaVoltSecondsSquaredPerMeter),
        DriveConstants.kDriveKinematics,
        m_robotDrive::getWheelSpeeds,
        new PIDController(DriveConstants.kPDriveVel, 0, 0),
        new PIDController(DriveConstants.kPDriveVel, 0, 0),
        m_robotDrive::move,
        m_robotDrive
);

.
.
.

public void move(double leftVoltage, double rightVoltage) {
    rightSide.setVoltage(-rightVoltage);
    leftSide.setVoltage(leftVoltage);
    drive.feed();
}

if you wanted to simplify the creation of the Ramsete commands to reduce the number of arguments you need to enter every time you make one, you can write a wrapper function that fills out most of the arguments that are repeated, like what my team does:

public static RamseteCommand generateRamseteCommand(DriveTrain driveTrain, ArrayList<Pose2d> path, TrajectoryConfig config) {
        Trajectory trajectory = TrajectoryGenerator.generateTrajectory(path, config);

        return generateRamseteCommand(driveTrain, trajectory);
}

public static RamseteCommand generateRamseteCommand(DriveTrain driveTrain, Trajectory trajectory) {

        RamseteCommand ramseteCommand = new RamseteCommand (
                trajectory,
                driveTrain::getRobotPose,
                new RamseteController(),
                driveTrain.getFeedforward(),
                driveTrain.getDriveTrainKinematics(),
                driveTrain::getSpeeds,
                driveTrain.getLeftPIDController(),
                driveTrain.getRightPIDController(),
                driveTrain::setVoltageOutput,
                driveTrain
        );
        return ramseteCommand;
}