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

Example 16 with ChassisSpeeds

use of edu.wpi.first.math.kinematics.ChassisSpeeds in project Dreadbots2022 by Dreadbot.

the class VelocityControlTestCommand method end.

@Override
public void end(boolean interrupted) {
    drive.setChassisSpeeds(new ChassisSpeeds(0, 0, 0));
    drive.stopMotors();
}

Example 17 with ChassisSpeeds

use of edu.wpi.first.math.kinematics.ChassisSpeeds in project frc-2022-rapid-react by team581.

the class VelocityControlTestCommand method end.

// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
    driveSubsystem.setChassisSpeeds(new ChassisSpeeds(0, 0, 0));
    driveSubsystem.stopMotors();
}

Example 18 with ChassisSpeeds

use of edu.wpi.first.math.kinematics.ChassisSpeeds in project Entropy2022 by Team138Entropy.

the class Drive method autoomousDrive.

/**
 * Drives the robot with the given linear velocity and angular velocity.
 *
 * @param xSpeed Linear velocity in m/s.
 * @param rot Angular velocity in rad/s.
 */
public void autoomousDrive(double xSpeed, double rot) {
    var wheelSpeeds = mKinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0.0, rot));
    setAutoSpeeds(wheelSpeeds);
}

Example 19 with ChassisSpeeds

use of edu.wpi.first.math.kinematics.ChassisSpeeds in project RoboCode2022 by HarkerRobo.

the class SwerveManualPercentOutput method execute.

@Override
public void execute() {
    double angularVelocity = MathUtil.mapJoystickOutput(OI.getInstance().getDriverGamepad().getRightX(), OI.DEADBAND);
    double translationy = -MathUtil.mapJoystickOutput(OI.getInstance().getDriverGamepad().getLeftX(), OI.DEADBAND);
    double translationx = MathUtil.mapJoystickOutput(OI.getInstance().getDriverGamepad().getLeftY(), OI.DEADBAND);
    double chasisMagnitude = Math.sqrt(Math.pow(translationx, 2) + Math.pow(translationy, 2));
    if (Math.abs(chasisMagnitude) < Drivetrain.MIN_OUTPUT) {
        translationx = 0;
        translationy = 0;
        if (Math.abs(angularVelocity) < Drivetrain.MIN_OUTPUT) {
            // Drivetrain.getInstance().getPigeon().getYaw();
            angularVelocity = 0;
        }
    }
    SmartDashboard.putNumber("trans X", translationx);
    angularVelocity *= Drivetrain.MAX_ANGULAR_VEL * OUTPUT_MULTIPLIER;
    translationx *= Drivetrain.MAX_DRIVE_VEL * OUTPUT_MULTIPLIER;
    translationy *= Drivetrain.MAX_DRIVE_VEL * OUTPUT_MULTIPLIER;
    if (debouncer.calculate(Math.abs(MathUtil.mapJoystickOutput(OI.getInstance().getDriverGamepad().getRightX(), OI.DEADBAND)) < Drivetrain.MIN_OUTPUT)) {
        angularVelocity = -PIGEON_KP * (pigeonAngle - Drivetrain.getInstance().getPigeon().getYaw());
        SmartDashboard.putBoolean("holding pigeon angle", true);
    } else {
        pigeonAngle = Drivetrain.getInstance().getPigeon().getYaw();
        SmartDashboard.putBoolean("holding pigeon angle", false);
    }
    // ChassisSpeeds chassis = ChassisSpeeds.fromFieldRelativeSpeeds(translationx, translationy, -angularVelocity, new Rotation2d(Math.toRadians(Drivetrain.getInstance().getPigeon().getYaw())));
    ChassisSpeeds chassis;
    if (Drivetrain.getInstance().isFieldCentric())
        chassis = ChassisSpeeds.fromFieldRelativeSpeeds(translationx, translationy, -angularVelocity, Rotation2d.fromDegrees(-Drivetrain.getInstance().getPigeon().getYaw()));
    else
        chassis = ChassisSpeeds.fromFieldRelativeSpeeds(translationx, translationy, -angularVelocity, Rotation2d.fromDegrees(0));
    Drivetrain.getInstance().setAngleAndDriveVelocity(Drivetrain.getInstance().getKinematics().toSwerveModuleStates(chassis), true);
}

Example 20 with ChassisSpeeds

use of edu.wpi.first.math.kinematics.ChassisSpeeds 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();
}