From 32da32e39c8b79032124b05f9028c4f4d1eea9c9 Mon Sep 17 00:00:00 2001 From: Braden Santiano Date: Mon, 27 Nov 2023 18:57:16 -0500 Subject: [PATCH 01/10] driving works --- .vscode/settings.json | 2 +- networktables.json | 1 + simgui-ds.json | 98 +++++++++++++++++++++++++++++++++++++++++++ simgui.json | 18 ++++++++ 4 files changed, 118 insertions(+), 1 deletion(-) create mode 100644 networktables.json create mode 100644 simgui-ds.json create mode 100644 simgui.json diff --git a/.vscode/settings.json b/.vscode/settings.json index cbd2e69..5e51565 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -14,7 +14,7 @@ "**/.factorypath": true, "**/*~": true }, - "wpilib.skipSelectSimulateExtension": true, + "wpilib.skipSelectSimulateExtension": false, "java.test.config": [ { "name": "WPIlibUnitTests", diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..c4b7efd --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,98 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..08662d3 --- /dev/null +++ b/simgui.json @@ -0,0 +1,18 @@ +{ + "HALProvider": { + "Other Devices": { + "RomiGyro": { + "header": { + "open": true + } + } + } + }, + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/Field": "Field2d", + "/SmartDashboard/SendableChooser[0]": "String Chooser" + } + } +} From d6ed2710edd1d381c5ebe6c1b82255780637ed4b Mon Sep 17 00:00:00 2001 From: Nilesh Agarwalla Date: Wed, 6 Dec 2023 19:12:31 -0500 Subject: [PATCH 02/10] Add gripper support (right stick, left/right) --- src/main/java/frc/robot/RobotContainer.java | 6 +- .../frc/robot/commands/GripperCommand.java | 64 +++++++++++++++++++ .../java/frc/robot/subsystems/Gripper.java | 40 ++++++++++++ 3 files changed, 109 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/commands/GripperCommand.java create mode 100644 src/main/java/frc/robot/subsystems/Gripper.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 87d582f..7836da4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,8 +21,10 @@ import frc.robot.commands.AutonomousDistance; import frc.robot.commands.AutonomousTime; import frc.robot.commands.DriveVision; +import frc.robot.commands.GripperCommand; import frc.robot.subsystems.Arm; import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Gripper; import frc.robot.subsystems.OnBoardIO; import frc.robot.subsystems.OnBoardIO.ChannelMode; import frc.robot.subsystems.drive.DriveIORomi; @@ -41,6 +43,7 @@ public class RobotContainer { // Create an arm sub-system private final Arm m_arm = new Arm(); // END: Setup arm + private final Gripper m_gripper = new Gripper(); // Assumes a XBox controller plugged into channnel 0 private final XboxController m_controller = new XboxController(0); @@ -108,8 +111,9 @@ private void configureButtonBindings() { // In this case, we want "forward" = "arm up" = positive value, but forward is reported as a // negative value from // the controller's stick, so we negate the returned value. - m_arm.setDefaultCommand(new ArmCommand(m_arm, () -> -m_controller.getRightY())); + m_arm.setDefaultCommand(new ArmCommand(m_arm, () -> m_controller.getRightX())); // END: Setup arm + m_gripper.setDefaultCommand(new GripperCommand(m_gripper, () -> m_controller.getRightY())); } /** diff --git a/src/main/java/frc/robot/commands/GripperCommand.java b/src/main/java/frc/robot/commands/GripperCommand.java new file mode 100644 index 0000000..be49167 --- /dev/null +++ b/src/main/java/frc/robot/commands/GripperCommand.java @@ -0,0 +1,64 @@ +package frc.robot.commands; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Gripper; +import java.util.function.DoubleSupplier; + +public class GripperCommand extends CommandBase { + // the gripper subsystem used by this comment + private final Gripper m_gripper; + + // the supplier of the desired speed + private final DoubleSupplier m_baseSpeed; + + // default start position for the gripper when the command is first scheduled + private final double m_defaultPosition = 0.5; + + private final double m_deadband = + 0.05; // to prevent stick drift, this value sets the min absolute value the speed needs to be + + // to prevent stick drift, this value sets the min absolute value the speed needs to be + // before we assume it is not zero + private final double m_speedScale = + 16; // to reduce stick sensitivity, this value indicates how much to scale the returned speed + // by + private final double m_minBaseRange = 0; // min range for the gripper's base + private final double m_maxBaseRange = 1; // max range for the gripper's base + + public GripperCommand(Gripper gripper, DoubleSupplier baseSpeed) { + m_gripper = gripper; + m_baseSpeed = baseSpeed; + + // To prevent scheduling conflicts, all commands need to indicate which sub-system(s) it uses + addRequirements(gripper); + } + + @Override + public void initialize() { + // We set the gripper's base position to the default position when this command is first + // scheduled + m_gripper.setClawPosition(m_defaultPosition); + } + + @Override + public void execute() { + // Variable that will store the new calculated gripper position + double newClawPosition; + + // Determine the requested speed, but ignoring inputs near-zero (i.e. +/= m_deadband) + double newBasePositionDelta = MathUtil.applyDeadband(m_baseSpeed.getAsDouble(), m_deadband); + // Scale the speed as desired to reduce sensitivity + newBasePositionDelta /= m_speedScale; + + // Calculate the gripper position by adding the computer speed to the gripper base's current + // position + newClawPosition = m_gripper.getClawPosition() + newBasePositionDelta; + + // Clamp the gripper's upper/lower position to the min/max range allowed + newClawPosition = MathUtil.clamp(newClawPosition, m_minBaseRange, m_maxBaseRange); + + // Finally, set the gripper base positon to the new calculated gripper position. + m_gripper.setClawPosition(newClawPosition); + } +} diff --git a/src/main/java/frc/robot/subsystems/Gripper.java b/src/main/java/frc/robot/subsystems/Gripper.java new file mode 100644 index 0000000..8742472 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Gripper.java @@ -0,0 +1,40 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.Servo; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Gripper extends SubsystemBase { + // This code assumes we are using the ROMI's external IO configuration and the arm claw servo is + // connected to Romi Pin EXT4 which is configured as PWM and mapped to port 2 + // https://docs.wpilib.org/en/stable/docs/romi-robot/web-ui.html#external-io-configuration + private final Servo m_armClawServo = new Servo(2); + + // Note: If you have a multi-axis arm, you could create another Servo object using a differemt PWM + // channel and add the corresponding methods for controlling the position. + // private final Servo m_armElbowServo = new Servo(2); + + /** + * Moves the arm claw to the desired position. -1.0 is max down, and 1.0 is max up + * + *

Note: The concept of "up and down" depends on how the servo is mounted to the arm. E.g. if + * the servo is flipped, the directions would be reversed. This module abstracts the hardware + * implementation details and follows the convention of 1.0 is up. + * + * @param position desired target position for arm claw [0.0 to 1.0] + */ + public void setClawPosition(double position) { + // Note: This code doesn't validate the requested position. If we don't want the arm to move + // within the entire range (e.g. due to physical constraints), we could clamp the position to + // the min/max values before setting the servo's position + m_armClawServo.set(position); + } + + /** + * @return the current desired target position for arm claw. Note: since the basic servo doesn't + * have any feedback, this is just the _requested_ position. The actual servo position is not + * known. + */ + public double getClawPosition() { + return m_armClawServo.get(); + } +} From e018d49f74b5db6261a4dad68f2affa991f50171 Mon Sep 17 00:00:00 2001 From: Braden Santiano Date: Wed, 6 Dec 2023 19:51:11 -0500 Subject: [PATCH 03/10] WIP arm/gripper --- simgui.json | 7 +++++++ src/main/java/frc/robot/commands/ArmCommand.java | 2 +- src/main/java/frc/robot/commands/GripperCommand.java | 6 +++++- src/main/java/frc/robot/subsystems/Arm.java | 2 +- src/main/java/frc/robot/subsystems/Gripper.java | 2 +- 5 files changed, 15 insertions(+), 4 deletions(-) diff --git a/simgui.json b/simgui.json index 08662d3..f5c6abf 100644 --- a/simgui.json +++ b/simgui.json @@ -14,5 +14,12 @@ "/SmartDashboard/Field": "Field2d", "/SmartDashboard/SendableChooser[0]": "String Chooser" } + }, + "NetworkTables": { + "transitory": { + "SmartDashboard": { + "open": true + } + } } } diff --git a/src/main/java/frc/robot/commands/ArmCommand.java b/src/main/java/frc/robot/commands/ArmCommand.java index 8e55b4c..21089f9 100644 --- a/src/main/java/frc/robot/commands/ArmCommand.java +++ b/src/main/java/frc/robot/commands/ArmCommand.java @@ -24,7 +24,7 @@ public class ArmCommand extends CommandBase { 16; // to reduce stick sensitivity, this value indicates how much to scale the returned speed // by private final double m_minBaseRange = 0; // min range for the arm's base - private final double m_maxBaseRange = 1; // max range for the arm's base + private final double m_maxBaseRange = 0.5; // max range for the arm's base public ArmCommand(Arm arm, DoubleSupplier baseSpeed) { m_arm = arm; diff --git a/src/main/java/frc/robot/commands/GripperCommand.java b/src/main/java/frc/robot/commands/GripperCommand.java index be49167..2bcc61e 100644 --- a/src/main/java/frc/robot/commands/GripperCommand.java +++ b/src/main/java/frc/robot/commands/GripperCommand.java @@ -1,6 +1,7 @@ package frc.robot.commands; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.Gripper; import java.util.function.DoubleSupplier; @@ -21,7 +22,7 @@ public class GripperCommand extends CommandBase { // to prevent stick drift, this value sets the min absolute value the speed needs to be // before we assume it is not zero private final double m_speedScale = - 16; // to reduce stick sensitivity, this value indicates how much to scale the returned speed + 64; // to reduce stick sensitivity, this value indicates how much to scale the returned speed // by private final double m_minBaseRange = 0; // min range for the gripper's base private final double m_maxBaseRange = 1; // max range for the gripper's base @@ -60,5 +61,8 @@ public void execute() { // Finally, set the gripper base positon to the new calculated gripper position. m_gripper.setClawPosition(newClawPosition); + + SmartDashboard.putNumber("Gripper Position", m_gripper.getClawPosition()); + SmartDashboard.putNumber("New Gripper Position", newClawPosition); } } diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index cb428e9..38c9fe5 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -7,7 +7,7 @@ public class Arm extends SubsystemBase { // This code assumes we are using the ROMI's external IO configuration and the arm base servo is // connected to Romi Pin EXT4 which is configured as PWM and mapped to port 3 // https://docs.wpilib.org/en/stable/docs/romi-robot/web-ui.html#external-io-configuration - private final Servo m_armBaseServo = new Servo(3); + private final Servo m_armBaseServo = new Servo(2); // Note: If you have a multi-axis arm, you could create another Servo object using a differemt PWM // channel and add the corresponding methods for controlling the position. diff --git a/src/main/java/frc/robot/subsystems/Gripper.java b/src/main/java/frc/robot/subsystems/Gripper.java index 8742472..8e6b6b1 100644 --- a/src/main/java/frc/robot/subsystems/Gripper.java +++ b/src/main/java/frc/robot/subsystems/Gripper.java @@ -7,7 +7,7 @@ public class Gripper extends SubsystemBase { // This code assumes we are using the ROMI's external IO configuration and the arm claw servo is // connected to Romi Pin EXT4 which is configured as PWM and mapped to port 2 // https://docs.wpilib.org/en/stable/docs/romi-robot/web-ui.html#external-io-configuration - private final Servo m_armClawServo = new Servo(2); + private final Servo m_armClawServo = new Servo(3); // Note: If you have a multi-axis arm, you could create another Servo object using a differemt PWM // channel and add the corresponding methods for controlling the position. From f60c0fd5024f2e3b462b2585f7c7411f7f00808f Mon Sep 17 00:00:00 2001 From: Braden Santiano Date: Wed, 13 Dec 2023 19:50:12 -0500 Subject: [PATCH 04/10] Fixed_teleop-need_auto --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7836da4..eb9c36d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -111,9 +111,9 @@ private void configureButtonBindings() { // In this case, we want "forward" = "arm up" = positive value, but forward is reported as a // negative value from // the controller's stick, so we negate the returned value. - m_arm.setDefaultCommand(new ArmCommand(m_arm, () -> m_controller.getRightX())); + m_arm.setDefaultCommand(new ArmCommand(m_arm, () -> m_controller.getRightY())); // END: Setup arm - m_gripper.setDefaultCommand(new GripperCommand(m_gripper, () -> m_controller.getRightY())); + m_gripper.setDefaultCommand(new GripperCommand(m_gripper, () -> m_controller.getRightX())); } /** From 111a3fa0ccc0cbbf6df8c72200932d82aef955cb Mon Sep 17 00:00:00 2001 From: Braden Santiano Date: Wed, 13 Dec 2023 20:01:40 -0500 Subject: [PATCH 05/10] fixedTeleop --- simgui.json | 6 ++++++ src/main/java/frc/robot/commands/ArcadeDrive.java | 2 +- src/main/java/frc/robot/commands/ArmCommand.java | 11 ++++++++--- src/main/java/frc/robot/commands/GripperCommand.java | 6 +++--- 4 files changed, 18 insertions(+), 7 deletions(-) diff --git a/simgui.json b/simgui.json index f5c6abf..0bf79a1 100644 --- a/simgui.json +++ b/simgui.json @@ -17,7 +17,13 @@ }, "NetworkTables": { "transitory": { + "Shuffleboard": { + "open": true + }, "SmartDashboard": { + "SendableChooser[0]": { + "open": true + }, "open": true } } diff --git a/src/main/java/frc/robot/commands/ArcadeDrive.java b/src/main/java/frc/robot/commands/ArcadeDrive.java index 1fd9c85..9849b1c 100644 --- a/src/main/java/frc/robot/commands/ArcadeDrive.java +++ b/src/main/java/frc/robot/commands/ArcadeDrive.java @@ -38,7 +38,7 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_drivetrain.arcadeDriveKinematics(m_xaxisSpeedSupplier.get(), m_zaxisRotateSupplier.get()); + m_drivetrain.arcadeDriveKinematics(m_xaxisSpeedSupplier.get()*0.5, m_zaxisRotateSupplier.get()*0.5); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/ArmCommand.java b/src/main/java/frc/robot/commands/ArmCommand.java index 21089f9..a030c34 100644 --- a/src/main/java/frc/robot/commands/ArmCommand.java +++ b/src/main/java/frc/robot/commands/ArmCommand.java @@ -1,9 +1,11 @@ package frc.robot.commands; +import java.util.function.DoubleSupplier; + import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.Arm; -import java.util.function.DoubleSupplier; public class ArmCommand extends CommandBase { // the arm subsystem used by this comment @@ -21,9 +23,9 @@ public class ArmCommand extends CommandBase { // to prevent stick drift, this value sets the min absolute value the speed needs to be // before we assume it is not zero private final double m_speedScale = - 16; // to reduce stick sensitivity, this value indicates how much to scale the returned speed + 64; // to reduce stick sensitivity, this value indicates how much to scale the returned speed // by - private final double m_minBaseRange = 0; // min range for the arm's base + private final double m_minBaseRange = 0.2; // min range for the arm's base private final double m_maxBaseRange = 0.5; // max range for the arm's base public ArmCommand(Arm arm, DoubleSupplier baseSpeed) { @@ -58,5 +60,8 @@ public void execute() { // Finally, set the arm base positon to the new calculated arm position. m_arm.setArmBasePosition(newArmPosition); + + SmartDashboard.putNumber("Arm Position", m_arm.getArmBasePosition()); + SmartDashboard.putNumber("New Arm Position", newArmPosition); } } diff --git a/src/main/java/frc/robot/commands/GripperCommand.java b/src/main/java/frc/robot/commands/GripperCommand.java index 2bcc61e..809da2c 100644 --- a/src/main/java/frc/robot/commands/GripperCommand.java +++ b/src/main/java/frc/robot/commands/GripperCommand.java @@ -22,10 +22,10 @@ public class GripperCommand extends CommandBase { // to prevent stick drift, this value sets the min absolute value the speed needs to be // before we assume it is not zero private final double m_speedScale = - 64; // to reduce stick sensitivity, this value indicates how much to scale the returned speed + 16; // to reduce stick sensitivity, this value indicates how much to scale the returned speed // by - private final double m_minBaseRange = 0; // min range for the gripper's base - private final double m_maxBaseRange = 1; // max range for the gripper's base + private final double m_minBaseRange = -1.50; // min range for the gripper's base + private final double m_maxBaseRange = 4.00; // max range for the gripper's base public GripperCommand(Gripper gripper, DoubleSupplier baseSpeed) { m_gripper = gripper; From bec7df071d63c0c0342cde24820465969345fe1b Mon Sep 17 00:00:00 2001 From: Braden Santiano Date: Thu, 14 Dec 2023 18:48:51 -0500 Subject: [PATCH 06/10] FinishedRomi --- src/main/java/frc/robot/commands/AutonomousDistance.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/AutonomousDistance.java b/src/main/java/frc/robot/commands/AutonomousDistance.java index d8ca422..99e880d 100644 --- a/src/main/java/frc/robot/commands/AutonomousDistance.java +++ b/src/main/java/frc/robot/commands/AutonomousDistance.java @@ -16,7 +16,7 @@ public class AutonomousDistance extends SequentialCommandGroup { */ public AutonomousDistance(Drivetrain drivetrain) { addCommands( - new DriveDistance(-0.5, 10, drivetrain), + new DriveDistance(-0.6, 12, drivetrain), new TurnDegrees(-0.5, 180, drivetrain), new DriveDistance(-0.5, 10, drivetrain), new TurnDegrees(0.5, 180, drivetrain)); From 119c1a061a0ef9c1b6cddc5cae3cec99d027d0f8 Mon Sep 17 00:00:00 2001 From: Braden Santiano Date: Thu, 14 Dec 2023 18:49:15 -0500 Subject: [PATCH 07/10] FinalRomiProgram --- build.gradle | 2 +- src/main/java/frc/robot/commands/ArcadeDrive.java | 2 +- src/main/java/frc/robot/commands/ArmCommand.java | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/build.gradle b/build.gradle index 81d2b25..3a31c7e 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2023.4.2" + id "edu.wpi.first.GradleRIO" version "2023.4.3" // START: Setup spotless id 'com.diffplug.spotless' version '6.18.0' // END: Setup spotless diff --git a/src/main/java/frc/robot/commands/ArcadeDrive.java b/src/main/java/frc/robot/commands/ArcadeDrive.java index 9849b1c..15b9ad1 100644 --- a/src/main/java/frc/robot/commands/ArcadeDrive.java +++ b/src/main/java/frc/robot/commands/ArcadeDrive.java @@ -38,7 +38,7 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_drivetrain.arcadeDriveKinematics(m_xaxisSpeedSupplier.get()*0.5, m_zaxisRotateSupplier.get()*0.5); + m_drivetrain.arcadeDriveKinematics(m_xaxisSpeedSupplier.get()*0.75, m_zaxisRotateSupplier.get()*0.5); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/ArmCommand.java b/src/main/java/frc/robot/commands/ArmCommand.java index a030c34..f369cf4 100644 --- a/src/main/java/frc/robot/commands/ArmCommand.java +++ b/src/main/java/frc/robot/commands/ArmCommand.java @@ -26,7 +26,7 @@ public class ArmCommand extends CommandBase { 64; // to reduce stick sensitivity, this value indicates how much to scale the returned speed // by private final double m_minBaseRange = 0.2; // min range for the arm's base - private final double m_maxBaseRange = 0.5; // max range for the arm's base + private final double m_maxBaseRange = 0.525; // max range for the arm's base public ArmCommand(Arm arm, DoubleSupplier baseSpeed) { m_arm = arm; From 44e65328fb98c510b8aa24b207a1d00ca4497ae6 Mon Sep 17 00:00:00 2001 From: Braden Santiano Date: Thu, 14 Dec 2023 19:32:55 -0500 Subject: [PATCH 08/10] autoAndProgramFinished --- src/main/java/frc/robot/commands/AutonomousDistance.java | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/commands/AutonomousDistance.java b/src/main/java/frc/robot/commands/AutonomousDistance.java index 99e880d..13650df 100644 --- a/src/main/java/frc/robot/commands/AutonomousDistance.java +++ b/src/main/java/frc/robot/commands/AutonomousDistance.java @@ -16,9 +16,10 @@ public class AutonomousDistance extends SequentialCommandGroup { */ public AutonomousDistance(Drivetrain drivetrain) { addCommands( - new DriveDistance(-0.6, 12, drivetrain), - new TurnDegrees(-0.5, 180, drivetrain), - new DriveDistance(-0.5, 10, drivetrain), - new TurnDegrees(0.5, 180, drivetrain)); + new DriveDistance(0.65, 11, drivetrain), + new TurnDegrees(0.3, 80, drivetrain), + new DriveDistance(0.5, 5, drivetrain), + new TurnDegrees(0.35, 180, drivetrain) + ); } } From 90f14b1aa7fc8521344f54f48b913cadaa0f78da Mon Sep 17 00:00:00 2001 From: Braden Santiano Date: Mon, 18 Dec 2023 17:29:53 -0500 Subject: [PATCH 09/10] TrulyAbsolutlyTotallyFinishedRomiCode --- src/main/java/frc/robot/commands/ArmCommand.java | 2 +- src/main/java/frc/robot/commands/AutonomousDistance.java | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/commands/ArmCommand.java b/src/main/java/frc/robot/commands/ArmCommand.java index f369cf4..42999eb 100644 --- a/src/main/java/frc/robot/commands/ArmCommand.java +++ b/src/main/java/frc/robot/commands/ArmCommand.java @@ -15,7 +15,7 @@ public class ArmCommand extends CommandBase { private final DoubleSupplier m_baseSpeed; // default start position for the arm when the command is first scheduled - private final double m_defaultPosition = 0.5; + private final double m_defaultPosition = 0.2; private final double m_deadband = 0.05; // to prevent stick drift, this value sets the min absolute value the speed needs to be diff --git a/src/main/java/frc/robot/commands/AutonomousDistance.java b/src/main/java/frc/robot/commands/AutonomousDistance.java index 13650df..10e71f5 100644 --- a/src/main/java/frc/robot/commands/AutonomousDistance.java +++ b/src/main/java/frc/robot/commands/AutonomousDistance.java @@ -16,10 +16,10 @@ public class AutonomousDistance extends SequentialCommandGroup { */ public AutonomousDistance(Drivetrain drivetrain) { addCommands( - new DriveDistance(0.65, 11, drivetrain), - new TurnDegrees(0.3, 80, drivetrain), - new DriveDistance(0.5, 5, drivetrain), - new TurnDegrees(0.35, 180, drivetrain) + new DriveDistance(0.65, 11, drivetrain) + //new TurnDegrees(0.3, 80, drivetrain), + //new DriveDistance(0.5, 5, drivetrain), + //new TurnDegrees(0.35, 90, drivetrain) ); } } From fa47dfb1d5970dc27dfc3626565a2bbdd8858476 Mon Sep 17 00:00:00 2001 From: Braden Santiano Date: Wed, 3 Jan 2024 19:13:36 -0500 Subject: [PATCH 10/10] postGameLagCorrections --- build.gradle | 2 +- src/main/java/frc/robot/commands/ArcadeDrive.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/build.gradle b/build.gradle index 3a31c7e..9934460 100644 --- a/build.gradle +++ b/build.gradle @@ -47,7 +47,7 @@ wpi.sim.addGui().defaultEnabled = true wpi.sim.addDriverstation() //Sets the websocket client remote host. -wpi.sim.envVar("HALSIMWS_HOST", "10.0.0.2") +wpi.sim.envVar("HALSIMWS_HOST", "192.168.50.221") wpi.sim.addWebsocketsServer().defaultEnabled = true wpi.sim.addWebsocketsClient().defaultEnabled = true diff --git a/src/main/java/frc/robot/commands/ArcadeDrive.java b/src/main/java/frc/robot/commands/ArcadeDrive.java index 15b9ad1..1fd9c85 100644 --- a/src/main/java/frc/robot/commands/ArcadeDrive.java +++ b/src/main/java/frc/robot/commands/ArcadeDrive.java @@ -38,7 +38,7 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_drivetrain.arcadeDriveKinematics(m_xaxisSpeedSupplier.get()*0.75, m_zaxisRotateSupplier.get()*0.5); + m_drivetrain.arcadeDriveKinematics(m_xaxisSpeedSupplier.get(), m_zaxisRotateSupplier.get()); } // Called once the command ends or is interrupted.