fixed drive to position

src/main/java/competition/subsystems/drive/DriveSubsystem.java

@@ -22,6 +22,7 @@ public class DriveSubsystem extends BaseDriveSubsystem implements DataFrameRefre
 
     DoubleProperty dp;
 
+    public boolean isPrecisionModeOn = false;
     @Inject
     public DriveSubsystem(XCANMotorController.XCANMotorControllerFactory motorControllerFactory, ElectricalContract electricalContract, PropertyFactory pf) {
         log.info("Creating DriveSubsystem");
@@ -36,13 +37,14 @@ public DriveSubsystem(XCANMotorController.XCANMotorControllerFactory motorContro
         dp = pf.createPersistentProperty("DriveSubsystem", 1.5);
     }
 
-    public void tankDrive(double leftPower, double rightPower) {
+    public double 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:
         frontLeft.setPower(leftPower);
         // TODO: Add code to set the right motors to the rightPower value.
 
+        return leftPower;
     }
 
 

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

@@ -9,17 +9,33 @@
 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
@@ -34,18 +50,23 @@ public void execute() {
         // - 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();
-    }
+        // some hints.
+        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() {
+    public boolean isFinished(
+    ) {
         // Modify this to return true once you have met your goal,
         // and you're moving fairly slowly (ideally stopped)
-        return false;
+        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/TankDriveWithJoysticksCommand.java

@@ -35,11 +35,15 @@ public void execute() {
         // 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
-
+        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);
+        if(drive.isPrecisionModeOn = true) {
+            leftValue = leftValue / 2;
+            rightValue = rightValue / 2;
+        }
+        drive.tankDrive(leftValue, rightValue);
     }
 
 }

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

@@ -18,6 +18,7 @@ public TogglePrecisionDriveCommand(DriveSubsystem driveSubsystem) {
     public void initialize() {
         // Here, you want to call the DriveSubsystem and tell it to change its precision
         // mode.
+        drive.isPrecisionModeOn = !drive.isPrecisionModeOn;
         // This means you'll need to add a new method into DriveSubsystem, and there are
         // two
         // major ways to do this:

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

@@ -1,6 +1,7 @@
 package competition.subsystems.drive.commands;
 
 import javax.inject.Inject;
+import javax.swing.*;
 
 import xbot.common.command.BaseCommand;
 import competition.subsystems.drive.DriveSubsystem;
@@ -23,6 +24,12 @@ public void initialize() {
 
     @Override
     public void execute() {
-    }
 
-}
+        if (pose.getCurrentHeading().getDegrees() == 90) {
+                drive.tankDrive(-10, -10);
+        }
+        else{
+            drive.tankDrive(2,2);
+        }
+    }
+}

src/test/java/xbot/edubot/basic_understanding/AObserveHowCommandsWork.java

@@ -7,7 +7,7 @@
 import competition.BaseCompetitionTest;
 import xbot.common.command.XScheduler;
 
-public class AObserveHowCommandsWork extends BaseCompetitionTest {
+public class    AObserveHowCommandsWork extends BaseCompetitionTest {
 
     protected Logger log;