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

Example 1 with MecanumDriveMotorVoltages

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