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

Example 1 with SwerveDriveKinematics

use of edu.wpi.first.math.kinematics.SwerveDriveKinematics in project FRC2022-Rapid-React by BitBucketsFRC4183.

the class DrivetrainSubsystem method init.

@Override
public void init() {
    this.maxVelocity_metersPerSecond = 6380.0 / 60.0 * SdsModuleConfigurations.MK4_L2.getDriveReduction() * SdsModuleConfigurations.MK4_L2.getWheelDiameter() * Math.PI;
    this.maxAngularVelocity_radiansPerSecond = maxVelocity_metersPerSecond / Math.hypot(config.drive.drivetrainTrackWidth_meters / 2.0, config.drive.drivetrainWheelBase_meters / 2.0);
    this.speedModifier = 1.0;
    this.moduleFrontLeftLocation = new Translation2d(config.drive.drivetrainTrackWidth_meters / 2.0, config.drive.drivetrainWheelBase_meters / 2.0);
    this.moduleFrontRightLocation = new Translation2d(config.drive.drivetrainTrackWidth_meters / 2.0, -config.drive.drivetrainWheelBase_meters / 2.0);
    this.moduleBackLeftLocation = new Translation2d(-config.drive.drivetrainTrackWidth_meters / 2.0, config.drive.drivetrainWheelBase_meters / 2.0);
    this.moduleBackRightLocation = new Translation2d(-config.drive.drivetrainTrackWidth_meters / 2.0, -config.drive.drivetrainWheelBase_meters / 2.0);
    this.kinematics = new SwerveDriveKinematics(moduleFrontLeftLocation, moduleFrontRightLocation, moduleBackLeftLocation, moduleBackRightLocation);
    this.gyro = new AHRS(SPI.Port.kMXP, (byte) 200);
    this.chassisSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0);
    this.initializeModules();
    setOdometry(new Pose2d());
}