From dc227fecfebaa2abf33d649ad08a9baac29df259 Mon Sep 17 00:00:00 2001
From: Clulogh <197954988+Clulogh@users.noreply.github.com>
Date: Sat, 4 Oct 2025 14:25:06 -0700
Subject: [PATCH 1/9] driving system

---
 .../subsystems/SubsystemDefaultCommandMap.java |  3 ++-
 .../subsystems/drive/DriveSubsystem.java       | 15 +++++++++++++--
 .../ArcadeDriveWithJoysticksCommand.java       | 11 ++++++++++-
 .../TankDriveWithJoysticksCommand.java         | 10 ++++++++--
 .../commands/TogglePrecisionDriveCommand.java  | 18 ++----------------
 5 files changed, 35 insertions(+), 22 deletions(-)

diff --git a/src/main/java/competition/subsystems/SubsystemDefaultCommandMap.java b/src/main/java/competition/subsystems/SubsystemDefaultCommandMap.java
index c2d39c6..d1da7ec 100644
--- a/src/main/java/competition/subsystems/SubsystemDefaultCommandMap.java
+++ b/src/main/java/competition/subsystems/SubsystemDefaultCommandMap.java
@@ -2,6 +2,7 @@
 
 import competition.subsystems.drive.DriveSubsystem;
 import competition.subsystems.drive.SwerveDriveSubsystem;
+import competition.subsystems.drive.commands.ArcadeDriveWithJoysticksCommand;
 import competition.subsystems.drive.commands.SwerveDriveWithJoysticksCommand;
 import competition.subsystems.drive.commands.TankDriveWithJoysticksCommand;
 
@@ -16,7 +17,7 @@ public class SubsystemDefaultCommandMap {
     public SubsystemDefaultCommandMap() {}
 
     @Inject
-    public void setupDriveSubsystem(DriveSubsystem driveSubsystem, TankDriveWithJoysticksCommand command) {
+    public void setupDriveSubsystem(DriveSubsystem driveSubsystem, ArcadeDriveWithJoysticksCommand command) {
         driveSubsystem.setDefaultCommand(command);
     }
 
diff --git a/src/main/java/competition/subsystems/drive/DriveSubsystem.java b/src/main/java/competition/subsystems/drive/DriveSubsystem.java
index 07f4966..b2b59bc 100644
--- a/src/main/java/competition/subsystems/drive/DriveSubsystem.java
+++ b/src/main/java/competition/subsystems/drive/DriveSubsystem.java
@@ -22,6 +22,8 @@ public class DriveSubsystem extends BaseDriveSubsystem implements DataFrameRefre
 
     DoubleProperty dp;
 
+    public boolean isPrecisionModeActive = false;
+
     @Inject
     public DriveSubsystem(XCANMotorController.XCANMotorControllerFactory motorControllerFactory, ElectricalContract electricalContract, PropertyFactory pf) {
         log.info("Creating DriveSubsystem");
@@ -36,13 +38,22 @@ public DriveSubsystem(XCANMotorController.XCANMotorControllerFactory motorContro
         dp = pf.createPersistentProperty("DriveSubsystem", 1.5);
     }
 
+    //A method that lets the outside user get if precision mode is on/off
+public boolean getPrecisionModeActive() {
+        return isPrecisionModeActive;
+}
+
     public void 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:
+        if(isPrecisionModeActive) { //precision mode is on when its true & will divide by 2
+            leftPower = leftPower * 0.5;
+            rightPower = rightPower * 0.5;
+        }
         frontLeft.setPower(leftPower);
-        // TODO: Add code to set the right motors to the rightPower value.
-
+        frontRight.setPower(rightPower);
+        // TODO: Add code to set the right motors to the rightPower value. (donee)
     }
 
 
diff --git a/src/main/java/competition/subsystems/drive/commands/ArcadeDriveWithJoysticksCommand.java b/src/main/java/competition/subsystems/drive/commands/ArcadeDriveWithJoysticksCommand.java
index 03d0dff..92672d4 100644
--- a/src/main/java/competition/subsystems/drive/commands/ArcadeDriveWithJoysticksCommand.java
+++ b/src/main/java/competition/subsystems/drive/commands/ArcadeDriveWithJoysticksCommand.java
@@ -15,6 +15,7 @@ public class ArcadeDriveWithJoysticksCommand extends BaseCommand {
     public ArcadeDriveWithJoysticksCommand(DriveSubsystem driveSubsystem, OperatorInterface oi) {
         this.operatorInterface = oi;
         this.drive = driveSubsystem;
+        this.addRequirements(drive);
     }
 
     @Override
@@ -24,6 +25,14 @@ public void initialize() {
 
     @Override
     public void execute() {
+        //this is from tank drive, make it so your only using one joystick to move forward & backwards.
+        //Left value will only move the left wheels, Right value will only move the right wheels.
+        //Move both value to move forwards, try to make it so both moves
+        //leftvector = left joystick, xy value = xy axis
+        double xValue = operatorInterface.gamepad.getLeftVector().getX();
+        double yValue = operatorInterface.gamepad.getLeftVector().getY();
+        // when calling x and y values from the operator interface, use leftJoystick.
+        //1 is full forward -1 is full backwards
+        drive.tankDrive(yValue-xValue,yValue+xValue); //only use 2 values, however using the +, - can bypass that.
     }
-
 }
diff --git a/src/main/java/competition/subsystems/drive/commands/TankDriveWithJoysticksCommand.java b/src/main/java/competition/subsystems/drive/commands/TankDriveWithJoysticksCommand.java
index 088dcc2..3935c61 100644
--- a/src/main/java/competition/subsystems/drive/commands/TankDriveWithJoysticksCommand.java
+++ b/src/main/java/competition/subsystems/drive/commands/TankDriveWithJoysticksCommand.java
@@ -34,12 +34,18 @@ public void execute() {
         // Get values from the joysticks:
         // 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
+        //Left value will only move the left wheels, Right value will only move the right wheels.
+        //Move both value to move forwards
+        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);
+
+
+
+
+        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..745f747 100644
--- a/src/main/java/competition/subsystems/drive/commands/TogglePrecisionDriveCommand.java
+++ b/src/main/java/competition/subsystems/drive/commands/TogglePrecisionDriveCommand.java
@@ -16,6 +16,8 @@ public TogglePrecisionDriveCommand(DriveSubsystem driveSubsystem) {
 
     @Override
     public void initialize() {
+        drive.isPrecisionModeActive = !drive.isPrecisionModeActive; //same as the highlighted part
+
         // Here, you want to call the DriveSubsystem and tell it to change its precision
         // mode.
         // This means you'll need to add a new method into DriveSubsystem, and there are
@@ -31,20 +33,4 @@ public void initialize() {
         // In all of these cases you'll need to have the mode somehow affect the
         // TankDrive method.
     }
-
-    @Override
-    public void execute() {
-        // No need to put any code here - since we want the toggle to run once,
-        // initialize() is the
-        // best place to put the code.
-    }
-
-    @Override
-    public boolean isFinished() {
-        // Commands keep running until they are finished.
-        // Since we want this command to just run once (toggling precision mode), we
-        // say that the command is finished right away.
-        return true;
-    }
-
 }

From b3a5321b98c009206c372ed9303fb8c73e4308d7 Mon Sep 17 00:00:00 2001
From: Clulogh <197954988+Clulogh@users.noreply.github.com>
Date: Sat, 11 Oct 2025 12:30:30 -0700
Subject: [PATCH 2/9] Changing speed

---
 .../operator_interface/OperatorCommandMap.java            | 8 +++++---
 .../java/competition/subsystems/drive/DriveSubsystem.java | 1 -
 .../drive/commands/TogglePrecisionDriveCommand.java       | 5 +++++
 3 files changed, 10 insertions(+), 4 deletions(-)

diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java
index 164338e..9a25446 100644
--- a/src/main/java/competition/operator_interface/OperatorCommandMap.java
+++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java
@@ -3,6 +3,7 @@
 import javax.inject.Inject;
 import javax.inject.Singleton;
 
+import competition.subsystems.drive.commands.TogglePrecisionDriveCommand;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 
@@ -24,12 +25,13 @@ public OperatorCommandMap() {}
     @Inject
     public void setupMyCommands(
             OperatorInterface operatorInterface,
-            SetRobotHeadingCommand resetHeading
-            )
+            SetRobotHeadingCommand resetHeading,
+            TogglePrecisionDriveCommand togglePrecisionDriveCommand
+    )
     {
         resetHeading.setHeadingToApply(90);
         operatorInterface.gamepad.getifAvailable(XboxButton.Start).whileTrue(resetHeading);
-
+        operatorInterface.gamepad.getifAvailable(XboxButton.A).onTrue(togglePrecisionDriveCommand);
         // Add new button mappings here!
     }
 
diff --git a/src/main/java/competition/subsystems/drive/DriveSubsystem.java b/src/main/java/competition/subsystems/drive/DriveSubsystem.java
index b2b59bc..9c065d4 100644
--- a/src/main/java/competition/subsystems/drive/DriveSubsystem.java
+++ b/src/main/java/competition/subsystems/drive/DriveSubsystem.java
@@ -53,7 +53,6 @@ public void tankDrive(double leftPower, double rightPower) {
         }
         frontLeft.setPower(leftPower);
         frontRight.setPower(rightPower);
-        // TODO: Add code to set the right motors to the rightPower value. (donee)
     }
 
 
diff --git a/src/main/java/competition/subsystems/drive/commands/TogglePrecisionDriveCommand.java b/src/main/java/competition/subsystems/drive/commands/TogglePrecisionDriveCommand.java
index 745f747..b67c901 100644
--- a/src/main/java/competition/subsystems/drive/commands/TogglePrecisionDriveCommand.java
+++ b/src/main/java/competition/subsystems/drive/commands/TogglePrecisionDriveCommand.java
@@ -33,4 +33,9 @@ public void initialize() {
         // In all of these cases you'll need to have the mode somehow affect the
         // TankDrive method.
     }
+
+    @Override
+    public boolean isFinished() {
+        return true;
+    }
 }

From e020d95654708a200eb150df35395eb5e0cb33ed Mon Sep 17 00:00:00 2001
From: Clulogh <197954988+Clulogh@users.noreply.github.com>
Date: Sat, 11 Oct 2025 14:45:56 -0700
Subject: [PATCH 3/9] Updates, comments

---
 .../drive/commands/DriveToPositionCommand.java     | 14 +++++++++-----
 1 file changed, 9 insertions(+), 5 deletions(-)

diff --git a/src/main/java/competition/subsystems/drive/commands/DriveToPositionCommand.java b/src/main/java/competition/subsystems/drive/commands/DriveToPositionCommand.java
index 036e1f4..0dc2716 100644
--- a/src/main/java/competition/subsystems/drive/commands/DriveToPositionCommand.java
+++ b/src/main/java/competition/subsystems/drive/commands/DriveToPositionCommand.java
@@ -29,14 +29,18 @@ public void initialize() {
 
     @Override
     public void execute() {
+        //starts at 0, stops at 300. Slowly goes up by 1 (momentum) << we wanna remove it, so we can stop exactly on 300
         // Here you'll need to figure out a technique that:
         // - Gets the robot to move to the target position
-        // - 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
+        // - Gets the robot stop (or at least be moving really really slowly) at the target position
+        //y=mx , power= p * error - d * speed d = deriative, speed = "velocity", error = displacement p, = proportion
+        //if(isPrecisionModeActive) {
+            //leftPower = leftPower * 0.1;
+           // rightPower = rightPower * 0.1; //move very slowly to target positistion
+        }
+        //stop when you reach target
+    }//cordinates or target/end position in the brackets
 
-        // 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();
     }

From 1db38cf57db356bf0081a54c6f2e1710a4ed64ca Mon Sep 17 00:00:00 2001
From: Clulogh <197954988+Clulogh@users.noreply.github.com>
Date: Sat, 25 Oct 2025 14:47:25 -0700
Subject: [PATCH 4/9] Reworded code + get prev and current positions

---
 .../subsystems/drive/DriveSubsystem.java      |  1 +
 .../commands/DriveToPositionCommand.java      | 44 +++++++++++++++----
 2 files changed, 36 insertions(+), 9 deletions(-)

diff --git a/src/main/java/competition/subsystems/drive/DriveSubsystem.java b/src/main/java/competition/subsystems/drive/DriveSubsystem.java
index 9c065d4..b46b06f 100644
--- a/src/main/java/competition/subsystems/drive/DriveSubsystem.java
+++ b/src/main/java/competition/subsystems/drive/DriveSubsystem.java
@@ -86,6 +86,7 @@ public void move(XYPair translate, double rotate) {
     public double getLeftTotalDistance() {
         // TODO: Auto-generated method stub
         return 0;
+
     }
 
     @Override
diff --git a/src/main/java/competition/subsystems/drive/commands/DriveToPositionCommand.java b/src/main/java/competition/subsystems/drive/commands/DriveToPositionCommand.java
index 0dc2716..963d6c4 100644
--- a/src/main/java/competition/subsystems/drive/commands/DriveToPositionCommand.java
+++ b/src/main/java/competition/subsystems/drive/commands/DriveToPositionCommand.java
@@ -5,21 +5,34 @@
 import xbot.common.command.BaseCommand;
 import competition.subsystems.drive.DriveSubsystem;
 import competition.subsystems.pose.PoseSubsystem;
+import xbot.common.properties.DoubleProperty;
+import xbot.common.properties.PropertyFactory;
 
 public class DriveToPositionCommand extends BaseCommand {
 
     DriveSubsystem drive;
     PoseSubsystem pose;
+    double targetPosition;
+    DoubleProperty pProperty;
+    DoubleProperty dProperty;
+    double previousPosition = 0;
+
+    //DriveSubsystem driveSubsystem
 
     @Inject
-    public DriveToPositionCommand(DriveSubsystem driveSubsystem, PoseSubsystem pose) {
+    public DriveToPositionCommand(DriveSubsystem driveSubsystem, PoseSubsystem pose, PropertyFactory propertyFactory) {
+        propertyFactory.setPrefix(this);
         this.drive = driveSubsystem;
         this.pose = pose;
+
+        pProperty = propertyFactory.createPersistentProperty("p", 1);
+        dProperty = propertyFactory.createPersistentProperty("d", 1);
     }
 
     public void setTargetPosition(double position) {
         // 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.
+        this.targetPosition = position;
     }
 
     @Override
@@ -33,16 +46,29 @@ public void execute() {
         // Here you'll need to figure out a technique that:
         // - Gets the robot to move to the target position
         // - Gets the robot stop (or at least be moving really really slowly) at the target position
-        //y=mx , power= p * error - d * speed d = deriative, speed = "velocity", error = displacement p, = proportion
-        //if(isPrecisionModeActive) {
-            //leftPower = leftPower * 0.1;
-           // rightPower = rightPower * 0.1; //move very slowly to target positistion
-        }
+        //y=mx+b , power = p*(error) + d or - d * speed
+        //d = deriative, speed = "velocity" how fast your going, error = distance from goal  p, = proportion
+
         //stop when you reach target
-    }//cordinates or target/end position in the brackets
+    //cordinates or target/end position in the brackets
+
+        double error = this.targetPosition - pose.getPosition();
+//faster speed less power, further away more power,
+        // if (error < .001) {
+        //   drive.tankDrive(0, 0);
+        //  }else if(error < 2) {
+        //    drive.tankDrive(0.005, 0.005);
+
+        // } else {
+        //      drive.tankDrive(0.10, 0.10);
+        double speed = pose.getPosition() - previousPosition;
+        double power = pProperty.get() * error - dProperty.get() * speed;
+
+        drive.tankDrive(power, power);
+        previousPosition = pose.getPosition(); //previous to current position
+
+
 
-        drive.tankDrive(0.25,0.25);
-        pose.getPosition();
     }
 
     @Override

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 5/9] 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 6/9] 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 7/9] 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 8/9] 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 9/9] 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