diff --git a/build.gradle b/build.gradle index 3c96375..4cae7e9 100644 --- a/build.gradle +++ b/build.gradle @@ -2,7 +2,7 @@ import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.3.1" + id "edu.wpi.first.GradleRIO" version "2025.3.2" id 'checkstyle' id 'jacoco' id 'org.hidetake.ssh' version "2.9.0" diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index 164338e..9a25446 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java @@ -3,6 +3,7 @@ import javax.inject.Inject; import javax.inject.Singleton; +import competition.subsystems.drive.commands.TogglePrecisionDriveCommand; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -24,12 +25,13 @@ public OperatorCommandMap() {} @Inject public void setupMyCommands( OperatorInterface operatorInterface, - SetRobotHeadingCommand resetHeading - ) + SetRobotHeadingCommand resetHeading, + TogglePrecisionDriveCommand togglePrecisionDriveCommand + ) { resetHeading.setHeadingToApply(90); operatorInterface.gamepad.getifAvailable(XboxButton.Start).whileTrue(resetHeading); - + operatorInterface.gamepad.getifAvailable(XboxButton.A).onTrue(togglePrecisionDriveCommand); // Add new button mappings here! } diff --git a/src/main/java/competition/subsystems/SubsystemDefaultCommandMap.java b/src/main/java/competition/subsystems/SubsystemDefaultCommandMap.java index c2d39c6..d1da7ec 100644 --- a/src/main/java/competition/subsystems/SubsystemDefaultCommandMap.java +++ b/src/main/java/competition/subsystems/SubsystemDefaultCommandMap.java @@ -2,6 +2,7 @@ import competition.subsystems.drive.DriveSubsystem; import competition.subsystems.drive.SwerveDriveSubsystem; +import competition.subsystems.drive.commands.ArcadeDriveWithJoysticksCommand; import competition.subsystems.drive.commands.SwerveDriveWithJoysticksCommand; import competition.subsystems.drive.commands.TankDriveWithJoysticksCommand; @@ -16,7 +17,7 @@ public class SubsystemDefaultCommandMap { public SubsystemDefaultCommandMap() {} @Inject - public void setupDriveSubsystem(DriveSubsystem driveSubsystem, TankDriveWithJoysticksCommand command) { + public void setupDriveSubsystem(DriveSubsystem driveSubsystem, ArcadeDriveWithJoysticksCommand command) { driveSubsystem.setDefaultCommand(command); } diff --git a/src/main/java/competition/subsystems/drive/DriveSubsystem.java b/src/main/java/competition/subsystems/drive/DriveSubsystem.java index 07f4966..b46b06f 100644 --- a/src/main/java/competition/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/competition/subsystems/drive/DriveSubsystem.java @@ -22,6 +22,8 @@ public class DriveSubsystem extends BaseDriveSubsystem implements DataFrameRefre DoubleProperty dp; + public boolean isPrecisionModeActive = false; + @Inject public DriveSubsystem(XCANMotorController.XCANMotorControllerFactory motorControllerFactory, ElectricalContract electricalContract, PropertyFactory pf) { log.info("Creating DriveSubsystem"); @@ -36,13 +38,21 @@ public DriveSubsystem(XCANMotorController.XCANMotorControllerFactory motorContro dp = pf.createPersistentProperty("DriveSubsystem", 1.5); } + //A method that lets the outside user get if precision mode is on/off +public boolean getPrecisionModeActive() { + return isPrecisionModeActive; +} + 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 // according to the value of leftPower: + if(isPrecisionModeActive) { //precision mode is on when its true & will divide by 2 + leftPower = leftPower * 0.5; + rightPower = rightPower * 0.5; + } frontLeft.setPower(leftPower); - // TODO: Add code to set the right motors to the rightPower value. - + frontRight.setPower(rightPower); } @@ -76,6 +86,7 @@ public void move(XYPair translate, double rotate) { public double getLeftTotalDistance() { // TODO: Auto-generated method stub return 0; + } @Override diff --git a/src/main/java/competition/subsystems/drive/commands/ArcadeDriveWithJoysticksCommand.java b/src/main/java/competition/subsystems/drive/commands/ArcadeDriveWithJoysticksCommand.java index 03d0dff..92672d4 100644 --- a/src/main/java/competition/subsystems/drive/commands/ArcadeDriveWithJoysticksCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/ArcadeDriveWithJoysticksCommand.java @@ -15,6 +15,7 @@ public class ArcadeDriveWithJoysticksCommand extends BaseCommand { public ArcadeDriveWithJoysticksCommand(DriveSubsystem driveSubsystem, OperatorInterface oi) { this.operatorInterface = oi; this.drive = driveSubsystem; + this.addRequirements(drive); } @Override @@ -24,6 +25,14 @@ public void initialize() { @Override public void execute() { + //this is from tank drive, make it so your only using one joystick to move forward & backwards. + //Left value will only move the left wheels, Right value will only move the right wheels. + //Move both value to move forwards, try to make it so both moves + //leftvector = left joystick, xy value = xy axis + double xValue = operatorInterface.gamepad.getLeftVector().getX(); + double yValue = operatorInterface.gamepad.getLeftVector().getY(); + // when calling x and y values from the operator interface, use leftJoystick. + //1 is full forward -1 is full backwards + drive.tankDrive(yValue-xValue,yValue+xValue); //only use 2 values, however using the +, - can bypass that. } - } diff --git a/src/main/java/competition/subsystems/drive/commands/DriveToOrientationCommand.java b/src/main/java/competition/subsystems/drive/commands/DriveToOrientationCommand.java index 832771f..1773060 100644 --- a/src/main/java/competition/subsystems/drive/commands/DriveToOrientationCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/DriveToOrientationCommand.java @@ -2,43 +2,79 @@ import javax.inject.Inject; +import competition.subsystems.pose.PoseSubsystem; import xbot.common.command.BaseCommand; import competition.subsystems.drive.DriveSubsystem; +import static java.lang.Math.abs; + public class DriveToOrientationCommand extends BaseCommand { DriveSubsystem drive; + PoseSubsystem pose; + double goal; + double currentPostion; + double previousPos; + @Inject - public DriveToOrientationCommand(DriveSubsystem driveSubsystem) { + public DriveToOrientationCommand(DriveSubsystem driveSubsystem, PoseSubsystem pose) { this.drive = driveSubsystem; + this.pose = pose; } public void setTargetHeading(double heading) { + goal = heading; // heading = direction you want face in degrees // This method will be called by the test, and will give you a goal heading. + if (goal > 180) { + goal = goal - 360; //get the position between 0 to 180 + } // You'll need to remember this target position and use it in your calculations. } @Override public void initialize() { - // If you have some one-time setup, do it here. + } @Override public void execute() { // 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 - // target position - - // How you do this is up to you. If you get stuck, ask a mentor or student for - // some hints! + // - Gets the robot stop (or at least be moving really really slowly) at the target position + //pose.getCurrentHeading(); //use current and goal/target & where you are facing rn + //always turn to left, but turn to right at some point + currentPostion = pose.getCurrentHeading().getDegrees(); + double speed = pose.getCurrentHeading().getDegrees() - previousPos; + double error = goal - pose.getCurrentHeading().getDegrees(); + double power = error - speed; + // higher distance error higher power,more power less speed ||| power = error - speed + drive.tankDrive(-power, power); + previousPos = pose.getCurrentHeading().getDegrees(); } + @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; + double error = pose.getCurrentHeading().getDegrees() - goal; + double speed = pose.getCurrentHeading().getDegrees() - previousPos; + + return Math.abs(error) < 0.05 && Math.abs(speed) < 0.05; //simple version of both of the ones at the bottom + + + // boolean closeEnough = Math.abs(error) < 0.05; simple version of the one at the bottom + // boolean slowEnough = Math.abs(speed) < 0.05; simple version of the one at the bottom + + // if (Math.abs(error) < 0.05) { + // closeEnough = true; + // } + + // if (Math.abs(speed) < 0.05) { + // slowEnough = true; + // } + // return closeEnough && slowEnough; + + } } + diff --git a/src/main/java/competition/subsystems/drive/commands/DriveToPositionCommand.java b/src/main/java/competition/subsystems/drive/commands/DriveToPositionCommand.java index 036e1f4..acf5c4f 100644 --- a/src/main/java/competition/subsystems/drive/commands/DriveToPositionCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/DriveToPositionCommand.java @@ -5,21 +5,34 @@ import xbot.common.command.BaseCommand; import competition.subsystems.drive.DriveSubsystem; import competition.subsystems.pose.PoseSubsystem; +import xbot.common.properties.DoubleProperty; +import xbot.common.properties.PropertyFactory; public class DriveToPositionCommand extends BaseCommand { DriveSubsystem drive; PoseSubsystem pose; + double targetPosition; + DoubleProperty pProperty; + DoubleProperty dProperty; + double previousPosition = 0; + + //DriveSubsystem driveSubsystem @Inject - public DriveToPositionCommand(DriveSubsystem driveSubsystem, PoseSubsystem pose) { + public DriveToPositionCommand(DriveSubsystem driveSubsystem, PoseSubsystem pose, PropertyFactory propertyFactory) { + propertyFactory.setPrefix(this); this.drive = driveSubsystem; this.pose = pose; + + pProperty = propertyFactory.createPersistentProperty("p", 1); + dProperty = propertyFactory.createPersistentProperty("d", 1); } public void setTargetPosition(double position) { // 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. + this.targetPosition = position; } @Override @@ -29,23 +42,43 @@ public void initialize() { @Override public void execute() { + //starts at 0, stops at 300. Slowly goes up by 1 (momentum) << we wanna remove it, so we can stop exactly on 300 // Here you'll need to figure out a technique that: // - Gets the robot to move to the target position - // - 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(); + // - Gets the robot stop (or at least be moving really really slowly) at the target position + //y=mx+b , power = p*(error) + d or - d * speed + //d = deriative, speed = "velocity" how fast your going, error = distance from goal p, = proportion + + //stop when you reach target + //cordinates or target/end position in the brackets + + double error = this.targetPosition - pose.getPosition(); +//faster speed less power, further away more power, + // if (error < .001) { + // drive.tankDrive(0, 0); + // }else if(error < 2) { + // drive.tankDrive(0.005, 0.005); + + // } else { + // drive.tankDrive(0.10, 0.10); + double speed = pose.getPosition() - previousPosition; + double power = pProperty.get() * error - dProperty.get() * speed; + + drive.tankDrive(power, power); + previousPosition = pose.getPosition(); //previous to current position + + + } @Override public boolean isFinished() { + + //speed, current position, + return true; + // Modify this to return true once you have met your goal, // and you're moving fairly slowly (ideally stopped) - return false; } } diff --git a/src/main/java/competition/subsystems/drive/commands/TankDriveWithJoysticksCommand.java b/src/main/java/competition/subsystems/drive/commands/TankDriveWithJoysticksCommand.java index 088dcc2..3935c61 100644 --- a/src/main/java/competition/subsystems/drive/commands/TankDriveWithJoysticksCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/TankDriveWithJoysticksCommand.java @@ -34,12 +34,18 @@ public void execute() { // Get values from the joysticks: // 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 + //Left value will only move the left wheels, Right value will only move the right wheels. + //Move both value to move forwards + 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); + + + + + 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..b67c901 100644 --- a/src/main/java/competition/subsystems/drive/commands/TogglePrecisionDriveCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/TogglePrecisionDriveCommand.java @@ -16,6 +16,8 @@ public TogglePrecisionDriveCommand(DriveSubsystem driveSubsystem) { @Override public void initialize() { + drive.isPrecisionModeActive = !drive.isPrecisionModeActive; //same as the highlighted part + // Here, you want to call the DriveSubsystem and tell it to change its precision // mode. // This means you'll need to add a new method into DriveSubsystem, and there are @@ -32,19 +34,8 @@ public void initialize() { // TankDrive method. } - @Override - public void execute() { - // No need to put any code here - since we want the toggle to run once, - // initialize() is the - // best place to put the code. - } - @Override public boolean isFinished() { - // Commands keep running until they are finished. - // Since we want this command to just run once (toggling precision mode), we - // say that the command is finished right away. return true; } - } diff --git a/src/main/java/competition/subsystems/drive/commands/TurnLeft90DegreesCommand.java b/src/main/java/competition/subsystems/drive/commands/TurnLeft90DegreesCommand.java index 392ae70..c8d3ea6 100644 --- a/src/main/java/competition/subsystems/drive/commands/TurnLeft90DegreesCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/TurnLeft90DegreesCommand.java @@ -5,24 +5,50 @@ import xbot.common.command.BaseCommand; import competition.subsystems.drive.DriveSubsystem; import competition.subsystems.pose.PoseSubsystem; +import xbot.common.properties.DoubleProperty; +import xbot.common.properties.PropertyFactory; public class TurnLeft90DegreesCommand extends BaseCommand { DriveSubsystem drive; PoseSubsystem pose; + double target; + double previousPos = 0; + @Inject public TurnLeft90DegreesCommand(DriveSubsystem driveSubsystem, PoseSubsystem pose) { this.drive = driveSubsystem; this.pose = pose; + + } @Override public void initialize() { + target = pose.getCurrentHeading().getDegrees() + 90; //90 is the target not the error + // 150+90=240 .. 240-360=-120 might have to minus 360 to target + if (target > 180) { //target is out of bounds so bigger than 180. + target = target - 360; //since its out of bounds and bigger than 180, you have to subtract it by 360 + } + } @Override public void execute() { + //turn 90 degrees + double speed = pose.getCurrentHeading().getDegrees() - previousPos; + double power = target - speed; + double error = target - pose.getCurrentHeading().getDegrees(); + // higher distance error higher power,more power less speed ||| power = error - speed + drive.tankDrive(-power, power); + + } -} + @Override + public boolean isFinished() { + + return false; + } +} \ No newline at end of file diff --git a/src/main/java/competition/subsystems/pose/PoseSubsystem.java b/src/main/java/competition/subsystems/pose/PoseSubsystem.java index 03d09cb..b8ca513 100644 --- a/src/main/java/competition/subsystems/pose/PoseSubsystem.java +++ b/src/main/java/competition/subsystems/pose/PoseSubsystem.java @@ -16,7 +16,7 @@ public class PoseSubsystem extends BasePoseSubsystem { private final DriveSubsystem drive; public double scalingFactorFromRotationsToMeters = 0.5; - + @Inject public PoseSubsystem(XGyroFactory gyroFactory, PropertyFactory propManager, DriveSubsystem drive) { super(gyroFactory, propManager); @@ -24,7 +24,7 @@ public PoseSubsystem(XGyroFactory gyroFactory, PropertyFactory propManager, Driv } public double getPosition() { - return (getLeftDriveDistance() + getRightDriveDistance()) / 2.0; + return (getLeftDriveDistance() + getRightDriveDistance()) / 2.0; } @Override