Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 08:48:07 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'swerve' of https://github.com/Team4388/2022NoWayHome into swerve

Browse Source
This commit is contained in:
aarav18
2022-01-29 15:15:51 -07:00
parent bb28c6d436 fb1afe0f62
commit 665a832a72
12 changed files with 114441 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+1 -1
View File
@@ -63,7 +63,7 @@ public final class Constants {
// swerve configuration
public static final double NEUTRAL_DEADBAND = 0.04;
public static final double OPEN_LOOP_RAMP_RATE = 0.4;
public static final double OPEN_LOOP_RAMP_RATE = 0.2;
public static final int REMOTE_0 = 0;
// conversions
src/main/java/frc4388/robot/RobotContainer.java
+9 -14
View File
@@ -17,8 +17,12 @@ import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
@@ -30,6 +34,7 @@ import frc4388.robot.Constants.*;
import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.LEDPatterns;
import frc4388.utility.controller.DeadbandedRawXboxController;
import frc4388.utility.controller.DeadbandedXboxController;
/**
@@ -62,10 +67,9 @@ public class RobotContainer {
// drives the swerve drive with a two-axis input from the driver controller
m_robotSwerveDrive.setDefaultCommand(
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
clampJoystickAxes(
getDriverController().getLeftX(),
getDriverController().getLeftY()),
-getDriverController().getRightX(),
getDriverController().getLeftY(),
-getDriverController().getRightX(),
true),
m_robotSwerveDrive));
@@ -83,7 +87,7 @@ public class RobotContainer {
/* Driver Buttons */
new JoystickButton(getDriverController(), XboxController.Button.kY.value)
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.Y_BUTTON)
.whenPressed(() -> m_robotSwerveDrive.m_gyro.reset());
.whenPressed(m_robotSwerveDrive.m_gyro::reset);
new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.LEFT_BUMPER_BUTTON)
@@ -145,7 +149,7 @@ public class RobotContainer {
);
m_robotSwerveDrive.resetOdometry(testFirstPath.getInitialPose());
return swerveControllerCommand.andThen(() -> m_robotSwerveDrive.driveWithInput(new double[] {0, 0}, 0, true));
return swerveControllerCommand.andThen(() -> m_robotSwerveDrive.driveWithInput(0, 0, 0, true));
//return new InstantCommand();
}
@@ -170,13 +174,4 @@ public class RobotContainer {
public XboxController getOperatorController() {
return m_operatorXbox;
}
public static double[] clampJoystickAxes(double x, double y) {
double square_mag = x * x + y * y;
if (square_mag > 1.d) {
double mag = Math.sqrt(square_mag);
return new double[] { x / mag, y / mag };
}
return new double[] { x, y };
}
}
src/main/java/frc4388/robot/RobotMap.java
+4 -4
View File
@@ -92,13 +92,13 @@ public class RobotMap {
rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
leftFrontSteerMotor.setNeutralMode(NeutralMode.Brake);
leftFrontWheelMotor.setNeutralMode(NeutralMode.Coast);
leftFrontWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast
rightFrontSteerMotor.setNeutralMode(NeutralMode.Brake);
rightFrontWheelMotor.setNeutralMode(NeutralMode.Coast);
rightFrontWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast
leftBackSteerMotor.setNeutralMode(NeutralMode.Brake);
leftBackWheelMotor.setNeutralMode(NeutralMode.Coast);
leftBackWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast
rightBackSteerMotor.setNeutralMode(NeutralMode.Brake);
rightBackWheelMotor.setNeutralMode(NeutralMode.Coast);
rightBackWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast
leftFront = new SwerveModule(leftFrontWheelMotor, leftFrontSteerMotor, leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET);
leftBack = new SwerveModule(leftBackWheelMotor, leftBackSteerMotor, leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+26 -13
View File
@@ -5,6 +5,7 @@
package frc4388.robot.subsystems;
import com.ctre.phoenix.sensors.PigeonIMU;
import com.ctre.phoenix.sensors.PigeonIMUConfiguration;
import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame;
import com.ctre.phoenix.sensors.WPI_PigeonIMU;
import com.ctre.phoenix.sensors.PigeonIMU.FusionStatus;
@@ -19,8 +20,11 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.OIConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.Gains;
@@ -104,21 +108,30 @@ public class SwerveDrive extends SubsystemBase {
* @param rot Angular rate of the robot.
* @param fieldRelative Whether the provided x and y speeds are relative to the field.
*/
public void driveWithInput(double[] speeds, double rot, boolean fieldRelative)
public void driveWithInput(double speedX, double speedY, double rot, boolean fieldRelative)
{
if (speeds[0] == 0 && speeds[1] == 0 && rot == 0) ignoreAngles = true;
else ignoreAngles = false;
speeds[0] *= speeds[0] * speeds[0];
speeds[1] *= speeds[1] * speeds[1];
// Mechanism2d controllerMechanism = new Mechanism2d(2, 2);
// controllerMechanism.getRoot("Left Axes", speedX, speedY);
// double[] speedsClamped = clampJoystickAxes(speedX, speedY);
// double speedXClamped = speedsClamped[0];
// double speedYClamped = speedsClamped[1];
// controllerMechanism.getRoot("Left Axes (Clamped)", speedXClamped, speedYClamped);
// SmartDashboard.putData("Driver Controller", controllerMechanism);
// SmartDashboard.putNumberArray("Left Axes -> Left Axes (Clamped)", new double[] {speedX, speedY, speedXClamped, speedYClamped});
if (speedX == 0 && speedY == 0 && rot == 0) ignoreAngles = true;
else ignoreAngles = false;
Translation2d speed = new Translation2d(speedX, speedY);
double mag = speed.getNorm();
speed = speed.times(mag * speedAdjust);
double xSpeedMetersPerSecond = -speeds[0] * speedAdjust;
double ySpeedMetersPerSecond = speeds[1] * speedAdjust;
SwerveModuleState[] states =
m_kinematics.toSwerveModuleStates(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED));
setModuleStates(states);
double xSpeedMetersPerSecond = -speed.getX();
double ySpeedMetersPerSecond = speed.getY();
SwerveModuleState[] states =
m_kinematics.toSwerveModuleStates(
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED));
setModuleStates(states);
}
public void setModuleStates(SwerveModuleState[] desiredStates) {
src/main/java/frc4388/utility/controller/DeadbandedRawXboxController.java
+14 -7
View File
@@ -1,13 +1,20 @@
package frc4388.utility.controller;
import edu.wpi.first.math.MathUtil;
import frc4388.robot.Constants;
import static frc4388.robot.Constants.OIConstants.AXIS_DEADBAND;
import edu.wpi.first.math.geometry.Translation2d;
public class DeadbandedRawXboxController extends RawXboxController {
public DeadbandedRawXboxController(int port) { super(port); }
@Override public double getLeftX() { return applyDeadband(super.getLeftX()); }
@Override public double getLeftY() { return applyDeadband(super.getLeftY()); }
@Override public double getRightX() { return applyDeadband(super.getRightX()); }
@Override public double getRightY() { return applyDeadband(super.getRightY()); }
private static double applyDeadband(double value) { return MathUtil.applyDeadband(value, Constants.OIConstants.AXIS_DEADBAND); }
@Override public double getLeftX() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()).getX(); }
@Override public double getLeftY() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()).getY(); }
@Override public double getRightX() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()).getX(); }
@Override public double getRightY() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()).getY(); }
public static Translation2d skewToDeadzonedCircle(double x, double y) {
Translation2d translation2d = new Translation2d(x, y);
double magnitude = translation2d.getNorm();
if (magnitude >= 1) return translation2d.div(magnitude);
if (magnitude < AXIS_DEADBAND) return new Translation2d(0,0);
return translation2d;
}
}
src/main/java/frc4388/utility/controller/DeadbandedXboxController.java
+14 -7
View File
@@ -1,14 +1,21 @@
package frc4388.utility.controller;
import edu.wpi.first.math.MathUtil;
import static frc4388.robot.Constants.OIConstants.AXIS_DEADBAND;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj.XboxController;
import frc4388.robot.Constants;
public class DeadbandedXboxController extends XboxController {
public DeadbandedXboxController(int port) { super(port); }
@Override public double getLeftX() { return applyDeadband(super.getLeftX()); }
@Override public double getLeftY() { return applyDeadband(super.getLeftY()); }
@Override public double getRightX() { return applyDeadband(super.getRightX()); }
@Override public double getRightY() { return applyDeadband(super.getRightY()); }
private static double applyDeadband(double value) { return MathUtil.applyDeadband(value, Constants.OIConstants.AXIS_DEADBAND); }
@Override public double getLeftX() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()).getX(); }
@Override public double getLeftY() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()).getY(); }
@Override public double getRightX() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()).getX(); }
@Override public double getRightY() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()).getY(); }
public static Translation2d skewToDeadzonedCircle(double x, double y) {
Translation2d translation2d = new Translation2d(x, y);
double magnitude = translation2d.getNorm();
if (magnitude >= 1) return translation2d.div(magnitude);
if (magnitude < AXIS_DEADBAND) return new Translation2d(0,0);
return translation2d;
}
}