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

Example 6 with ChassisSpeeds

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

the class TrajectoryDriveCommand method end.

// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {
    m_timer.stop();
    m_subsystem.drive(new ChassisSpeeds(0, 0, 0));
}

Example 7 with ChassisSpeeds

use of edu.wpi.first.math.kinematics.ChassisSpeeds in project RobotCode2022 by Mechanical-Advantage.

the class MotionProfileCommand method execute.

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
    State setpoint = trajectory.sample(timer.get());
    Logger.getInstance().recordOutput("Odometry/ProfileSetpoint", new double[] { setpoint.poseMeters.getX(), setpoint.poseMeters.getY(), setpoint.poseMeters.getRotation().getRadians() });
    ChassisSpeeds chassisSpeeds = controller.calculate(drive.getPose(), setpoint);
    DifferentialDriveWheelSpeeds wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
    drive.driveVelocity(wheelSpeeds.leftMetersPerSecond, wheelSpeeds.rightMetersPerSecond);
}

Example 8 with ChassisSpeeds

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

the class HSSwerveDriveController method execute.

@Override
public void execute() {
    if (Timer.getFPGATimestamp() - startTime < TURN_TIME) {
        super.initialize();
        Pose2d initialState = trajectory.sample(0).poseMeters;
        Pose2d nextState = trajectory.sample(0.1).poseMeters;
        double dx = nextState.getX() - initialState.getX();
        double dy = nextState.getY() - initialState.getY();
        ChassisSpeeds chassis = ChassisSpeeds.fromFieldRelativeSpeeds(dx * 0.01, dy * 0.01, 0, Drivetrain.getInstance().getHeadingRotation());
        Drivetrain.getInstance().setAngleAndDriveVelocity(Drivetrain.getInstance().getKinematics().toSwerveModuleStates(chassis));
        thetaController.reset(-Drivetrain.getInstance().getHeading());
        xController.reset();
        yController.reset();
    } else {
        super.execute();
        SmartDashboard.putNumber("mp x error", xController.getPositionError());
        SmartDashboard.putNumber("mp y error", yController.getPositionError());
        SmartDashboard.putNumber("mp theta error", thetaController.getPositionError());
        SmartDashboard.putNumber("mp theta target position", thetaController.getSetpoint().position);
        SmartDashboard.putNumber("mp theta target velocity", thetaController.getSetpoint().velocity);
    }
}

Example 9 with ChassisSpeeds

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

the class SwerveManual method execute.

@Override
public void execute() {
    double angularVelocity = MathUtil.mapJoystickOutput(OI.getInstance().getDriverGamepad().getRightX(), OI.DEADBAND);
    angularVelocity *= Math.abs(angularVelocity);
    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) {
            angularVelocity = 0.001;
        }
    }
    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;
    double mag = Math.sqrt(translationx * translationx + translationy * translationy);
    double limitedMag = limiter.calculate(mag);
    if (Math.abs(mag) > 1e-3) {
        translationx = translationx / mag * limitedMag;
        translationy = translationy / mag * limitedMag;
    }
    if (OI.getInstance().getDriverGamepad().getButtonBumperLeft().get()) {
        translationy *= 0.6;
        translationx *= 0.6;
    }
    if (debouncer.calculate(Math.abs(MathUtil.mapJoystickOutput(OI.getInstance().getDriverGamepad().getRightX(), OI.DEADBAND)) < Drivetrain.MIN_OUTPUT)) {
        angularVelocity = -PIGEON_KP * (pigeonAngle - Drivetrain.getInstance().getHeading());
        SmartDashboard.putBoolean("holding pigeon angle", true);
    } else {
        pigeonAngle = Drivetrain.getInstance().getHeading();
        SmartDashboard.putBoolean("holding pigeon angle", false);
    }
    if (OI.getInstance().getDriverGamepad().getButtonBumperRightState() && Limelight.isTargetVisible()) {
        Limelight.update();
        angularVelocity = -txController.calculate(Limelight.getTx());
        pigeonAngle = Drivetrain.getInstance().getHeading();
        SmartDashboard.putBoolean("holding pigeon angle", false);
        SmartDashboard.putNumber("limelight setpoint", txController.getSetpoint().position);
        SmartDashboard.putNumber("limelight goal", txController.getGoal().position);
    } else {
    // txController.reset(0);
    }
    SmartDashboard.putNumber("angular vel", angularVelocity);
    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), false);
}

Example 10 with ChassisSpeeds

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

the class AlignWithLimelight method execute.

public void execute() {
    Limelight.update();
    double angularVelocity = -txController.calculate(Limelight.getTx(), 0);
    ChassisSpeeds chassis = new ChassisSpeeds(0, 0, -angularVelocity);
    Drivetrain.getInstance().setAngleAndDriveVelocity(Drivetrain.getInstance().getKinematics().toSwerveModuleStates(chassis), false);
}