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

Example 6 with SwerveModuleState

use of edu.wpi.first.math.kinematics.SwerveModuleState in project RapidReact2022-340 by Greater-Rochester-Robotics.

the class SwerveDrive method driveOneModule.

/**
 * This function is meant to drive one module at a time for testing purposes.
 * @param moduleNumber which of the four modules(0-3) we are using
 * @param moveSpeed move speed -1.0 to 1.0, where 0.0 is stopped
 * @param rotatePos a position between -PI and PI where we want the module to be
 * @param isVeloMode changes between velocity mode and dutyCycle mode
 */
public void driveOneModule(int moduleNumber, double moveSpeed, double rotatePos, boolean isVeloMode) {
    // test that moduleNumber is between 0-3, return if not(return;)
    if (moduleNumber > 3 && moduleNumber < 0) {
        System.out.println("Module " + moduleNumber + " is out of bounds.");
        return;
    } else if (rotatePos < -Math.PI || rotatePos > Math.PI) {
        System.out.println("Input angle out of range.");
        return;
    }
    SwerveModuleState oneSwerveState = new SwerveModuleState(moveSpeed, new Rotation2d(rotatePos));
    // code to drive one module in a testing form
    swerveModules[moduleNumber].setModuleState(oneSwerveState, isVeloMode);
}

Example 7 with SwerveModuleState

use of edu.wpi.first.math.kinematics.SwerveModuleState in project 2022-Robot-Development by SirLanceABot.

the class SwerveModule method setDesiredState.

/**
 * Sets the desired state for the module.
 *
 * @param desiredState Desired state with speed and angle.
 */
public void setDesiredState(SwerveModuleState desiredState) {
    // Optimize the reference state to avoid spinning further than 90 degrees
    // FIXME Changing radians to degrees
    SwerveModuleState state = SwerveModuleState.optimize(desiredState, Rotation2d.fromDegrees(getTurningEncoderPosition()));
    // SwerveModuleState state =
    // SwerveModuleState.optimize(desiredState, new Rotation2d(getTurningEncoderPosition()));
    double driveP = SmartDashboard.getNumber("Drive P", 0.0);
    double driveD = SmartDashboard.getNumber("Drive D", 0.0);
    drivePIDController.setP(driveP);
    drivePIDController.setD(driveD);
    // Calculate the drive output from the drive PID controller.
    final double driveOutput = drivePIDController.calculate(getDrivingEncoderRate(), state.speedMetersPerSecond);
    final double driveFeedforwardValue = driveFeedforward.calculate(state.speedMetersPerSecond);
    // Calculate the turning motor output from the turning PID controller.
    // double p = SmartDashboard.getNumber("Turn P", 0.0);
    // double d = SmartDashboard.getNumber("Turn D", 0.0);
    // turningPIDController.setP(p);
    // turningPIDController.setD(d);
    // FIXME Changing radians to degrees, changed PID to take degrees
    final double turnOutput = turningPIDController.calculate(getTurningEncoderPosition(), state.angle.getDegrees());
    // final double turnOutput =
    // turningPIDController.calculate(Math.toRadians(getTurningEncoderPosition()), state.angle.getRadians());
    // FIXME Changing radians to degrees, Changed volts per radian to volts per degree for kS, kV, kA
    final double turnFeedforwardValue = turnFeedforward.calculate(turningPIDController.getSetpoint().velocity);
    // FIXME Convert to Talon FX
    var normalizedDriveVoltage = normalizeVoltage(driveOutput + driveFeedforwardValue);
    var normalizedTurnVoltage = normalizeVoltage(turnOutput + turnFeedforwardValue);
    driveMotor.set(ControlMode.PercentOutput, normalizedDriveVoltage);
    // Used for running PIDF on TalonFX
    // var ticksPer100MS = state.speedMetersPerSecond * (1 / Constants.DRIVE_ENCODER_RATE_TO_METERS_PER_SEC);
    // System.out.println("M/S: " + state.speedMetersPerSecond + ", Ticks/100MS: " + ticksPer100MS);
    // driveMotor.set(ControlMode.Velocity, ticksPer100MS);
    // getDrivingEncoderRate();
    turnMotor.set(ControlMode.PercentOutput, normalizedTurnVoltage);
// FIXME Changing radians to degrees
// SmartDashboard.putNumber(moduleName + " Optimized Angle Radians", state.angle.getRadians());
// SmartDashboard.putNumber(moduleName + " Optimized Angle Degrees", state.angle.getDegrees());
// SmartDashboard.putNumber(moduleName + " Optimized Speed", state.speedMetersPerSecond);
// SmartDashboard.putNumber(moduleName + " Turn Output", turnOutput);
// SmartDashboard.putNumber(moduleName + " Turn Feedforward", turnFeedforwardValue);
// SmartDashboard.putNumber(moduleName + " Normalized Turn Percent", normalizedTurnVoltage);
// SmartDashboard.putNumber(moduleName + " Drive Output", driveOutput);
// SmartDashboard.putNumber(moduleName + " Drive Feedforward", driveFeedforwardValue);
// SmartDashboard.putNumber(moduleName + " Normalized Drive Percent", normalizedDriveVoltage);
// SmartDashboard.putNumber(moduleName + " Drive Encoder Rate", getDrivingEncoderRate());
}

Example 8 with SwerveModuleState

use of edu.wpi.first.math.kinematics.SwerveModuleState in project 2022-Robot-Development by SirLanceABot.

the class Drivetrain method drive.

/**
 * Method to drive the robot using joystick info.
 *
 * @param xSpeed Speed of the robot in the x direction (forward).
 * @param ySpeed Speed of the robot in the y direction (sideways).
 * @param turn Angular rate of the robot.
 * @param fieldRelative Whether the provided x and y speeds are relative to the field.
 */
@SuppressWarnings("ParameterName")
public void drive(double xSpeed, double ySpeed, double turn, boolean fieldRelative) {
    ChassisSpeeds chassisSpeeds;
    SwerveModuleState[] swerveModuleStates;
    if (fieldRelative)
        chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, turn, navX.getRotation2d());
    else
        chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, turn);
    swerveModuleStates = kinematics.toSwerveModuleStates(chassisSpeeds);
    SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, Constant.MAX_DRIVE_SPEED);
    if (xSpeed == 0 && ySpeed == 0 && turn == 0 && previousSwerveModuleStates != null) {
        for (int i = 0; i < swerveModuleStates.length; i++) {
            swerveModuleStates[i].angle = previousSwerveModuleStates[i].angle;
        }
    }
    frontLeft.setDesiredState(swerveModuleStates[0]);
    frontRight.setDesiredState(swerveModuleStates[1]);
    backLeft.setDesiredState(swerveModuleStates[2]);
    backRight.setDesiredState(swerveModuleStates[3]);
    previousSwerveModuleStates = swerveModuleStates;
    feedWatchdog();
}

Example 9 with SwerveModuleState

use of edu.wpi.first.math.kinematics.SwerveModuleState in project 2022-Robot by team1389.

the class Drivetrain method setChassisSpeeds.

// Manually set the speed of the drivetrain
public void setChassisSpeeds(ChassisSpeeds speeds) {
    SwerveModuleState[] moduleStates = kinematics.toSwerveModuleStates(speeds);
    frontLeft.setState(new SwerveModuleState(-moduleStates[0].speedMetersPerSecond, moduleStates[0].angle));
    frontRight.setState(new SwerveModuleState(-moduleStates[1].speedMetersPerSecond, moduleStates[1].angle));
    backLeft.setState(new SwerveModuleState(-moduleStates[2].speedMetersPerSecond, moduleStates[2].angle));
    backRight.setState(new SwerveModuleState(-moduleStates[3].speedMetersPerSecond, moduleStates[3].angle));
}

Example 10 with SwerveModuleState

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

the class SwerveModuleTest method execute.

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
    double angle = Math.toDegrees(Math.atan2(m_controller.getRightY(), m_controller.getRightX()));
    SwerveModuleState target = new SwerveModuleState(m_controller.getLeftY(), Rotation2d.fromDegrees(angle));
    m_drive.setSwerveModuleState(m_moduleID, target);
}