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

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/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) {