diff --git a/src/main/java/competition/subsystems/drive/DriveSubsystem.java b/src/main/java/competition/subsystems/drive/DriveSubsystem.java index 07f4966..ecbe2a5 100644 --- a/src/main/java/competition/subsystems/drive/DriveSubsystem.java +++ b/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; } diff --git a/src/main/java/competition/subsystems/drive/commands/DriveToPositionCommand.java b/src/main/java/competition/subsystems/drive/commands/DriveToPositionCommand.java index 036e1f4..a245b83 100644 --- a/src/main/java/competition/subsystems/drive/commands/DriveToPositionCommand.java +++ b/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; } - } diff --git a/src/main/java/competition/subsystems/drive/commands/TankDriveWithJoysticksCommand.java b/src/main/java/competition/subsystems/drive/commands/TankDriveWithJoysticksCommand.java index 088dcc2..c706de8 100644 --- a/src/main/java/competition/subsystems/drive/commands/TankDriveWithJoysticksCommand.java +++ b/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); } } diff --git a/src/main/java/competition/subsystems/drive/commands/TogglePrecisionDriveCommand.java b/src/main/java/competition/subsystems/drive/commands/TogglePrecisionDriveCommand.java index 184a1c6..e934109 100644 --- a/src/main/java/competition/subsystems/drive/commands/TogglePrecisionDriveCommand.java +++ b/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: diff --git a/src/main/java/competition/subsystems/drive/commands/TurnLeft90DegreesCommand.java b/src/main/java/competition/subsystems/drive/commands/TurnLeft90DegreesCommand.java index 392ae70..d4bdce9 100644 --- a/src/main/java/competition/subsystems/drive/commands/TurnLeft90DegreesCommand.java +++ b/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); + } + } +} \ No newline at end of file diff --git a/src/test/java/xbot/edubot/basic_understanding/AObserveHowCommandsWork.java b/src/test/java/xbot/edubot/basic_understanding/AObserveHowCommandsWork.java index ed5708d..8d160e9 100644 --- a/src/test/java/xbot/edubot/basic_understanding/AObserveHowCommandsWork.java +++ b/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;