From 5c67be93712a4aab8c7e8126f85db1a02c9bda1a Mon Sep 17 00:00:00 2001 From: varun7654 Date: Mon, 18 Jul 2022 18:31:20 -0700 Subject: [PATCH 1/8] create initial code that controls the robot using forces instead of speeds --- src/main/java/frc/robot/Constants.java | 36 +- src/main/java/frc/subsystem/Drive.java | 320 +++++++++--------- src/main/java/frc/utility/MathUtil.java | 29 ++ .../geometry/MutableTranslation2d.java | 31 +- src/test/java/frc/subsystem/DriveTest.java | 207 ----------- 5 files changed, 243 insertions(+), 380 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 017403a5..e6740409 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -95,11 +95,8 @@ public final class Constants { *

* 3 -> Right Back */ - public static final SimpleMotorFeedforward[] DRIVE_FEEDFORWARD = { - new SimpleMotorFeedforward(0.17763, 2.7731, 0.5), - new SimpleMotorFeedforward(0.17763, 2.7731, 0.5), - new SimpleMotorFeedforward(0.17763, 2.7731, 0.5), - new SimpleMotorFeedforward(0.17763, 2.7731, 0.5)}; + public static final SimpleMotorFeedforward DRIVE_FEEDFORWARD = new SimpleMotorFeedforward(0.17763, 2.7731, 0.5); + public static final double DRIVE_ROTATIONAL_KA = 0.4; /** @@ -150,10 +147,10 @@ public final class Constants { public static final double DRIVE_LOW_SPEED_MOD = 0.65; public static final double SWERVE_ACCELERATION = - ((360 * 10 * FALCON_ENCODER_TICKS_PER_ROTATIONS) / SWERVE_MOTOR_POSITION_CONVERSION_FACTOR) / 360; //5 rot/s^2 + ((360 * 10 * FALCON_ENCODER_TICKS_PER_ROTATIONS) / SWERVE_MOTOR_POSITION_CONVERSION_FACTOR) / 360; //10 rot/s^2 public static final double SWERVE_CRUISE_VELOCITY = - ((360 * 7 * FALCON_ENCODER_TICKS_PER_ROTATIONS) / SWERVE_MOTOR_POSITION_CONVERSION_FACTOR) / 360; //6.5 rot/s + ((360 * 7 * FALCON_ENCODER_TICKS_PER_ROTATIONS) / SWERVE_MOTOR_POSITION_CONVERSION_FACTOR) / 360; //7 rot/s /** * Allowed Turn Error in degrees. */ @@ -182,28 +179,35 @@ public enum AccelerationLimits { /** * Normal acceleration limit while driving. This ensures that the driver can't tip the robot. */ - NORMAL_DRIVING(24), + NORMAL_DRIVING(24, Math.toRadians(360 * 22)), /** * Acceleration limit when we're trying to shoot and move. This is very low to prevent the driver from making sudden * movements that would mess up the shot */ - SHOOT_AND_MOVE(4), + SHOOT_AND_MOVE(4, Math.toRadians(360 * 22)), /** * Slower acceleration limit to allow the robot to aim while slowing */ - STOP_AND_SHOOT(12); + STOP_AND_SHOOT(12, Math.toRadians(360 * 22)); - public double acceleration; + public final double acceleration; + public final double turningTranslationDeceleration; + public final double angularAcceleration; - AccelerationLimits(double acceleration) { + AccelerationLimits(double acceleration, double angularAcceleration) { + this(acceleration, acceleration, angularAcceleration); + } + + AccelerationLimits(double acceleration, double turningTranslationDeceleration, double angularAcceleration) { this.acceleration = acceleration; + this.turningTranslationDeceleration = turningTranslationDeceleration; + this.angularAcceleration = angularAcceleration; } } - /** - * Units are in Radians per Second Squared - */ - public static final double MAX_ANGULAR_ACCELERATION = Math.toRadians(360 * 22); + public static final double DRIVE_ANGULAR_BINARY_SEARCH_TOLERANCE = 0.01; + public static final double DRIVE_TRANSLATIONAL_BINARY_SEARCH_TOLERANCE = 0.01; + //field/Vision Manager constants public static final Translation2d GOAL_POSITION = new Translation2d(8.25, 0); diff --git a/src/main/java/frc/subsystem/Drive.java b/src/main/java/frc/subsystem/Drive.java index fb9d134f..1b133566 100644 --- a/src/main/java/frc/subsystem/Drive.java +++ b/src/main/java/frc/subsystem/Drive.java @@ -29,6 +29,7 @@ import frc.utility.ControllerDriveInputs; import frc.utility.Timer; import frc.utility.controllers.LazyTalonFX; +import frc.utility.geometry.MutableTranslation2d; import frc.utility.wpimodified.HolonomicDriveController; import frc.utility.wpimodified.PIDController; import org.jetbrains.annotations.Contract; @@ -39,6 +40,7 @@ import java.util.concurrent.locks.ReentrantLock; import static frc.robot.Constants.*; +import static frc.utility.MathUtil.*; public final class Drive extends AbstractSubsystem { @@ -336,32 +338,151 @@ public void swerveDrive(@NotNull ControllerDriveInputs inputs) { swerveDrive(chassisSpeeds); } - public void swerveDriveFieldRelative(@NotNull ControllerDriveInputs inputs) { - setDriveState(DriveState.TELEOP); - - ChassisSpeeds chassisSpeeds; - if (useFieldRelative) { - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(DRIVE_HIGH_SPEED_M * inputs.getX(), - DRIVE_HIGH_SPEED_M * inputs.getY(), - inputs.getRotation() * 7, - RobotTracker.getInstance().getGyroAngle()); + /** + * @param targetRobotRelativeSpeed The target speed of the robot in meters per second + * @param targetAngularSpeed The target angular speed of the robot in radians per second + * @param maxDesiredAcceleration The maximum wanted acceleration of the robot in meters per second squared + */ + public void swerveDrive(@NotNull Translation2d targetRobotRelativeSpeed, + double targetAngularSpeed, + double maxDesiredAcceleration) { + final ChassisSpeeds currentSpeeds = SWERVE_DRIVE_KINEMATICS.toChassisSpeeds(getSwerveModuleStates()); + + final Translation2d currentTranslationalVelocity = + new Translation2d(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond); + + final Translation2d neededVelocityChange = targetRobotRelativeSpeed.minus(currentTranslationalVelocity); + + // Create two different accelerations with: + // lowRobotAcceleration = translational acceleration which will result in the least voltage used for translating + // highRobotAcceleration = translational acceleration which will result in the most voltage used for translating + + // This acceleration will slow down the robot as quickly as possible, but don't overshoot a velocity of 0. + // Also ensures that don't use move voltage decelerating. + // TODO: Compensate for the fact that we'll be adding the voltage of our current velocity + double maxDeceleration = min(accelerationLimit.turningTranslationDeceleration, + currentTranslationalVelocity.getNorm() * (DRIVE_PERIOD / 1000.0), + (currentTranslationalVelocity.getNorm() * DRIVE_FEEDFORWARD.kv) / DRIVE_FEEDFORWARD.ka); + MutableTranslation2d lowRobotAcceleration = new MutableTranslation2d(currentTranslationalVelocity). + normalize().times(maxDeceleration).unaryMinus(); + + // Acceleration to reach the target velocity as quickly as possible + double maxAcceleration = min(accelerationLimit.acceleration, + neededVelocityChange.minus(targetRobotRelativeSpeed).getNorm() * (DRIVE_PERIOD / 1000.0), + maxDesiredAcceleration, + (currentTranslationalVelocity.minus(targetRobotRelativeSpeed).getNorm() * DRIVE_FEEDFORWARD.kv) + / DRIVE_FEEDFORWARD.ka); + // Do we need to limit the acceleration, so we don't go over the max voltage here? (Do we need the above line?) + // or will the binary search below do that for us? + MutableTranslation2d highRobotAcceleration = new MutableTranslation2d(neededVelocityChange). + normalize().times(maxAcceleration); + + double currentAngleSpeed = currentSpeeds.omegaRadiansPerSecond; + double neededAngleChange = targetAngularSpeed - currentAngleSpeed; + double highAngularAcceleration = Math.copySign(min(accelerationLimit.angularAcceleration, + Math.abs(targetAngularSpeed - currentAngleSpeed) * (DRIVE_PERIOD / 1000.0)), neededAngleChange); + double lowAngularAcceleration = Math.copySign(min( + accelerationLimit.angularAcceleration, + (Math.abs(currentAngleSpeed) * DRIVE_FEEDFORWARD.kv) / DRIVE_ROTATIONAL_KA), + -currentAngleSpeed); + + double maxUsableVoltage = getMaxUsableVoltage(); + + if (isUnderMaxVoltage(getChassisSpeedsFromAcceleration(highRobotAcceleration, highAngularAcceleration, currentSpeeds), + maxUsableVoltage)) { + // Everything is using their highest possible voltage, so we can't go any higher. + swerveDrive(getChassisSpeedsFromAcceleration(highRobotAcceleration, highAngularAcceleration, currentSpeeds)); + } else if (isUnderMaxVoltage( + getChassisSpeedsFromAcceleration(lowRobotAcceleration, lowAngularAcceleration, currentSpeeds), + maxUsableVoltage)) { + // Everything is using their lowest possible voltage, so we can't go any lower. + swerveDrive(getChassisSpeedsFromAcceleration(lowRobotAcceleration, highAngularAcceleration, currentSpeeds)); } else { - chassisSpeeds = new ChassisSpeeds(DRIVE_HIGH_SPEED_M * inputs.getX(), - DRIVE_HIGH_SPEED_M * inputs.getY(), - inputs.getRotation() * 7); + // Binary search for the angular acceleration that will result in the robot being under the max voltage. + // Angular speed has priority over translational speed, so we'll assume that we're using the lowest translational + // acceleration and try to find the highest angular acceleration that will result in the robot being under the max + // voltage. + double angularAcceleration; + { + double low = lowAngularAcceleration; + double high = highAngularAcceleration; + while (high - low > Constants.DRIVE_ANGULAR_BINARY_SEARCH_TOLERANCE) { + double mid = (low + high) / 2; + ChassisSpeeds chassisSpeeds = getChassisSpeedsFromAcceleration(lowRobotAcceleration, mid, currentSpeeds); + if (isUnderMaxVoltage(chassisSpeeds, maxUsableVoltage)) { + low = mid; + } else { + high = mid; + } + } + angularAcceleration = low; + } + + // Binary search for the highest translational acceleration that will result in the robot being under the max voltage. + // We'll use the highest angular acceleration we found above, and try to maximize the translational acceleration + // within the allowed voltage budget. + Translation2d translationalAcceleration; + { + final MutableTranslation2d low = lowRobotAcceleration; + final MutableTranslation2d high = highRobotAcceleration; + while (dist2(high.minus(low)) > Constants.DRIVE_TRANSLATIONAL_BINARY_SEARCH_TOLERANCE) { + // This is technically changing the direction of the vector. Would this cause issues? + low.plus(high).times(0.5); + ChassisSpeeds chassisSpeeds = getChassisSpeedsFromAcceleration(low, angularAcceleration, currentSpeeds); + if (!isUnderMaxVoltage(chassisSpeeds, maxUsableVoltage)) { + low.times(2).minus(high); // Undo changes to low + high.plus(low).times(0.5); // Set high to what low was + } // else, low has already been updated to be the correct value. + } + + translationalAcceleration = low.getTranslation2d(); + } + + swerveDrive(getChassisSpeedsFromAcceleration(translationalAcceleration, angularAcceleration, currentSpeeds)); } + } - swerveDrive(chassisSpeeds); + @Contract(pure = true) + private boolean isUnderMaxVoltage(ChassisSpeeds chassisSpeeds, double maxUsableVoltage) { + SwerveModuleState[] swerveModuleStates = SWERVE_DRIVE_KINEMATICS.toSwerveModuleStates(chassisSpeeds); + for (SwerveModuleState swerveModuleState : swerveModuleStates) { + if (swerveModuleState.speedMetersPerSecond > maxUsableVoltage) { // Speed m/s is actually voltage + return false; + } + } + return false; } - public void swerveDrive(ChassisSpeeds chassisSpeeds) { + private double getMaxUsableVoltage() { + return 10 - DRIVE_FEEDFORWARD.ks; + } - Translation2d accelerationFR = limitAcceleration(chassisSpeeds); + @Contract(mutates = "", pure = true) + public @NotNull ChassisSpeeds getChassisSpeedsFromAcceleration(@NotNull Translation2d robotRelativeAcceleration, + double angularAcceleration, + @NotNull ChassisSpeeds currentSpeeds) { + final double kV = DRIVE_FEEDFORWARD.kv; + final double kA = DRIVE_FEEDFORWARD.ka; + final double kAR = DRIVE_ROTATIONAL_KA; - SmartDashboard.putNumber("Drive Command X Velocity", chassisSpeeds.vxMetersPerSecond); - SmartDashboard.putNumber("Drive Command Y Velocity", chassisSpeeds.vyMetersPerSecond); - SmartDashboard.putNumber("Drive Command Rotation", chassisSpeeds.omegaRadiansPerSecond); + ChassisSpeeds chassisSpeeds = new ChassisSpeeds( + currentSpeeds.vxMetersPerSecond * kV + robotRelativeAcceleration.getX() * kA, + currentSpeeds.vyMetersPerSecond * kV + robotRelativeAcceleration.getY() * kA, + currentSpeeds.omegaRadiansPerSecond * kV + angularAcceleration * kAR + ); + + + SWERVE_DRIVE_KINEMATICS.toSwerveModuleStates(chassisSpeeds); + + return chassisSpeeds; + } + + + public void swerveDrive(ChassisSpeeds chassisSpeeds) { + SmartDashboard.putNumber("Drive Command X Force", chassisSpeeds.vxMetersPerSecond); + SmartDashboard.putNumber("Drive Command Y Force", chassisSpeeds.vyMetersPerSecond); + SmartDashboard.putNumber("Drive Command Rotational Force", chassisSpeeds.omegaRadiansPerSecond); SwerveModuleState[] moduleStates = SWERVE_DRIVE_KINEMATICS.toSwerveModuleStates(chassisSpeeds); @@ -369,51 +490,46 @@ public void swerveDrive(ChassisSpeeds chassisSpeeds) { chassisSpeeds.vyMetersPerSecond != 0 || chassisSpeeds.omegaRadiansPerSecond != 0; - SwerveDriveKinematics.desaturateWheelSpeeds(moduleStates, DRIVE_HIGH_SPEED_M); - setSwerveModuleStates(moduleStates, rotate, accelerationFR); - - lastAcceleration = accelerationFR; + SwerveDriveKinematics.desaturateWheelSpeeds(moduleStates, getMaxUsableVoltage()); + setSwerveModuleStates(moduleStates, rotate); } public void setSwerveModuleStates(SwerveModuleState[] moduleStates, boolean rotate) { - setSwerveModuleStates(moduleStates, rotate, new Translation2d()); - } - - public void setSwerveModuleStates(SwerveModuleState[] moduleStates, boolean rotate, Translation2d acceleration) { - acceleration = toRobotRelative(acceleration); - double accelNorm = acceleration.getNorm(); for (int i = 0; i < 4; i++) { - SwerveModuleState targetState = SwerveModuleState.optimize(moduleStates[i], - Rotation2d.fromDegrees(getWheelRotation(i))); - double rotatedAccel = accelNorm; - if (!(Math.abs(targetState.angle.minus(moduleStates[i].angle).getDegrees()) < 5)) { - rotatedAccel = -rotatedAccel; - } + double currentAngle = getWheelRotation(i); + SwerveModuleState targetState = SwerveModuleState.optimize(moduleStates[i], Rotation2d.fromDegrees(currentAngle)); - //SwerveModuleState targetState = moduleStates[i]; - // TODO: flip the acceleration if we flip the module - double targetAngle = targetState.angle.getDegrees() % 360; - if (targetAngle < 0) { // Make sure the angle is positive - targetAngle += 360; - } - double currentAngle = getWheelRotation(i); //swerveEncoders[i].getPosition(); + double targetAngle = mod(targetState.angle.getDegrees(), 360); double angleDiff = getAngleDiff(targetAngle, currentAngle); - if (Math.abs(angleDiff) > 0.1 && rotate) { // Only update the setpoint if we're already there and moving + if (Math.abs(angleDiff) > 0.1 && rotate) { // Only update the setpoint if we're not already there and moving setSwerveMotorPosition(i, getRelativeSwervePosition(i) + angleDiff); } - double speedModifier = 1; //= 1 - (OrangeUtility.coercedNormalize(Math.abs(angleDiff), 5, 180, 0, 180) / 180); - - setMotorSpeed(i, targetState.speedMetersPerSecond * speedModifier, rotatedAccel); + setMotorVoltage(i, targetState.speedMetersPerSecond); - SmartDashboard.putNumber("Swerve Motor " + i + " Speed Modifier", speedModifier); SmartDashboard.putNumber("Swerve Motor " + i + " Target Position", getRelativeSwervePosition(i) + angleDiff); SmartDashboard.putNumber("Swerve Motor " + i + " Error", angleDiff); } } + /** + * Sets the motor voltage + * + * @param module The module to set the voltage on + * @param outputVoltage The target outputVoltage + */ + public void setMotorVoltage(int module, double outputVoltage) { + if (module < 0 || module > 3) { + throw new IllegalArgumentException("Module must be between 0 and 3"); + } + + swerveDriveMotors[module].set(ControlMode.PercentOutput, + (outputVoltage / Constants.SWERVE_DRIVE_VOLTAGE_LIMIT) + DRIVE_FEEDFORWARD.ks); + SmartDashboard.putNumber("Out Volts " + module, outputVoltage); + } + /** * @param targetAngle The angle we want to be at (0-360) * @param currentAngle The current angle of the module (0-360) @@ -436,85 +552,14 @@ public double getAngleDiff(double targetAngle, double currentAngle) { private double lastLoopTime = 0; - /** - * Puts a limit on the acceleration. This method should be called before setting a chassis speeds to the robot drivebase. - *

- * Limits the acceleration by ensuring that the difference between the command and previous velocity doesn't exceed a set - * value - * - * @param commandedVelocity Desired velocity (The chassis speeds is mutated to the limited acceleration) (robot centric) - * @return The requested acceleration of the robot (field centric) - */ - @Contract(mutates = "param") - @NotNull Translation2d limitAcceleration(@NotNull ChassisSpeeds commandedVelocity) { - double dt; - if ((Timer.getFPGATimestamp() - lastLoopTime) > ((double) Constants.DRIVE_PERIOD / 1000) * 20) { - // If the dt is a lot greater than our nominal dt reset the acceleration limiting - // (ex. we've been disabled for a while) - ChassisSpeeds currentChassisSpeeds = RobotTracker.getInstance().getLatencyCompedChassisSpeeds(); - lastRequestedVelocity = new Translation2d( - currentChassisSpeeds.vxMetersPerSecond, - currentChassisSpeeds.vyMetersPerSecond - ); - - lastRequestedRotation = currentChassisSpeeds.omegaRadiansPerSecond; - dt = (double) Constants.DRIVE_PERIOD / 1000; - } else { - dt = Timer.getFPGATimestamp() - lastLoopTime; - } - lastLoopTime = Timer.getFPGATimestamp(); - - double maxVelocityChange = accelerationLimit.acceleration * dt; - double maxAngularVelocityChange = Constants.MAX_ANGULAR_ACCELERATION * dt; - - //field relative - Translation2d velocityCommand = toFieldRelative( - new Translation2d(commandedVelocity.vxMetersPerSecond, commandedVelocity.vyMetersPerSecond - )); - - Translation2d velocityChange = velocityCommand.minus(lastRequestedVelocity); - double velocityChangeAngle = Math.atan2(velocityChange.getY(), velocityChange.getX()); //Radians - - Translation2d limitedVelocityVectorChange = velocityChange; - Translation2d limitedVelocityVector = velocityCommand; - // Check if velocity change exceeds max limit - if (velocityChange.getNorm() > maxVelocityChange) { - // Get limited velocity vector difference in cartesian coordinate system - limitedVelocityVectorChange = new Translation2d(maxVelocityChange, new Rotation2d(velocityChangeAngle)); - limitedVelocityVector = lastRequestedVelocity.plus(limitedVelocityVectorChange); - - //robot relative - Translation2d limitedVelocityVectorRotated = toRobotRelative(limitedVelocityVector); - - commandedVelocity.vxMetersPerSecond = limitedVelocityVectorRotated.getX(); - commandedVelocity.vyMetersPerSecond = limitedVelocityVectorRotated.getY(); - } - - // Checks if requested change in Angular Velocity is greater than allowed - if (Math.abs(commandedVelocity.omegaRadiansPerSecond - lastRequestedRotation) - > maxAngularVelocityChange) { - // Add the lastCommandVelocity and the maxAngularVelocityChange (changed to have the same sign as the actual change) - commandedVelocity.omegaRadiansPerSecond = lastRequestedRotation + - Math.copySign(maxAngularVelocityChange, - commandedVelocity.omegaRadiansPerSecond - lastRequestedRotation); - } - - - // save our current commanded velocity to be used in next iteration - lastRequestedRotation = commandedVelocity.omegaRadiansPerSecond; - lastRequestedVelocity = limitedVelocityVector;//field - - return limitedVelocityVectorChange;//field - } - /** * Converts a field relative velocity to be robot relative * - * @param fieldRelativeTranslation a field relative velocity + * @param fieldRelativeVelocity a field relative velocity * @return a robot relative velocity */ - private Translation2d toRobotRelative(Translation2d fieldRelativeTranslation) { - return fieldRelativeTranslation.rotateBy(RobotTracker.getInstance().getGyroAngle().unaryMinus()); + private Translation2d toRobotRelative(Translation2d fieldRelativeVelocity) { + return fieldRelativeVelocity.rotateBy(RobotTracker.getInstance().getGyroAngle().unaryMinus()); } /** @@ -530,37 +575,6 @@ private Translation2d toFieldRelative(Translation2d robotRelativeTranslation) { private final double[] lastWheelSpeeds = new double[4]; private final double[] lastWheelSpeedsTime = new double[4]; - /** - * Sets the motor voltage - * - * @param module The module to set the voltage on - * @param velocity The target velocity - */ - public void setMotorSpeed(int module, double velocity, double acceleration) { - if (module < 0 || module > 3) { - throw new IllegalArgumentException("Module must be between 0 and 3"); - } - -// /* -// * This might have issues in certain situations (ie. when the wheel speed switches directions from optimization), but -// * it should be fine since we'll only see that for one iteration. -// */ -// double currentTime = Timer.getFPGATimestamp(); -// double dt = currentTime - lastWheelSpeedsTime[module]; -// double acceleration = (velocity - lastWheelSpeeds[module]) / dt; -// lastWheelSpeeds[module] = velocity; -// lastWheelSpeedsTime[module] = currentTime; -// if (dt > 0.15) { -// acceleration = 0; -// } - - double ffv = Constants.DRIVE_FEEDFORWARD[module].calculate(velocity, acceleration); - // Converts ffv voltage to percent output and sets it to motor - swerveDriveMotors[module].set(ControlMode.PercentOutput, ffv / Constants.SWERVE_DRIVE_VOLTAGE_LIMIT); - SmartDashboard.putNumber("Out Volts " + module, ffv); - //swerveDriveMotors[module].setVoltage(10 * velocity/Constants.SWERVE_METER_PER_ROTATION); - } - /** * {@link Drive#getSpeedSquared()} is faster if you don't need the square root * @@ -870,9 +884,7 @@ public void logData() { */ public double getWheelRotation(int moduleNumber) { if (useRelativeEncoderPosition) { - double relPos = getRelativeSwervePosition(moduleNumber) % 360; - if (relPos < 0) relPos += 360; - return relPos; + return mod(getRelativeSwervePosition(moduleNumber), 360); } else { return swerveCanCoders[moduleNumber].getAbsolutePosition(); } diff --git a/src/main/java/frc/utility/MathUtil.java b/src/main/java/frc/utility/MathUtil.java index 55b1a3fa..6dd85018 100644 --- a/src/main/java/frc/utility/MathUtil.java +++ b/src/main/java/frc/utility/MathUtil.java @@ -58,4 +58,33 @@ public static double dist2(@NotNull Translation2d translation2d) { double y = translation2d.getY(); return x * x + y * y; } + + /** + * Returns the output of the modulo operation, but always with a positive result. + * + * @param x The number to modulo. + * @param y The modulus. + * @return The output of the modulo operation. + */ + public static double mod(double x, double y) { + double result = x % y; + if (result < 0) { + result += y; + } + return result; + } + + /** + * Returns the minimum of the supplied values. + * + * @param values The values to compare. + * @return The minimum of the values. + */ + public static double min(double... values) { + double min = Double.MAX_VALUE; + for (double value : values) { + min = Math.min(min, value); + } + return min; + } } diff --git a/src/main/java/frc/utility/geometry/MutableTranslation2d.java b/src/main/java/frc/utility/geometry/MutableTranslation2d.java index 9eac001e..6d0391ad 100644 --- a/src/main/java/frc/utility/geometry/MutableTranslation2d.java +++ b/src/main/java/frc/utility/geometry/MutableTranslation2d.java @@ -7,6 +7,7 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import org.jetbrains.annotations.Contract; import java.util.Objects; @@ -75,6 +76,7 @@ public MutableTranslation2d(double distance, Rotation2d angle) { * @param x The x component of the translation. * @param y The y component of the translation. */ + @Contract(mutates = "this") public MutableTranslation2d set(double x, double y) { m_x = x; m_y = y; @@ -95,6 +97,12 @@ public MutableTranslation2d set(double distance, Rotation2d angle) { return this; } + @Contract(mutates = "this") + public void set(MutableTranslation2d other) { + m_x = other.getX(); + m_y = other.getY(); + } + /** * Calculates the distance between two translations in 2d space. * @@ -154,6 +162,7 @@ public double getNorm() { * @return The rotated translation. */ @Override + @Contract(mutates = "this") public MutableTranslation2d rotateBy(Rotation2d other) { return set(m_x * other.getCos() - m_y * other.getSin(), m_x * other.getSin() + m_y * other.getCos()); } @@ -167,6 +176,7 @@ public MutableTranslation2d rotateBy(Rotation2d other) { * @return The sum of the translations. */ @Override + @Contract(mutates = "this") public MutableTranslation2d plus(edu.wpi.first.math.geometry.Translation2d other) { return set(m_x + other.getX(), m_y + other.getY()); } @@ -179,6 +189,7 @@ public MutableTranslation2d plus(edu.wpi.first.math.geometry.Translation2d other * @param other The translation to add. * @return The sum of the translations. */ + @Contract(mutates = "this") public MutableTranslation2d plus(MutableTranslation2d other) { return set(m_x + other.getX(), m_y + other.getY()); } @@ -192,6 +203,7 @@ public MutableTranslation2d plus(MutableTranslation2d other) { * @return The difference between the two translations. */ @Override + @Contract(mutates = "this") public MutableTranslation2d minus(edu.wpi.first.math.geometry.Translation2d other) { return set(m_x - other.getX(), m_y - other.getY()); } @@ -203,8 +215,9 @@ public MutableTranslation2d minus(edu.wpi.first.math.geometry.Translation2d othe * @return The inverse of the current translation. */ @Override - public edu.wpi.first.math.geometry.Translation2d unaryMinus() { - return new edu.wpi.first.math.geometry.Translation2d(-m_x, -m_y); + @Contract(mutates = "this") + public MutableTranslation2d unaryMinus() { + return set(-m_x, -m_y); } /** @@ -216,6 +229,7 @@ public edu.wpi.first.math.geometry.Translation2d unaryMinus() { * @return The scaled translation. */ @Override + @Contract(mutates = "this") public MutableTranslation2d times(double scalar) { return set(m_x * scalar, m_y * scalar); } @@ -229,6 +243,7 @@ public MutableTranslation2d times(double scalar) { * @return This mutated object. */ @Override + @Contract(mutates = "this") public MutableTranslation2d div(double scalar) { return set(m_x / scalar, m_y / scalar); } @@ -238,6 +253,16 @@ public String toString() { return String.format("MutableTranslation2d(X: %.2f, Y: %.2f)", m_x, m_y); } + /** + * Normalizes the translation to a unit vector. + * + * @return This mutated object. + */ + @Contract(mutates = "this") + public MutableTranslation2d normalize() { + return this.div(this.getNorm()); + } + /** * Checks equality between this MutableTranslation2d and another object. * @@ -261,7 +286,7 @@ public int hashCode() { return Objects.hash(m_x, m_y); } - + public MutableTranslation2d interpolate(MutableTranslation2d endValue, double t) { return set(MathUtil.interpolate(this.getX(), endValue.getX(), t), MathUtil.interpolate(this.getY(), endValue.getY(), t)); diff --git a/src/test/java/frc/subsystem/DriveTest.java b/src/test/java/frc/subsystem/DriveTest.java index 38c1f1d5..d38e139c 100644 --- a/src/test/java/frc/subsystem/DriveTest.java +++ b/src/test/java/frc/subsystem/DriveTest.java @@ -1,20 +1,12 @@ package frc.subsystem; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import frc.robot.Constants; -import frc.utility.Timer; -import frc.utility.controllers.LazyTalonFX; import org.junit.jupiter.api.AfterEach; import org.junit.jupiter.api.BeforeEach; -import org.junit.jupiter.api.Disabled; import org.junit.jupiter.api.Test; -import java.lang.reflect.Field; import java.util.Random; import static org.junit.jupiter.api.Assertions.assertEquals; -import static org.junit.jupiter.api.Assertions.assertNotEquals; public class DriveTest { @@ -38,205 +30,6 @@ void tearDown() throws Exception { } - @Disabled - void testStopMovement1() throws NoSuchFieldException, IllegalAccessException { - Field lastRequestedVelocity = Drive.class.getDeclaredField("lastRequestedVelocity"); - lastRequestedVelocity.setAccessible(true); - lastRequestedVelocity.set(drive, new Translation2d()); - - Field lastRequestedRotation = Drive.class.getDeclaredField("lastRequestedRotation"); - lastRequestedRotation.setAccessible(true); - lastRequestedRotation.set(drive, 0.0); - - Timer.setTime(55); - drive.swerveDrive(new ChassisSpeeds(1, 1, 0.0)); - for (LazyTalonFX swerveDriveMotor : drive.swerveDriveMotors) { - assertNotEquals(0.0, swerveDriveMotor.getSetpoint(), DELTA); - } - Timer.setTime(55.14); - - drive.stopMovement(); - - drive.update(); - - for (int i = 0; i < drive.swerveMotors.length; i++) { - assertEquals(0.0, drive.swerveDriveMotors[i].getSetpoint(), DELTA); - } - } - - /** - * Test that the acceleration limit is working - */ - @Test - void testStopMovement2() throws NoSuchFieldException, IllegalAccessException { - Field lastRequestedVelocity = Drive.class.getDeclaredField("lastRequestedVelocity"); - lastRequestedVelocity.setAccessible(true); - lastRequestedVelocity.set(drive, new Translation2d(10, 10)); - - Field lastRequestedRotation = Drive.class.getDeclaredField("lastRequestedRotation"); - lastRequestedRotation.setAccessible(true); - lastRequestedRotation.set(drive, 0.0); - - Timer.setTime(54.9); - drive.swerveDrive(new ChassisSpeeds(10, 10, 0.0)); - - Timer.setTime(55); - drive.swerveDrive(new ChassisSpeeds(10, 10, 0.0)); - for (LazyTalonFX swerveDriveMotor : drive.swerveDriveMotors) { - assertNotEquals(0.0, swerveDriveMotor.getSetpoint(), DELTA); - } - Timer.setTime(55.01); - - drive.stopMovement(); - - drive.update(); - - for (int i = 0; i < drive.swerveMotors.length; i++) { - assertNotEquals(0.0, drive.swerveDriveMotors[i].getSetpoint(), DELTA); - } - } - - /** - * Test that the acceleration limit is working - */ - @Disabled - void testStopMovement3() throws NoSuchFieldException, IllegalAccessException { - Field lastRequestedVelocity = Drive.class.getDeclaredField("lastRequestedVelocity"); - lastRequestedVelocity.setAccessible(true); - lastRequestedVelocity.set(drive, new Translation2d(0.2, 0.2)); - - Field lastRequestedRotation = Drive.class.getDeclaredField("lastRequestedRotation"); - lastRequestedRotation.setAccessible(true); - lastRequestedRotation.set(drive, 0.0); - - Timer.setTime(54.9); - drive.swerveDrive(new ChassisSpeeds(0.2, 0.2, 0.0)); - - Timer.setTime(55); - drive.swerveDrive(new ChassisSpeeds(0.2, 0.2, 0.0)); - for (LazyTalonFX swerveDriveMotor : drive.swerveDriveMotors) { - assertNotEquals(0.0, swerveDriveMotor.getSetpoint(), DELTA); - } - Timer.setTime(55.1); - - drive.stopMovement(); - - drive.update(); - - - for (int i = 0; i < drive.swerveMotors.length; i++) { - double expectedVoltage = Constants.DRIVE_FEEDFORWARD[i].calculate(0, -Math.hypot(0.2, 0.2) / 0.1) / 12; - assertEquals(expectedVoltage, drive.swerveDriveMotors[i].getSetpoint(), DELTA); - } - } - - /* - @Disabled("Limiting angular acceleration breaks test") - void testLimitAcceleration() throws Exception { - ChassisSpeeds commandedVelocity = null; - ChassisSpeeds limitedVelocity = null; - double velocity = 0; - double actualVelocity = 0; - - Field currentRobotState = RobotTracker.class.getDeclaredField("latencyCompensatedChassisSpeeds"); - currentRobotState.setAccessible(true); - - Field lastLoopTime = Drive.class.getDeclaredField("lastLoopTime"); - lastLoopTime.setAccessible(true); - - // Set time to 50 seconds - - lastLoopTime.set(drive, 50); - - Timer.setTime(50.1); - // Set Current Velocity - ChassisSpeeds currentRobotStateChassisSpeed = new ChassisSpeeds(20, 10, 3); - currentRobotState.set(robotTracker, currentRobotStateChassisSpeed); - - // Set Commanded Velocity - commandedVelocity = new ChassisSpeeds(40, 20, 6); - // call limitAcceleration - limitedVelocity = drive.limitAcceleration(commandedVelocity); - // Gets mag of limitedVelocity - velocity = Math.sqrt((limitedVelocity.vxMetersPerSecond * limitedVelocity.vxMetersPerSecond) - + (limitedVelocity.vyMetersPerSecond * limitedVelocity.vyMetersPerSecond)); - - // Gets mag of actualVelocity - actualVelocity = Math.sqrt( - (currentRobotStateChassisSpeed.vxMetersPerSecond * currentRobotStateChassisSpeed.vxMetersPerSecond) - + (currentRobotStateChassisSpeed.vyMetersPerSecond * currentRobotStateChassisSpeed.vyMetersPerSecond)); - // Check if acceleration is at maximum - assertEquals(Constants.MAX_ACCELERATION * 0.1, Math.abs(velocity - actualVelocity), DELTA); - assertEquals(limitedVelocity.omegaRadiansPerSecond, 6, DELTA); - } - - @Disabled("Limiting angular acceleration breaks test") - void testLimitAcceleration2() throws Exception { - - Field currentRobotState = RobotTracker.class.getDeclaredField("latencyCompensatedChassisSpeeds"); - currentRobotState.setAccessible(true); - - Field lastLoopTime = Drive.class.getDeclaredField("lastLoopTime"); - lastLoopTime.setAccessible(true); - - // Set time to 50 seconds - - lastLoopTime.set(drive, 50); - - Timer.setTime(50.05); - // Set Current Velocity - ChassisSpeeds currentRobotStateChassisSpeed = new ChassisSpeeds(23.22, 11.05, -3.1); - currentRobotState.set(robotTracker, currentRobotStateChassisSpeed); - - // Set Commanded Velocity - ChassisSpeeds commandedVelocity = new ChassisSpeeds(-62.3, -23.44, 2.05); - // call limitAcceleration - ChassisSpeeds limitedVelocity = drive.limitAcceleration(commandedVelocity); - // Gets mag of limitedVelocity - double velocity = Math.sqrt((limitedVelocity.vxMetersPerSecond * limitedVelocity.vxMetersPerSecond) - + (limitedVelocity.vyMetersPerSecond * limitedVelocity.vyMetersPerSecond)); - - // Gets mag of actualVelocity - double actualVelocity = Math.sqrt( - (currentRobotStateChassisSpeed.vxMetersPerSecond * currentRobotStateChassisSpeed.vxMetersPerSecond) - + (currentRobotStateChassisSpeed.vyMetersPerSecond * currentRobotStateChassisSpeed.vyMetersPerSecond)); - // Check if acceleration is at maximum - assertEquals(-Constants.MAX_ACCELERATION * 0.05, velocity - actualVelocity, DELTA); - assertEquals(2.05, limitedVelocity.omegaRadiansPerSecond, DELTA); - } - - @Disabled("Limiting angular acceleration breaks test") - void testLimitAcceleration3() throws Exception { - - Field currentRobotState = RobotTracker.class.getDeclaredField("latencyCompensatedChassisSpeeds"); - currentRobotState.setAccessible(true); - - Field lastLoopTime = Drive.class.getDeclaredField("lastLoopTime"); - lastLoopTime.setAccessible(true); - - // Set time to 50 seconds - - lastLoopTime.set(drive, 50); - - Timer.setTime(50.2); - // Set Current Velocity - ChassisSpeeds currentRobotStateChassisSpeed = new ChassisSpeeds(-20, -10, 15); - currentRobotState.set(robotTracker, currentRobotStateChassisSpeed); - - // Set Commanded Velocity - ChassisSpeeds commandedVelocity = new ChassisSpeeds(-14.4, 20, -15.5); - // call limitAcceleration - ChassisSpeeds limitedVelocity = drive.limitAcceleration(commandedVelocity); - // Gets mag of limitedVelocity - double velocity = Math.hypot(limitedVelocity.vxMetersPerSecond - currentRobotStateChassisSpeed.vxMetersPerSecond, - limitedVelocity.vyMetersPerSecond - currentRobotStateChassisSpeed.vyMetersPerSecond); - - // Check if acceleration is at maximum - assertEquals(Constants.MAX_ACCELERATION * 0.05, velocity, DELTA); - assertEquals(-15.5, limitedVelocity.omegaRadiansPerSecond, DELTA); - } - */ - @Test void testAngleDiff1() { assertEquals(0, drive.getAngleDiff(0, 0)); From 5989fbfd5783b3d2172ea227bf3e70965204caa7 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Mon, 18 Jul 2022 21:16:52 -0700 Subject: [PATCH 2/8] fix some calculation issues when calculating the low and high accelerations --- src/main/java/frc/subsystem/Drive.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/subsystem/Drive.java b/src/main/java/frc/subsystem/Drive.java index 1b133566..2aac12b9 100644 --- a/src/main/java/frc/subsystem/Drive.java +++ b/src/main/java/frc/subsystem/Drive.java @@ -361,14 +361,14 @@ public void swerveDrive(@NotNull Translation2d targetRobotRelativeSpeed, // Also ensures that don't use move voltage decelerating. // TODO: Compensate for the fact that we'll be adding the voltage of our current velocity double maxDeceleration = min(accelerationLimit.turningTranslationDeceleration, - currentTranslationalVelocity.getNorm() * (DRIVE_PERIOD / 1000.0), + currentTranslationalVelocity.getNorm() / (DRIVE_PERIOD / 1000.0), (currentTranslationalVelocity.getNorm() * DRIVE_FEEDFORWARD.kv) / DRIVE_FEEDFORWARD.ka); MutableTranslation2d lowRobotAcceleration = new MutableTranslation2d(currentTranslationalVelocity). normalize().times(maxDeceleration).unaryMinus(); // Acceleration to reach the target velocity as quickly as possible double maxAcceleration = min(accelerationLimit.acceleration, - neededVelocityChange.minus(targetRobotRelativeSpeed).getNorm() * (DRIVE_PERIOD / 1000.0), + neededVelocityChange.getNorm() / (DRIVE_PERIOD / 1000.0), maxDesiredAcceleration, (currentTranslationalVelocity.minus(targetRobotRelativeSpeed).getNorm() * DRIVE_FEEDFORWARD.kv) / DRIVE_FEEDFORWARD.ka); @@ -380,7 +380,7 @@ public void swerveDrive(@NotNull Translation2d targetRobotRelativeSpeed, double currentAngleSpeed = currentSpeeds.omegaRadiansPerSecond; double neededAngleChange = targetAngularSpeed - currentAngleSpeed; double highAngularAcceleration = Math.copySign(min(accelerationLimit.angularAcceleration, - Math.abs(targetAngularSpeed - currentAngleSpeed) * (DRIVE_PERIOD / 1000.0)), neededAngleChange); + Math.abs(targetAngularSpeed - currentAngleSpeed) / (DRIVE_PERIOD / 1000.0)), neededAngleChange); double lowAngularAcceleration = Math.copySign(min( accelerationLimit.angularAcceleration, (Math.abs(currentAngleSpeed) * DRIVE_FEEDFORWARD.kv) / DRIVE_ROTATIONAL_KA), From 5d897048c533ef6659e39783b032f01362331206 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Thu, 21 Jul 2022 20:42:21 -0700 Subject: [PATCH 3/8] fix: resetI method resetting the wrong value --- src/main/java/frc/utility/wpimodified/PIDController.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/utility/wpimodified/PIDController.java b/src/main/java/frc/utility/wpimodified/PIDController.java index e0da5287..bfda9cfa 100644 --- a/src/main/java/frc/utility/wpimodified/PIDController.java +++ b/src/main/java/frc/utility/wpimodified/PIDController.java @@ -353,7 +353,7 @@ public void reset() { * Resets the integral term. */ public void resetI() { - m_prevError = 0; + m_totalError = 0; } @Override From b796c16392ab4dbbe07335d526636c289d6525c7 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Thu, 21 Jul 2022 20:51:59 -0700 Subject: [PATCH 4/8] new: add helper methods for the main swerveDrive() method. rename: swerveDrive(ChassisSpeeds chassisSpeeds) -> setCommandedVoltages(ChassisSpeeds commandVoltage) to prevent people from mistakenly using it. new: add an additionalAccleration argument to the main swerveDrive method. new: add a method to determine the maximum acceleration to prevent tipping fix: getTurnPidDeltaSpeed resetting the turnPID to often. fix: miscellaneous compilation issues --- src/main/java/frc/robot/Constants.java | 22 +- src/main/java/frc/robot/Robot.java | 12 +- src/main/java/frc/subsystem/Drive.java | 191 ++++++++++++------ .../java/frc/subsystem/VisionManager.java | 2 +- 4 files changed, 142 insertions(+), 85 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e6740409..97bbcee7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -86,14 +86,6 @@ public final class Constants { /** * Feed forward constants for the drivetrain. - *

- * 0 -> Left Front - *

- * 1 -> Left Back - *

- * 2 -> Right Front - *

- * 3 -> Right Back */ public static final SimpleMotorFeedforward DRIVE_FEEDFORWARD = new SimpleMotorFeedforward(0.17763, 2.7731, 0.5); public static final double DRIVE_ROTATIONAL_KA = 0.4; @@ -141,6 +133,7 @@ public final class Constants { public static final Translation2d SWERVE_RIGHT_FRONT_LOCATION = new Translation2d(0.307975, -0.307975); public static final Translation2d SWERVE_RIGHT_BACK_LOCATION = new Translation2d(-0.307975, -0.307975); + public static final Translation2d ROBOT_CENTER_OF_GRAVITY = new Translation2d(0, 0.1); public static final double DRIVE_HIGH_SPEED_M = 4.2; @SuppressWarnings("unused") public static final double DRIVE_HIGH_SPEED_IN = Units.metersToInches(DRIVE_HIGH_SPEED_M); @@ -154,24 +147,23 @@ public final class Constants { /** * Allowed Turn Error in degrees. */ - public static final double MAX_TURN_ERROR = 30; - - public static final double TURN_SPEED_LIMIT_TO_SHOOT = 3; + public static final double MAX_TURN_ERROR = 10; /** - * Allowed Turn Error in degrees. + * Maximum turn speed while aiming in radians per second. */ - public static final double MAX_PID_STOP_SPEED = 100; + public static final double TURN_SPEED_LIMIT_WHILE_AIMING = 4.0; + public static final double MAX_DRIVE_TURN_SPEED = 4.0; public static final int SWERVE_MOTOR_CURRENT_LIMIT = 15; public static final int SWERVE_DRIVE_MOTOR_CURRENT_LIMIT = 15; - public static final int SWERVE_DRIVE_VOLTAGE_LIMIT = 12; + public static final int SWERVE_DRIVE_VOLTAGE_LIMIT = 10; public static final double SWERVE_DRIVE_MOTOR_REDUCTION = 1 / 8.14; public static final Rotation2d CLIMB_LINEUP_ANGLE = Rotation2d.fromDegrees(180); // TurnPID - public static final double DEFAULT_TURN_P = IS_PRACTICE ? 10.0 : 10.0; + public static final double DEFAULT_TURN_P = 10.0; public static final double DEFAULT_TURN_I = 0; public static final double DEFAULT_TURN_D = 0.4; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c81b7ad2..ed61d96f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -10,7 +10,7 @@ import com.dacubeking.AutoBuilder.robot.serialization.Serializer; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.networktables.*; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID.RumbleType; @@ -340,7 +340,7 @@ public void teleopPeriodic() { } } else { // Stop the robot from moving if we're not issuing other commands to the drivebase - drive.swerveDrive(new ChassisSpeeds(0, 0, 0)); + drive.swerveDrive(new Translation2d(0, 0), 0, null); } } @@ -523,14 +523,10 @@ private void doNormalDriving() { if (xbox.getRawButton(XboxButtons.Y)) { drive.doHold(); } else { - drive.swerveDrive(NO_MOTION_CONTROLLER_INPUTS); + drive.swerveDrive(new Translation2d(), 0, null); } } else { - if (useFieldRelative) { - drive.swerveDriveFieldRelative(controllerDriveInputs); - } else { - drive.swerveDrive(controllerDriveInputs); - } + drive.swerveDrive(controllerDriveInputs, useFieldRelative); } } diff --git a/src/main/java/frc/subsystem/Drive.java b/src/main/java/frc/subsystem/Drive.java index 2aac12b9..a2880bfe 100644 --- a/src/main/java/frc/subsystem/Drive.java +++ b/src/main/java/frc/subsystem/Drive.java @@ -22,7 +22,6 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile.State; import edu.wpi.first.networktables.EntryListenerFlags; import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants; @@ -41,18 +40,10 @@ import static frc.robot.Constants.*; import static frc.utility.MathUtil.*; +import static frc.utility.geometry.GeometryUtils.angleOf; public final class Drive extends AbstractSubsystem { - - /** - * Last requested field relative acceleration - */ - public @NotNull volatile Translation2d lastAcceleration = new Translation2d(); - - // PID TUNING - final @NotNull NetworkTableInstance networkTableInstance = NetworkTableInstance.getDefault(); - final @NotNull NetworkTableEntry turnP = SmartDashboard.getEntry("TurnPIDP"); final @NotNull NetworkTableEntry turnI = SmartDashboard.getEntry("TurnPIDI"); final @NotNull NetworkTableEntry turnD = SmartDashboard.getEntry("TurnPIDD"); @@ -193,9 +184,9 @@ private Drive() { swerveDriveMotors[i].configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, Constants.SWERVE_DRIVE_MOTOR_CURRENT_LIMIT, Constants.SWERVE_DRIVE_MOTOR_CURRENT_LIMIT, 0)); - swerveDriveMotors[i].configVoltageCompSaturation(Constants.SWERVE_DRIVE_VOLTAGE_LIMIT); + swerveDriveMotors[i].configVoltageCompSaturation(Constants.SWERVE_DRIVE_VOLTAGE_LIMIT, 100); + - // This makes motors brake when no RPM is set swerveDriveMotors[i].setNeutralMode(NeutralMode.Coast); swerveMotors[i].setNeutralMode(NeutralMode.Coast); swerveMotors[i].setInverted(true); @@ -327,56 +318,78 @@ public void doHold() { setSwerveModuleStates(Constants.HOLD_MODULE_STATES, true); } - public void swerveDrive(@NotNull ControllerDriveInputs inputs) { + public void swerveDrive(@NotNull ControllerDriveInputs inputs, boolean useFieldRelative) { + Translation2d targetVelocity = new Translation2d(inputs.getX(), inputs.getY()).times(DRIVE_HIGH_SPEED_M); + double targetRotationSpeed = inputs.getRotation() * MAX_DRIVE_TURN_SPEED; + if (this.useFieldRelative && useFieldRelative) { + swerveDriveFieldRelative(targetVelocity, targetRotationSpeed, null); + } else { + swerveDrive(targetVelocity, targetRotationSpeed, null); + } + } - setDriveState(DriveState.TELEOP); + public void swerveDriveFieldRelative(@Nullable Translation2d targetFieldRelativeSpeed, + double targetAngularSpeed, + @Nullable Translation2d additionalFieldRelativeAcceleration) { + Rotation2d gyroAngle = RobotTracker.getInstance().getGyroAngle().unaryMinus(); + Translation2d targetRobotRelativeSpeed = null; + if (targetFieldRelativeSpeed != null) { + targetRobotRelativeSpeed = targetFieldRelativeSpeed.rotateBy(gyroAngle); + } + Translation2d additionalRobotRelativeAcceleration = null; + if (additionalFieldRelativeAcceleration != null) { + additionalRobotRelativeAcceleration = additionalFieldRelativeAcceleration.rotateBy(gyroAngle); + } - ChassisSpeeds chassisSpeeds = new ChassisSpeeds(DRIVE_HIGH_SPEED_M * inputs.getX(), - DRIVE_HIGH_SPEED_M * inputs.getY(), - inputs.getRotation() * 7); - swerveDrive(chassisSpeeds); + swerveDrive(targetRobotRelativeSpeed, targetAngularSpeed, additionalRobotRelativeAcceleration); } + private @NotNull Translation2d expectedAcceleration = new Translation2d(0, 0); + /** - * @param targetRobotRelativeSpeed The target speed of the robot in meters per second + * @param targetRobotRelativeSpeed The target speed of the robot in meters per second. If null, the robot will assume that you + * only want to control the robot via the acceleration. * @param targetAngularSpeed The target angular speed of the robot in radians per second - * @param maxDesiredAcceleration The maximum wanted acceleration of the robot in meters per second squared */ - public void swerveDrive(@NotNull Translation2d targetRobotRelativeSpeed, + @Contract(mutates = "this") + public void swerveDrive(@Nullable Translation2d targetRobotRelativeSpeed, double targetAngularSpeed, - double maxDesiredAcceleration) { + @Nullable Translation2d additionalRobotRelativeAcceleration) { final ChassisSpeeds currentSpeeds = SWERVE_DRIVE_KINEMATICS.toChassisSpeeds(getSwerveModuleStates()); final Translation2d currentTranslationalVelocity = new Translation2d(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond); - final Translation2d neededVelocityChange = targetRobotRelativeSpeed.minus(currentTranslationalVelocity); + final Translation2d neededVelocityChange = targetRobotRelativeSpeed != null ? + targetRobotRelativeSpeed.minus(currentTranslationalVelocity) : new Translation2d(0, 0); + // Create two different accelerations with: // lowRobotAcceleration = translational acceleration which will result in the least voltage used for translating // highRobotAcceleration = translational acceleration which will result in the most voltage used for translating // This acceleration will slow down the robot as quickly as possible, but don't overshoot a velocity of 0. - // Also ensures that don't use move voltage decelerating. - // TODO: Compensate for the fact that we'll be adding the voltage of our current velocity + // Also ensures that don't use extra voltage to decelerate. + final double currentTranslationalVelocityNorm = currentTranslationalVelocity.getNorm(); double maxDeceleration = min(accelerationLimit.turningTranslationDeceleration, - currentTranslationalVelocity.getNorm() / (DRIVE_PERIOD / 1000.0), - (currentTranslationalVelocity.getNorm() * DRIVE_FEEDFORWARD.kv) / DRIVE_FEEDFORWARD.ka); + currentTranslationalVelocityNorm / (DRIVE_PERIOD / 1000.0), + getMaxAccelerationToAvoidTipping(angleOf(currentTranslationalVelocity)), + (currentTranslationalVelocityNorm * DRIVE_FEEDFORWARD.kv) / DRIVE_FEEDFORWARD.ka); MutableTranslation2d lowRobotAcceleration = new MutableTranslation2d(currentTranslationalVelocity). normalize().times(maxDeceleration).unaryMinus(); // Acceleration to reach the target velocity as quickly as possible double maxAcceleration = min(accelerationLimit.acceleration, - neededVelocityChange.getNorm() / (DRIVE_PERIOD / 1000.0), - maxDesiredAcceleration, - (currentTranslationalVelocity.minus(targetRobotRelativeSpeed).getNorm() * DRIVE_FEEDFORWARD.kv) - / DRIVE_FEEDFORWARD.ka); - // Do we need to limit the acceleration, so we don't go over the max voltage here? (Do we need the above line?) - // or will the binary search below do that for us? + neededVelocityChange.getNorm() / (DRIVE_PERIOD / 1000.0)); MutableTranslation2d highRobotAcceleration = new MutableTranslation2d(neededVelocityChange). normalize().times(maxAcceleration); + if (additionalRobotRelativeAcceleration != null) { + highRobotAcceleration.plus(additionalRobotRelativeAcceleration); + } + + double currentAngleSpeed = currentSpeeds.omegaRadiansPerSecond; double neededAngleChange = targetAngularSpeed - currentAngleSpeed; double highAngularAcceleration = Math.copySign(min(accelerationLimit.angularAcceleration, @@ -391,12 +404,12 @@ public void swerveDrive(@NotNull Translation2d targetRobotRelativeSpeed, if (isUnderMaxVoltage(getChassisSpeedsFromAcceleration(highRobotAcceleration, highAngularAcceleration, currentSpeeds), maxUsableVoltage)) { // Everything is using their highest possible voltage, so we can't go any higher. - swerveDrive(getChassisSpeedsFromAcceleration(highRobotAcceleration, highAngularAcceleration, currentSpeeds)); + setCommandedVoltages(getChassisSpeedsFromAcceleration(highRobotAcceleration, highAngularAcceleration, currentSpeeds)); } else if (isUnderMaxVoltage( getChassisSpeedsFromAcceleration(lowRobotAcceleration, lowAngularAcceleration, currentSpeeds), maxUsableVoltage)) { // Everything is using their lowest possible voltage, so we can't go any lower. - swerveDrive(getChassisSpeedsFromAcceleration(lowRobotAcceleration, highAngularAcceleration, currentSpeeds)); + setCommandedVoltages(getChassisSpeedsFromAcceleration(lowRobotAcceleration, highAngularAcceleration, currentSpeeds)); } else { // Binary search for the angular acceleration that will result in the robot being under the max voltage. // Angular speed has priority over translational speed, so we'll assume that we're using the lowest translational @@ -435,10 +448,17 @@ public void swerveDrive(@NotNull Translation2d targetRobotRelativeSpeed, } // else, low has already been updated to be the correct value. } + // Should we be doing the anti-tip acceleration here? + double maxAccel = getMaxAccelerationToAvoidTipping(angleOf(low)); + if (low.getNorm() > maxAccel) { + low.normalize().times(maxAccel); + } + translationalAcceleration = low.getTranslation2d(); } + expectedAcceleration = translationalAcceleration; - swerveDrive(getChassisSpeedsFromAcceleration(translationalAcceleration, angularAcceleration, currentSpeeds)); + setCommandedVoltages(getChassisSpeedsFromAcceleration(translationalAcceleration, angularAcceleration, currentSpeeds)); } } @@ -479,18 +499,61 @@ private double getMaxUsableVoltage() { } - public void swerveDrive(ChassisSpeeds chassisSpeeds) { - SmartDashboard.putNumber("Drive Command X Force", chassisSpeeds.vxMetersPerSecond); - SmartDashboard.putNumber("Drive Command Y Force", chassisSpeeds.vyMetersPerSecond); - SmartDashboard.putNumber("Drive Command Rotational Force", chassisSpeeds.omegaRadiansPerSecond); + private static final double RIGHT_BACK_ANGLE_RADIANS; + private static final double RIGHT_FRONT_ANGLE_RADIANS; + private static final double LEFT_FRONT_ANGLE_RADIANS; + private static final double LEFT_BACK_LEFT_ANGLE_RADIANS; - SwerveModuleState[] moduleStates = SWERVE_DRIVE_KINEMATICS.toSwerveModuleStates(chassisSpeeds); + static { + RIGHT_FRONT_ANGLE_RADIANS = angleOf(SWERVE_RIGHT_FRONT_LOCATION.plus(ROBOT_CENTER_OF_GRAVITY)).getRadians(); + LEFT_FRONT_ANGLE_RADIANS = angleOf(SWERVE_LEFT_FRONT_LOCATION.plus(ROBOT_CENTER_OF_GRAVITY)).getRadians(); + LEFT_BACK_LEFT_ANGLE_RADIANS = angleOf(SWERVE_LEFT_BACK_LOCATION.plus(ROBOT_CENTER_OF_GRAVITY)).getRadians(); + RIGHT_BACK_ANGLE_RADIANS = angleOf(SWERVE_RIGHT_BACK_LOCATION.plus(ROBOT_CENTER_OF_GRAVITY)).getRadians(); + } + + /** + * This method assumes that the robot is a square. + * + * @param accelerationAngle The angle of the acceleration vector in radians. + * @return The maximum translational acceleration in that direction. + */ + @Contract(mutates = "", pure = true) + public double getMaxAccelerationToAvoidTipping(Rotation2d accelerationAngle) { + double accelerationAngleRadians = mod(accelerationAngle.getRadians() + Math.PI, Math.PI * 2) - Math.PI; + + double distanceCgToFrame; + + if (accelerationAngleRadians > RIGHT_BACK_ANGLE_RADIANS && accelerationAngleRadians <= LEFT_FRONT_ANGLE_RADIANS) { + distanceCgToFrame = Math.cos(accelerationAngleRadians) * + (SWERVE_RIGHT_FRONT_LOCATION.getX() - ROBOT_CENTER_OF_GRAVITY.getX()); + } else if (accelerationAngleRadians > LEFT_FRONT_ANGLE_RADIANS && accelerationAngleRadians <= LEFT_BACK_LEFT_ANGLE_RADIANS) { + distanceCgToFrame = Math.cos(accelerationAngleRadians - (Math.PI / 4)) * + (SWERVE_LEFT_FRONT_LOCATION.getY() - ROBOT_CENTER_OF_GRAVITY.getY()); + } else if (accelerationAngleRadians > LEFT_BACK_LEFT_ANGLE_RADIANS && accelerationAngleRadians <= RIGHT_FRONT_ANGLE_RADIANS) { + distanceCgToFrame = Math.cos(accelerationAngleRadians - (Math.PI / 2)) * + (SWERVE_LEFT_BACK_LOCATION.getX() - ROBOT_CENTER_OF_GRAVITY.getX()); + } else { + distanceCgToFrame = Math.cos(accelerationAngleRadians - (3 * (Math.PI / 4))) * + (SWERVE_RIGHT_BACK_LOCATION.getY() - ROBOT_CENTER_OF_GRAVITY.getY()); + } + + return distanceCgToFrame * 10; // TODO: Tune this + } + + + public void setCommandedVoltages(ChassisSpeeds commandVoltage) { + SmartDashboard.putNumber("Drive Command X Voltage", commandVoltage.vxMetersPerSecond); + SmartDashboard.putNumber("Drive Command Y Voltage", commandVoltage.vyMetersPerSecond); + SmartDashboard.putNumber("Drive Command Rotational Voltage", commandVoltage.omegaRadiansPerSecond); - boolean rotate = chassisSpeeds.vxMetersPerSecond != 0 || - chassisSpeeds.vyMetersPerSecond != 0 || - chassisSpeeds.omegaRadiansPerSecond != 0; + SwerveModuleState[] moduleStates = SWERVE_DRIVE_KINEMATICS.toSwerveModuleStates(commandVoltage); - SwerveDriveKinematics.desaturateWheelSpeeds(moduleStates, getMaxUsableVoltage()); + boolean rotate = commandVoltage.vxMetersPerSecond != 0 || + commandVoltage.vyMetersPerSecond != 0 || + commandVoltage.omegaRadiansPerSecond != 0; + + SwerveDriveKinematics.desaturateWheelSpeeds(moduleStates, getMaxUsableVoltage()); // In some rare cases, we can still + // command above the max voltage. Ex: if the available voltage drops faster than we can decelerate. setSwerveModuleStates(moduleStates, rotate); } @@ -676,7 +739,10 @@ private void updateRamsete() { adjustedSpeeds.omegaRadiansPerSecond = getTurnPidDeltaSpeed(autoAimingRotationGoal); } - swerveDrive(adjustedSpeeds); + swerveDrive(new Translation2d(adjustedSpeeds.vxMetersPerSecond, adjustedSpeeds.vyMetersPerSecond), + adjustedSpeeds.omegaRadiansPerSecond, + new Translation2d(goal.accelerationMetersPerSecondSq, 0) + .rotateBy(new Rotation2d(goal.curvatureRadPerMeter))); // Is this correct? if (swerveAutoController.atReference() && (Timer.getFPGATimestamp() - autoStartTime) >= currentAutoTrajectory.getTotalTimeSeconds()) { setDriveState(DriveState.DONE); stopMovement(); @@ -692,16 +758,23 @@ private void updateRamsete() { } } + /** + * @param autoAimingRotationGoal The goal to aim at (in radians) + * @return The speed to turn at (in radians/s) + */ private double getTurnPidDeltaSpeed(@NotNull TrapezoidProfile.State autoAimingRotationGoal) { turnPID.setSetpoint(autoAimingRotationGoal.position); - if (Timer.getFPGATimestamp() - 0.2 > lastTurnUpdate || turnPID.getPositionError() > Math.toRadians(7)) { + if (Timer.getFPGATimestamp() - 0.2 > lastTurnUpdate) { turnPID.reset(); } else if (turnPID.getPositionError() > Math.toRadians(7)) { + // This is basically an I-Zone turnPID.resetI(); } lastTurnUpdate = Timer.getFPGATimestamp(); - return turnPID.calculate(RobotTracker.getInstance().getGyroAngle().getRadians()) + autoAimingRotationGoal.velocity; + return Math.max( + turnPID.calculate(RobotTracker.getInstance().getGyroAngle().getRadians()) + autoAimingRotationGoal.velocity, + Constants.TURN_SPEED_LIMIT_WHILE_AIMING); } public void setAutoRotation(@NotNull Rotation2d rotation) { @@ -738,7 +811,7 @@ public void update() { updateRamsete(); break; case STOP: - swerveDrive(new ChassisSpeeds(0, 0, 0)); + swerveDrive(new Translation2d(0, 0), 0, null); } } @@ -809,22 +882,14 @@ public void updateTurn(ControllerDriveInputs controllerDriveInputs, State goal, double pidDeltaSpeed = getTurnPidDeltaSpeed(goal); - -// System.out.println( -// "turn error: " + Math.toDegrees(turnPID.getPositionError()) + " delta speed: " + Math.toDegrees(pidDeltaSpeed)); double curSpeed = RobotTracker.getInstance().getLatencyCompedChassisSpeeds().omegaRadiansPerSecond; + Translation2d targetVelocity = new Translation2d(controllerDriveInputs.getX(), controllerDriveInputs.getY()) + .times(DRIVE_HIGH_SPEED_M); if (useFieldRelative) { - swerveDrive(ChassisSpeeds.fromFieldRelativeSpeeds( - controllerDriveInputs.getX() * DRIVE_HIGH_SPEED_M * 0.45, - controllerDriveInputs.getY() * DRIVE_HIGH_SPEED_M * 0.45, - pidDeltaSpeed, - RobotTracker.getInstance().getGyroAngle())); + swerveDriveFieldRelative(targetVelocity, pidDeltaSpeed, null); } else { - swerveDrive(new ChassisSpeeds( - controllerDriveInputs.getX() * DRIVE_HIGH_SPEED_M * 0.45, - controllerDriveInputs.getY() * DRIVE_HIGH_SPEED_M * 0.45, - pidDeltaSpeed)); + swerveDrive(targetVelocity, pidDeltaSpeed, null); } if (Math.abs(goal.position - RobotTracker.getInstance().getGyroAngle().getRadians()) < turnErrorRadians) { @@ -920,6 +985,10 @@ public double[] getModuleSpeeds() { return speeds; } + public Translation2d getExpectedAcceleration() { + return expectedAcceleration; + } + /** * Checks if gyro is connected. If disconnected, switches to robot-centric drive for the rest of the match. Reports error to * driver station when this happens. diff --git a/src/main/java/frc/subsystem/VisionManager.java b/src/main/java/frc/subsystem/VisionManager.java index 8f28a81f..dfc48d52 100644 --- a/src/main/java/frc/subsystem/VisionManager.java +++ b/src/main/java/frc/subsystem/VisionManager.java @@ -686,7 +686,7 @@ private Translation2d getAdjustedTranslation(double predictAheadTime) { private Translation2d getAccel() { final @NotNull RobotTracker robotTracker = RobotTracker.getInstance(); - return Drive.getInstance().lastAcceleration; + return Drive.getInstance().getExpectedAcceleration(); //return robotTracker.getAcceleration(); //return ZERO; } From 3753d958395a5d70d8b3025aaae25654d72bffc0 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sun, 24 Jul 2022 12:55:57 -0700 Subject: [PATCH 5/8] ensure that the translational acceleration limiting is always active --- src/main/java/frc/subsystem/Drive.java | 32 +++++++++++++++----------- 1 file changed, 18 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/subsystem/Drive.java b/src/main/java/frc/subsystem/Drive.java index a2880bfe..e96ab39b 100644 --- a/src/main/java/frc/subsystem/Drive.java +++ b/src/main/java/frc/subsystem/Drive.java @@ -346,6 +346,7 @@ public void swerveDriveFieldRelative(@Nullable Translation2d targetFieldRelative } private @NotNull Translation2d expectedAcceleration = new Translation2d(0, 0); + private double expectedAngularAcceleration = 0; /** * @param targetRobotRelativeSpeed The target speed of the robot in meters per second. If null, the robot will assume that you @@ -401,21 +402,25 @@ public void swerveDrive(@Nullable Translation2d targetRobotRelativeSpeed, double maxUsableVoltage = getMaxUsableVoltage(); + MutableTranslation2d wantedRobotAcceleration; + double wantedAngularAcceleration; + if (isUnderMaxVoltage(getChassisSpeedsFromAcceleration(highRobotAcceleration, highAngularAcceleration, currentSpeeds), maxUsableVoltage)) { // Everything is using their highest possible voltage, so we can't go any higher. - setCommandedVoltages(getChassisSpeedsFromAcceleration(highRobotAcceleration, highAngularAcceleration, currentSpeeds)); + wantedRobotAcceleration = highRobotAcceleration; + wantedAngularAcceleration = highAngularAcceleration; } else if (isUnderMaxVoltage( getChassisSpeedsFromAcceleration(lowRobotAcceleration, lowAngularAcceleration, currentSpeeds), maxUsableVoltage)) { // Everything is using their lowest possible voltage, so we can't go any lower. - setCommandedVoltages(getChassisSpeedsFromAcceleration(lowRobotAcceleration, highAngularAcceleration, currentSpeeds)); + wantedRobotAcceleration = lowRobotAcceleration; + wantedAngularAcceleration = lowAngularAcceleration; } else { // Binary search for the angular acceleration that will result in the robot being under the max voltage. // Angular speed has priority over translational speed, so we'll assume that we're using the lowest translational // acceleration and try to find the highest angular acceleration that will result in the robot being under the max // voltage. - double angularAcceleration; { double low = lowAngularAcceleration; double high = highAngularAcceleration; @@ -428,20 +433,19 @@ public void swerveDrive(@Nullable Translation2d targetRobotRelativeSpeed, high = mid; } } - angularAcceleration = low; + wantedAngularAcceleration = low; } // Binary search for the highest translational acceleration that will result in the robot being under the max voltage. // We'll use the highest angular acceleration we found above, and try to maximize the translational acceleration // within the allowed voltage budget. - Translation2d translationalAcceleration; { final MutableTranslation2d low = lowRobotAcceleration; final MutableTranslation2d high = highRobotAcceleration; while (dist2(high.minus(low)) > Constants.DRIVE_TRANSLATIONAL_BINARY_SEARCH_TOLERANCE) { // This is technically changing the direction of the vector. Would this cause issues? low.plus(high).times(0.5); - ChassisSpeeds chassisSpeeds = getChassisSpeedsFromAcceleration(low, angularAcceleration, currentSpeeds); + ChassisSpeeds chassisSpeeds = getChassisSpeedsFromAcceleration(low, wantedAngularAcceleration, currentSpeeds); if (!isUnderMaxVoltage(chassisSpeeds, maxUsableVoltage)) { low.times(2).minus(high); // Undo changes to low high.plus(low).times(0.5); // Set high to what low was @@ -449,17 +453,17 @@ public void swerveDrive(@Nullable Translation2d targetRobotRelativeSpeed, } // Should we be doing the anti-tip acceleration here? - double maxAccel = getMaxAccelerationToAvoidTipping(angleOf(low)); - if (low.getNorm() > maxAccel) { - low.normalize().times(maxAccel); - } - - translationalAcceleration = low.getTranslation2d(); + wantedRobotAcceleration = low; } - expectedAcceleration = translationalAcceleration; + } - setCommandedVoltages(getChassisSpeedsFromAcceleration(translationalAcceleration, angularAcceleration, currentSpeeds)); + double maxAccel = getMaxAccelerationToAvoidTipping(angleOf(wantedRobotAcceleration)); + if (wantedRobotAcceleration.getNorm() > maxAccel) { + wantedRobotAcceleration.normalize().times(maxAccel); } + + setCommandedVoltages(getChassisSpeedsFromAcceleration(wantedRobotAcceleration.getTranslation2d(), + wantedAngularAcceleration, currentSpeeds)); } @Contract(pure = true) From d5886ca663163d5f64e1fbee3c5076b11300d521 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sun, 24 Jul 2022 12:59:29 -0700 Subject: [PATCH 6/8] log the expected accelerations and add logging for the actual angular acceleration --- src/main/java/frc/subsystem/Drive.java | 3 +++ src/main/java/frc/subsystem/RobotTracker.java | 19 ++++++++++++++++--- 2 files changed, 19 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/subsystem/Drive.java b/src/main/java/frc/subsystem/Drive.java index e96ab39b..c519c5b3 100644 --- a/src/main/java/frc/subsystem/Drive.java +++ b/src/main/java/frc/subsystem/Drive.java @@ -942,6 +942,9 @@ public void logData() { logData("Swerve Motor " + i + " Current", swerveMotors[i].getStatorCurrent()); } logData("Drive State", driveState.toString()); + logData("Expected Acceleration X", expectedAcceleration.getX()); + logData("Expected Acceleration Y", expectedAcceleration.getY()); + logData("Expected Acceleration Theta", expectedAngularAcceleration); } diff --git a/src/main/java/frc/subsystem/RobotTracker.java b/src/main/java/frc/subsystem/RobotTracker.java index 1778562d..ed4cf8f1 100644 --- a/src/main/java/frc/subsystem/RobotTracker.java +++ b/src/main/java/frc/subsystem/RobotTracker.java @@ -72,7 +72,9 @@ public final class RobotTracker extends AbstractSubsystem { private final ReentrantReadWriteLock lock = new ReentrantReadWriteLock(); - private Translation2d acceleration = new Translation2d(); + private @NotNull Translation2d acceleration = new Translation2d(); + + private double angularAcceleration = 0; private final @NotNull EvictingQueue poseHistory = EvictingQueue.create(50); private final @NotNull EvictingQueue chassisSpeedsHistory = EvictingQueue.create(12); @@ -259,8 +261,9 @@ public void update() { acceleration = new Translation2d( prevChassisSpeeds.vxMetersPerSecond - latestChassisSpeeds.vxMetersPerSecond, prevChassisSpeeds.vyMetersPerSecond - latestChassisSpeeds.vyMetersPerSecond) - .times(1.0 / (ROBOT_TRACKER_PERIOD * 2 * (chassisSpeedsHistory.size()))); // currentOdometryTime is the - // last loop time + .times(1.0 / (ROBOT_TRACKER_PERIOD * 2 * (chassisSpeedsHistory.size()))); + angularAcceleration = (latestChassisSpeeds.omegaRadiansPerSecond - prevChassisSpeeds.omegaRadiansPerSecond) + * (1.0 / (ROBOT_TRACKER_PERIOD * 2 * (chassisSpeedsHistory.size()))); } else { acceleration = new Translation2d(); } @@ -586,6 +589,7 @@ public void logData() { logData("Acceleration", acceleration.getNorm()); logData("Acceleration X", acceleration.getX()); logData("Acceleration Y", acceleration.getY()); + logData("Acceleration Theta", angularAcceleration); RobotPositionSender.addRobotPosition(new RobotState( getLastEstimatedPoseMeters(), @@ -666,4 +670,13 @@ public Translation2d getAcceleration() { lock.readLock().unlock(); } } + + public double getAngularAcceleration() { + lock.readLock().lock(); + try { + return angularAcceleration; + } finally { + lock.readLock().unlock(); + } + } } \ No newline at end of file From f98f60a01a52a5af7dd3a0f1d7a24634cb9a47c9 Mon Sep 17 00:00:00 2001 From: varun7654 Date: Sun, 24 Jul 2022 13:09:19 -0700 Subject: [PATCH 7/8] add a debug method in drive to control the robot by it's acceleration This method is useful to check if the drive constants are correct --- src/main/java/frc/subsystem/Drive.java | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/main/java/frc/subsystem/Drive.java b/src/main/java/frc/subsystem/Drive.java index c519c5b3..f5a525d1 100644 --- a/src/main/java/frc/subsystem/Drive.java +++ b/src/main/java/frc/subsystem/Drive.java @@ -318,6 +318,22 @@ public void doHold() { setSwerveModuleStates(Constants.HOLD_MODULE_STATES, true); } + /** + * Debug method to check if the drive constants are correct + * + * @param inputs the inputs to the drive + * @param useFieldRelative whether or not to use field relative directions + */ + public void swerveDriveAcceleration(@NotNull ControllerDriveInputs inputs, boolean useFieldRelative) { + Translation2d targetAcceleration = new Translation2d(inputs.getX(), inputs.getY()).times(4.0); + double targetRotationSpeed = inputs.getRotation() * MAX_DRIVE_TURN_SPEED; + if (this.useFieldRelative && useFieldRelative) { + swerveDriveFieldRelative(null, targetRotationSpeed, targetAcceleration); + } else { + swerveDrive(null, targetRotationSpeed, targetAcceleration); + } + } + public void swerveDrive(@NotNull ControllerDriveInputs inputs, boolean useFieldRelative) { Translation2d targetVelocity = new Translation2d(inputs.getX(), inputs.getY()).times(DRIVE_HIGH_SPEED_M); double targetRotationSpeed = inputs.getRotation() * MAX_DRIVE_TURN_SPEED; From dc804c0ec2cf0cb039242fe732c0d077661af0cc Mon Sep 17 00:00:00 2001 From: varun7654 Date: Tue, 2 Aug 2022 13:45:59 -0700 Subject: [PATCH 8/8] Use the voltage of the battery to determine the maximum usable voltage --- src/main/java/frc/robot/Constants.java | 4 +++- src/main/java/frc/subsystem/Drive.java | 3 ++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 97bbcee7..5e9ffbbe 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -90,6 +90,8 @@ public final class Constants { public static final SimpleMotorFeedforward DRIVE_FEEDFORWARD = new SimpleMotorFeedforward(0.17763, 2.7731, 0.5); public static final double DRIVE_ROTATIONAL_KA = 0.4; + public static final double VOLTAGE_OVERHEAD = 0.2; + /** * What the module states should be in hold mode. The wheels will be put in an X pattern to prevent the robot from moving. @@ -157,7 +159,7 @@ public final class Constants { public static final double MAX_DRIVE_TURN_SPEED = 4.0; public static final int SWERVE_MOTOR_CURRENT_LIMIT = 15; public static final int SWERVE_DRIVE_MOTOR_CURRENT_LIMIT = 15; - public static final int SWERVE_DRIVE_VOLTAGE_LIMIT = 10; + public static final int SWERVE_DRIVE_VOLTAGE_LIMIT = 14; public static final double SWERVE_DRIVE_MOTOR_REDUCTION = 1 / 8.14; public static final Rotation2d CLIMB_LINEUP_ANGLE = Rotation2d.fromDegrees(180); diff --git a/src/main/java/frc/subsystem/Drive.java b/src/main/java/frc/subsystem/Drive.java index f5a525d1..22064259 100644 --- a/src/main/java/frc/subsystem/Drive.java +++ b/src/main/java/frc/subsystem/Drive.java @@ -23,6 +23,7 @@ import edu.wpi.first.networktables.EntryListenerFlags; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants; import frc.utility.ControllerDriveInputs; @@ -494,7 +495,7 @@ private boolean isUnderMaxVoltage(ChassisSpeeds chassisSpeeds, double maxUsableV } private double getMaxUsableVoltage() { - return 10 - DRIVE_FEEDFORWARD.ks; + return RobotController.getBatteryVoltage() - DRIVE_FEEDFORWARD.ks - VOLTAGE_OVERHEAD; }