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

Fix Xbox controller diagonals

Browse Source 
- Add some Sysid data
- Make drive motors break in neutral
This commit is contained in:
nathanrsxtn
2022-01-29 14:49:18 -07:00
parent a69de7795d
commit fb1afe0f62
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/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 };
}
}