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

Example 26 with ChassisSpeeds

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

the class SwerveDrive method moveFieldRelative.

public void moveFieldRelative(double sidewaysSpeed, double forwardSpeed, double theta) {
    ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds(sidewaysSpeed, forwardSpeed, theta, getRotation());
    move(speeds);
}

Example 27 with ChassisSpeeds

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

the class SwerveDrive method moveRobotRelative.

public void moveRobotRelative(double sidewaysSpeed, double forwardSpeed, double theta) {
    ChassisSpeeds speeds = new ChassisSpeeds(forwardSpeed, sidewaysSpeed, theta);
    move(speeds);
}

Example 28 with ChassisSpeeds

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

the class SwerveDrive method simulationPeriodic.

@Override
public void simulationPeriodic() {
    super.simulationPeriodic();
    ChassisSpeeds speeds = m_kinematics.toChassisSpeeds(getModuleStates());
    var currentYaw = m_gyro.getYaw();
    setSimulationAngle(currentYaw + new Rotation2d(speeds.omegaRadiansPerSecond / Constants.RobotUpdate_hz).getDegrees());
}

Example 29 with ChassisSpeeds

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

Example 30 with ChassisSpeeds

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

the class Robot method autonomousInit.

/**
 * This autonomous (along with the chooser code above) shows how to select
 * between different
 * autonomous modes using the dashboard. The sendable chooser code works with
 * the Java
 * SmartDashboard. If you prefer the LabVIEW Dashboard, remove all of the
 * chooser code and
 * uncomment the getString line to get the auto name from the text box below the
 * Gyro
 *
 * <p>
 * You can add additional auto modes by adding additional comparisons to the
 * switch structure
 * below with additional strings. If using the make sure to add them to the
 * chooser code above as well.
 */
@Override
public void autonomousInit() {
    if (config.enableDriveSubsystem && config.enableAutonomousSubsystem) {
        this.info.log(LogLevel.GENERAL, "auton started");
        Command command;
        switch(this.autonomousPathChooser.getSelected()) {
            case NOTHING:
                command = new AutonomousFollowPathCommand(config.auto.nothingPath, this.autonomousSubsystem, this.drivetrainSubsystem);
                break;
            case PATH_PLANNER_DRIVE_BACKWARDS:
                command = new AutonomousFollowPathCommand(config.auto.driveBackwardsPath, this.autonomousSubsystem, this.drivetrainSubsystem);
                break;
            case HARDCODED_SHOOT_DRIVE_BACK:
                command = new AutonomousCommand(this.autonomousSubsystem, this.drivetrainSubsystem, this.intakeSubsystem, this.shooterSubsystem).executeShootPreload().executeAction((d, i, s) -> {
                    i.forceIntaking();
                    i.spinForward();
                    // Run the feeder in reverse so that ball stays inside bms
                    s.antiFeed();
                }).executeAction((d, i, s) -> d.drive(new ChassisSpeeds(1.5, 0.0, 0)), // Drive out of the tarmac
                1).executeAction((d, i, s) -> d.stop(), // Drive out of the tarmac pt 2
                2.0).complete();
                break;
            case HARDCODED_SHOOT_DRIVE_BACK_AND_SHOOT_LOW:
                drivetrainSubsystem.resetGyroWithOffset(Rotation2d.fromDegrees(-150));
                command = new AutonomousCommand(this.autonomousSubsystem, this.drivetrainSubsystem, this.intakeSubsystem, this.shooterSubsystem).executeShootPreload().executeAction((d, i, s) -> {
                    i.forceIntaking();
                    i.spinForward();
                    // Run the feeder in reverse so that ball stays inside bms
                    s.antiFeed();
                }).executeAction((d, i, s) -> d.drive(new ChassisSpeeds(1.5, 0.0, 0)), // Drive out of the tarmac
                1).executeAction((d, i, s) -> d.stop(), // Drive out of the tarmac pt 2
                2.0).executeAction((d, i, s) -> d.drive(new ChassisSpeeds(-1.5, 0.0, 0)), // Drive back to the hub
                2).executeAction((d, i, s) -> d.stop(), // Drive back to the hub pt 2
                2.5).executeAction((d, i, s) -> d.stop(), // Drive back to the hub pt 2
                .5).executeShootPreloadLow().complete();
                break;
            case HARDCODED_SHOOT_DRIVE_BACK_AND_SHOOT_HIGH:
                drivetrainSubsystem.resetGyroWithOffset(Rotation2d.fromDegrees(-150));
                command = new AutonomousCommand(this.autonomousSubsystem, this.drivetrainSubsystem, this.intakeSubsystem, this.shooterSubsystem).executeShootPreload().executeAction((d, i, s) -> {
                    i.forceIntaking();
                    i.spinForward();
                    // Run the feeder in reverse so that ball stays inside bms
                    s.antiFeed();
                }).executeAction((d, i, s) -> d.drive(new ChassisSpeeds(1.5, 0.0, 0)), // Drive out of the tarmac
                1).executeAction((d, i, s) -> d.stop(), // Drive out of the tarmac pt 2
                2.0).executeAction((d, i, s) -> d.drive(new ChassisSpeeds(-1.5, 0.0, 0)), // Drive back to the hub
                2).executeAction((d, i, s) -> d.stop(), // Drive back to the hub pt 2
                2.5).executeAction((d, i, s) -> d.stop(), // Drive back to the hub pt 2
                .5).executeShootPreload().complete();
                break;
            case PATH_PLANNER_SHOOT_AND_DRIVE_BACKWARDS:
                command = new AutonomousCommand(this.autonomousSubsystem, this.drivetrainSubsystem, this.intakeSubsystem, this.shooterSubsystem).executeShootPreload().executeAction((d, i, s) -> i.spinForward()).executeDrivePath("Drive Backwards Single Ball", 1).executeAction((d, i, s) -> i.stopSpin(), 2).complete();
                break;
            case PATH_PLANNER_SHOOT_INTAKE_TWO_BALLS:
                command = new AutonomousCommand(this.autonomousSubsystem, this.drivetrainSubsystem, this.intakeSubsystem, this.shooterSubsystem).executeShootPreload().executeDrivePath("Drive Backwards Double Ball P1").executeAction((d, i, s) -> i.spinForward()).executeDrivePath("Drive Backwards Double Ball P2", 2).executeAction((d, i, s) -> i.stopSpin(), 2).complete();
                break;
            case MAIN_NO_TERMINAL:
                command = new AutonomousCommand(this.autonomousSubsystem, this.drivetrainSubsystem, this.intakeSubsystem, this.shooterSubsystem).executeShootPreload().executeDrivePath(// Drive to the first ball
                "Main P1").executeAction(// Activate intake
                (d, i, s) -> i.spinForward()).executeDrivePath("Main P2 Ball", // Skip terminal, go straight to the second ball
                2.0).executeAction((d, i, s) -> i.spinBackward(), // Turn off the intake after getting the ball
                2.0).executeDrivePath(// Drive to the base of the hub
                "Main P3").executeAction(// Shoot - Spin up Top
                (d, i, s) -> s.spinUpTop()).executeAction((d, i, s) -> {
                    // Activate feeders
                    s.turnOnFeeders();
                    // Activate BMS in case a ball doesn't get pulled by the feeders
                    i.ballManagementForward();
                }, // Wait 2 seconds for the shooter to spin up
                2).complete();
                break;
            case MAIN_WITH_TERMINAL:
                command = new AutonomousCommand(this.autonomousSubsystem, this.drivetrainSubsystem, this.intakeSubsystem, this.shooterSubsystem).executeShootPreload().executeDrivePath(// Drive to the first ball
                "Main P1").executeAction(// Activate intake
                (d, i, s) -> i.spinForward()).executeDrivePath("Main P2 Terminal", // Head to the Terminal ball and push it in
                2.0).executeDrivePath(// Head to the second ball
                "Main P2.5 Terminal").executeAction((d, i, s) -> i.spinBackward(), // Turn off the intake after getting the ball
                2.0).executeDrivePath(// Drive to the base of the hub
                "Main P3").executeAction(// Shoot - Spin up Top
                (d, i, s) -> s.spinUpTop()).executeAction((d, i, s) -> {
                    // Activate feeders
                    s.turnOnFeeders();
                    // Activate BMS in case a ball doesn't get pulled by the feeders
                    i.ballManagementForward();
                }, // Wait 2 seconds for the shooter to spin up
                2).complete();
                break;
            default:
                info.log(LogLevel.CRITICAL, "Invalid Autonomous Path! (SendableChooser Output: " + this.autonomousPathChooser.getSelected() + ")");
                return;
        }
        command.schedule();
    }
}