Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 00:38:05 -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 14:40:43 -07:00
parent ce6d43878d 01b755eee0
commit a69de7795d
15 changed files with 334 additions and 458 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+1
View File
@@ -102,5 +102,6 @@ public final class Constants {
public static final class OIConstants {
public static final int XBOX_DRIVER_ID = 0;
public static final int XBOX_OPERATOR_ID = 1;
public static final double AXIS_DEADBAND = 0.1;
}
}
src/main/java/frc4388/robot/Robot.java
+2
View File
@@ -4,6 +4,7 @@
package frc4388.robot;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -115,6 +116,7 @@ public class Robot extends TimedRobot {
m_autonomousCommand.cancel();
}
m_robotTime.startMatchTime();
DriverStation.silenceJoystickConnectionWarning(true);
}
/**
src/main/java/frc4388/robot/RobotContainer.java
+23 -27
View File
@@ -18,6 +18,7 @@ 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.wpilibj.Joystick;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
@@ -29,8 +30,7 @@ import frc4388.robot.Constants.*;
import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.utility.LEDPatterns;
import frc4388.utility.controller.IHandController;
import frc4388.utility.controller.XboxController;
import frc4388.utility.controller.DeadbandedXboxController;
/**
* This class is where the bulk of the robot should be declared. Since
@@ -50,8 +50,8 @@ public class RobotContainer {
private final LED m_robotLED = new LED(m_robotMap.LEDController);
/* Controllers */
private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
@@ -62,10 +62,10 @@ public class RobotContainer {
// drives the swerve drive with a two-axis input from the driver controller
m_robotSwerveDrive.setDefaultCommand(
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
XboxController.ClampJoystickAxis(
getDriverController().getLeftXAxis(),
getDriverController().getLeftYAxis()),
-getDriverController().getRightXAxis(),
clampJoystickAxes(
getDriverController().getLeftX(),
getDriverController().getLeftY()),
-getDriverController().getRightX(),
true),
m_robotSwerveDrive));
@@ -81,26 +81,27 @@ public class RobotContainer {
*/
private void configureButtonBindings() {
/* Driver Buttons */
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
new JoystickButton(getDriverController(), XboxController.Button.kY.value)
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.Y_BUTTON)
.whenPressed(() -> m_robotSwerveDrive.m_gyro.reset());
new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON)
new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.LEFT_BUMPER_BUTTON)
.whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON)
new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.RIGHT_BUMPER_BUTTON)
.whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
new JoystickButton(getDriverController(), XboxController.Button.kA.value)
.whenPressed(() -> zeroOdometry(new Pose2d(0, 0, new Rotation2d(0))));
//.whenPressed(this::resetOdometry);
/* Operator Buttons */
// activates "Lit Mode"
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.A_BUTTON)
.whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
.whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
@@ -152,7 +153,7 @@ public class RobotContainer {
/**
* Add your docs here.
*/
public IHandController getDriverController() {
public XboxController getDriverController() {
return m_driverXbox;
}
@@ -166,21 +167,16 @@ public class RobotContainer {
/**
* Add your docs here.
*/
public IHandController getOperatorController() {
public XboxController getOperatorController() {
return m_operatorXbox;
}
/**
* Add your docs here.
*/
public Joystick getOperatorJoystick() {
return m_operatorXbox.getJoyStick();
}
/**
* Add your docs here.
*/
public Joystick getDriverJoystick() {
return m_driverXbox.getJoyStick();
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/subsystems/SwerveDrive.java
+6 -1
View File
@@ -18,6 +18,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds;
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.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc4388.robot.Constants.SwerveDriveConstants;
@@ -53,6 +54,8 @@ public class SwerveDrive extends SubsystemBase {
public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
public boolean ignoreAngles;
private final Field2d m_field = new Field2d();
public SwerveDrive(SwerveModule leftFront, SwerveModule leftBack, SwerveModule rightFront, SwerveModule rightBack, WPI_PigeonIMU gyro) {
// m_leftFrontSteerMotor = leftFrontSteerMotor;
// m_leftFrontWheelMotor = leftFrontWheelMotor;
@@ -89,7 +92,8 @@ public class SwerveDrive extends SubsystemBase {
VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
VecBuilder.fill(Units.degreesToRadians(0.01)),
VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
m_gyro.reset();
m_gyro.reset();
SmartDashboard.putData("Field", m_field);
}
//https://github.com/ZachOrr/MK3-Swerve-Example
/**
@@ -140,6 +144,7 @@ public class SwerveDrive extends SubsystemBase {
// m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_6_SensorFusion, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_1_General, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
super.periodic();
}