diff --git a/.github/.idea/.github.iml b/.github/.idea/.github.iml new file mode 100644 index 0000000..d6ebd48 --- /dev/null +++ b/.github/.idea/.github.iml @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/.github/.idea/misc.xml b/.github/.idea/misc.xml new file mode 100644 index 0000000..639900d --- /dev/null +++ b/.github/.idea/misc.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/.github/.idea/modules.xml b/.github/.idea/modules.xml new file mode 100644 index 0000000..854e742 --- /dev/null +++ b/.github/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/.github/.idea/workspace.xml b/.github/.idea/workspace.xml new file mode 100644 index 0000000..6d6a0bc --- /dev/null +++ b/.github/.idea/workspace.xml @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + 1761191873112 + + + + \ No newline at end of file diff --git a/.run/Build Robot.run.xml b/.run/Build Robot.run.xml index 6d07797..790501e 100644 --- a/.run/Build Robot.run.xml +++ b/.run/Build Robot.run.xml @@ -4,7 +4,7 @@ diff --git a/gradle/.idea/.gitignore b/gradle/.idea/.gitignore new file mode 100644 index 0000000..26d3352 --- /dev/null +++ b/gradle/.idea/.gitignore @@ -0,0 +1,3 @@ +# Default ignored files +/shelf/ +/workspace.xml diff --git a/gradle/.idea/gradle.iml b/gradle/.idea/gradle.iml new file mode 100644 index 0000000..d6ebd48 --- /dev/null +++ b/gradle/.idea/gradle.iml @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/gradle/.idea/libraries/gradle_wrapper.xml b/gradle/.idea/libraries/gradle_wrapper.xml new file mode 100644 index 0000000..d2b3f27 --- /dev/null +++ b/gradle/.idea/libraries/gradle_wrapper.xml @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/gradle/.idea/misc.xml b/gradle/.idea/misc.xml new file mode 100644 index 0000000..639900d --- /dev/null +++ b/gradle/.idea/misc.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/gradle/.idea/modules.xml b/gradle/.idea/modules.xml new file mode 100644 index 0000000..be92c07 --- /dev/null +++ b/gradle/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/gradle/.idea/vcs.xml b/gradle/.idea/vcs.xml new file mode 100644 index 0000000..f51b4aa --- /dev/null +++ b/gradle/.idea/vcs.xml @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/src/main/java/competition/subsystems/drive/DriveSubsystem.java b/src/main/java/competition/subsystems/drive/DriveSubsystem.java index 07f4966..2712cbc 100644 --- a/src/main/java/competition/subsystems/drive/DriveSubsystem.java +++ b/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 diff --git a/src/main/java/competition/subsystems/drive/commands/DriveToOrientationCommand.java b/src/main/java/competition/subsystems/drive/commands/DriveToOrientationCommand.java index 832771f..718247b 100644 --- a/src/main/java/competition/subsystems/drive/commands/DriveToOrientationCommand.java +++ b/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 diff --git a/src/main/java/competition/subsystems/drive/commands/DriveToPositionCommand.java b/src/main/java/competition/subsystems/drive/commands/DriveToPositionCommand.java index 036e1f4..9662770 100644 --- a/src/main/java/competition/subsystems/drive/commands/DriveToPositionCommand.java +++ b/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; } } diff --git a/src/main/java/competition/subsystems/drive/commands/TankDriveWithJoysticksCommand.java b/src/main/java/competition/subsystems/drive/commands/TankDriveWithJoysticksCommand.java index 088dcc2..1d1a622 100644 --- a/src/main/java/competition/subsystems/drive/commands/TankDriveWithJoysticksCommand.java +++ b/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); } } diff --git a/src/main/java/competition/subsystems/drive/commands/TogglePrecisionDriveCommand.java b/src/main/java/competition/subsystems/drive/commands/TogglePrecisionDriveCommand.java index 184a1c6..8cda738 100644 --- a/src/main/java/competition/subsystems/drive/commands/TogglePrecisionDriveCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/TogglePrecisionDriveCommand.java @@ -1,3 +1,4 @@ + package competition.subsystems.drive.commands; import javax.inject.Inject; diff --git a/src/main/java/competition/subsystems/drive/commands/TurnLeft90DegreesCommand.java b/src/main/java/competition/subsystems/drive/commands/TurnLeft90DegreesCommand.java index 392ae70..68a0814 100644 --- a/src/main/java/competition/subsystems/drive/commands/TurnLeft90DegreesCommand.java +++ b/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; + } } - -} 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;