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

Example 1 with SwerveModuleState

use of edu.wpi.first.math.kinematics.SwerveModuleState in project 2022-Rapid-React by Manchester-Central.

the class SwerveDrive method adjustToDefaultPosition.

public void adjustToDefaultPosition() {
    m_moduleFL.setTargetState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45)));
    m_moduleFR.setTargetState(new SwerveModuleState(0, Rotation2d.fromDegrees(45)));
    m_moduleBR.setTargetState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45)));
    m_moduleBL.setTargetState(new SwerveModuleState(0, Rotation2d.fromDegrees(45)));
}

Example 2 with SwerveModuleState

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

the class DrivetrainSubsystem method zeroStates.

public void zeroStates(Pose2d start) {
    SwerveModuleState zeroState = new SwerveModuleState(0, start.getRotation());
    SwerveModuleState[] states = { zeroState, zeroState, zeroState, zeroState };
    this.setStates(states);
    this.odometry.resetPosition(start, start.getRotation());
}

Example 3 with SwerveModuleState

use of edu.wpi.first.math.kinematics.SwerveModuleState in project FRC2022 by 2202Programming.

the class SwerveModuleMK3 method setDesiredState.

/**
 * Set the speed + rotation of the swerve module from a SwerveModuleState object
 *
 * @param desiredState - A SwerveModuleState representing the desired new state
 *                     of the module
 */
public void setDesiredState(SwerveModuleState state) {
    // should favor reversing direction over
    SwerveModuleState m_state = SwerveModuleState.optimize(state, Rotation2d.fromDegrees(m_internalAngle));
    // turning > 90 degrees
    // uncomment to use optimized angle command
    state = m_state;
    // use position control on angle with INTERNAL encoder, scaled internally for
    // degrees
    m_angle_target = m_state.angle.getDegrees();
    // figure out how far we need to move, target - current, bounded +/-180
    double delta = ModMath.delta360(m_angle_target, m_internalAngle);
    // if we aren't moving, keep the wheels pointed where they are
    if (Math.abs(m_state.speedMetersPerSecond) < .01)
        delta = 0;
    // now add that delta to unbounded Neo angle, m_internal isn't range bound
    angleMotorPID.setReference(angleCmdInvert * (m_internalAngle + delta), ControlType.kPosition);
    // use velocity control
    driveMotorPID.setReference(m_state.speedMetersPerSecond, ControlType.kVelocity);
}

Example 4 with SwerveModuleState

use of edu.wpi.first.math.kinematics.SwerveModuleState in project FRC2022 by 2202Programming.

the class SwerveDrivetrain method stop.

/**
 * stop() - zero the current state's velocity component and leave angles as they are
 */
public void stop() {
    SwerveModuleState state = new SwerveModuleState();
    state.speedMetersPerSecond = 0.0;
    // output the angle and velocity for each module
    for (int i = 0; i < modules.length; i++) {
        state.angle = Rotation2d.fromDegrees(modules[i].getAngle());
        modules[i].setDesiredState(state);
    }
}

Example 5 with SwerveModuleState

use of edu.wpi.first.math.kinematics.SwerveModuleState in project FRC-2022-First-Iteration by SkylineSpartabots.

the class DrivetrainSubsystem method applyDrive.

public void applyDrive() {
    SwerveModuleState[] states = DriveConstants.kDriveKinematics.toSwerveModuleStates(m_chassisSpeeds);
    SwerveDriveKinematics.desaturateWheelSpeeds(states, DriveConstants.kMaxSpeedMetersPerSecond);
    m_frontLeftModule.set(getVoltageByVelocity(states[0].speedMetersPerSecond), states[0].angle.getRadians());
    m_frontRightModule.set(getVoltageByVelocity(states[1].speedMetersPerSecond), states[1].angle.getRadians());
    m_backLeftModule.set(getVoltageByVelocity(states[2].speedMetersPerSecond), states[2].angle.getRadians());
    m_backRightModule.set(getVoltageByVelocity(states[3].speedMetersPerSecond), states[3].angle.getRadians());
    m_odometry.update(getGyroscopeRotation(), new SwerveModuleState(m_frontLeftModule.getDriveVelocity(), new Rotation2d(m_frontLeftModule.getSteerAngle())), new SwerveModuleState(m_frontRightModule.getDriveVelocity(), new Rotation2d(m_frontRightModule.getSteerAngle())), new SwerveModuleState(m_backLeftModule.getDriveVelocity(), new Rotation2d(m_backLeftModule.getSteerAngle())), new SwerveModuleState(m_backRightModule.getDriveVelocity(), new Rotation2d(m_backRightModule.getSteerAngle())));
}