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

Example 1 with ChassisSpeeds

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

the class SwerveDrive method moveDriverRelative.

public void moveDriverRelative(double sidewaysSpeed, double forwardSpeed, double theta) {
    ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds(forwardSpeed, sidewaysSpeed * -1, theta, getRotation());
    move(speeds);
}

Example 2 with ChassisSpeeds

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

the class DefaultDriveCommand method execute.

@Override
public void execute() {
    switch(orientationChooser.getSelected()) {
        case "Field Oriented":
            driveSubsystem.drive(ChassisSpeeds.fromFieldRelativeSpeeds(limiterX.calculate(translationXSupplier.getAsDouble()) * driveSubsystem.getMaxVelocity(), limiterY.calculate(translationYSupplier.getAsDouble()) * driveSubsystem.getMaxVelocity(), rotationSupplier.getAsDouble() * driveSubsystem.getMaxAngularVelocity(), driveSubsystem.getGyroAngle()));
            break;
        case "Robot Oriented":
            ChassisSpeeds robotOrient = new ChassisSpeeds(limiterX.calculate(translationXSupplier.getAsDouble() * driveSubsystem.getMaxVelocity()), limiterY.calculate(translationYSupplier.getAsDouble() * driveSubsystem.getMaxVelocity()), rotationSupplier.getAsDouble() * driveSubsystem.getMaxAngularVelocity());
            driveSubsystem.drive(robotOrient);
            break;
    }
}

Example 3 with ChassisSpeeds

use of edu.wpi.first.math.kinematics.ChassisSpeeds in project FRC_Rapid_React_2022 by CommandoRobotics.

the class FollowTrajectoryCommand method execute.

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
    double curTime = m_timer.get();
    double dt = curTime - m_prevTime;
    // Get the desired state of the robot (cast to a PathPlannerState to get rotation)
    var desiredState = (PathPlannerState) m_trajectory.sample(curTime);
    ChassisSpeeds targetChassisSpeeds;
    // Use the Holonomic Controller to get the ChassisSpeeds required to get to the next state based on the previous state
    if (usingCustomdriveRotationInput) {
        SmartDashboard.putBoolean("Using PathPlanner Rotation", false);
        targetChassisSpeeds = m_controller.calculate(m_pose.get(), desiredState, m_desiredRotation.get());
    } else {
        SmartDashboard.putBoolean("Using PathPlanner Rotation", true);
        targetChassisSpeeds = m_controller.calculate(m_pose.get(), desiredState, desiredState.holonomicRotation);
    }
    // Change ChassisSpeeds to WheelSpeeds (m/s) using mecanum kinematics
    var targetWheelSpeeds = m_kinematics.toWheelSpeeds(targetChassisSpeeds);
    targetWheelSpeeds.desaturate(m_maxWheelVelocityMetersPerSecond);
    // Set the PID setpoints for each wheel
    var frontLeftSpeedSetpoint = targetWheelSpeeds.frontLeftMetersPerSecond;
    var rearLeftSpeedSetpoint = targetWheelSpeeds.rearLeftMetersPerSecond;
    var frontRightSpeedSetpoint = targetWheelSpeeds.frontRightMetersPerSecond;
    var rearRightSpeedSetpoint = targetWheelSpeeds.rearRightMetersPerSecond;
    if (outputWheelSpeeds) {
        m_outputWheelSpeeds.accept(targetWheelSpeeds);
    } else {
        double frontLeftOutput;
        double rearLeftOutput;
        double frontRightOutput;
        double rearRightOutput;
        // Calculate the feedforward for each wheel based on the given feedforward for the chassis
        final double frontLeftFeedforward = m_feedforward.calculate(frontLeftSpeedSetpoint, (frontLeftSpeedSetpoint - m_prevSpeeds.frontLeftMetersPerSecond) / dt);
        final double rearLeftFeedforward = m_feedforward.calculate(rearLeftSpeedSetpoint, (rearLeftSpeedSetpoint - m_prevSpeeds.rearLeftMetersPerSecond) / dt);
        final double frontRightFeedforward = m_feedforward.calculate(frontRightSpeedSetpoint, (frontRightSpeedSetpoint - m_prevSpeeds.frontRightMetersPerSecond) / dt);
        final double rearRightFeedforward = m_feedforward.calculate(rearRightSpeedSetpoint, (rearRightSpeedSetpoint - m_prevSpeeds.rearRightMetersPerSecond) / dt);
        // Get the outputs of each PID controllers for each wheel based on the setpoint and feedforward
        frontLeftOutput = frontLeftFeedforward + m_frontLeftController.calculate(m_currentWheelSpeeds.get().frontLeftMetersPerSecond, frontLeftSpeedSetpoint);
        rearLeftOutput = rearLeftFeedforward + m_rearLeftController.calculate(m_currentWheelSpeeds.get().rearLeftMetersPerSecond, rearLeftSpeedSetpoint);
        frontRightOutput = frontRightFeedforward + m_frontRightController.calculate(m_currentWheelSpeeds.get().frontRightMetersPerSecond, frontRightSpeedSetpoint);
        rearRightOutput = rearRightFeedforward + m_rearRightController.calculate(m_currentWheelSpeeds.get().rearRightMetersPerSecond, rearRightSpeedSetpoint);
        // Feed the output voltages of the PID loops to the drive train as a MecanumDriveMotorVoltages
        m_outputDriveVoltages.accept(new MecanumDriveMotorVoltages(frontLeftOutput, frontRightOutput, rearLeftOutput, rearRightOutput));
    }
    m_prevTime = curTime;
    m_prevSpeeds = targetWheelSpeeds;
}

Example 4 with ChassisSpeeds

use of edu.wpi.first.math.kinematics.ChassisSpeeds in project command by MilwaukeeCyberCheese.

the class DriveCommand method execute.

@Override
public void execute() {
    inputX = m_translationXSupplier.getAsDouble();
    inputY = m_translationYSupplier.getAsDouble();
    inputRot = m_rotationSupplier.getAsDouble();
    m_driveSubsystem.drive(new ChassisSpeeds(inputX, inputY, inputRot));
}

Example 5 with ChassisSpeeds

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

the class TrajectoryDriveCommand method execute.

@Override
public void execute() {
    var desiredSpeed = m_trajectory.sample(m_timer.get());
    ChassisSpeeds targetChassisSpeeds = m_controller.calculate(new Pose2d(m_subsystem.getPose().getX(), m_subsystem.getPose().getY(), m_subsystem.getGyroscopeRotation()), desiredSpeed, m_endRotation);
    m_subsystem.drive(targetChassisSpeeds);
    SmartDashboard.putNumber("Elapsed Time", m_timer.get());
    SmartDashboard.putNumber("Desired acceleration", desiredSpeed.accelerationMetersPerSecondSq);
    SmartDashboard.putNumber("Desired velocity", desiredSpeed.velocityMetersPerSecond);
    SmartDashboard.putNumber("DesiredX", desiredSpeed.poseMeters.getX());
    SmartDashboard.putNumber("DesiredY", desiredSpeed.poseMeters.getY());
    SmartDashboard.putNumber("DesiredRot", desiredSpeed.poseMeters.getRotation().getDegrees());
    SmartDashboard.putNumber("Desired Time", desiredSpeed.timeSeconds);
    SmartDashboard.putNumber("End Rotation", m_endRotation.getDegrees());
    SmartDashboard.putNumber("Auto X Speed", targetChassisSpeeds.vxMetersPerSecond);
    SmartDashboard.putNumber("Auto Y Speed", targetChassisSpeeds.vxMetersPerSecond);
    SmartDashboard.putNumber("Auto Rot Speed", targetChassisSpeeds.omegaRadiansPerSecond);
}