Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-09 08:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

commit stuff

Browse Source
This commit is contained in:
Aarav
2023-03-15 13:03:02 -06:00
parent f7b804680b
commit 29eeee1f7f
5 changed files with 24 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+4 -1
View File
@@ -27,7 +27,10 @@ public final class Constants {
public static final double MAX_ROT_SPEED = 1.5;
public static final double MIN_ROT_SPEED = 0.8;
public static double ROTATION_SPEED = MAX_ROT_SPEED;
public static double ROT_CORRECTION_SPEED = MIN_ROT_SPEED;
public static double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED;
public static final double CORRECTION_MIN = 10;
public static final double CORRECTION_MAX = 50;
public static final class IDs {
public static final int LEFT_FRONT_WHEEL_ID = 2;
src/main/java/frc4388/robot/RobotContainer.java
+4 -2
View File
@@ -17,6 +17,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.*;
import frc4388.robot.commands.AutoBalance;
@@ -122,7 +123,7 @@ public class RobotContainer {
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.toggleGear(), m_robotSwerveDrive));
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.toggleGear(m_robotArm.getArmRotation()-135), m_robotSwerveDrive));
// // .onFalse()
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
@@ -164,8 +165,9 @@ public class RobotContainer {
new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
.onTrue(new ParallelCommandGroup(
new InstantCommand(() -> m_robotClaw.toggle()),
new RunArmIn(m_robotArm),
new SequentialCommandGroup(
new RunArmIn(m_robotArm),
new WaitCommand(.25),
new PivotCommand(m_robotArm, 135)
)
));
src/main/java/frc4388/robot/commands/RunArmIn.java
+1 -1
View File
@@ -12,7 +12,7 @@ public class RunArmIn extends CommandBase {
public RunArmIn(Arm arm) {
m_arm = arm;
addRequirements(arm);
addRequirements();
}
// Called when the command is initially scheduled.
src/main/java/frc4388/robot/subsystems/Arm.java
+2 -8
View File
@@ -55,9 +55,9 @@ public class Arm extends SubsystemBase {
if ((degrees < 2 && vel < 0) || (degrees > 110 && vel > 0)) {
m_pivot.set(ControlMode.PercentOutput, 0);
} else if (degrees > 90 && vel > 0) {
m_pivot.set(ControlMode.PercentOutput, .15 * vel);
m_pivot.set(ControlMode.PercentOutput, .1 * vel);
} else {
m_pivot.set(ControlMode.PercentOutput, .3 * vel);
m_pivot.set(ControlMode.PercentOutput, .25 * vel);
}
}
@@ -169,12 +169,6 @@ public class Arm extends SubsystemBase {
boolean soft_limits = true;
public void killSoftLimits() {
resetTeleSoftLimit();
var pivot_soft = m_pivot.getSelectedSensorPosition();
var tele_soft = m_tele.getSelectedSensorPosition();
m_pivot.configForwardSoftLimitEnable(!soft_limits);
m_pivot.configReverseSoftLimitEnable(!soft_limits);
soft_limits = !soft_limits;
}
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+13 -7
View File
@@ -12,6 +12,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.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import frc4388.robot.Constants.SwerveDriveConstants;
@@ -37,7 +38,7 @@ public class SwerveDrive extends SubsystemBase {
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
public Rotation2d rotTarget = new Rotation2d();
public double rotTarget = 0;
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
/** Creates a new SwerveDrive. */
@@ -56,11 +57,13 @@ public class SwerveDrive extends SubsystemBase {
if (fieldRelative) {
double rot = 0;
if (rightStick.getNorm() > 0.01) {
rotTarget = gyro.getRotation2d();
if (rightStick.getNorm() > 0.05) {
rotTarget = gyro.getAngle();
rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
SmartDashboard.putBoolean("drift correction", false);
} else {
rot = rotTarget.minus(gyro.getRotation2d()).getRadians() * SwerveDriveConstants.ROT_CORRECTION_SPEED;
SmartDashboard.putBoolean("drift correction", true);
rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
}
// Use the left joystick to set speed. Apply a cubic curve and the set max speed.
@@ -95,7 +98,7 @@ public class SwerveDrive extends SubsystemBase {
public void resetGyro() {
gyro.reset();
rotTarget = new Rotation2d(0);
rotTarget = 0;
}
public void stopModules() {
@@ -113,11 +116,14 @@ public class SwerveDrive extends SubsystemBase {
// This method will be called once per scheduler run
}
public void toggleGear() {
if (this.speedAdjust == SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) {
public void toggleGear(double angle) {
if (this.speedAdjust == SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW
&& Math.abs(angle) < 2) {
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MAX;
} else {
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
}
}