Examples with DifferentialDriveOdometry edu.wpi.first.math.kinematics.DifferentialDriveOdometry used on opensource projects

Example 1 with DifferentialDriveOdometry

use of edu.wpi.first.math.kinematics.DifferentialDriveOdometry in project K-9 by TheGreenMachine.

the class TankDrive method resetEncoders.

public synchronized void resetEncoders() {
    mLeftMaster.setSelectedSensorPosition(0, 0, 0);
    mRightMaster.setSelectedSensorPosition(0, 0, 0);
    mPeriodicIO = new PeriodicIO();
    leftEncoderSimPosition = 0;
    rightEncoderSimPosition = 0;
    mRobotState.field.setRobotPose(Constants.StartingPose);
    odometry = new DifferentialDriveOdometry(Constants.StartingPose.getRotation(), Constants.StartingPose);
}

Example 2 with DifferentialDriveOdometry

use of edu.wpi.first.math.kinematics.DifferentialDriveOdometry in project RobotCore by FRC-568.

the class TalonSRXDrive method buildDrive.

private DifferentialDrive buildDrive() {
    boolean invert;
    int[] ports;
    ports = configIntArray("leftMotors");
    invert = configBoolean("leftInverted");
    motorsL = new WPI_TalonSRX[ports.length];
    for (int i = 0; i < motorsL.length; i++) {
        motorsL[i] = new WPI_TalonSRX(ports[i]);
        motorsL[i].configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0);
        motorsL[i].setInverted(invert);
        motorsL[i].setNeutralMode(NeutralMode.Coast);
        motorsL[i].configOpenloopRamp(1);
        motorsL[i].configPeakCurrentLimit(20);
        motorsL[i].configContinuousCurrentLimit(27);
        if (i > 0)
            motorsL[i].follow(motorsL[0]);
    }
    ports = configIntArray("rightMotors");
    invert = configBoolean("rightInverted");
    motorsR = new WPI_TalonSRX[ports.length];
    for (int i = 0; i < motorsR.length; i++) {
        motorsR[i] = new WPI_TalonSRX(ports[i]);
        motorsR[i].configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 0);
        motorsR[i].setInverted(invert);
        motorsR[i].setNeutralMode(NeutralMode.Coast);
        motorsL[i].configOpenloopRamp(1);
        motorsR[i].configPeakCurrentLimit(20);
        motorsR[i].configContinuousCurrentLimit(27);
        if (i > 0)
            motorsR[i].follow(motorsR[0]);
    }
    DifferentialDrive d = new DifferentialDrive(motorsL[0], motorsR[0]);
    SendableRegistry.addChild(this, d);
    leftMotors = new MotorControllerGroup(motorsL[0], motorsL[1]);
    rightMotors = new MotorControllerGroup(motorsR[0], motorsR[1]);
    odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading()));
    return d;
}

Example 3 with DifferentialDriveOdometry

use of edu.wpi.first.math.kinematics.DifferentialDriveOdometry in project NerdyLib by nerdherd.

the class Drivetrain method configKinematics.

public void configKinematics(double trackwidth, Rotation2d startingAngle, Pose2d startingPose) {
    m_kinematics = new DifferentialDriveKinematics(trackwidth);
    m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getRawYaw()));
}

Example 4 with DifferentialDriveOdometry

use of edu.wpi.first.math.kinematics.DifferentialDriveOdometry in project RapidReact2022 by team223.

the class DriveSubsystem method reset.

// RESETS VALUES
public void reset(Pose2d pose2d) {
    gyroscope.reset(pose2d.getRotation().getDegrees());
    System.out.println("ANGLE- " + gyroscope.getAngle());
    odometry = new DifferentialDriveOdometry(pose2d.getRotation(), pose2d);
    for (int i = 0; i < 2; i++) {
        leftMotors[i].getEncoder().setPosition(0);
        rightMotors[i].getEncoder().setPosition(0);
    }
}