diff --git a/Diagrams/.obsidian/community-plugins.json b/Diagrams/.obsidian/community-plugins.json index db856a363b41..3c32eb2f31b3 100644 --- a/Diagrams/.obsidian/community-plugins.json +++ b/Diagrams/.obsidian/community-plugins.json @@ -1,3 +1,4 @@ [ - "advanced-canvas" + "advanced-canvas", + "theme-toggle" ] \ No newline at end of file diff --git a/Diagrams/.obsidian/plugins/theme-toggle/main.js b/Diagrams/.obsidian/plugins/theme-toggle/main.js new file mode 100644 index 000000000000..58e3b3ffe6ba --- /dev/null +++ b/Diagrams/.obsidian/plugins/theme-toggle/main.js @@ -0,0 +1,69 @@ +/* +THIS IS A GENERATED/BUNDLED FILE BY ESBUILD +if you want to view the source, please visit the github repository of this plugin +*/ + +var __defProp = Object.defineProperty; +var __getOwnPropDesc = Object.getOwnPropertyDescriptor; +var __getOwnPropNames = Object.getOwnPropertyNames; +var __hasOwnProp = Object.prototype.hasOwnProperty; +var __export = (target, all) => { + for (var name in all) + __defProp(target, name, { get: all[name], enumerable: true }); +}; +var __copyProps = (to, from, except, desc) => { + if (from && typeof from === "object" || typeof from === "function") { + for (let key of __getOwnPropNames(from)) + if (!__hasOwnProp.call(to, key) && key !== except) + __defProp(to, key, { get: () => from[key], enumerable: !(desc = __getOwnPropDesc(from, key)) || desc.enumerable }); + } + return to; +}; +var __toCommonJS = (mod) => __copyProps(__defProp({}, "__esModule", { value: true }), mod); + +// main.ts +var main_exports = {}; +__export(main_exports, { + default: () => ThemeToggle +}); +module.exports = __toCommonJS(main_exports); +var import_obsidian = require("obsidian"); +var ThemeToggle = class extends import_obsidian.Plugin { + async onload() { + this.ribbon = this.addRibbonIcon(this.getThemeIcon(), "Toggle to " + (this.getCurrentTheme() === "obsidian" ? "light" : "dark") + " mode", (evt) => { + this.app.changeTheme(this.getCurrentTheme() === "obsidian" ? "moonstone" : "obsidian"); + (0, import_obsidian.setIcon)(evt.target, this.getThemeIcon()); + evt.target.setAttr("aria-label", "Toggle to " + (this.getCurrentTheme() === "obsidian" ? "light" : "dark") + " mode"); + }); + this.ribbon.addClass("ribbon-theme-toggle-plugin"); + this.registerEvent( + this.app.workspace.on("css-change", () => { + setTimeout(() => { + (0, import_obsidian.setIcon)(this.ribbon, this.getCurrentTheme() === "obsidian" ? "sun" : "moon"); + this.ribbon.setAttr("aria-label", "Toggle to " + (this.getCurrentTheme() === "obsidian" ? "light" : "dark") + " mode"); + }, 10); + }) + ); + this.addCommand({ + id: "toggle-theme", + name: "Toggle theme", + callback: () => { + this.app.changeTheme(this.getCurrentTheme() === "obsidian" ? "moonstone" : "obsidian"); + (0, import_obsidian.setIcon)(this.ribbon, this.getCurrentTheme() === "obsidian" ? "sun" : "moon"); + } + }); + } + onunload() { + this.ribbon.remove(); + } + getCurrentTheme() { + let currentTheme = this.app.getTheme() === "obsidian" ? "obsidian" : "moonstone"; + return currentTheme; + } + getThemeIcon() { + let moonOrSunIcon = this.getCurrentTheme() === "obsidian" ? "sun" : "moon"; + return moonOrSunIcon; + } +}; + +/* nosourcemap */ \ No newline at end of file diff --git a/Diagrams/.obsidian/plugins/theme-toggle/manifest.json b/Diagrams/.obsidian/plugins/theme-toggle/manifest.json new file mode 100644 index 000000000000..43ebe4123cd1 --- /dev/null +++ b/Diagrams/.obsidian/plugins/theme-toggle/manifest.json @@ -0,0 +1,10 @@ +{ + "id": "theme-toggle", + "name": "Theme toggle", + "version": "0.1.6", + "minAppVersion": "0.15.0", + "description": "Dark/light theme toggle via ribbon icon or command", + "author": "@gapmiss", + "authorUrl": "https://github.com/gapmiss", + "isDesktopOnly": false +} diff --git a/Diagrams/.obsidian/workspace.json b/Diagrams/.obsidian/workspace.json index 2fc2bcc72d34..1b4a878220bb 100644 --- a/Diagrams/.obsidian/workspace.json +++ b/Diagrams/.obsidian/workspace.json @@ -17,14 +17,32 @@ "viewState": { "x": -670, "y": 70, - "zoom": -0.2612571403908114 + "zoom": -0.6381710945544748 } }, "icon": "lucide-layout-dashboard", "title": "File Structure" } + }, + { + "id": "3c98fecc6f5ddbb9", + "type": "leaf", + "state": { + "type": "canvas", + "state": { + "file": "Robot Wiring.canvas", + "viewState": { + "x": 210, + "y": 40, + "zoom": -0.39938423496735836 + } + }, + "icon": "lucide-layout-dashboard", + "title": "Robot Wiring" + } } - ] + ], + "currentTab": 1 } ], "direction": "vertical" @@ -180,12 +198,21 @@ "daily-notes:Open today's daily note": false, "templates:Insert template": false, "command-palette:Open command palette": false, - "bases:Create new base": false + "bases:Create new base": false, + "theme-toggle:Toggle to dark mode": false } }, - "active": "d3b4cee07f48b93b", + "active": "3c98fecc6f5ddbb9", "lastOpenFiles": [ + "File Structure.canvas", + "File Structure Dark.png", + "File Structure Light.png", + "Robot Wiring Light.png", + "Robot Wiring Dark.png", + "Robot Wiring.canvas", + "File Structure_Light.png", + "Robot_Wiring.png", "File Structure.png", - "File Structure Light.png" + "Robot Controls.png" ] } \ No newline at end of file diff --git a/Diagrams/File Structure Dark.png b/Diagrams/File Structure Dark.png new file mode 100644 index 000000000000..0cc9af8efa8a Binary files /dev/null and b/Diagrams/File Structure Dark.png differ diff --git a/Diagrams/File Structure Light.png b/Diagrams/File Structure Light.png new file mode 100644 index 000000000000..31f64079c9eb Binary files /dev/null and b/Diagrams/File Structure Light.png differ diff --git a/Diagrams/File Structure.png b/Diagrams/File Structure.png deleted file mode 100644 index 093392959be2..000000000000 Binary files a/Diagrams/File Structure.png and /dev/null differ diff --git a/Diagrams/Robot_Controls.png b/Diagrams/Robot Controls.png similarity index 100% rename from Diagrams/Robot_Controls.png rename to Diagrams/Robot Controls.png diff --git a/Diagrams/Robot Wiring Dark.png b/Diagrams/Robot Wiring Dark.png new file mode 100644 index 000000000000..b5a73b3e1e31 Binary files /dev/null and b/Diagrams/Robot Wiring Dark.png differ diff --git a/Diagrams/Robot Wiring Light.png b/Diagrams/Robot Wiring Light.png new file mode 100644 index 000000000000..760215b579a6 Binary files /dev/null and b/Diagrams/Robot Wiring Light.png differ diff --git a/Diagrams/Wiring.canvas b/Diagrams/Robot Wiring.canvas similarity index 74% rename from Diagrams/Wiring.canvas rename to Diagrams/Robot Wiring.canvas index ea4cd43f646f..80d89dca2987 100644 --- a/Diagrams/Wiring.canvas +++ b/Diagrams/Robot Wiring.canvas @@ -1,104 +1,71 @@ { "nodes":[ - {"id":"85dd3f12ac72d900","type":"group","x":-360,"y":-440,"width":380,"height":260,"label":"Legend"}, + {"id":"85dd3f12ac72d900","type":"group","x":-440,"y":-440,"width":380,"height":260,"label":"Legend"}, { - "id":"c2eee2656a3ddfa1", - "type":"text", - "text":"Pinpoint Computer\nI2C Port 1", - "styleAttributes":{"textAlign":"center"}, - "x":80, - "y":-260, - "width":160, - "height":60, - "color":"6" - }, - { - "id":"e20a52ef21d57dcf", - "type":"text", - "text":"LED Controller\nI2C Port 2", - "styleAttributes":{"textAlign":"center"}, - "x":80, - "y":-340, - "width":160, - "height":60, - "color":"6" - }, - { - "id":"f9a634b23b5064c5", + "id":"bfb02b2a92b84ad2", "type":"text", - "text":"Servos", + "text":"Belt Servo\nConfiguration Name: \"belt\"\nConfiguration Type: CRServo\nPort 0 on Control Hub\nPort 1 on Servo Power Injector", "styleAttributes":{"textAlign":"center"}, - "x":-340, - "y":-340, - "width":160, - "height":60, + "x":-300, + "y":-80, + "width":280, + "height":140, "color":"4" }, { - "id":"48faa728dd92705d", - "type":"text", - "text":"Motors", - "styleAttributes":{"textAlign":"center"}, - "x":-160, - "y":-340, - "width":160, - "height":60, - "color":"5" - }, - { - "id":"26e535ad3414017c", + "id":"41c5560b3c8edc13", "type":"text", - "text":"Control System", + "text":"Hood Servo\nConfiguration Name: \"hood\"\nConfiguration Type: Servo\nPort 1 on Control Hub\nPort 2 on Servo Power Injector", "styleAttributes":{"textAlign":"center"}, - "x":-340, - "y":-260, - "width":160, - "height":60, - "color":"2" + "x":-300, + "y":80, + "width":280, + "height":139, + "color":"4" }, { - "id":"2cad408dc2aae848", + "id":"b2918b70ae0b4303", "type":"text", - "text":"I2C Devices", + "text":"Gate Servo\nConfiguration Name: \"gate\"\nConfiguration Type: Servo\nPort 2 on Control Hub\nPort 3 on Servo Power Injector", "styleAttributes":{"textAlign":"center"}, - "x":-160, - "y":-260, - "width":160, - "height":60, - "color":"6" + "x":-300, + "y":240, + "width":280, + "height":140, + "color":"4" }, { - "id":"a0abe0901b65ac56", + "id":"8c94376760075734", "type":"text", - "text":"Upper Color Sensors\nI2C Ports 0-1", + "text":"Transfer Servo\nConfiguration Name: \"transfer\"\nConfiguration Type: Servo\nPort 3 on Control Hub\nPort 4 on Servo Power Injector", "styleAttributes":{"textAlign":"center"}, - "x":300, - "y":-340, - "width":160, - "height":60, - "color":"6" + "x":-300, + "y":400, + "width":280, + "height":139, + "color":"4" }, { - "id":"e3fdefe17fe2310d", + "id":"f667221e45b8fc86", "type":"text", - "text":"Analog Devices", + "text":"Intake Motor\nConfiguration Name: \"intake\"\nConfiguration Type: \"\"\nPort 0 - No Encoder", "styleAttributes":{"textAlign":"center"}, - "x":-160, - "y":-420, - "width":160, - "height":60, - "color":"3" + "x":520, + "y":-80, + "width":320, + "height":140, + "color":"5" }, { - "id":"7a1b2108fa3bc30e", + "id":"0075a4079a1a472d", "type":"text", - "text":"Hall Effect Sensor\nPort 0", + "text":"Front Right Drive Motor\nConfiguration Name: \"FR\"\nConfiguration Type: \"\"\nPort 1 - No Encoder", "styleAttributes":{"textAlign":"center"}, - "x":80, - "y":-420, - "width":160, - "height":60, - "color":"1" + "x":520, + "y":80, + "width":320, + "height":140, + "color":"5" }, { "id":"81a20594db9883e0", @@ -111,6 +78,17 @@ "height":60, "color":"2" }, + { + "id":"c2eee2656a3ddfa1", + "type":"text", + "text":"Pinpoint Computer\nConfiguration Name: \"pinpoint\"\nI2C Port 1", + "styleAttributes":{"textAlign":"center"}, + "x":80, + "y":-260, + "width":280, + "height":80, + "color":"6" + }, { "id":"d9c005396002882a", "type":"text", @@ -123,55 +101,66 @@ "color":"2" }, { - "id":"f667221e45b8fc86", + "id":"5587942ea5903dbe", "type":"text", - "text":"Intake Motor\nPort 0 - No Encoder", + "text":"Back Right Drive Motor\nConfiguration Name: \"BR\"\nConfiguration Type: \"\"\nPort 2 - No Encoder", "styleAttributes":{"textAlign":"center"}, "x":520, - "y":-60, - "width":260, - "height":60, + "y":240, + "width":320, + "height":140, "color":"5" }, { - "id":"0075a4079a1a472d", + "id":"c6d790ec667c829f", "type":"text", - "text":"Front Right Drive Motor\nPort 1 - No Encoder", + "text":"Turret Rotation Motor\nConfiguration Name: \"turret\"\nConfiguration Type: \"\"\nPort 3 - With Encoder", "styleAttributes":{"textAlign":"center"}, "x":520, - "y":20, - "width":260, - "height":60, + "y":400, + "width":320, + "height":140, "color":"5" }, { - "id":"5587942ea5903dbe", + "id":"a0abe0901b65ac56", "type":"text", - "text":"Back Right Drive Motor\nPort 2 - No Encoder", + "text":"Upper Color Sensors\nConfiguration Names: \"color1\", \"color2\"\nI2C Ports 0-1", "styleAttributes":{"textAlign":"center"}, "x":520, - "y":100, - "width":260, + "y":-360, + "width":340, + "height":80, + "color":"6" + }, + { + "id":"e3fdefe17fe2310d", + "type":"text", + "text":"Analog Devices", + "styleAttributes":{"textAlign":"center"}, + "x":-240, + "y":-420, + "width":160, "height":60, - "color":"5" + "color":"3" }, { - "id":"c6d790ec667c829f", + "id":"48faa728dd92705d", "type":"text", - "text":"Turret Rotation Motor\nPort 3 - With Encoder", + "text":"Motors", "styleAttributes":{"textAlign":"center"}, - "x":520, - "y":180, - "width":260, + "x":-240, + "y":-340, + "width":160, "height":60, "color":"5" }, { - "id":"d97803361edf47ba", + "id":"2cad408dc2aae848", "type":"text", - "text":"Lower Color Sensors\nI2C Ports 2-3", + "text":"I2C Devices", "styleAttributes":{"textAlign":"center"}, - "x":300, + "x":-240, "y":-260, "width":160, "height":60, @@ -189,117 +178,114 @@ "color":"2" }, { - "id":"88b48890e8f34fc1", + "id":"b7514b6ae54e27cf", "type":"text", - "text":"The Servo Power Injector ports correspond to the Control Hub Ports.\nChub P0 -> SPI P1, Chub P1 -> SPI P2, etc. The given ports are for the SPI, not the control hub.", - "styleAttributes":{ - "textAlign":"center", - "shape":null, - "border":null - }, - "x":-540, - "y":-40, + "text":"Digital Devices", + "styleAttributes":{"textAlign":"center"}, + "x":-420, + "y":-420, "width":160, - "height":260 + "height":60, + "color":"1" }, { - "id":"bfb02b2a92b84ad2", + "id":"f9a634b23b5064c5", "type":"text", - "text":"Belt Servo - Port 1", + "text":"Servos", "styleAttributes":{"textAlign":"center"}, - "x":-300, - "y":-60, - "width":200, + "x":-420, + "y":-340, + "width":160, "height":60, "color":"4" }, { - "id":"440a120e5375c485", + "id":"26e535ad3414017c", "type":"text", - "text":"Front Left Drive Motor\nPort 0 - No Encoder", + "text":"Control System", "styleAttributes":{"textAlign":"center"}, - "x":80, - "y":-60, - "width":260, + "x":-420, + "y":-260, + "width":160, "height":60, - "color":"5" + "color":"2" }, { "id":"76edd4abcf05a2a3", "type":"text", - "text":"Back Left Drive Motor\nPort 1 - No Encoder", + "text":"Back Left Drive Motor\nConfiguration Name: \"BL\"\nConfiguration Type: \"\"\nPort 3 - No Encoder", "styleAttributes":{"textAlign":"center"}, "x":80, - "y":20, - "width":260, - "height":60, + "y":400, + "width":320, + "height":140, "color":"5" }, { - "id":"336d50b5d4cb968f", + "id":"440a120e5375c485", "type":"text", - "text":"Flywheel Motor 1\nPort 2 - With Encoder", + "text":"Front Left Drive Motor\nConfiguration Name: \"FL\"\nConfiguration Type: \"\"\nPort 2 - No Encoder", "styleAttributes":{"textAlign":"center"}, "x":80, - "y":100, - "width":260, - "height":60, + "y":240, + "width":320, + "height":140, "color":"5" }, { - "id":"f8b39583a022e492", + "id":"336d50b5d4cb968f", "type":"text", - "text":"Flywheel Motor 2\nPort 3 - No Encoder", + "text":"Flywheel Motor 1\nConfiguration Name: \"flywheel\"\nConfiguration Type: \"\"\nPort 0 - With Encoder", "styleAttributes":{"textAlign":"center"}, "x":80, - "y":180, - "width":260, - "height":60, + "y":-80, + "width":320, + "height":140, "color":"5" }, { - "id":"41c5560b3c8edc13", + "id":"f8b39583a022e492", "type":"text", - "text":"Hood Servo - Port 2", + "text":"Flywheel Motor 2\nConfiguration Name: \"flywheel2\"\nConfiguration Type: \"\"\nPort 1 - No Encoder", "styleAttributes":{"textAlign":"center"}, - "x":-300, - "y":20, - "width":200, - "height":60, - "color":"4" + "x":80, + "y":80, + "width":320, + "height":140, + "color":"5" }, { - "id":"b2918b70ae0b4303", + "id":"e20a52ef21d57dcf", "type":"text", - "text":"Gate Servo - Port 3", + "text":"LED Controller\nConfiguration Name: \"prism\"\nI2C Port 2", "styleAttributes":{"textAlign":"center"}, - "x":-300, - "y":100, - "width":200, - "height":60, - "color":"4" + "x":80, + "y":-360, + "width":280, + "height":80, + "color":"6" }, { - "id":"8c94376760075734", + "id":"7a1b2108fa3bc30e", "type":"text", - "text":"Transfer Servo - Port 4", + "text":"Hall Effect Sensor\nConfiguration Name: \"hall\"\nPort 0", "styleAttributes":{"textAlign":"center"}, - "x":-300, - "y":180, - "width":200, - "height":60, - "color":"4" + "x":80, + "y":-460, + "width":280, + "height":80, + "color":"1" }, { - "id":"b7514b6ae54e27cf", + "id":"d97803361edf47ba", "type":"text", - "text":"Digital Devices", + "text":"Lower Color Sensors\nConfiguration Names: \"color3\", \"color4\"\nI2C Ports 2-3", "styleAttributes":{"textAlign":"center"}, - "x":-340, - "y":-420, - "width":160, - "height":60, - "color":"1" + "x":520, + "y":-260, + "width":340, + "height":80, + "color":"6" } ], "edges":[ @@ -451,30 +437,30 @@ "toSide":"left" }, { - "id":"5c79a21468a43736", + "id":"21b147d9374eee41", "styleAttributes":{"pathfindingMethod":"square"}, "toFloating":false, - "fromNode":"d9c005396002882a", + "fromNode":"81a20594db9883e0", "fromSide":"top", - "toNode":"a0abe0901b65ac56", - "toSide":"right" + "toNode":"7a1b2108fa3bc30e", + "toSide":"left" }, { - "id":"a6ee77457b2001f7", + "id":"3fe0580e43888e49", "styleAttributes":{"pathfindingMethod":"square"}, "toFloating":false, "fromNode":"d9c005396002882a", "fromSide":"top", "toNode":"d97803361edf47ba", - "toSide":"right" + "toSide":"left" }, { - "id":"21b147d9374eee41", + "id":"7c79e47353d69416", "styleAttributes":{"pathfindingMethod":"square"}, "toFloating":false, - "fromNode":"81a20594db9883e0", + "fromNode":"d9c005396002882a", "fromSide":"top", - "toNode":"7a1b2108fa3bc30e", + "toNode":"a0abe0901b65ac56", "toSide":"left" } ], diff --git a/Diagrams/Robot_Wiring.png b/Diagrams/Robot_Wiring.png deleted file mode 100644 index 8b4808bb1128..000000000000 Binary files a/Diagrams/Robot_Wiring.png and /dev/null differ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java index f04d78dffea1..a454bdb146db 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Auto_/Auto_Main_.java @@ -97,6 +97,7 @@ public void onWaitForStart() { @Override public void onStartButtonPressed() { + Robot.lights.setAllianceDisplay(selectedAlliance); // Schedule the proper auto selectedAuto.runAuto(); turretOn = selectedAuto.getTurretEnabled(); @@ -122,6 +123,7 @@ public void onUpdate() { @Override public void onStop(){ + Robot.lights.setStripRainbow(); Robot.drivetrain.follower.holdPoint(Robot.drivetrain.follower.getPose()); MecanumDrivetrainClass.robotPose = Robot.drivetrain.follower.getPose(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Devices.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Devices.java index 17e38a9656e7..f0a2d149add2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Devices.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Devices.java @@ -13,8 +13,11 @@ import com.qualcomm.robotcore.hardware.NormalizedRGBA; import com.qualcomm.robotcore.hardware.Servo; +import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.LoadHardwareClass; import org.firstinspires.ftc.teamcode.Prism.Color; +import org.firstinspires.ftc.teamcode.Prism.Direction; import org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver; import org.firstinspires.ftc.teamcode.Prism.PrismAnimations; @@ -258,8 +261,11 @@ public void setRPM(double rpm){ velPID.setGoal(new KineticState(0, degreesPerSecond)); setPower(velPID.calculate(currentKineticState)); } - } + public double getCurrent(CurrentUnit units){ + return motorObject.getCurrent(units); + } + } public static class ServoClass { private Servo servo; @@ -289,7 +295,6 @@ public double getAngle(){ return servo.getPosition(); } } - public static class REVColorSensorV3Class { private NormalizedColorSensor sensor; @@ -313,7 +318,6 @@ public double getDistance(DistanceUnit units){ return ((DistanceSensor) sensor).getDistance(units); } } - public static class DualProximitySensorClass { private final REVColorSensorV3Class sensor1 = new REVColorSensorV3Class(); private final REVColorSensorV3Class sensor2 = new REVColorSensorV3Class(); @@ -339,7 +343,6 @@ public double[] getDistances(){ return new double[]{sensor1.getDistance(units), sensor2.getDistance(units)}; } } - public static class REVHallEffectSensorClass { private DigitalChannel sensor; @@ -352,21 +355,51 @@ public Boolean getTriggered(){ return !sensor.getState(); } } - + public enum StripState { + PROGRESS, + BLINK, + OFF + } public static class GoBildaPrismBarClass { + // Maximum length of 4 daisy chained strips is 36 (12 + 12 + 6 + 6) + // Scrimmage length of 2 daisy chained strips is 24 (12 + 12) GoBildaPrismDriver prism; + int stripBrightness = 50; + public void init(@NonNull OpMode opmode, int stripLength){ + prism = opmode.hardwareMap.get(GoBildaPrismDriver.class, "prism"); + prism.setStripLength(stripLength); + } - PrismAnimations.Solid solidColor = new PrismAnimations.Solid(Color.BLUE); - PrismAnimations.Blink blinkAnim = new PrismAnimations.Blink(Color.BLUE); + public void setStripSolidColor(Color color){ + prism.insertAndUpdateAnimation(GoBildaPrismDriver.LayerHeight.LAYER_0, new PrismAnimations.Solid(color, stripBrightness)); + } - public void init(@NonNull OpMode opmode, String prismDeviceName, Integer stripLength){ - prism = opmode.hardwareMap.get(GoBildaPrismDriver.class,prismDeviceName); - prism.setStripLength(stripLength); + public void setStripRainbow(){ + PrismAnimations.AnimationBase colorA = new PrismAnimations.Rainbow(Direction.Backward); + colorA.setIndexes(0, 12); + PrismAnimations.AnimationBase colorB = new PrismAnimations.Rainbow(Direction.Forward); + colorB.setIndexes(13, 24); + prism.insertAndUpdateAnimation(GoBildaPrismDriver.LayerHeight.LAYER_1, colorA); + prism.insertAndUpdateAnimation(GoBildaPrismDriver.LayerHeight.LAYER_2, colorB); + } + + public void clearStripAnimations(){ + prism.clearAllAnimations(); } - public void setColor(Color color){ + public void setStripBrightness(int brightness){ + stripBrightness = brightness; + } + public void setAllianceDisplay(LoadHardwareClass.Alliance alliance){ + clearStripAnimations(); + if (alliance == LoadHardwareClass.Alliance.RED){ + setStripSolidColor(Color.RED); + }else if (alliance == LoadHardwareClass.Alliance.BLUE){ + setStripSolidColor(Color.BLUE); + } } } -} \ No newline at end of file +} + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Intake.java index 08afdf72a825..a4a9398bbbac 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Intake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Actuators_/Intake.java @@ -5,6 +5,7 @@ import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; +import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; @Configurable @@ -73,7 +74,7 @@ public void setMode(intakeMode direction) { intake.setPower(1); belt.setPower(1); }else if (direction == intakeMode.SHOOTING){ - intake.setPower(0); + intake.setPower(0); //TODO change this to 0 if using servo powered intake belt belt.setPower(1); }else if (direction == intakeMode.REVERSING){ intake.setPower(-1); @@ -87,6 +88,10 @@ public void setMode(intakeMode direction) { } } + public double getCurrent(){ + return intake.getCurrent(CurrentUnit.AMPS); + } + /** * Outputs one of the following modes * */ public gatestate getGate(){ - if (gate.getAngle() == 0.47){ + if (gate.getAngle() == 0.5){ return gatestate.OPEN; } else { return gatestate.CLOSED; } } - /** - * Calculates the target angle to rotate the turret to in order to aim at the correct goal.
- * Currently uses Pinpoint Odometry and trigonometry to get the angle. - */ - public double calcLocalizer (){ - Pose goalPose = calcGoalPose(); - - return (Math.toDegrees(Math.atan2( - goalPose.getY()-Robot.drivetrain.follower.getPose().getY(), - goalPose.getX()-Robot.drivetrain.follower.getPose().getX()) - ) - Math.toDegrees(Robot.drivetrain.follower.getPose().getHeading()) + 90)%360; - } - public Pose calcGoalPose(){ - robotZone.setPosition(Robot.drivetrain.follower.getPose().getX(), Robot.drivetrain.follower.getPose().getY()); - robotZone.setRotation(Robot.drivetrain.follower.getPose().getHeading()); - - Pose nearGoalPose = new Pose(8,140,0); - if (LoadHardwareClass.selectedAlliance == LoadHardwareClass.Alliance.RED) {nearGoalPose = new Pose(140, 140, 0);} - Pose farGoalPose = new Pose(16,140,0); - if (LoadHardwareClass.selectedAlliance == LoadHardwareClass.Alliance.RED) {farGoalPose = new Pose(136, 140, 0);} - - if(robotZone.isInside(LoadHardwareClass.FarLaunchZone)){ - return farGoalPose; - }else{ - return nearGoalPose; - } - } - /** * Sets the RPM of the flywheel. * @param rpm diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/AprilTagVisionSystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/AprilTagVisionSystem.java new file mode 100644 index 000000000000..e3a960c3897a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/AprilTagVisionSystem.java @@ -0,0 +1,296 @@ +/* Copyright (c) 2023 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_; + +import androidx.annotation.NonNull; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagGameDatabase; +import org.firstinspires.ftc.vision.apriltag.AprilTagMetadata; +import org.firstinspires.ftc.vision.apriltag.AprilTagPoseFtc; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +import java.util.List; + +public class AprilTagVisionSystem { + private AprilTagProcessor tagProcessor; + private VisionPortal visionPortal; + private List detections; + private OpMode opMode; + + public boolean initialized = false; + + /** + * Initialize the AprilTag processor. + */ + public void init(OpMode opmode) { + initialized = true; + opMode = opmode; + + // Create the AprilTag processor. + tagProcessor = new AprilTagProcessor.Builder() + + // The following default settings are available to un-comment and edit as needed. + .setDrawAxes(true) + //.setDrawCubeProjection(false) + .setDrawTagOutline(true) + //.setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11) + .setTagLibrary(AprilTagGameDatabase.getCurrentGameTagLibrary()) + .setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES) + + // == CAMERA CALIBRATION == + // If you do not manually specify calibration parameters, the SDK will attempt + // to load a predefined calibration for your camera. + //.setLensIntrinsics(578.272, 578.272, 402.145, 221.506) + // ... these parameters are fx, fy, cx, cy. + + .build(); + + // Adjust Image Decimation to trade-off detection-range for detection-rate. + // eg: Some typical detection data using a Logitech C920 WebCam + // Decimation = 1 .. Detect 2" Tag from 10 feet away at 10 Frames per second + // Decimation = 2 .. Detect 2" Tag from 6 feet away at 22 Frames per second + // Decimation = 3 .. Detect 2" Tag from 4 feet away at 30 Frames Per Second (default) + // Decimation = 3 .. Detect 5" Tag from 10 feet away at 30 Frames Per Second (default) + // Note: Decimation can be changed on-the-fly to adapt during a match. + //aprilTag.setDecimation(3); + + // Create the vision portal by using a builder. + VisionPortal.Builder builder = new VisionPortal.Builder(); + + // Set the camera (webcam vs. built-in RC phone camera). + builder.setCamera(opMode.hardwareMap.get(WebcamName.class, "Webcam 1")); + + // Choose a camera resolution. Not all cameras support all resolutions. + //builder.setCameraResolution(new Size(640, 480)); + + // Enable the RC preview (LiveView). Set "false" to omit camera monitoring. + builder.enableLiveView(true); + + // Set the stream format; MJPEG uses less bandwidth than default YUY2. + //builder.setStreamFormat(VisionPortal.StreamFormat.YUY2); + + // Choose whether or not LiveView stops if no processors are enabled. + // If set "true", monitor shows solid orange screen if no processors enabled. + // If set "false", monitor shows camera view without annotations. + //builder.setAutoStopLiveView(false); + + // Set and enable the processor. + builder.addProcessor(tagProcessor); + + // Build the Vision Portal, using the above settings. + visionPortal = builder.build(); + + // Disable or re-enable the aprilTag processor at any time. + visionPortal.setProcessorEnabled(tagProcessor, true); + } + /** + * Add telemetry about AprilTag detections. + */ + public void printDebugTelemetry() { + updateAprilTagProcessor(); + opMode.telemetry.addData("# AprilTags Detected", detections.size()); + + // Step through the list of detections and display info for each one. + for (int i = 0; i < detections.size(); i++) { + AprilTagDetection detection = detections.get(i); + if (detection.metadata != null) { + opMode.telemetry.addLine(String.format("\n==== (ID %d) %s", getID(i), getMetadata(i).name)); + opMode.telemetry.addLine(getXYZbyIndex(i).toString()); + opMode.telemetry.addLine(getPRYbyIndex(i).toString()); + opMode.telemetry.addLine(getRBEbyIndex(i).toString()); + } else { + opMode.telemetry.addLine(String.format("\n==== (ID %d) Unknown", getID(i))); + opMode.telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); + } + } // end for() loop + + // Add "key" information to opMode.telemetry + opMode.telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist."); + opMode.telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)"); + opMode.telemetry.addLine("RBE = Range, Bearing & Elevation"); + + } + + /** + * @return a boolean representing whether a tag is detected by the camera + */ + public boolean tagDetected(){ + updateAprilTagProcessor(); + return !detections.isEmpty(); + } + public boolean tagDetected(int id){ + updateAprilTagProcessor(); + if (detections != null){ + for (int i = 0; i < detections.size(); i++){ + if (detections.get(i).id == id){ + return true; + } + } + } + return false; + } + + /** + * Fetches the most recent AprilTag detections + */ + public void updateAprilTagProcessor(){ + detections = tagProcessor.getDetections(); + } + + public PoseXYZ getXYZbyIndex(int detectionIndex){ + updateAprilTagProcessor(); + return new PoseXYZ(detections.get(detectionIndex).ftcPose); + } + public PoseXYZ getXYZ(int id){ + for (int i = 0; i < detections.size(); i++){ + if (detections.get(i).id == id){ + return getXYZbyIndex(i); + } + } + return null; + } + + public PosePRY getPRYbyIndex(int detectionIndex){ + updateAprilTagProcessor(); + return new PosePRY(detections.get(detectionIndex).ftcPose); + } + public PosePRY getPRY(int id){ + for (int i = 0; i < detections.size(); i++){ + if (detections.get(i).id == id){ + return getPRYbyIndex(i); + } + } + return null; + } + + public PoseRBE getRBEbyIndex(int detectionIndex){ + if (!detections.isEmpty()){ + return new PoseRBE(detections.get(detectionIndex).ftcPose); + } + return null; + } + public PoseRBE getRBE(int id){ + if (!detections.isEmpty()){ + for (int i = 0; i < detections.size(); i++){ + if (detections.get(i).id == id){ + return getRBEbyIndex(i); + } + } + } + return null; + } + + public AprilTagMetadata getMetadata(int detectionIndex){ + updateAprilTagProcessor(); + return detections.get(detectionIndex).metadata; + } + public AprilTagMetadata getMetadata(){ + return getMetadata(0); + } + + public int getID(int detectionIndex){ + updateAprilTagProcessor(); + return detections.get(detectionIndex).id; + } + public int getID(){ + return getID(0); + } + + public class PoseXYZ { + public double x; + public double y; + public double z; + + PoseXYZ(double X, double Y, double Z){ + x = X; + y = Y; + z = Z; + } + PoseXYZ(AprilTagPoseFtc pose){ + x = pose.x; + y = pose.y; + z = pose.z; + } + @NonNull + @Override + public String toString(){ + return "[X: " + x + ", Y: " + y + ", Z: " + z + "]"; + } + } + public class PosePRY { + public double p; + public double r; + public double y; + + PosePRY(double P, double R, double Y){ + p = P; + r = R; + y = Y; + } + PosePRY(AprilTagPoseFtc pose){ + p = pose.pitch; + r = pose.roll; + y = pose.yaw; + } + @NonNull + @Override + public String toString(){ + return "[Pitch: " + p + ", Roll: " + r + ", Yaw: " + y + "]"; + } + } + public class PoseRBE { + public final double r; + public final double b; + public final double e; + + PoseRBE(double range, double bearing, double elevation){ + r = range; + b = bearing; + e = elevation; + } + PoseRBE(AprilTagPoseFtc pose){ + r = pose.range; + b = pose.bearing; + e = pose.elevation; + } + @NonNull + @Override + public String toString(){ + return "[Range: " + r + ",\n Bearing: " + b + ",\n Elevation: " + e + "]"; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java index 64854aad2535..6f5d165ebbce 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/Commands.java @@ -9,6 +9,7 @@ import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Actuators_.Turret; import dev.nextftc.core.commands.Command; +import dev.nextftc.core.commands.delays.Delay; import dev.nextftc.core.commands.delays.WaitUntil; import dev.nextftc.core.commands.groups.ParallelRaceGroup; import dev.nextftc.core.commands.groups.SequentialGroup; @@ -118,4 +119,35 @@ public Command shootBalls(){ ); } + public Command shootTestBalls(){ + return new ParallelRaceGroup( + new SequentialGroup( + // Ensure the flywheel is up to speed, if not, spin up first + setFlywheelState(Turret.flywheelState.ON), + + // Shoot the first two balls + setGateState(Turret.gatestate.OPEN), + resetShootingTimerFifthsec(), + new WaitUntil(shootingTimerFifthSec::isDone), + setIntakeMode(Intake.intakeMode.INTAKING), + resetShootingTimer2sec(), + new WaitUntil(() -> (Robot.intake.getTopSensorState() && !Robot.intake.getBottomSensorState() && shootingTimer2sec.isDone())), + + // Shoot the last ball + setIntakeMode(Intake.intakeMode.SHOOTING), + setTransferState(Intake.transferState.UP), + resetShootingTimerHalfsec(), + new WaitUntil(shootingTimerHalfSec::isDone), + + // Reset the systems + setIntakeMode(Intake.intakeMode.OFF), + setGateState(Turret.gatestate.CLOSED), + setTransferState(Intake.transferState.DOWN) + ), + new SequentialGroup( + new Delay(8) + ) + ); + } + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadHardwareClass.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadHardwareClass.java index 2e023e363459..d1c3d3453d67 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadHardwareClass.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadHardwareClass.java @@ -36,6 +36,7 @@ import com.skeletonarmy.marrow.zones.Point; import com.skeletonarmy.marrow.zones.PolygonZone; +import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Actuators_.Devices; import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Actuators_.Intake; import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Actuators_.Turret; import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Drivetrain_.MecanumDrivetrainClass; @@ -54,6 +55,7 @@ public class LoadHardwareClass { public final MecanumDrivetrainClass drivetrain; public final Turret turret; public final Intake intake; + public final Devices.GoBildaPrismBarClass lights; // Declare various enums & other variables that are useful across files public static Alliance selectedAlliance = null; @@ -96,6 +98,7 @@ public LoadHardwareClass(OpMode opmode) { this.drivetrain = new MecanumDrivetrainClass(); this.turret = new Turret(); this.intake = new Intake(); + this.lights = new Devices.GoBildaPrismBarClass(); } /** @@ -107,6 +110,7 @@ public void init(Pose initialPose) { drivetrain.init(myOpMode, initialPose); turret.init(myOpMode, this); intake.init(myOpMode); + lights.init(myOpMode, 24); // Misc telemetry myOpMode.telemetry.addData(">", "Hardware Initialized"); @@ -122,6 +126,7 @@ public void init(Pose initialPose, Follower follower) { drivetrain.init(myOpMode, initialPose, follower); turret.init(myOpMode, this); intake.init(myOpMode); + lights.init(myOpMode, 24); // Misc telemetry myOpMode.telemetry.addData(">", "Hardware Initialized"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadPrismLights.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadPrismLights.java new file mode 100644 index 000000000000..667aceca18b4 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Hardware_/LoadPrismLights.java @@ -0,0 +1,32 @@ +package org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_; + +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import org.firstinspires.ftc.teamcode.Prism.Color; +import org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver; +import org.firstinspires.ftc.teamcode.Prism.PrismAnimations; + +public class LoadPrismLights { + private GoBildaPrismDriver prism; + + public void init(OpMode opMode, String name){ + prism = opMode.hardwareMap.get(GoBildaPrismDriver.class, name); + prism.setStripLength(24); + } + + public void setStripsRainbow(){ + PrismAnimations.AnimationBase rainbow = new PrismAnimations.Rainbow(); + rainbow.setIndexes(0, 12); + prism.insertAndUpdateAnimation(GoBildaPrismDriver.LayerHeight.LAYER_0, rainbow); + } + public void setStripsRed(){ + prism.insertAndUpdateAnimation(GoBildaPrismDriver.LayerHeight.LAYER_1, new PrismAnimations.Solid(Color.RED, 12, 29)); + } + public void setStripsBlue(){ + prism.insertAndUpdateAnimation(GoBildaPrismDriver.LayerHeight.LAYER_1, new PrismAnimations.Solid(Color.BLUE, 12, 29)); + } + + public void clearStrips(){ + prism.clearAllAnimations(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java index f8b4860829c4..3fc8362109ba 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Main_.java @@ -37,11 +37,13 @@ import com.pedropathing.geometry.Pose; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.util.ElapsedTime; import com.skeletonarmy.marrow.TimerEx; import com.skeletonarmy.marrow.prompts.OptionPrompt; import com.skeletonarmy.marrow.prompts.Prompter; +import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Actuators_.Intake; import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Actuators_.Intake.intakeMode; import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Actuators_.Intake.transferState; @@ -52,6 +54,8 @@ import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Drivetrain_.Pedro_Paths; import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.LoadHardwareClass; +import java.util.concurrent.TimeUnit; + // FIRST COMMENT FROM MY NEW COMPUTER =D // - ARI @@ -65,10 +69,13 @@ public class Teleop_Main_ extends LinearOpMode { private final TelemetryManager panelsTelemetry = PanelsTelemetry.INSTANCE.getTelemetry(); public int shootingState = 0; - public TimerEx stateTimer = new TimerEx(1); + public TimerEx stateTimerFifthSec = new TimerEx(0.2); + public TimerEx stateTimerFullSec = new TimerEx(1); + public TimerEx stateTimerHalfSec = new TimerEx(0.5); public double hoodOffset = 0; public double turretOffset = 0; public boolean turretOn = true; + public boolean hoodOn = true; public Pose holdPoint = new Pose(72, 72, 90); public Boolean holdJustTriggered = false; @@ -78,6 +85,7 @@ public class Teleop_Main_ extends LinearOpMode { Pedro_Paths Paths = new Pedro_Paths(); // Create a new instance of Prompter for selecting the alliance Prompter prompter = null; + enum startPoses { FAR, NEAR @@ -131,6 +139,8 @@ public void runOpMode() { telemetry.update(); }); + Robot.turret.initVision(this); + // Runs repeatedly while in init while (opModeInInit()) { prompter.run(); @@ -148,6 +158,7 @@ public void runOpMode() { Paths.buildPaths(Robot.drivetrain.follower); Robot.drivetrain.startTeleOpDrive(); Robot.intake.setTransfer(transferState.DOWN); + Robot.lights.setAllianceDisplay(selectedAlliance); // run until the end of the match (driver presses STOP) while (opModeIsActive()) { @@ -157,6 +168,11 @@ public void runOpMode() { } } + int targetTagID = 24; + if (LoadHardwareClass.selectedAlliance == LoadHardwareClass.Alliance.BLUE){ + targetTagID = 20; + } + Gamepad1(); Gamepad2(); @@ -168,17 +184,26 @@ public void runOpMode() { telemetry.addData("ALLIANCE", selectedAlliance); telemetry.addData("SpeedMult", Robot.drivetrain.speedMultiplier); - telemetry.addLine(); //positional telemetry telemetry.addData("Robot Position [X, Y, H]", "[" + Robot.drivetrain.follower.getPose().getX() + ", " + Robot.drivetrain.follower.getPose().getY() + ", " + Robot.drivetrain.follower.getPose().getHeading() + "]"); telemetry.addData("Distance From Goal", Robot.drivetrain.distanceFromGoal()); + telemetry.addLine(); + panelsTelemetry.addData("FL Wheel Current", hardwareMap.get(DcMotorEx.class, "FL").getCurrent(CurrentUnit.AMPS)); + panelsTelemetry.addData("FR Wheel Current", hardwareMap.get(DcMotorEx.class, "FR").getCurrent(CurrentUnit.AMPS)); + panelsTelemetry.addData("BL Wheel Current", hardwareMap.get(DcMotorEx.class, "BL").getCurrent(CurrentUnit.AMPS)); + panelsTelemetry.addData("BR Wheel Current", hardwareMap.get(DcMotorEx.class, "BR").getCurrent(CurrentUnit.AMPS)); telemetry.addLine(); // Turret-related Telemetry + telemetry.addData("Tag Detected", Robot.turret.vision.tagDetected(targetTagID)); + telemetry.addData("Is using Camera PID", Robot.turret.useCameraAim); + telemetry.addData("Camera Error", Robot.turret.cameraTurretError); + panelsTelemetry.addData("Camera Turret Error", Robot.turret.cameraTurretError); panelsTelemetry.addData("Turret Target Angle", Robot.turret.rotation.target); panelsTelemetry.addData("Turret Actual Angle", Robot.turret.rotation.getAngleAbsolute()); telemetry.addData("Turret Target Angle", Robot.turret.rotation.target); telemetry.addData("Turret Actual Angle", Robot.turret.rotation.getAngleAbsolute()); + telemetry.addData("Turret Rotation Offset", turretOffset); telemetry.addData("Turret Hood Angle", Robot.turret.getHood()); telemetry.addData("Turret Hood Offset", hoodOffset); telemetry.addData("Turret Target [X, Y]", "[" + Robot.turret.calcGoalPose().getX() + ", " + Robot.turret.calcGoalPose().getY() + "]"); @@ -195,6 +220,7 @@ public void runOpMode() { // Intake-related Telemetry telemetry.addLine(); telemetry.addData("Intake Mode", Robot.intake.getMode()); + telemetry.addData("Intake Current", Robot.intake.getCurrent()); // Color Sensor Telemetry telemetry.addLine(); @@ -204,9 +230,13 @@ public void runOpMode() { // System-related Telemetry telemetry.addLine(); telemetry.addData("Status", "Run Time: " + runtime); - telemetry.addData("Version: ", "12/12/25"); + telemetry.addData("Version: ", "2/13/25"); telemetry.update(); panelsTelemetry.update(); + + if (runtime.time(TimeUnit.SECONDS) > 115){ + Robot.lights.setStripRainbow(); + } } } @@ -298,6 +328,10 @@ public void Gamepad1() { gamepad1.right_stick_x / turnMult, true ); + + if (gamepad1.dpadDownWasPressed()){ + hoodOn = !hoodOn; + } } /** @@ -344,11 +378,11 @@ public void Gamepad1() { * */ public void Gamepad2() { - if (gamepad2.aWasPressed()){ + if (gamepad1.yWasPressed()){ turretOn = !turretOn; } - Robot.turret.updateAimbot(turretOn, true, hoodOffset); - Robot.turret.rotation.setOffsetDegrees(Robot.turret.rotation.getOffsetDegrees() + turretOffset); + Robot.turret.updateAimbot(turretOn, hoodOn, hoodOffset); + Robot.turret.rotation.setOffsetDegrees(Turret.turretOffset + turretOffset); double dylanStickDeadzones = 0.2; @@ -395,9 +429,9 @@ public void Gamepad2() { hoodOffset -= 10; } if (gamepad2.dpadLeftWasPressed()){ - turretOffset += 2; + turretOffset += 10; }else if (gamepad2.dpadLeftWasPressed()){ - turretOffset -= 2; + turretOffset -= 10; } @@ -407,38 +441,48 @@ public void Gamepad2() { shootingState++; } if (gamepad2.xWasPressed()){ - shootingState = 3; + shootingState = 4; } switch (shootingState) { case 0: telemetry.addData("Shooting State", "OFF"); return; case 1: - if (Robot.intake.getMode() == intakeMode.OFF){ - stateTimer.restart(); - stateTimer.start(); + Robot.turret.setFlywheelState(flywheelState.ON); + if (Robot.turret.getGate() == gatestate.CLOSED){ + stateTimerFifthSec.restart(); + stateTimerFifthSec.start(); } - Robot.intake.setMode(intakeMode.INTAKING); Robot.turret.setGateState(gatestate.OPEN); - telemetry.addData("Shooting State", "STARTED"); - if (stateTimer.isDone() && Robot.intake.getTopSensorState() && !Robot.intake.getBottomSensorState()){ - shootingState++; + telemetry.addData("Shooting State", "GATE OPENING"); + if (stateTimerFifthSec.isDone()){ + shootingState = 2; } return; case 2: + if (Robot.intake.getMode() == intakeMode.OFF){ + stateTimerFullSec.restart(); + stateTimerFullSec.start(); + } + Robot.intake.setMode(intakeMode.INTAKING); + telemetry.addData("Shooting State", "SHOOTING 2"); + if (stateTimerFullSec.isDone() && Robot.intake.getTopSensorState() && !Robot.intake.getBottomSensorState()){ + shootingState = 3; + } + return; + case 3: if (Robot.intake.getMode() == intakeMode.INTAKING){ - stateTimer.restart(); - stateTimer.start(); + stateTimerHalfSec.restart(); + stateTimerHalfSec.start(); } Robot.intake.setMode(Intake.intakeMode.SHOOTING); Robot.intake.setTransfer(transferState.UP); - telemetry.addData("Shooting State", "TRANSFERRED"); - if (stateTimer.isDone()) { - shootingState++; + telemetry.addData("Shooting State", "SHOOTING FINAL"); + if (stateTimerHalfSec.isDone()) { + shootingState = 4; } return; - case 3: - Robot.turret.setFlywheelState(flywheelState.OFF); + case 4: Robot.turret.setGateState(gatestate.CLOSED); Robot.intake.setMode(intakeMode.OFF); Robot.intake.setTransfer(transferState.DOWN); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Tuning_.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Tuning_.java index ea765929be47..e1cdea2eef6f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Tuning_.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LOADCode/Main_/Teleop_/Teleop_Tuning_.java @@ -37,9 +37,8 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.util.ElapsedTime; -import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Actuators_.Intake; -import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.Actuators_.Turret; import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.LoadHardwareClass; +import org.firstinspires.ftc.teamcode.LOADCode.Main_.Hardware_.LoadPrismLights; @Configurable @TeleOp(name="Teleop_Tuning_", group="TeleOp") @@ -64,107 +63,50 @@ public void runOpMode() { // Create a new instance of our Robot class LoadHardwareClass Robot = new LoadHardwareClass(this); + LoadPrismLights prism = new LoadPrismLights(); // Initialize all hardware of the robot Robot.init(startPose); - while (opModeInInit() && Robot.turret.zeroTurret()){ - sleep(0); - } telemetry.addData("Status", "Initialized"); telemetry.update(); + while (!isStopRequested() && Robot.turret.zeroTurret()){ + sleep(0); + } + Robot.turret.setHood(0); + // Wait for the game to start (driver presses START) waitForStart(); runtime.reset(); // Begin TeleOp driving Robot.drivetrain.startTeleOpDrive(); - Robot.turret.setGateState(Turret.gatestate.OPEN); + + prism.init(this, "prism"); // run until the end of the match (driver presses STOP) while (opModeIsActive()) { // Pass the joystick positions to our mecanum drive controller Robot.drivetrain.pedroMecanumDrive( - gamepad1.left_stick_y/2, - gamepad1.left_stick_x/2, - gamepad1.right_stick_x/2, + gamepad1.left_stick_y, + gamepad1.left_stick_x, + gamepad1.right_stick_x, true ); - // Color sensor telemetry - telemetry.addLine("COLOR SENSOR DATA"); - telemetry.addData("Color Sensor Threshold", Intake.proximitySensorThreshold); - telemetry.addData("Upper Sensor Array Triggered", Robot.intake.getTopSensorState()); - telemetry.addData("Lower Sensor Array Triggered", Robot.intake.getBottomSensorState()); - - // Controls for turret rotation testing - if (gamepad1.x){ - Robot.turret.rotation.setAngle(turretTurnAngle); - turretTurnAngle += turretTurnIncrement * turretTurnMultiplier; - if (turretTurnAngle > 360){ - turretTurnAngle = 360; - turretTurnMultiplier *= -1; - }else if (turretTurnAngle < 0){ - turretTurnAngle = 0; - turretTurnMultiplier *= -1; - } - - }else if (gamepad1.b){ - Robot.turret.updateAimbotWithVelocity(); - }else if (gamepad1.a){ - Robot.turret.rotation.setAngle(0); - }else{ - Robot.turret.rotation.setAngle(90); + if (gamepad1.aWasPressed()){ + prism.setStripsRainbow(); } - Robot.turret.updatePIDs(); - telemetry.addLine(); - telemetry.addLine("TURRET DATA"); - telemetry.addData("Turret Target Angle", Robot.turret.rotation.target); - telemetry.addData("Turret Actual Angle", Robot.turret.rotation.getAngleAbsolute()); - telemetry.addData("Hall Effect Detected", Robot.turret.hall.getTriggered()); - panelsTelemetry.addData("Turret Target Angle", Robot.turret.rotation.target); - panelsTelemetry.addData("Turret Actual Angle", Robot.turret.rotation.getAngleAbsolute()); - - - // Controls for hood testing -// if (gamepad1.dpad_up){ -// Robot.turret.setHood(Robot.turret.getHood() + 2); -// }else if (gamepad1.dpad_down){ -// Robot.turret.setHood(Robot.turret.getHood() - 2); -// }else if (gamepad1.dpadLeftWasPressed()){ -// Robot.turret.setHood(hoodTargetPos); -// } - telemetry.addLine(); - telemetry.addLine("HOOD DATA"); - telemetry.addData("Hood Angle", Robot.turret.getHood()); - - // Controls for flywheel testing - if (gamepad1.backWasPressed()){ - if (Robot.turret.flywheelMode == Turret.flywheelState.OFF){ - Robot.turret.setFlywheelState(Turret.flywheelState.ON); - }else{ - Robot.turret.setFlywheelState(Turret.flywheelState.OFF); - } + if (gamepad1.bWasPressed()){ + prism.setStripsRed(); } - Robot.turret.updateFlywheel(); - telemetry.addLine(); - telemetry.addLine("FLYWHEEL DATA"); - telemetry.addData("Flywheel Target Velocity", Robot.turret.flywheel.target); - telemetry.addData("Flywheel Actual Velocity", Robot.turret.getFlywheelRPM()); - telemetry.addData("Flywheel Motor Power", Robot.turret.flywheel.getPower()); - panelsTelemetry.addData("Flywheel Target Velocity", Robot.turret.flywheel.target); - panelsTelemetry.addData("Flywheel Actual Velocity", Robot.turret.getFlywheelRPM()); - panelsTelemetry.addData("Flywheel Motor Power", Robot.turret.flywheel.getPower()); - - if (gamepad1.aWasPressed()){ - if (Robot.intake.getMode() == Intake.intakeMode.INTAKING){ - Robot.intake.setMode(Intake.intakeMode.OFF); - }else{ - Robot.intake.setMode(Intake.intakeMode.INTAKING); - } + if (gamepad1.xWasPressed()){ + prism.setStripsBlue(); + } + if (gamepad1.yWasPressed()){ + prism.clearStrips(); } - // System-related Telemetry telemetry.addLine(); telemetry.addLine("SYSTEM DATA"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/PrismAnimations.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/PrismAnimations.java index 6474bc42f2fe..b61be1a194c6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/PrismAnimations.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/PrismAnimations.java @@ -494,6 +494,10 @@ public Rainbow(float startHue) { this(); this.startHue = startHue; } + public Rainbow(Direction direction) { + this(); + this.direction = direction; + } public Rainbow(float startHue, float stopHue){ this(startHue); this.stopHue = stopHue;