Drive To Position Command

.run/Build Robot.run.xml

@@ -4,7 +4,7 @@
       <option name="executionName" />
       <option name="externalProjectPath" value="$PROJECT_DIR$" />
       <option name="externalSystemIdString" value="GRADLE" />
-      <option name="scriptParameters" />
+      <option name="scriptParameters" value="--scan" />
       <option name="taskDescriptions">
         <list />
       </option>

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

@@ -1,3 +1,4 @@
+
 package competition.subsystems.drive;
 
 import javax.inject.Inject;
@@ -21,7 +22,7 @@ public class DriveSubsystem extends BaseDriveSubsystem implements DataFrameRefre
     public final XCANMotorController frontRight;
 
     DoubleProperty dp;
-
+    public boolean isPrecisionModeOn = false;
     @Inject
     public DriveSubsystem(XCANMotorController.XCANMotorControllerFactory motorControllerFactory, ElectricalContract electricalContract, PropertyFactory pf) {
         log.info("Creating DriveSubsystem");
@@ -35,7 +36,6 @@ public DriveSubsystem(XCANMotorController.XCANMotorControllerFactory motorContro
         pf.setPrefix(this);
         dp = pf.createPersistentProperty("DriveSubsystem", 1.5);
     }
-
     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
@@ -69,7 +69,7 @@ public PIDManager getRotateDecayPid() {
 
     @Override
     public void move(XYPair translate, double rotate) {
-       throw new RuntimeException("Not yet implemented");
+        throw new RuntimeException("Not yet implemented");
     }
 
     @Override

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

@@ -2,30 +2,62 @@
 
 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 goal;
+
+    double currentRotation;
+
+    double targetHeading;
 
     @Inject
-    public DriveToOrientationCommand(DriveSubsystem driveSubsystem) {
+    public DriveToOrientationCommand(DriveSubsystem driveSubsystem, PoseSubsystem a) {
         this.drive = driveSubsystem;
+        this.pose = a;
     }
 
     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() {
+        currentRotation = pose.getCurrentHeading().getDegrees();
+        goal = targetHeading;
+        previousPosition = currentRotation;
+        currentRotation = -currentRotation;
+        System.out.println("MY GOAL:" + goal);
+        System.out.println("MY starting pos:" + currentRotation);
         // If you have some one-time setup, do it here.
     }
 
     @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;
         // 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

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

@@ -9,17 +9,31 @@
 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 +48,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();
-    }
+        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;
+        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

@@ -1,3 +1,4 @@
+
 package competition.subsystems.drive.commands;
 
 import javax.inject.Inject;
@@ -35,11 +36,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

@@ -1,3 +1,4 @@
+
 package competition.subsystems.drive.commands;
 
 import javax.inject.Inject;

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

@@ -9,8 +9,21 @@
 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) {
         this.drive = driveSubsystem;
@@ -19,10 +32,24 @@ 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;
+        }
     }
-
-}

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;