Update DriveToPositionCommand.java

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

@@ -2,26 +2,41 @@
 
 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 p = 10;
+    double d =10;
+    double speed = 0;
+    double previousPosition;
+    double currentRotation;
+    double targetHeading;
 
     @Inject
-    public DriveToOrientationCommand(DriveSubsystem driveSubsystem) {
+    public DriveToOrientationCommand(DriveSubsystem driveSubsystem, PoseSubsystem pose) {
         this.drive = driveSubsystem;
+        this.pose = pose;
     }
 
     public void setTargetHeading(double heading) {
         // This method will be called by the test, and will give you a goal heading.
         // You'll need to remember this target position and use it in your calculations.
+        targetHeading = heading;
     }
 
     @Override
     public void initialize() {
         // If you have some one-time setup, do it here.
+        currentRotation = pose.getCurrentHeading().getDegrees();
+        previousPosition = currentRotation;
+        currentRotation =-currentRotation;
+        System.out.println("MY GOAL: " + targetHeading);
+        System.out.println("MY starting pos: " + currentRotation);
     }
 
     @Override
@@ -30,7 +45,17 @@ 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
+        currentRotation = pose.getCurrentHeading().getDegrees();
+        double error = targetHeading - currentRotation;
+        speed = currentRotation - previousPosition;
+        if(error >= 180) {
 
+            error = error - 360;
+
+        }
+        double power = p * error - d * speed;
+        drive.tankDrive(-power, power);
+        previousPosition = currentRotation;
         // How you do this is up to you. If you get stuck, ask a mentor or student for
         // some hints!
     }

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

@@ -10,16 +10,23 @@ public class DriveToPositionCommand extends BaseCommand {
 
     DriveSubsystem drive;
     PoseSubsystem pose;
+    double goalPosition = 0;
+    double d = 50;
+    double p = 50;
+    double previousPosition = 0;
+    double currentPosition = 0;
+    double speed = 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.
+        goalPosition = newGoalPosition;
     }
 
     @Override
@@ -35,17 +42,38 @@ 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!
-        drive.tankDrive(0.25,0.25);
-        pose.getPosition();
+        // goal: drive.tankdrive(power, power)
+        // need: power
+        // power = p * error - d * speed
+        // p & d we alr have
+        // need: error
+        // ---> error = goalposition - currentposition
+        // need : speed
+        // ----> speed = change in position over time distance over time
+        // current position - previous position
+        // getting our error
+        currentPosition = pose.getPosition();
+        double error = goalPosition - currentPosition;
+        speed = currentPosition - previousPosition;
+        double power = p * error - d * speed;
+        drive.tankDrive(power, power);
+
+        previousPosition = currentPosition;
     }
 
     @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;
+
+        // returns true if finished
+        // returns false if not finished
+        // finished conditions:
+        // 1. at goal position
+        // 2. basically no speed (not moving)
+        boolean atGoalPosition = Math.abs(currentPosition - goalPosition) < 0.01;
+        boolean notMoving = Math.abs(speed) < 0.001;
+        return atGoalPosition && notMoving;
     }
 
 }

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

@@ -10,6 +10,12 @@ public class TurnLeft90DegreesCommand extends BaseCommand {
 
     DriveSubsystem drive;
     PoseSubsystem pose;
+    double p = 10;
+    double d =10;
+    double speed = 0;
+    double previousPosition;
+    double goal;
+    double currentRotation;
 
     @Inject
     public TurnLeft90DegreesCommand(DriveSubsystem driveSubsystem, PoseSubsystem pose) {
@@ -19,10 +25,29 @@ public TurnLeft90DegreesCommand(DriveSubsystem driveSubsystem, PoseSubsystem pos
 
     @Override
     public void initialize() {
+        currentRotation = pose.getCurrentHeading().getDegrees();
+        goal = currentRotation + 90;
+        previousPosition = currentRotation;
+        currentRotation =-currentRotation;
+        System.out.println("MY GOAL: " + goal);
+        System.out.println("MY starting pos: " + currentRotation);
     }
 
     @Override
     public void execute() {
+        currentRotation = pose.getCurrentHeading().getDegrees();
+        double error = goal - currentRotation;
+        speed = currentRotation - previousPosition;
+        if(error >= 180) {
+
+            error = error - 360;
+
+        }
+        double power = p * error - d * speed;
+        drive.tankDrive(-power, power);
+        previousPosition = currentRotation;
     }
 
+
+
 }