Rotation

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"

src/main/java/competition/subsystems/drive/commands/DriveToOrientationCommand.java

@@ -2,43 +2,79 @@
 
 import javax.inject.Inject;
 
+import competition.subsystems.pose.PoseSubsystem;
 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 currentPostion;
+    double previousPos;
+
 
     @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 (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.
     }
 
     @Override
     public void initialize() {
-        // If you have some one-time setup, do it here.
+
     }
 
     @Override
     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
-
-        // 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
+        currentPostion = pose.getCurrentHeading().getDegrees();
+        double speed = pose.getCurrentHeading().getDegrees() - previousPos;
+        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);
+        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;
+
+
     }
 }
+

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;
     }
 
 }

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;
+import xbot.common.properties.PropertyFactory;
 
 public class TurnLeft90DegreesCommand extends BaseCommand {
 
     DriveSubsystem drive;
     PoseSubsystem pose;
 
+    double target;
+    double previousPos = 0;
+
     @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
+        // 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
+        }
+
     }
 
     @Override
     public void execute() {
+        //turn 90 degrees
+        double speed = pose.getCurrentHeading().getDegrees() - previousPos;
+        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);
+
+
     }
 
-}
+    @Override
+    public boolean isFinished() {
+
+        return false;
+    }
+}

src/main/java/competition/subsystems/pose/PoseSubsystem.java

@@ -16,15 +16,15 @@ 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);
         this.drive = drive;
     }
 
     public double getPosition() {
-        return (getLeftDriveDistance() + getRightDriveDistance()) / 2.0; 
+        return (getLeftDriveDistance() + getRightDriveDistance()) / 2.0;
     }
 
     @Override