Encoders! #9

ky28059 · 2021-11-14T02:19:56Z

Resources

proan · 2021-11-14T15:58:52Z

src/main/java/frc/robot/Constants.java

        public static final int bLeftMotorPort = 14;
+


extra line?

ky28059 · 2021-11-20T23:55:10Z

Resources for the odometry portion of this PR

navX website and AHRS docs
DifferentialDrivePoseEstimator docs and guide

e3l · 2021-12-02T03:46:21Z

src/main/java/frc/robot/RobotContainer.java

@@ -42,6 +43,7 @@

  // Controllers
  private XboxController controlXbox = new XboxController(0);
+  private JoystickButton xboxAButton = new JoystickButton(controlXbox, XboxController.Button.kA.value);


why not use the controller directly, is that not possible

https://docs.wpilib.org/en/stable/docs/software/commandbased/binding-commands-to-triggers.html
https://first.wpi.edu/FRC/roborio/beta/docs/java/edu/wpi/first/wpilibj/XboxController.html#getAButtonPressed--
I think the JoystickButton class just wraps the XboxController's button methods so you can do event based "when pressed" stuff instead of having to check periodically whether the button was pressed using getAButtonPressed() and then going from there. Je crois que setting up button bindings via JoystickButton is the reason for the controllerBindings method.

e3l · 2021-12-02T03:47:41Z

src/main/java/frc/robot/commands/tank/DriveTankCommand.java

+    // for the third.
+    tankSubsystem.zeroPosition();
+
+    tankSubsystem.setCarDrivePowers(0.5, 0.0);
  }

  @Override
  public boolean isFinished() {
    return true;


src/main/java/frc/robot/commands/tank/DriveTankCommand.java

e3l · 2021-12-02T03:52:29Z

src/main/java/frc/robot/subsystems/tank/TankSubsystem.java

@@ -20,6 +30,14 @@
  private final WPI_TalonSRX rightMain;
  private final WPI_TalonSRX rightFollow;

+  private final AHRS ahrs;
+
+  private final DifferentialDrivePoseEstimator odometry;


in another comment I mentioned "all localization stuff"... we should take all the localization logic and put it in its own class and run it on its own thread. logically and from my experience, dead reckoning is more accurate the less time each loop is -- no matter your odometry method (e.g. circular), you're making an assumption about what happens between each tick, so reducing the tick interval reduces the fudginess of your guess.

if we use the periodic() method, the commandbased system actively waits until an interval is hit before calling periodics, it's something like 20ms by default? i can't remember

I second the localization system approach.

e3l · 2021-12-02T03:53:49Z

src/main/java/frc/robot/subsystems/tank/TankSubsystem.java

+    rightMain.setSelectedSensorPosition(0);
+
+    // https://first.wpi.edu/wpilib/allwpilib/docs/release/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimator.html#resetPosition(edu.wpi.first.wpilibj.geometry.Pose2d,edu.wpi.first.wpilibj.geometry.Rotation2d)
+    odometry.resetPosition(new Pose2d(), Rotation2d.fromDegrees(ahrs.getAngle()));


why not reset the gyro

https://first.wpi.edu/wpilib/allwpilib/docs/release/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimator.html#resetPosition(edu.wpi.first.wpilibj.geometry.Pose2d,edu.wpi.first.wpilibj.geometry.Rotation2d)

The gyroscope angle does not need to be reset here on the user's robot code. The library automatically takes care of offsetting the gyro angle.

And the angle was reset to 0 during testing so it seems to work.

proan · 2021-12-06T22:52:00Z

src/main/java/frc/robot/RobotContainer.java

@@ -70,7 +72,7 @@ public RobotContainer() {
    tankSubsystem = new TankSubsystem();

    // Instantiate commands
-    tankCommand = new DriveTankCommand(tankSubsystem, 0, 0);
+    tankCommand = new DriveTankCommand(tankSubsystem, 12);


Is this "12" related to the "12 inches" comment on L117?
I don't love magic numbers, but a comment here would be helpful if you don't want to make this a well-named constant.

Yeah, the idea was for it to drive forward "12 inches" in autonomous to then measure that distance and check if the constant was working properly, but the different speeds on each side prevented that from really happening. I'll add a comment to this.

proan · 2021-12-06T23:45:24Z

src/main/java/frc/robot/subsystems/tank/TankSubsystem.java

+
+  private final DifferentialDrivePoseEstimator odometry;
+
+  public static final double ENCODER_TICKS_TO_INCHES = 32 / 142.40;


Fortunately you have the .40 in this equation, otherwise the result is 0. Ask me how I know and how many hours I spent hunting for this bug.

If you have integer division, you get an integer result.

proan · 2021-12-06T23:58:30Z

src/main/java/frc/robot/subsystems/tank/TankSubsystem.java

+    double leftDistance = leftMain.getSelectedSensorPosition() * ENCODER_TICKS_TO_INCHES;
+    double rightDistance = rightMain.getSelectedSensorPosition() * ENCODER_TICKS_TO_INCHES;
+
+    odometry.update(gyroAngle, wheelVelocities, leftDistance, rightDistance);


Seems like this interface isn't great. We're passing in a DifferentialDriveWheelSpeeds object and then separate positions for left and right wheels. These two interfaces should match. I know this isn't your interface, but keep this in mind.

proan · 2021-12-06T23:59:14Z

src/main/java/frc/robot/subsystems/tank/TankSubsystem.java

+
+    odometry.update(gyroAngle, wheelVelocities, leftDistance, rightDistance);
+
+    System.out.println(odometry.getEstimatedPosition());


Maybe print out a little text for context in the logs/terminal?

ky28059 · 2021-12-17T22:39:02Z

CANSparkMax has a similar API to WPI_TalonSRX, but some notable differences:

Followers automatically inherit their leader's inversion (and can invert their leader's inversion if needed).

Percent output motor setting is the default.

The type of motor (brushed or brushless) must be given in the constructor.

Brushless hall encoders can be accessed via getEncoder() (but this is only for temporary testing, as on the build robot we'll use external magencoders)

ky28059 · 2021-12-19T20:52:11Z

src/main/java/frc/robot/subsystems/tank/TankSubsystem.java

+  public void setMotorVoltages(double left, double right) {
+    // TODO: stub on purpose, see comment in FollowPathCommand
+    // This doesn't really match our current interface, but neither does the other option;
+    // Should a state on this subsystem be added to switch between voltage and percent output?


Alternatively, instead of tankSubsystem containing states and continually setting the motor controller output in periodic, the motor controllers can be the ones to store the state and methods can just set their values. Instead of

private int leftPower; private int rightPower; public void setTankDrivePowers(double leftScale, double rightScale) { this.leftPower = leftScale; this.rightPower = rightScale; } @Override public void periodic() { // These must be in percent output mode leftMain.set(leftPower); rightMain.set(rightPower); }

why not have

public void setTankDrivePowers(double leftScale, double rightScale) { leftMain.set(leftScale); rightMain.set(rightScale); }

? This would also allow for more flexibility with the output mode, such as

public void setTankDrivePowers(double leftScale, double rightScale) { leftMain.set(leftScale); rightMain.set(rightScale); } public void setTankDriveVoltages(double leftVoltage, double rightVoltage) { leftMain.setVoltage(leftVoltage); rightMain.setVoltage(rightVoltage); }

Allowing voltages would also resolve 1/2 of the current problems with the path following command.

ky28059 · 2021-12-20T02:13:34Z

Resources for the pathfinding portion of this PR

WPILib trajectory tutorial and example project
Feedforwards in WPILib
DifferentialDriveKinematics
RamseteController

ky28059 · 2021-12-20T06:43:14Z

It seems like WPILib's path/trajectory things must be in meters (see PathWeaver export unit and all the WPILib trajectory docs) so I will convert the odometry to that as well. It seems like PathWeaver can have its interface be in feet or inches while exporting paths as meters, which should make the forced meterization of everything less painful.

This commit will be overridden by using WPI's builtin odometry classes, but I am pushing it to keep it in history.

This can be tested and tuned over time

Wasn't able to test this that much because the drivetrain became possessed by ghosts, but can test next week on the actual drivetrain.

See comment in FollowPathCommand

Prepared to do https://docs.wpilib.org/en/stable/docs/software/wpilib-tools/robot-characterization/characterization-routine.html#running-the-characterization-routine and measure the rest on Sunday, then perhaps test finally!

These will probably have to be remeasured with the Robot Characterization tool. When testing pathfinding with these, the robot immediately swerves sharply to the left (which it shouldn't be doing). It might have something to do with the right encoder readings being inverted during characterization (left reports 4 meters, right reports -4), but I've tried setting various options in the config to try and fix that to no avail. Will definitely look into it further at some point.

Recharacterized using process described in https://www.chiefdelphi.com/t/the-future-of-pathweaver/393014/82. `Kp` might need to be tuned, as does `ENCODER_TICKS_TO_METERS` most likely.

Also push experimental changes to PID constants and auton testing, which still doesn't quite work

With the new REV version, CANSparkMaxes with the same can ID cannot be created (it throws errors which kill the robot program)

Hopefully these ones work better!

ky28059 · 2022-01-20T06:06:19Z

Closing(ish) thoughts

A lot of things made sense when I realized what RamseteCommand and associated WPILib path following things actually did -- given a trajectory, the path following generates a list of left and right velocities for the drivetrain to follow the trajectory. My confusion from the PID oscillation stemmed from the assumption that the PID loop in the command was for position, so an oscillation would surely cause the robot to move back and forth; it never did because the PID loop was for velocity instead (this is why kI and kD were set to 0 in the example; accelerating to a target velocity is a use case for just a P controller). Oscillations due to a high kP were between 1.9m/s and 2.1m/s and not 1.9m and 2.1m so there was no back and forth driving. Furthermore, the confusion on why the robot stopped before the end position when constants were misconfigured can also be explained by realizing that the list of velocities probably ended, with the constants being incorrect leading to the robot not following the velocities properly and ending in an incorrect spot.

A lot of these confusions could probably have been resolved sooner had I read the docs for RamseteController a little more carefully.

eggaskin

Some constants to be fine tuned/changed, but Kevin's computer is not working so first merge.

ky28059 · 2022-01-21T05:53:47Z

Some constants to be fine tuned/changed, but Kevin's computer is not working so first merge.

I don't think constants need to be changed, I just wanted to test shuffleboard (which wasn't cooperating). I think launching it from driver station as opposed to vscode fixed the issue, so the branch can probably be deleted.

KEVIN: Closing thoughts. A lot of things made sense when I realized what RamseteCommand and associated WPILib path following things actually did -- given a trajectory, the path following generates a list of left and right velocities for the drivetrain to follow the trajectory. My confusion from the PID oscillation stemmed from the assumption that the PID loop in the command was for position, so an oscillation would surely cause the robot to move back and forth; it never did because the PID loop was for velocity instead (this is why kI and kD were set to 0 in the example; accelerating to a target velocity is a use case for just a P controller). Oscillations due to a high kP were between 1.9m/s and 2.1m/s and not 1.9m and 2.1m so there was no back and forth driving. Furthermore, the confusion on why the robot stopped before the end position when constants were misconfigured can also be explained by realizing that the list of velocities probably ended, with the constants being incorrect leading to the robot not following the velocities properly and ending in an incorrect spot. A lot of these confusions could probably have been resolved sooner had I read the docs for RamseteController a little more carefully. * Initial testing * Add wheel circumference to hopefully calculate distance * Add position getting/setting, fix constants * Slow drive speed, zero sensors * Start odometry threading * Odometry package and class This commit will be overridden by using WPI's builtin odometry classes, but I am pushing it to keep it in history. * Use WPI odometry class * Small changes * navX AHRS * Merge left and right sensor methods to use odometry class * Change `new Pose2d()` to `getRobotPosition()` * Measure the constant! This can be tested and tuned over time * Bind A button to zero position * Change constant to inches, remeasure * Add better documentation * Remove joysticks from encoder branch * Readd odometry thread Wasn't able to test this that much because the drivetrain became possessed by ghosts, but can test next week on the actual drivetrain. * Talon -> Spark Max * Implement preliminary path following command See comment in FollowPathCommand * Remove states from `tankSubsystem` * Inches -> meters * Add secondary constructor to FollowPathCommand * Fix turning direction; remove CAN ID 0 * Edit to functional drivetrain state * Measure some constants Prepared to do https://docs.wpilib.org/en/stable/docs/software/wpilib-tools/robot-characterization/characterization-routine.html#running-the-characterization-routine and measure the rest on Sunday, then perhaps test finally! * Measure more constants! These will probably have to be remeasured with the Robot Characterization tool. When testing pathfinding with these, the robot immediately swerves sharply to the left (which it shouldn't be doing). It might have something to do with the right encoder readings being inverted during characterization (left reports 4 meters, right reports -4), but I've tried setting various options in the config to try and fix that to no avail. Will definitely look into it further at some point. * Revert constants, invert gyro, invert left motors * Fix inverted gyro error in resetPosition * New constants! Recharacterized using process described in https://www.chiefdelphi.com/t/the-future-of-pathweaver/393014/82. `Kp` might need to be tuned, as does `ENCODER_TICKS_TO_METERS` most likely. * Update WPILib and vendor packages to 2022 Also push experimental changes to PID constants and auton testing, which still doesn't quite work * Add preliminary shuffleboard code * Move odometry to `tankSubsystem` With the new REV version, CANSparkMaxes with the same can ID cannot be created (it throws errors which kill the robot program) * Measure new constants with sysid Hopefully these ones work better! * `Field2d`, new constants * Cleanup, remove elevator subsystem * Change angle entry to use a gyro complex widget Co-authored-by: pinkbluesky <[email protected]>

ky28059 marked this pull request as draft November 14, 2021 02:20

proan reviewed Nov 14, 2021

View reviewed changes

src/main/java/frc/robot/Constants.java

public static final int bLeftMotorPort = 14;

Copy link

proan Nov 14, 2021

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

extra line?

e3l reacted with thumbs up emoji

ky28059 marked this pull request as ready for review December 2, 2021 02:33

e3l reviewed Dec 2, 2021

View reviewed changes

proan reviewed Dec 6, 2021

View reviewed changes

ky28059 commented Dec 19, 2021

View reviewed changes

ky28059 added 17 commits January 13, 2022 20:02

Initial testing

2460b9e

Add wheel circumference to hopefully calculate distance

404095c

Add position getting/setting, fix constants

25ccfd2

Slow drive speed, zero sensors

420ef19

Start odometry threading

0c5e011

Odometry package and class

21a1570

This commit will be overridden by using WPI's builtin odometry classes, but I am pushing it to keep it in history.

Use WPI odometry class

1201276

Small changes

afbe674

navX AHRS

6623342

Merge left and right sensor methods to use odometry class

7c57cad

Change new Pose2d() to getRobotPosition()

3e334b0

Measure the constant!

4efc7e2

This can be tested and tuned over time

Bind A button to zero position

4aedb1f

Change constant to inches, remeasure

e585eb1

Add better documentation

df902fc

Remove joysticks from encoder branch

5ac1cf8

Readd odometry thread

e2a7d74

Wasn't able to test this that much because the drivetrain became possessed by ghosts, but can test next week on the actual drivetrain.

ky28059 and others added 12 commits January 13, 2022 20:04

Talon -> Spark Max

abe5960

Implement preliminary path following command

34d8f32

See comment in FollowPathCommand

Remove states from tankSubsystem

f88d6f7

Inches -> meters

17dbc6b

Add secondary constructor to FollowPathCommand

87f6eda

Fix turning direction; remove CAN ID 0

e541d99

Edit to functional drivetrain state

c59f21b

Measure some constants

8eed3f4

Prepared to do https://docs.wpilib.org/en/stable/docs/software/wpilib-tools/robot-characterization/characterization-routine.html#running-the-characterization-routine and measure the rest on Sunday, then perhaps test finally!

Revert constants, invert gyro, invert left motors

5be5ef2

Fix inverted gyro error in resetPosition

62167e8

New constants!

8749cb5

Recharacterized using process described in https://www.chiefdelphi.com/t/the-future-of-pathweaver/393014/82. `Kp` might need to be tuned, as does `ENCODER_TICKS_TO_METERS` most likely.

ky28059 force-pushed the encoders branch from c310184 to 8749cb5 Compare January 14, 2022 04:06

ky28059 added 6 commits January 14, 2022 00:05

Update WPILib and vendor packages to 2022

4bae50d

Also push experimental changes to PID constants and auton testing, which still doesn't quite work

Add preliminary shuffleboard code

c06c585

Move odometry to tankSubsystem

ce720d7

With the new REV version, CANSparkMaxes with the same can ID cannot be created (it throws errors which kill the robot program)

Measure new constants with sysid

e937081

Hopefully these ones work better!

Field2d, new constants

1e35d01

Cleanup, remove elevator subsystem

cb01391

Change angle entry to use a gyro complex widget

88c91f9

eggaskin approved these changes Jan 21, 2022

View reviewed changes

eggaskin merged commit cf73d04 into develop Jan 21, 2022

ky28059 deleted the encoders branch January 21, 2022 05:53

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Encoders! #9

Encoders! #9

ky28059 commented Nov 14, 2021 •

edited

Loading

proan Nov 14, 2021

ky28059 commented Nov 20, 2021

e3l Dec 2, 2021

ky28059 Dec 3, 2021

e3l Dec 2, 2021

e3l Dec 2, 2021

proan Dec 6, 2021

e3l Dec 2, 2021

ky28059 Dec 2, 2021

proan Dec 6, 2021

ky28059 Dec 9, 2021

proan Dec 6, 2021

proan Dec 6, 2021

proan Dec 6, 2021

ky28059 commented Dec 17, 2021

ky28059 Dec 19, 2021

ky28059 Dec 19, 2021

ky28059 commented Dec 20, 2021 •

edited

Loading

ky28059 commented Dec 20, 2021

ky28059 commented Jan 20, 2022

eggaskin left a comment

ky28059 commented Jan 21, 2022


		private final DifferentialDrivePoseEstimator odometry;

		public static final double ENCODER_TICKS_TO_INCHES = 32 / 142.40;


		odometry.update(gyroAngle, wheelVelocities, leftDistance, rightDistance);

		System.out.println(odometry.getEstimatedPosition());

Encoders! #9

Encoders! #9

Conversation

ky28059 commented Nov 14, 2021 • edited Loading

Resources

Choose a reason for hiding this comment

ky28059 commented Nov 20, 2021

Resources for the odometry portion of this PR

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

ky28059 commented Dec 17, 2021

Choose a reason for hiding this comment

Choose a reason for hiding this comment

ky28059 commented Dec 20, 2021 • edited Loading

Resources for the pathfinding portion of this PR

ky28059 commented Dec 20, 2021

ky28059 commented Jan 20, 2022

Closing(ish) thoughts

eggaskin left a comment

Choose a reason for hiding this comment

ky28059 commented Jan 21, 2022

ky28059 commented Nov 14, 2021 •

edited

Loading

ky28059 commented Dec 20, 2021 •

edited

Loading