Rotation to left 90 and orientation

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/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!
     }
 

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

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,21 @@ 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);
     }
 
 
@@ -76,6 +86,7 @@ public void move(XYPair translate, double rotate) {
     public double getLeftTotalDistance() {
         // TODO: Auto-generated method stub
         return 0;
+
     }
 
     @Override

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

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

@@ -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
@@ -29,23 +42,43 @@ 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
-
-        // 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();
+        // - Gets the robot stop (or at least be moving really really slowly) at the target position
+        //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
+
+        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
+
+
+
     }
 
     @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/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);
     }
 
 }

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
@@ -32,19 +34,8 @@ public void initialize() {
         // 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;
     }
-
 }

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