From 423f16f4111734db9a8ef52f3c751cf7cd42a878 Mon Sep 17 00:00:00 2001 From: Darrell Teegarden Date: Sat, 4 Oct 2025 11:53:02 -0700 Subject: [PATCH 1/3] Add DriveToPosition exercise --- .../java/competition/subsystems/drive/DriveSubsystem.java | 1 + .../drive/commands/TankDriveWithJoysticksCommand.java | 8 ++++++-- .../drive/commands/TogglePrecisionDriveCommand.java | 1 + .../basic_understanding/AObserveHowCommandsWork.java | 2 +- 4 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/main/java/competition/subsystems/drive/DriveSubsystem.java b/src/main/java/competition/subsystems/drive/DriveSubsystem.java index 07f4966..d3bed96 100644 --- a/src/main/java/competition/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/competition/subsystems/drive/DriveSubsystem.java @@ -22,6 +22,7 @@ public class DriveSubsystem extends BaseDriveSubsystem implements DataFrameRefre DoubleProperty dp; + public boolean isPrecisionModeOn = false; @Inject public DriveSubsystem(XCANMotorController.XCANMotorControllerFactory motorControllerFactory, ElectricalContract electricalContract, PropertyFactory pf) { log.info("Creating DriveSubsystem"); diff --git a/src/main/java/competition/subsystems/drive/commands/TankDriveWithJoysticksCommand.java b/src/main/java/competition/subsystems/drive/commands/TankDriveWithJoysticksCommand.java index 088dcc2..c706de8 100644 --- a/src/main/java/competition/subsystems/drive/commands/TankDriveWithJoysticksCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/TankDriveWithJoysticksCommand.java @@ -35,11 +35,15 @@ public void execute() { // Here's how to get how far the left joystick's Y-axis is pushed: double leftValue = operatorInterface.gamepad.getLeftVector().getY(); // TODO: get how far the RIGHT joystick's Y-axis is pushed as well - + double rightValue = operatorInterface.gamepad.getRightVector().getY(); // Pass values into the DriveSubsystem so it can control motors: // right now, this just sends the left power to the left part of the drive. // You'll need to give it a right power value as well. - drive.tankDrive(leftValue, 0); + if(drive.isPrecisionModeOn = true) { + leftValue = leftValue / 2; + rightValue = rightValue / 2; + } + drive.tankDrive(leftValue, rightValue); } } diff --git a/src/main/java/competition/subsystems/drive/commands/TogglePrecisionDriveCommand.java b/src/main/java/competition/subsystems/drive/commands/TogglePrecisionDriveCommand.java index 184a1c6..e934109 100644 --- a/src/main/java/competition/subsystems/drive/commands/TogglePrecisionDriveCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/TogglePrecisionDriveCommand.java @@ -18,6 +18,7 @@ public TogglePrecisionDriveCommand(DriveSubsystem driveSubsystem) { public void initialize() { // Here, you want to call the DriveSubsystem and tell it to change its precision // mode. + drive.isPrecisionModeOn = !drive.isPrecisionModeOn; // This means you'll need to add a new method into DriveSubsystem, and there are // two // major ways to do this: diff --git a/src/test/java/xbot/edubot/basic_understanding/AObserveHowCommandsWork.java b/src/test/java/xbot/edubot/basic_understanding/AObserveHowCommandsWork.java index ed5708d..8d160e9 100644 --- a/src/test/java/xbot/edubot/basic_understanding/AObserveHowCommandsWork.java +++ b/src/test/java/xbot/edubot/basic_understanding/AObserveHowCommandsWork.java @@ -7,7 +7,7 @@ import competition.BaseCompetitionTest; import xbot.common.command.XScheduler; -public class AObserveHowCommandsWork extends BaseCompetitionTest { +public class AObserveHowCommandsWork extends BaseCompetitionTest { protected Logger log; From 6aa83b5d0fb2aad24369ddd9452406e0e68834d6 Mon Sep 17 00:00:00 2001 From: victor Date: Sat, 18 Oct 2025 11:38:40 -0700 Subject: [PATCH 2/3] Add DriveToPosition excercise --- .../subsystems/drive/DriveSubsystem.java | 3 +- .../commands/DriveToPositionCommand.java | 37 ++++++++++++++++--- 2 files changed, 34 insertions(+), 6 deletions(-) diff --git a/src/main/java/competition/subsystems/drive/DriveSubsystem.java b/src/main/java/competition/subsystems/drive/DriveSubsystem.java index d3bed96..ecbe2a5 100644 --- a/src/main/java/competition/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/competition/subsystems/drive/DriveSubsystem.java @@ -37,13 +37,14 @@ public DriveSubsystem(XCANMotorController.XCANMotorControllerFactory motorContro dp = pf.createPersistentProperty("DriveSubsystem", 1.5); } - public void tankDrive(double leftPower, double rightPower) { + public double tankDrive(double leftPower, double rightPower) { // You'll need to take these power values and assign them to all of the motors. // As an example, here is some code that has the frontLeft motor to spin // according to the value of leftPower: frontLeft.setPower(leftPower); // TODO: Add code to set the right motors to the rightPower value. + return leftPower; } diff --git a/src/main/java/competition/subsystems/drive/commands/DriveToPositionCommand.java b/src/main/java/competition/subsystems/drive/commands/DriveToPositionCommand.java index 036e1f4..531d096 100644 --- a/src/main/java/competition/subsystems/drive/commands/DriveToPositionCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/DriveToPositionCommand.java @@ -9,17 +9,37 @@ public class DriveToPositionCommand extends BaseCommand { DriveSubsystem drive; + PoseSubsystem pose; + double currentPosition = 0; + + double goalPosition = 0; + + double error = 0; + + double power = 0; + + double p = 55; + + double d = 60; + + double errorOfPreviousStep = 0; + + double b = 0; + + @Inject public DriveToPositionCommand(DriveSubsystem driveSubsystem, PoseSubsystem pose) { this.drive = driveSubsystem; this.pose = pose; } - public void setTargetPosition(double position) { + public void setTargetPosition(double newGoalPosition) { // This method will be called by the test, and will give you a goal distance. // You'll need to remember this target position and use it in your calculations. + newGoalPosition = 5; + goalPosition = newGoalPosition; } @Override @@ -34,13 +54,20 @@ public void execute() { // - Hint: use pose.getPosition() to find out where you are // - Gets the robot stop (or at least be moving really really slowly) at the // target position - // How you do this is up to you. If you get stuck, ask a mentor or student for // some hints! - drive.tankDrive(0.25,0.25); - pose.getPosition(); + b= (d*(error-errorOfPreviousStep)); + currentPosition = pose.getPosition(); + error = goalPosition-currentPosition; + if(currentPosition > 5.000000000000001){ + drive.tankDrive(power,power); + } + if(currentPosition < 4.9999999999999999){ + drive.tankDrive(power,power); + } + power=p * error - b; + errorOfPreviousStep = error; } - @Override public boolean isFinished() { // Modify this to return true once you have met your goal, From 708b8a43358101af7cdaa64c82abf6bfa6058110 Mon Sep 17 00:00:00 2001 From: victor <> Date: Sat, 25 Oct 2025 14:24:47 -0700 Subject: [PATCH 3/3] fixed drivetoposition fixed drivetoposition :D --- .../commands/DriveToPositionCommand.java | 40 ++++++++----------- .../commands/TurnLeft90DegreesCommand.java | 11 ++++- 2 files changed, 26 insertions(+), 25 deletions(-) diff --git a/src/main/java/competition/subsystems/drive/commands/DriveToPositionCommand.java b/src/main/java/competition/subsystems/drive/commands/DriveToPositionCommand.java index 531d096..a245b83 100644 --- a/src/main/java/competition/subsystems/drive/commands/DriveToPositionCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/DriveToPositionCommand.java @@ -12,21 +12,18 @@ public class DriveToPositionCommand extends BaseCommand { PoseSubsystem pose; - double currentPosition = 0; - double goalPosition = 0; - double error = 0; + double d = 50; - double power = 0; + double p = 50; - double p = 55; + double previousPosition = 0; - double d = 60; + double currentPosition = 0; - double errorOfPreviousStep = 0; + double speed = 0; - double b = 0; @Inject @@ -38,7 +35,6 @@ public DriveToPositionCommand(DriveSubsystem driveSubsystem, PoseSubsystem pose) public void setTargetPosition(double newGoalPosition) { // This method will be called by the test, and will give you a goal distance. // You'll need to remember this target position and use it in your calculations. - newGoalPosition = 5; goalPosition = newGoalPosition; } @@ -55,24 +51,22 @@ public void execute() { // - Gets the robot stop (or at least be moving really really slowly) at the // target position // How you do this is up to you. If you get stuck, ask a mentor or student for - // some hints! - b= (d*(error-errorOfPreviousStep)); + // some hints. currentPosition = pose.getPosition(); - error = goalPosition-currentPosition; - if(currentPosition > 5.000000000000001){ - drive.tankDrive(power,power); - } - if(currentPosition < 4.9999999999999999){ - drive.tankDrive(power,power); - } - power=p * error - b; - errorOfPreviousStep = error; + double error = goalPosition - currentPosition; + speed = currentPosition - previousPosition; + double power = p * error - d * speed; + drive.tankDrive(power,power); + + previousPosition = currentPosition; } @Override - public boolean isFinished() { + public boolean isFinished( + ) { // Modify this to return true once you have met your goal, // and you're moving fairly slowly (ideally stopped) - return false; + boolean atGoalPosition = Math.abs(currentPosition - goalPosition) < 0.01; + boolean notMoving = Math.abs(speed) <0.001; + return atGoalPosition && notMoving; } - } diff --git a/src/main/java/competition/subsystems/drive/commands/TurnLeft90DegreesCommand.java b/src/main/java/competition/subsystems/drive/commands/TurnLeft90DegreesCommand.java index 392ae70..d4bdce9 100644 --- a/src/main/java/competition/subsystems/drive/commands/TurnLeft90DegreesCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/TurnLeft90DegreesCommand.java @@ -1,6 +1,7 @@ package competition.subsystems.drive.commands; import javax.inject.Inject; +import javax.swing.*; import xbot.common.command.BaseCommand; import competition.subsystems.drive.DriveSubsystem; @@ -23,6 +24,12 @@ public void initialize() { @Override public void execute() { - } -} + if (pose.getCurrentHeading().getDegrees() == 90) { + drive.tankDrive(-10, -10); + } + else{ + drive.tankDrive(2,2); + } + } +} \ No newline at end of file