Hello!
My team just got back from competition last week, and we have decided to rework our autonomous as it left much to be desired. We have a little under two weeks, so we decided to attempt to learn Ramsete/Pathweaver.
We’ve followed the WPILIB documentation online, as well as this github repository for falcons, and incorporated it into our programming. After messing with some stuff, it seemed to only want to go in circles. Which I assume was due to the NavX. Our kP value seems fairly high as well, which we retrieved from sysid.
We decided to move all of the programming for trajectory to its own setup, to make it easier. We haven’t been able to run this yet, but we will be able to run it tonight. But I still think the NavX setup isn’t correct, and maybe some other things are incorrect?
Any feedback on things to correct would be amazing.
Here is our Github Repository for the programming.
If you just use a straight trajectory does it work?
I would recommend getting your bot to the point where you can do a simple 3 meter straight trajectory, then try to get fancier from there.
Here is what a straight trajectory looks like:
trajectory =
TrajectoryGenerator.generateTrajectory(
// Start at the origin facing the +X direction
new Pose2d(0, 3, new Rotation2d(0)),
// Pass through these two interior waypoints, making an 's' curve path
//List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
List.of(new Translation2d(1,3), new Translation2d(2,3)),
// End 3 meters straight ahead of where we started, facing forward
new Pose2d(3, 3, new Rotation2d(0)),
// Pass config
config);
Here is an example of a working code base that can follow trajectories with Falcons.
Gigantic NOTE: This code was written to be run in the WPILib simulator. We have directly used it on a real robot with a representative setup, but you can’t do that until you change all the generated constants etc you get from sysid.
That code will either try to find a trajectory in the deploy directory that was generated with Pathweaver, or if it doesn’t find it, it will run a straight trajectory.
Good luck.
1 Like
You wouldn’t want the waypoints to be (1,0), (2,0)? Again, trying to learn. What would the difference be?
Thanks!
I could be 0 for the y, just would change the starting position of the trajectory.
(0,3) starts in the middle of the field, whereas (0,0) would start in the bottom left of the field (thinking of the field in the Field2d() sense).
1 Like
Also, if you run that code in the simulator, and when Glass opens you can click on NetworkTables->SmartDashboard->Field2d and it will show you the robot’s representation on the field as the odometry is updated.
Doing that you can tinker with the starting coordinates and see how the changes correspond.
1 Like
I would check out WPILib’s troubleshooting documentation: Troubleshooting — FIRST Robotics Competition documentation
They have some steps for breaking down the process. You can spend an eternity tweaking code and trying to just run a path, but it’s much easier to validate that each part of your setup is correct.
To me, it sounds like your odometry has a flipped sign somewhere or applying a positive voltage doesn’t translate to forward movement on each wheel.
I also recommend setting kP to zero until you get simple feedforward path following to work
3 Likes
I want to double check, but the gyro should be counterclockwise is more positive for odmoetry? Or is that not correct?
Correct, Counter-Clockwise is positive.
1 Like
Correct that rotating your robot to the left (counter clockwise) should increase the angle
In case you’re like me and need claims backed up with the official doc: Differential Drive Odometry — FIRST Robotics Competition documentation
1 Like
public double getHeading() {
return m_navx.getRotation2d().getDegrees();
}
This is my current getHeading command in my DriveSubsystem. If I’m not mistaken, this will return the value backwards?
Do I need to do one like @NewtonCrosby has it in their repository?
public double getHeading() {
return Math.IEEEremainder(m_navx.getAngle(), 360) * (DriveConstants.kGyroReversed ? -1.0 : 1.0);
}
For the Navx yes, you need it reversed. CCW is negative to the native navx, at least it is on the navx-mxp that we have on our robots.
EDIT to add:
That simulator project is using Falcons and a Navx for the gyro. So, if you can get it to work in the simulator as you expect, then you should be able to modify the constants from sysid and have it come close to generally work on your robot.
My student that had never done it before translated the simulator project to a falcon robot with the navx in a few hours to running full Pathweaver generated paths.
2 Likes
The last major question I have is making sure I’m doing encoders properly.
My current get WheelSpeeds command:
public DifferentialDriveWheelSpeeds getWheelSpeeds() {
return new DifferentialDriveWheelSpeeds(
getLMasterRate() * Constants.gearRatio * 10 / Constants.EncoderCPR * Constants.wheelcircumference,
getRMasterRate() * Constants.gearRatio * 10 / Constants.EncoderCPR * Constants.wheelcircumference
);
}
Your code:
public DifferentialDriveWheelSpeeds getWheelSpeeds() {
double left_meters_sec = DriveConstants.k100msPerSecond * nativeUnitsToDistanceMeters(m_leftDrive.getSelectedSensorVelocity());
double right_meters_sec = DriveConstants.k100msPerSecond * nativeUnitsToDistanceMeters(m_rightDrive.getSelectedSensorVelocity());
return new DifferentialDriveWheelSpeeds(left_meters_sec, right_meters_sec);
}
I kept our project in Meters so I didn’t have to worry about translating from inches to meters. But if I’m not mistaken, I still have to translate CPR to meters?
I’m planning on taking your code into the simulator just to play with it, as I’m still around 5 hours away from being able to mess with our bot.
You can just use that code directly, just add it to your own drivetrain. I took that code directly from the CTRE examples, it’s not even mine. 
private double nativeUnitsToDistanceMeters(double sensorCounts){
double motorRotations = (double)sensorCounts / DriveConstants.kCountsPerRev;
double wheelRotations = motorRotations / DriveConstants.kSensorGearRatio;
double positionMeters = wheelRotations * (2 * Math.PI * Units.inchesToMeters(DriveConstants.kWheelRadiusInches));
return positionMeters;
}
That logic accounts for everything you need, the only consideration you have to worry about is where the encoder is relative to the gearbox (the gear ratio between the motor/encoder and the actual wheels).
1 Like
Should I also add the Velocity to Native Units and Distance to Native units? Or is that needed? I want to make sure I’m adding everything that needs to be used!
Thanks!
You definitely need all of them for the odometry update, so for the simulator to appear to be working correctly you’ll need all of them.
No penalty to having them there if they aren’t used either.
1 Like
So I finally got to our Robot and tried to run the updated program, and there are no volts being provided to the wheel motors. I’ve tried to double check the path, but it. seems like everything is correct. The robot moves in teleop, so I know it’s not mechanical. Something on my software side is messed up.
Things to check?
Did you run Sysid and plug in the new values for the constants?
Yes, I have our new values inputted.
I would suggest you plot the tank drive value calculations in Glass to visualize the results you’re seeing on your robot.
I would imagine that your constants are not providing sufficient values to overcome friction, or you’re not actually commanding the motors to move.
I think I’m not sending any voltage to the motors. I have the volts sent to the Smartdashboard, and they’re showing 0 during auto.
I have no idea what Glass is/how to use it. I’ve seen it in the Start Tools.