From 40f26024fde6124df4dd8df9482785d8d1acdd84 Mon Sep 17 00:00:00 2001 From: karmalover-ca Date: Tue, 27 Feb 2024 22:43:42 -0500 Subject: [PATCH 1/3] adds gyro system and navx vendor deps --- .vscode/settings.json | 5 +- src/main/java/frc/robot/RobotContainer.java | 4 + src/main/java/frc/robot/RobotMap.java | 5 + src/main/java/frc/robot/Tools.java | 7 + .../java/frc/robot/commands/GyroCommand.java | 46 +++++ src/main/java/frc/robot/subsystems/Gyro.java | 162 ++++++++++++++++++ vendordeps/NavX.json | 40 +++++ 7 files changed, 268 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/Tools.java create mode 100644 src/main/java/frc/robot/commands/GyroCommand.java create mode 100644 src/main/java/frc/robot/subsystems/Gyro.java create mode 100644 vendordeps/NavX.json diff --git a/.vscode/settings.json b/.vscode/settings.json index 8be11f2..10907c1 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -25,5 +25,8 @@ } }, ], - "java.test.defaultConfig": "WPIlibUnitTests" + "java.test.defaultConfig": "WPIlibUnitTests", + "cSpell.words": [ + "AHRS" + ] } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a90836a..5561715 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,19 +7,23 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.commands.DriveCommand; +import frc.robot.commands.GyroCommand; import frc.robot.commands.instant.ShiftDownCommand; import frc.robot.commands.instant.ShiftUpCommand; import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Gyro; import frc.robot.subsystems.Drivetrain.Gear; public class RobotContainer { public Drivetrain drivetrain = new Drivetrain(RobotMap.leftController1, RobotMap.rightController1, RobotMap.driveShitSolenoid); + public Gyro gyro = new Gyro(); public RobotContainer() { configureBindings(); drivetrain.setDefaultCommand(new DriveCommand(drivetrain, OI.leftDriveSupplier, OI.rightDriveSupplier)); + gyro.setDefaultCommand(new GyroCommand(gyro)); } private void configureBindings() { diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index e59fa80..cfa7dc5 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -1,5 +1,6 @@ package frc.robot; +import com.kauailabs.navx.frc.AHRS; import com.revrobotics.AlternateEncoderType; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; @@ -7,6 +8,7 @@ import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.PneumaticsModuleType; +import edu.wpi.first.wpilibj.SPI; public class RobotMap { @@ -31,6 +33,9 @@ private class PCM { public static final DoubleSolenoid driveShitSolenoid = new DoubleSolenoid(PneumaticsModuleType.CTREPCM, PCM.DRIVE_SHIFT_DOWN, PCM.DRIVE_SHIFT_UP); + // Gyro + public static final AHRS gyro = new AHRS(SPI.Port.kMXP); + // Static initializer will be run on first reference to RobotMap (stealing code from greg) static { diff --git a/src/main/java/frc/robot/Tools.java b/src/main/java/frc/robot/Tools.java new file mode 100644 index 0000000..f71e253 --- /dev/null +++ b/src/main/java/frc/robot/Tools.java @@ -0,0 +1,7 @@ +package frc.robot; + +public class Tools { + public static double getTimeSeconds() { + return (double) System.currentTimeMillis() / 1000.0; + } +} diff --git a/src/main/java/frc/robot/commands/GyroCommand.java b/src/main/java/frc/robot/commands/GyroCommand.java new file mode 100644 index 0000000..929dccf --- /dev/null +++ b/src/main/java/frc/robot/commands/GyroCommand.java @@ -0,0 +1,46 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Gyro; + +public class GyroCommand extends Command { + private boolean finished = false; + + Gyro gyro; + + public GyroCommand(Gyro gyro) { + addRequirements(gyro); + this.gyro = gyro; + } + + @Override + public void initialize() { + System.out.println("this works"); + } + + @Override + public void execute() { + double current_angle = gyro.getAngle(); + while (current_angle < 0) { // convert gyro angle to angle within -180 to 180 range + current_angle += 360; + } + current_angle %= 360; + if (current_angle > 180) { + current_angle = -360 + current_angle; + } + SmartDashboard.putNumber("Gyro", current_angle); + } + + @Override + public boolean isFinished() { + return finished; + } + + public void end() { + } + + public void interrupted() { + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Gyro.java b/src/main/java/frc/robot/subsystems/Gyro.java new file mode 100644 index 0000000..084e3f2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Gyro.java @@ -0,0 +1,162 @@ +package frc.robot.subsystems; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotMap; +import frc.robot.Tools; + +/** + * + */ +public class Gyro extends SubsystemBase { + @SuppressWarnings("unused") + private double offset_per_second = 0; + private double[] sample_1 = {0, 1}; + private boolean first = true; + @SuppressWarnings("unused") + private double reset_time = 0; + private double prev_sample; + + public void initDefaultCommand() { + // Set the default command for a subsystem here. + } + + public void calibrate() { + RobotMap.gyro.zeroYaw(); // TODO make it so it actually zeros instead of zeroing just yaw + sample_1[0] = Tools.getTimeSeconds(); + sample_1[1] = getAngle(); + + } + + public void reset() { + RobotMap.gyro.reset(); + } + + public Rotation2d getRotation2d() { + return RobotMap.gyro.getRotation2d(); + } + + public void resetAngle() { + if (first) { + offset_per_second = (getAngle() - sample_1[1]) / (Tools.getTimeSeconds() - sample_1[0]); + first = false; + } + reset_time = Tools.getTimeSeconds(); + prev_sample = -RobotMap.gyro.getAngle(); + } + + public double getAngle() { + return -RobotMap.gyro.getAngle() - prev_sample;// - (Tools.getTimeSeconds() - reset_time) * offset_per_second; // it is negative to make counterclockwise motion increase the angle like normal math + } + + public double getPitch() { + return RobotMap.gyro.getRoll(); + } + + public double getAbsoluteAngle() { + return -RobotMap.gyro.getAngle(); + } + + public double[] getStraightOutput(double l, double r) { + final double ANGLE_TOLERANCE = 1; + double k_p = 0.01; + double l_out = l; + double r_out = r; + double current_angle = getAngle(); + + if (Math.abs(current_angle) > ANGLE_TOLERANCE) { // adjust values to compensate for turning drift + double modifier = current_angle * k_p; // make amount of correction proportional to angle + l_out += modifier; + r_out -= modifier; + } + + l_out = l_out > 1 ? 1 : l_out; // ensure values are within -1 to 1 range + l_out = l_out < -1 ? -1 : l_out; + r_out = r_out > 1 ? 1 : r_out; + r_out = r_out < -1 ? -1 : r_out; + + return new double[] {l_out, r_out}; + } + + public double[] getStraightOutput(double l, double r, double target) { + final double ANGLE_TOLERANCE = 1; + double k_p = 0.03; + double l_out = l; + double r_out = r; + double current_angle = getAbsoluteAngle() - target; + + if (Math.abs(current_angle) > ANGLE_TOLERANCE) { // adjust values to compensate for turning drift + double modifier = current_angle * k_p; // make amount of correction proportional to angle + l_out += modifier; + r_out -= modifier; + } + + l_out = l_out > 1 ? 1 : l_out; // ensure values are within -1 to 1 range + l_out = l_out < -1 ? -1 : l_out; + r_out = r_out > 1 ? 1 : r_out; + r_out = r_out < -1 ? -1 : r_out; + + return new double[] {l_out, r_out}; + + } + + public double[] getArcadeGyroOutput(double x, double y) { + if (x == 0 && y == 0) { + return new double[] {0, 0}; + } + + double joystick_speed; + double joystick_angle; + double current_angle; + double angle_difference; + final double MAX_ARCADE_SPEED = 0.8; + final double FORWARD_THRESHOLD = 40; + + double l_gyro_out = 0; + double r_gyro_out = 0; + + joystick_speed = Math.abs(x) > Math.abs(y) ? Math.abs(x) : Math.abs(y); // use maximum axis to create speed + joystick_speed = Math.abs(joystick_speed) > Math.abs(MAX_ARCADE_SPEED) ? MAX_ARCADE_SPEED : joystick_speed; // ensure speed is not over the limit + + joystick_angle = -Math.toDegrees(Math.atan2(y, x)) - 90; // convert x y to angle within -180 to 180 range + if (joystick_angle < -180) { + joystick_angle = 360 + joystick_angle; + } + + current_angle = getAngle(); + while (current_angle < 0) { // convert gyro angle to angle within -180 to 180 range + current_angle += 360; + } + current_angle %= 360; + if (current_angle > 180) { + current_angle = -360 + current_angle; + } + + angle_difference = current_angle - joystick_angle; + if (angle_difference < -180) { + angle_difference = 360 + angle_difference; + } + else if (angle_difference > 180) { + angle_difference -= 360; + } + + if (angle_difference > FORWARD_THRESHOLD) { + l_gyro_out = joystick_speed; + r_gyro_out = -joystick_speed; + } + else if (angle_difference < -FORWARD_THRESHOLD) { + l_gyro_out = -joystick_speed; + r_gyro_out = joystick_speed; + } + else if (angle_difference < FORWARD_THRESHOLD && angle_difference > 0) { // rotate clockwise + l_gyro_out = joystick_speed; + r_gyro_out = (joystick_speed - angle_difference / FORWARD_THRESHOLD * joystick_speed * 2); + } + else if (angle_difference > -FORWARD_THRESHOLD && angle_difference < 0) { // rotate counterclockwise + l_gyro_out = (joystick_speed + angle_difference / FORWARD_THRESHOLD * joystick_speed * 2); + r_gyro_out = joystick_speed; + } + + return new double[] {l_gyro_out, r_gyro_out}; + } +} \ No newline at end of file diff --git a/vendordeps/NavX.json b/vendordeps/NavX.json new file mode 100644 index 0000000..e978a5f --- /dev/null +++ b/vendordeps/NavX.json @@ -0,0 +1,40 @@ +{ + "fileName": "NavX.json", + "name": "NavX", + "version": "2024.1.0", + "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", + "frcYear": "2024", + "mavenUrls": [ + "https://dev.studica.com/maven/release/2024/" + ], + "jsonUrl": "https://dev.studica.com/releases/2024/NavX.json", + "javaDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-frc-java", + "version": "2024.1.0" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.kauailabs.navx.frc", + "artifactId": "navx-frc-cpp", + "version": "2024.1.0", + "headerClassifier": "headers", + "sourcesClassifier": "sources", + "sharedLibrary": false, + "libName": "navx_frc", + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "linuxraspbian", + "linuxarm32", + "linuxarm64", + "linuxx86-64", + "osxuniversal", + "windowsx86-64" + ] + } + ] +} \ No newline at end of file From 82d2cbf2670a467fe6c38cae5aa20cfbd186c1a8 Mon Sep 17 00:00:00 2001 From: Liam Pilson Date: Fri, 1 Mar 2024 19:45:35 -0500 Subject: [PATCH 2/3] removes GyroCommand, and moves dashboard output to subsystem --- src/main/java/frc/robot/RobotContainer.java | 1 - .../java/frc/robot/commands/GyroCommand.java | 46 ------------------- src/main/java/frc/robot/subsystems/Gyro.java | 14 ++++++ 3 files changed, 14 insertions(+), 47 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/GyroCommand.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5561715..9049312 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,7 +23,6 @@ public RobotContainer() { configureBindings(); drivetrain.setDefaultCommand(new DriveCommand(drivetrain, OI.leftDriveSupplier, OI.rightDriveSupplier)); - gyro.setDefaultCommand(new GyroCommand(gyro)); } private void configureBindings() { diff --git a/src/main/java/frc/robot/commands/GyroCommand.java b/src/main/java/frc/robot/commands/GyroCommand.java deleted file mode 100644 index 929dccf..0000000 --- a/src/main/java/frc/robot/commands/GyroCommand.java +++ /dev/null @@ -1,46 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Gyro; - -public class GyroCommand extends Command { - private boolean finished = false; - - Gyro gyro; - - public GyroCommand(Gyro gyro) { - addRequirements(gyro); - this.gyro = gyro; - } - - @Override - public void initialize() { - System.out.println("this works"); - } - - @Override - public void execute() { - double current_angle = gyro.getAngle(); - while (current_angle < 0) { // convert gyro angle to angle within -180 to 180 range - current_angle += 360; - } - current_angle %= 360; - if (current_angle > 180) { - current_angle = -360 + current_angle; - } - SmartDashboard.putNumber("Gyro", current_angle); - } - - @Override - public boolean isFinished() { - return finished; - } - - public void end() { - } - - public void interrupted() { - } - -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Gyro.java b/src/main/java/frc/robot/subsystems/Gyro.java index 084e3f2..f5a6317 100644 --- a/src/main/java/frc/robot/subsystems/Gyro.java +++ b/src/main/java/frc/robot/subsystems/Gyro.java @@ -1,6 +1,7 @@ package frc.robot.subsystems; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.RobotMap; import frc.robot.Tools; @@ -159,4 +160,17 @@ else if (angle_difference > -FORWARD_THRESHOLD && angle_difference < 0) { // rot return new double[] {l_gyro_out, r_gyro_out}; } + + @Override + public void periodic() { + double current_angle = getAngle(); + while (current_angle < 0) { // convert gyro angle to angle within -180 to 180 range + current_angle += 360; + } + current_angle %= 360; + if (current_angle > 180) { + current_angle = -360 + current_angle; + } + SmartDashboard.putNumber("Gyro", current_angle); + } } \ No newline at end of file From f8feed87e4a3eab0d1ce139d90ad08e9bab675c6 Mon Sep 17 00:00:00 2001 From: Liam Pilson Date: Fri, 1 Mar 2024 19:47:55 -0500 Subject: [PATCH 3/3] stupid vscode building when it shouldn't --- src/main/java/frc/robot/RobotContainer.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9049312..c839b67 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,7 +7,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.commands.DriveCommand; -import frc.robot.commands.GyroCommand; import frc.robot.commands.instant.ShiftDownCommand; import frc.robot.commands.instant.ShiftUpCommand; import frc.robot.subsystems.Drivetrain;