Fix Robot's Odometry and Encoder Handling

[dlezcan1]

We have issues with the current swerve drive modeling and handling of swerve module drive encoders and driving. The following are a list of tasks we need to fix the system.

• [x] Update the SwerveModule's drive motor and motor's encoder usage
  • [x] Update the drive motor's encoder position and velocity conversion factors. The encoder values give fractions of a revolution the motor experiences. The equations are the following:

    private static kPositionConversionFactor = (Math.PI * Constants.kWheelDiameterInMeters) / Constants.kGearRatio; // convert revolutions @ motor to meters traveled at the wheel
    private static kVelocityConversionFactor = kPositionConversionFactor / 60; // convert revolutions per minute to meters per second
    ...
    m_driveEncoder.setPositionConversionFactor(kPositionConversionFactor);
    m_driveEncoder.setVelocityConversionFactor(kVelocityConversionFactor);

  • [x] Without PID, we set the motor's velocity power percentage (-1.0 to 1.0). Thus, we need to convert this driven value as a value of meters per second in the SwerveModule.setDesiredState(SwerveModuleState state) to change the state.speedMetersPerSecond to the motor's velocity percentage by

    m_drivingMotor.set(state.speedMetersPerSecond/Constants.kMaxSwerveModuleVelocity);

  • [x] Reset the drive motor's settings to factory in the SwerveModule Constructor with m_driveMotor.restoreFactoryDefaults();. Afterwards, reset the drive encoder's position to 0 with m_driveEncoder.setPosition(0).
• [x] Update turning SwerveModule turning motor's encoder's usage
  • There is DutyCycleEncoder attached directly to the wheel's turning axis.
  • [x] This DutyCycleEncoder needs to be created in the constructor with

    public SwerveModule(int driveMotorChannel, int turningMotorChannel, int turnEncoderPWMChannel, double turnOffset) { // constructor
    ...
    m_turningEncoder = new DutyCycleEncoder(turnEncoderChannel); 
    m_turningEncoder.reset(); // ALWAYS RESET YOUR ENCODERS
    m_turningEncoder.setPositionOffset(turnOffset); // set the encoder's offset
    m_turningEncoder.setDistancePerRotation(<number of radians per rotation of encoder>);
    ...
    }

  • [x] The relative encoder can get the distance of the encoder in fractions of a revolution with DutyCycleEncoder.getPosition function. However, we can add a conversion factor with the DutyCycleEncoder.setDistancePerRotation(<radian conversion factor to multiply the encoder position with>) function in the SwerveModule constructor.
• [x] With the following updates, we need the following functions
  • [x] A function to get the current SwerveModuleState, which contains 2 values: the drive motor's velocity in meters per second and the turn motor's current angle as a Rotation2d object.
  • [x] A function to get the current SwerveModulePosition, which contains 2 values: the drive motor's traveled distance in meters per second and the turn motor's current angle as a Rotation2d object.

    class SwerveModule {
    ...
    public SwerveModuleState getModuleState() {
            return new SwerveModuleState(<velocity of drive motor in meters per second>, Rotation2d.fromRadians(<angle of turn motor in radians>));
    }
    public SwerveModulePosition getModulePostion() {
            return new SwerveModulePosition(<position of drive motor in meters>, Rotation2d.fromRadians(<angle of turn motor in radians>));
    } 
    ...
    } 

• [x] Update DriveTrainPID with the following functions to handle odometry
  • [x] Function to return an array of the drivetrain's SwerveModulePosition. Order is important here and needs to be the same as the intialized SwerveDriveKinematics object. Current order is front left, front right, back left, back right
  • [x] Function to return an array of the drivetrain's SwerveModuleState. Order is important here and needs to be the same as the intialized SwerveDriveKinematics object. Current order is front left, front right, back left, back right
  • [x] Function to get the module's current ChassisSpeed. ChassisSpeed object contains 3 values: x direction velocity in meters per second, y direction velocity in meters per second, rotation velocity of robot in radians per second
  • [x] Function to update SwerveDriveOdometry object. This needs to be added to the classes periodic function.
  • [x] Function to reset the SwerveDriveOdometry object's position with a set Pose2d object.
  • [x] Function to get the robot's current Pose2d. The robot's current 2D position in meters and orientation
  • [x] Function to drive the robot with a desired ChassisSpeed. This should feed into the current DriveTrainPID.drive function implemented, so we avoid code duplication.
  • [x] Change the drive function by removing the boolean fieldRelative and boolean defenseHoldingMode arguments. These can be used directly with this.FieldRelativeEnable and this.WheelLock values.
  • The structure should look as follows:

    class DriveTrainPID {
    SwerveModule m_frontLeftModule, m_frontRightModule, m_backLeftModule, m_backRightModule;
    ...
    public SweveModulePosition [] getSwerveModulePositions() {
    return new SwerveModulePosition [] {
     m_frontLeftModule.getModulePosition(), ...
    };
    }
    public SweveModuleState [] getSwerveModuleStates() {
    return new SwerveModuleState [] {
     m_frontLeftModule.getModuleState(), ...
    };
    }
    public ChassisSpeed getChassisSpeed() {
    return m_kinematics.toChassisSpeed(getModuleStates());
    }
    public void updateOdometry() { // needs to be added to periodic() function
    m_odometry.update(gyro.getRotation2d(), getSwerveModulePositions()); 
    }
    public void resetOdometry(Pose2d pose) {
    m_odometry.resetPosition(<Rotation2d of gyro's current orientation>, getSwerveModulePositions(), pose);
    }
    pubilc Pose2d getPose2d() {
    return m_odometry.getPoseMeters(); // keep this in meters. We can always convert to inches in our displays, but it's best to be in meters for path following
    }
    public void driveChassisSpeed(ChassisSpeed desiredChassisSpeed) {
    this.drive(<unpack drive chassis speed with whatever you need>);
    }
    public void drive(double xSpeed, double ySpeed, double rotSpeed) { // remove unnecessary parameters. Use class variables
    ...
    }
    }

• [x] Finally, we need to update the controller's output to instead feed the DriveTrainPID.drive command with the desired speed velocities in meters per second and rotation speed in radians per second. We should tell the DriveTrainPID to command specific velocities, rather than (-1 to 1) values. This will make our software more consistent.

  class SwerveDriveCommand {
  public void execute() {
    ...
    double commandX   = m_LeftX.getAsDouble() * DriveTrainPID.kMaxVelocity;
    double commandY   = m_LeftY.getAsDouble() * DriveTrainPID.kMaxVelocity;
    double commandRot = m_RightX.getAsDouble() * DriveTrainPID.kMaxAngularVelocity;
    ...
  }
  }

[dlezcan1]

This has been implemented now.