From 805b17d64e84a5b35f33165fa75290de462ca277 Mon Sep 17 00:00:00 2001 From: Clulogh <197954988+Clulogh@users.noreply.github.com> Date: Sat, 1 Nov 2025 14:47:03 -0700 Subject: [PATCH 1/5] Worked on the rotation ciriculum, however many errors and mistakes will fix after --- .../commands/TurnLeft90DegreesCommand.java | 26 +++++++++++++++++++ .../subsystems/pose/PoseSubsystem.java | 4 +-- 2 files changed, 28 insertions(+), 2 deletions(-) diff --git a/src/main/java/competition/subsystems/drive/commands/TurnLeft90DegreesCommand.java b/src/main/java/competition/subsystems/drive/commands/TurnLeft90DegreesCommand.java index 392ae70..fa6b2be 100644 --- a/src/main/java/competition/subsystems/drive/commands/TurnLeft90DegreesCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/TurnLeft90DegreesCommand.java @@ -5,24 +5,50 @@ import xbot.common.command.BaseCommand; import competition.subsystems.drive.DriveSubsystem; import competition.subsystems.pose.PoseSubsystem; +import xbot.common.properties.DoubleProperty; public class TurnLeft90DegreesCommand extends BaseCommand { DriveSubsystem drive; PoseSubsystem pose; + double target; + //double error; + double previousPos = 0; + double speeD; + @Inject public TurnLeft90DegreesCommand(DriveSubsystem driveSubsystem, PoseSubsystem pose) { this.drive = driveSubsystem; this.pose = pose; + } @Override public void initialize() { + target = pose.getCurrentHeading().getDegrees() + 90; //90 is the target not the error + //you need to reduce the +90, so it goes down as it gets closer to target. If it keeps +90 then it will pass the target + + } + //circle is 360, 90 is one quarter of the circle, get degree is turning in place so no moving. @Override public void execute() { + //turn 90 degrees instantly + double power = pose.getCurrentHeading().getDegrees(); + double error = pose.getCurrentHeading().getDegrees() - previousPos; + + if (error < .001) { + drive.tankDrive(1, speeD); + + double speed = pose.getCurrentHeading().getDegrees() - previousPos; + double power = pose.getCurrentHeading().getDegrees() * error - previousPos * speed; + + + + } + } diff --git a/src/main/java/competition/subsystems/pose/PoseSubsystem.java b/src/main/java/competition/subsystems/pose/PoseSubsystem.java index 03d09cb..b8ca513 100644 --- a/src/main/java/competition/subsystems/pose/PoseSubsystem.java +++ b/src/main/java/competition/subsystems/pose/PoseSubsystem.java @@ -16,7 +16,7 @@ public class PoseSubsystem extends BasePoseSubsystem { private final DriveSubsystem drive; public double scalingFactorFromRotationsToMeters = 0.5; - + @Inject public PoseSubsystem(XGyroFactory gyroFactory, PropertyFactory propManager, DriveSubsystem drive) { super(gyroFactory, propManager); @@ -24,7 +24,7 @@ public PoseSubsystem(XGyroFactory gyroFactory, PropertyFactory propManager, Driv } public double getPosition() { - return (getLeftDriveDistance() + getRightDriveDistance()) / 2.0; + return (getLeftDriveDistance() + getRightDriveDistance()) / 2.0; } @Override From 6fa31b4f21b8b97b0ffecd5fe44134c6704498be Mon Sep 17 00:00:00 2001 From: Clulogh <197954988+Clulogh@users.noreply.github.com> Date: Sat, 15 Nov 2025 14:32:33 -0800 Subject: [PATCH 2/5] Updated turn left to 90, successfully worked --- .../commands/TurnLeft90DegreesCommand.java | 32 +++++++------------ 1 file changed, 12 insertions(+), 20 deletions(-) diff --git a/src/main/java/competition/subsystems/drive/commands/TurnLeft90DegreesCommand.java b/src/main/java/competition/subsystems/drive/commands/TurnLeft90DegreesCommand.java index fa6b2be..fac1288 100644 --- a/src/main/java/competition/subsystems/drive/commands/TurnLeft90DegreesCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/TurnLeft90DegreesCommand.java @@ -6,6 +6,7 @@ import competition.subsystems.drive.DriveSubsystem; import competition.subsystems.pose.PoseSubsystem; import xbot.common.properties.DoubleProperty; +import xbot.common.properties.PropertyFactory; public class TurnLeft90DegreesCommand extends BaseCommand { @@ -13,42 +14,33 @@ public class TurnLeft90DegreesCommand extends BaseCommand { PoseSubsystem pose; double target; - //double error; double previousPos = 0; - double speeD; @Inject public TurnLeft90DegreesCommand(DriveSubsystem driveSubsystem, PoseSubsystem pose) { this.drive = driveSubsystem; this.pose = pose; + } @Override public void initialize() { target = pose.getCurrentHeading().getDegrees() + 90; //90 is the target not the error - //you need to reduce the +90, so it goes down as it gets closer to target. If it keeps +90 then it will pass the target - + // 150+90=240 .. 240-360=-120 might have to minus 360 to target + if (target > 180) { //target is out of bounds so bigger than 180. + target = target - 360; //since its out of bounds and bigger than 180, you have to subtract it by 360 + } } - //circle is 360, 90 is one quarter of the circle, get degree is turning in place so no moving. @Override public void execute() { - //turn 90 degrees instantly - double power = pose.getCurrentHeading().getDegrees(); - double error = pose.getCurrentHeading().getDegrees() - previousPos; - - if (error < .001) { - drive.tankDrive(1, speeD); - + //turn 90 degrees double speed = pose.getCurrentHeading().getDegrees() - previousPos; - double power = pose.getCurrentHeading().getDegrees() * error - previousPos * speed; - - - - - } - + double power = target - speed; + double error = target - pose.getCurrentHeading().getDegrees(); + // higher distance error higher power,more power less speed ||| power = error - speed + drive.tankDrive(-power, power); -} + }} From 8cf3866b3a5c36bb9c19755e6e0330063a87246a Mon Sep 17 00:00:00 2001 From: Clulogh <197954988+Clulogh@users.noreply.github.com> Date: Sat, 22 Nov 2025 13:07:42 -0800 Subject: [PATCH 3/5] updates --- .../commands/DriveToOrientationCommand.java | 16 +++++++++++++--- .../drive/commands/TurnLeft90DegreesCommand.java | 11 ++++++++++- 2 files changed, 23 insertions(+), 4 deletions(-) diff --git a/src/main/java/competition/subsystems/drive/commands/DriveToOrientationCommand.java b/src/main/java/competition/subsystems/drive/commands/DriveToOrientationCommand.java index 832771f..3470e5c 100644 --- a/src/main/java/competition/subsystems/drive/commands/DriveToOrientationCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/DriveToOrientationCommand.java @@ -2,26 +2,35 @@ import javax.inject.Inject; +import competition.subsystems.pose.PoseSubsystem; import xbot.common.command.BaseCommand; import competition.subsystems.drive.DriveSubsystem; public class DriveToOrientationCommand extends BaseCommand { DriveSubsystem drive; + PoseSubsystem pose; + double goal; @Inject - public DriveToOrientationCommand(DriveSubsystem driveSubsystem) { + public DriveToOrientationCommand(DriveSubsystem driveSubsystem, PoseSubsystem pose) { this.drive = driveSubsystem; + this.pose = pose; } public void setTargetHeading(double heading) { + goal = heading; // heading= direction you want face in degrees // This method will be called by the test, and will give you a goal heading. + + //if (target > 180) { + // target = target - 360; //get the position between 0 to 180 + // You'll need to remember this target position and use it in your calculations. } @Override public void initialize() { - // If you have some one-time setup, do it here. + } @Override @@ -30,7 +39,8 @@ public void execute() { // - Gets the robot to turn to the target orientation // - Gets the robot stop (or at least be moving really really slowly) at the // target position - + // pose. getcurrentheading = where you face rn +//always turn to left, but turn to right at some point // How you do this is up to you. If you get stuck, ask a mentor or student for // some hints! } diff --git a/src/main/java/competition/subsystems/drive/commands/TurnLeft90DegreesCommand.java b/src/main/java/competition/subsystems/drive/commands/TurnLeft90DegreesCommand.java index fac1288..7d1a2b4 100644 --- a/src/main/java/competition/subsystems/drive/commands/TurnLeft90DegreesCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/TurnLeft90DegreesCommand.java @@ -43,4 +43,13 @@ public void execute() { // higher distance error higher power,more power less speed ||| power = error - speed drive.tankDrive(-power, power); - }} + + } + + @Override + public boolean isFinished() { + // Modify this to return true once you have met your goal, + // and you're moving fairly slowly (ideally stopped) + return false; + } +} \ No newline at end of file From bf262f4ebf7ee30ef52538368e88ae5a96c500d2 Mon Sep 17 00:00:00 2001 From: Clulogh <197954988+Clulogh@users.noreply.github.com> Date: Sat, 22 Nov 2025 14:43:31 -0800 Subject: [PATCH 4/5] Drive to orientation 11/22/25 works but just needs to adjust a bit to match the rotation heading. --- .../commands/DriveToOrientationCommand.java | 25 +++++++++++-------- .../commands/DriveToPositionCommand.java | 5 +++- 2 files changed, 19 insertions(+), 11 deletions(-) diff --git a/src/main/java/competition/subsystems/drive/commands/DriveToOrientationCommand.java b/src/main/java/competition/subsystems/drive/commands/DriveToOrientationCommand.java index 3470e5c..95d9bf2 100644 --- a/src/main/java/competition/subsystems/drive/commands/DriveToOrientationCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/DriveToOrientationCommand.java @@ -11,6 +11,8 @@ public class DriveToOrientationCommand extends BaseCommand { DriveSubsystem drive; PoseSubsystem pose; double goal; + double previousPos = 0; + @Inject public DriveToOrientationCommand(DriveSubsystem driveSubsystem, PoseSubsystem pose) { @@ -21,10 +23,9 @@ public DriveToOrientationCommand(DriveSubsystem driveSubsystem, PoseSubsystem po public void setTargetHeading(double heading) { goal = heading; // heading= direction you want face in degrees // This method will be called by the test, and will give you a goal heading. - - //if (target > 180) { - // target = target - 360; //get the position between 0 to 180 - + if (goal > 180) { + goal = goal - 360; //get the position between 0 to 180 + } // You'll need to remember this target position and use it in your calculations. } @@ -37,14 +38,18 @@ public void initialize() { public void execute() { // Here you'll need to figure out a technique that: // - Gets the robot to turn to the target orientation - // - Gets the robot stop (or at least be moving really really slowly) at the - // target position - // pose. getcurrentheading = where you face rn -//always turn to left, but turn to right at some point - // How you do this is up to you. If you get stuck, ask a mentor or student for - // some hints! + // - Gets the robot stop (or at least be moving really really slowly) at the target position + //pose.getCurrentHeading(); //use current and goal/target & where you are facing rn + //always turn to left, but turn to right at some point + double speed = pose.getCurrentHeading().getDegrees() - previousPos; + double power = goal - speed; + double error = pose.getCurrentHeading().getDegrees(); + + // higher distance error higher power,more power less speed ||| power = error - speed + drive.tankDrive(power, -power); } + @Override public boolean isFinished() { // Modify this to return true once you have met your goal, diff --git a/src/main/java/competition/subsystems/drive/commands/DriveToPositionCommand.java b/src/main/java/competition/subsystems/drive/commands/DriveToPositionCommand.java index 963d6c4..acf5c4f 100644 --- a/src/main/java/competition/subsystems/drive/commands/DriveToPositionCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/DriveToPositionCommand.java @@ -73,9 +73,12 @@ public void execute() { @Override public boolean isFinished() { + + //speed, current position, + return true; + // Modify this to return true once you have met your goal, // and you're moving fairly slowly (ideally stopped) - return false; } } From 77608fbc22617dfeb9718f1cb52cf0695482ab9c Mon Sep 17 00:00:00 2001 From: Clulogh <197954988+Clulogh@users.noreply.github.com> Date: Sat, 6 Dec 2025 14:49:14 -0800 Subject: [PATCH 5/5] Finished drive to orientation --- build.gradle | 2 +- .../commands/DriveToOrientationCommand.java | 39 ++++++++++++++----- .../commands/TurnLeft90DegreesCommand.java | 3 +- 3 files changed, 32 insertions(+), 12 deletions(-) diff --git a/build.gradle b/build.gradle index 3c96375..4cae7e9 100644 --- a/build.gradle +++ b/build.gradle @@ -2,7 +2,7 @@ import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.3.1" + id "edu.wpi.first.GradleRIO" version "2025.3.2" id 'checkstyle' id 'jacoco' id 'org.hidetake.ssh' version "2.9.0" diff --git a/src/main/java/competition/subsystems/drive/commands/DriveToOrientationCommand.java b/src/main/java/competition/subsystems/drive/commands/DriveToOrientationCommand.java index 95d9bf2..1773060 100644 --- a/src/main/java/competition/subsystems/drive/commands/DriveToOrientationCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/DriveToOrientationCommand.java @@ -6,12 +6,15 @@ import xbot.common.command.BaseCommand; import competition.subsystems.drive.DriveSubsystem; +import static java.lang.Math.abs; + public class DriveToOrientationCommand extends BaseCommand { DriveSubsystem drive; PoseSubsystem pose; double goal; - double previousPos = 0; + double currentPostion; + double previousPos; @Inject @@ -21,7 +24,7 @@ public DriveToOrientationCommand(DriveSubsystem driveSubsystem, PoseSubsystem po } public void setTargetHeading(double heading) { - goal = heading; // heading= direction you want face in degrees + goal = heading; // heading = direction you want face in degrees // This method will be called by the test, and will give you a goal heading. if (goal > 180) { goal = goal - 360; //get the position between 0 to 180 @@ -41,19 +44,37 @@ public void execute() { // - Gets the robot stop (or at least be moving really really slowly) at the target position //pose.getCurrentHeading(); //use current and goal/target & where you are facing rn //always turn to left, but turn to right at some point + currentPostion = pose.getCurrentHeading().getDegrees(); double speed = pose.getCurrentHeading().getDegrees() - previousPos; - double power = goal - speed; - double error = pose.getCurrentHeading().getDegrees(); - + double error = goal - pose.getCurrentHeading().getDegrees(); + double power = error - speed; // higher distance error higher power,more power less speed ||| power = error - speed - drive.tankDrive(power, -power); + drive.tankDrive(-power, power); + previousPos = pose.getCurrentHeading().getDegrees(); } @Override public boolean isFinished() { - // Modify this to return true once you have met your goal, - // and you're moving fairly slowly (ideally stopped) - return false; + double error = pose.getCurrentHeading().getDegrees() - goal; + double speed = pose.getCurrentHeading().getDegrees() - previousPos; + + return Math.abs(error) < 0.05 && Math.abs(speed) < 0.05; //simple version of both of the ones at the bottom + + + // boolean closeEnough = Math.abs(error) < 0.05; simple version of the one at the bottom + // boolean slowEnough = Math.abs(speed) < 0.05; simple version of the one at the bottom + + // if (Math.abs(error) < 0.05) { + // closeEnough = true; + // } + + // if (Math.abs(speed) < 0.05) { + // slowEnough = true; + // } + // return closeEnough && slowEnough; + + } } + diff --git a/src/main/java/competition/subsystems/drive/commands/TurnLeft90DegreesCommand.java b/src/main/java/competition/subsystems/drive/commands/TurnLeft90DegreesCommand.java index 7d1a2b4..c8d3ea6 100644 --- a/src/main/java/competition/subsystems/drive/commands/TurnLeft90DegreesCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/TurnLeft90DegreesCommand.java @@ -48,8 +48,7 @@ public void execute() { @Override public boolean isFinished() { - // Modify this to return true once you have met your goal, - // and you're moving fairly slowly (ideally stopped) + return false; } } \ No newline at end of file