diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java deleted file mode 100644 index f797c6b..0000000 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java +++ /dev/null @@ -1,141 +0,0 @@ -/* - * Copyright (c) 2024 DigitalChickenLabs - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package org.firstinspires.ftc.robotcontroller.external.samples; - -import com.qualcomm.hardware.digitalchickenlabs.OctoQuad; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import org.firstinspires.ftc.robotcore.external.Telemetry; - -/* - * This OpMode illustrates how to use the DigitalChickenLabs OctoQuad Quadrature Encoder & Pulse Width Interface Module - * - * The OctoQuad has 8 input channels that can used to read either Relative Quadrature Encoders or Pulse-Width Absolute Encoder inputs. - * Relative Quadrature encoders are found on most FTC motors, and some stand-alone position sensors like the REV Thru-Bore encoder. - * Pulse-Width encoders are less common. The REV Thru-Bore encoder can provide its absolute position via a variable pulse width, - * as can several sonar rangefinders such as the MaxBotix MB1000 series. - * - * This basic sample shows how an OctoQuad can be used to read the position three Odometry pods fitted - * with REV Thru-Bore encoders. For a more advanced example showing additional OctoQuad capabilities, see the SensorOctoQuadAdv sample. - * - * This OpMode assumes that the OctoQuad is attached to an I2C interface named "octoquad" in the robot configuration. - * - * The code assumes the first three OctoQuad inputs are connected as follows - * - Chan 0: for measuring forward motion on the left side of the robot. - * - Chan 1: for measuring forward motion on the right side of the robot. - * - Chan 2: for measuring Lateral (strafing) motion. - * - * The encoder values may be reset to zero by pressing the X (left most) button on Gamepad 1. - * - * This sample does not show how to interpret these readings, just how to obtain and display them. - * - * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. - * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list - * - * See the sensor's product page: https://www.tindie.com/products/35114/ - */ -@TeleOp(name = "OctoQuad Basic", group="OctoQuad") -@Disabled -public class SensorOctoQuad extends LinearOpMode { - - // Identify which encoder OctoQuad inputs are connected to each odometry pod. - private final int ODO_LEFT = 0; // Facing forward direction on left side of robot (Axial motion) - private final int ODO_RIGHT = 1; // Facing forward direction on right side or robot (Axial motion) - private final int ODO_PERP = 2; // Facing perpendicular direction at the center of the robot (Lateral motion) - - // Declare the OctoQuad object and members to store encoder positions and velocities - private OctoQuad octoquad; - - private int posLeft; - private int posRight; - private int posPerp; - - /** - * This function is executed when this OpMode is selected from the Driver Station. - */ - @Override - public void runOpMode() { - - // Connect to OctoQuad by referring to its name in the Robot Configuration. - octoquad = hardwareMap.get(OctoQuad.class, "octoquad"); - - // Read the Firmware Revision number from the OctoQuad and display it as telemetry. - telemetry.addData("OctoQuad Firmware Version ", octoquad.getFirmwareVersion()); - - // Reverse the count-direction of any encoder that is not what you require. - // e.g. if you push the robot forward and the left encoder counts down, then reverse it so it counts up. - octoquad.setSingleEncoderDirection(ODO_LEFT, OctoQuad.EncoderDirection.REVERSE); - octoquad.setSingleEncoderDirection(ODO_RIGHT, OctoQuad.EncoderDirection.FORWARD); - octoquad.setSingleEncoderDirection(ODO_PERP, OctoQuad.EncoderDirection.FORWARD); - - // Any changes that are made should be saved in FLASH just in case there is a sensor power glitch. - octoquad.saveParametersToFlash(); - - telemetry.addLine("\nPress START to read encoder values"); - telemetry.update(); - - waitForStart(); - - // Configure the telemetry for optimal display of data. - telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE); - telemetry.setMsTransmissionInterval(50); - - // Set all the encoder inputs to zero. - octoquad.resetAllPositions(); - - // Loop while displaying the odometry pod positions. - while (opModeIsActive()) { - telemetry.addData(">", "Press X to Reset Encoders\n"); - - // Check for X button to reset encoders. - if (gamepad1.x) { - // Reset the position of all encoders to zero. - octoquad.resetAllPositions(); - } - - // Read all the encoder data. Load into local members. - readOdometryPods(); - - // Display the values. - telemetry.addData("Left ", "%8d counts", posLeft); - telemetry.addData("Right", "%8d counts", posRight); - telemetry.addData("Perp ", "%8d counts", posPerp); - telemetry.update(); - } - } - - private void readOdometryPods() { - // For best performance, we should only perform ONE transaction with the OctoQuad each cycle. - // Since this example only needs to read positions from a few channels, we could use either - // readPositionRange(idxFirst, idxLast) to get a select number of sequential channels - // or - // readAllPositions() to get all 8 encoder readings - // - // Since both calls take almost the same amount of time, and the actual channels may not end up - // being sequential, we will read all of the encoder positions, and then pick out the ones we need. - int[] positions = octoquad.readAllPositions(); - posLeft = positions[ODO_LEFT]; - posRight = positions[ODO_RIGHT]; - posPerp = positions[ODO_PERP]; - } -} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadAdv.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadAdv.java deleted file mode 100644 index e763b9a..0000000 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadAdv.java +++ /dev/null @@ -1,278 +0,0 @@ -/* - * Copyright (c) 2024 DigitalChickenLabs - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -package org.firstinspires.ftc.robotcontroller.external.samples; - -import com.qualcomm.hardware.digitalchickenlabs.OctoQuad; -import com.qualcomm.hardware.digitalchickenlabs.OctoQuadBase; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.util.ElapsedTime; -import com.qualcomm.robotcore.util.MovingStatistics; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; - - -import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; - -import java.util.ArrayList; -import java.util.List; - -/* - * This OpMode illustrates how to use advanced features of the DigitalChickenLabs OctoQuad Quadrature Encoder & Pulse Width Interface Module - * - * The OctoQuad has 8 input channels that can used to read either Quadrature Encoder signals (like with most FTC motors) - * or Pulse-Width style Absolute Encoder inputs (eg: REV Through Bore encoder pulse width output). - * - * This OpMode illustrates several of the more advanced features of an OctoQuad, including Pulse Width measurement and velocity measurement. - * For a more basic OpMode just showing how to read encoder inputs, see the SensorOctoQuad sample. - * - * This OpMode assumes that the OctoQuad is attached to an I2C interface named "octoquad" in the robot configuration. - * - * One system that requires a lot of encoder inputs is a Swerve Drive system. - * Such a drive requires two encoders per drive module: one for position & velocity of the Drive motor/wheel, and one for position/angle of the Steering motor. - * The Drive motor usually requires a quadrature encoder, and the Steering motor requires an Absolute encoder. - * This quantity and combination of encoder inputs is a challenge, but a single OctoQuad could be used to read them all. - * - * This sample will configure an OctoQuad for a swerve drive, as follows - * - Configure 4 Relative Quadrature Encoder inputs and 4 Absolute Pulse-Width Encoder inputs - * - Configure a velocity sample interval of 25 mSec - * - Configure a pulse width input range of 1-1024 uSec for a REV Through Bore Encoder's Absolute Pulse output. - * - * An OctoSwerveDrive class will be created to initialize the OctoQuad, and manage the 4 swerve modules. - * An OctoSwerveModule class will be created to configure and read a single swerve module. - * - * Wiring: - * The OctoQuad will be configured to accept Quadrature encoders on the first four channels and Absolute (pulse width) encoders on the last four channels. - * - * The standard 4-pin to 4-pin cable can be used to connect each Driver Motor encoder to the OctoQuad. (channels 0-3) - * - * A modified version of the REV 6-4 pin cable (shipped with the encoder) connects the steering encoder to the OctoQuad. (channels 4-7) - * To connect the Absolute position signal from a REV Thru-Bore encoder to the OctoQuad, the Provided 6-pin to 4-pin cable will need to be modified. - * At the 6-pin connector end, move the yellow wire from its initial pin-3 position to the ABS pin-5 position. This can be done easily - * by using a small flathead screwdriver to lift the small white tab holding the metal wire crimp in place and gently pulling the wire out. - * See the OctoSwerveDrive() constructor below for the correct wheel/channel assignment. - * - * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. - * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list - * - * Note: If you prefer, you can move the two support classes from this file, and place them in their own files. - * But leaving them in place is simpler for this example. - * - * See the sensor's product page: https://www.tindie.com/products/35114/ - */ -@TeleOp(name="OctoQuad Advanced", group="OctoQuad") -@Disabled -public class SensorOctoQuadAdv extends LinearOpMode { - - @Override - public void runOpMode() throws InterruptedException { - // Connect to the OctoQuad by looking up its name in the hardwareMap. - OctoQuad octoquad = hardwareMap.get(OctoQuad.class, "octoquad"); - - // Create the interface for the Swerve Drive Encoders - OctoSwerveDrive octoSwerveDrive = new OctoSwerveDrive(octoquad); - - // Display the OctoQuad firmware revision - telemetry.addLine("OctoQuad Firmware v" + octoquad.getFirmwareVersion()); - telemetry.addLine("\nPress START to read encoder values"); - telemetry.update(); - - waitForStart(); - - // Configure the telemetry for optimal display of data. - telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE); - telemetry.setMsTransmissionInterval(50); - - // Run stats to determine cycle times. - MovingStatistics avgTime = new MovingStatistics(100); - ElapsedTime elapsedTime = new ElapsedTime(); - - while (opModeIsActive()) { - telemetry.addData(">", "Press X to Reset Encoders\n"); - - if(gamepad1.x) { - octoquad.resetAllPositions(); - } - - // read all the swerve drive encoders and update positions and velocities. - octoSwerveDrive.updateModules(); - octoSwerveDrive.show(telemetry); - - // Update cycle time stats - avgTime.add(elapsedTime.nanoseconds()); - elapsedTime.reset(); - - telemetry.addData("Loop time", "%.1f mS", avgTime.getMean()/1000000); - telemetry.update(); - } - } -} - -// ============================ Internal (Inner) Classes ============================= - -/*** - * OctoSwerveDrive class manages 4 Swerve Modules - * - Performs general OctoQuad initialization - * - Creates 4 module classes, one for each swerve module - * - Updates swerve drive status by reading all data from OctoQuad and Updating each module - * - Displays all swerve drive data as telemetry - */ -class OctoSwerveDrive { - - private final OctoQuad octoquad; - private final List allModules = new ArrayList<>(); - - // members to hold encoder data for each module. - public final OctoSwerveModule LeftFront; - public final OctoSwerveModule RightFront; - public final OctoSwerveModule LeftBack; - public final OctoSwerveModule RightBack; - - // Prepare an object to hold an entire OctoQuad encoder readable register bank (pos and vel) - private final OctoQuad.EncoderDataBlock encoderDataBlock = new OctoQuad.EncoderDataBlock(); - - public OctoSwerveDrive(OctoQuad octoquad) { - this.octoquad = octoquad; - - // Clear out all prior settings and encoder data before setting up desired configuration - octoquad.resetEverything(); - - // Assume first 4 channels are relative encoders and the next 4 are absolute encoders - octoquad.setChannelBankConfig(OctoQuad.ChannelBankConfig.BANK1_QUADRATURE_BANK2_PULSE_WIDTH); - - // Create the four OctoSwerveModules, and add them to a 'list' for easier reference. - - // Note: The wheel/channel order is set here. Ensure your encoder cables match. - // - // Note: The angleOffset must be set for each module so a forward facing wheel shows a steer angle of 0 degrees. - // The process for determining the correct angleOffsets is as follows: - // 1) Set all the angleValues (below) to zero then build and deploy the code. - // 2) Physically rotate all the swerve drives so the wheels are facing forward in the desired 0 degree position - // 3) Run the OpMode, view the telemetry and record the "Degrees" value for each module. - // 4) Update the code by entering the recorded Degrees value for each module as the angleOffset (last) parameter in the lines below. - // - // Rebuild and deploy the new code. Verify that the telemetry now indicates 0 degrees when the wheels are facing forward. - // Also verify that the correct module values change appropriately when you manually spin (drive) and rotate (steer) a wheel. - - allModules.add(LeftFront = new OctoSwerveModule(octoquad, "LF ",0,0));// Drive = 0, Steer = 4 - allModules.add(RightFront = new OctoSwerveModule(octoquad, "RF ",1,0));// Drive = 1, Steer = 5 - allModules.add(LeftBack = new OctoSwerveModule(octoquad, "LB ",2,0));// Drive = 2, Steer = 6 - allModules.add(RightBack = new OctoSwerveModule(octoquad, "RB ",3,0));// Drive = 3, Steer = 7 - - // now make sure the settings persist through any power glitches. - octoquad.saveParametersToFlash(); - } - - /** - * Updates all 4 swerve modules - */ - public void updateModules() { - // Read full OctoQuad data block and then pass DataBlock to each swerve module to update themselves. - octoquad.readAllEncoderData(encoderDataBlock); - for (OctoSwerveModule module : allModules) { - module.updateModule(encoderDataBlock); - } - } - - /** - * Generate telemetry data for all modules. - * @param telemetry OpMode Telemetry object - */ - public void show(Telemetry telemetry) { - // create general header block and then have each module add its own telemetry - telemetry.addData("pos", " Count CPS Degree DPS"); - for (OctoSwerveModule module : allModules) { - module.show(telemetry); - } - } -} - -/*** - * The OctoSwerveModule class manages a single swerve module - */ -class OctoSwerveModule { - - public double driveCounts; - public double driveCountsPerSec; - public double steerDegrees; - public double steerDegreesPerSec; - - private final String name; - private final int channel; - private final double angleOffset; - private final double steerDirMult; - - private static final int VELOCITY_SAMPLE_INTERVAL_MS = 25; // To provide 40 updates per second. - private static final double DEGREES_PER_US = (360.0 / 1024.0); // based on REV Through Bore Encoder - private static final double VELOCITY_SAMPLES_PER_S = (1000.0 / VELOCITY_SAMPLE_INTERVAL_MS); - - // The correct drive and turn directions must be set for the Swerve Module based on the specific hardware geometry. - // Forward motion must generate an increasing drive count. - // Counter Clockwise steer rotation must generate an increasing Steer Angle (degrees) - private static final boolean INVERT_DRIVE_ENCODER = false; // Set true if forward motion decreases drive "Count" - private static final boolean INVERT_STEER_ENCODER = false; // Set true if counter clockwise steer action decreases steer "Degree" - - /*** - * @param octoquad provide access to configure OctoQuad - * @param name name used for telemetry display - * @param quadChannel Quadrature encoder channel. Pulse Width channel is this + 4 - * @param angleOffset Angle to subtract from absolute encoder to calibrate zero position. (see comments above) - */ - public OctoSwerveModule (OctoQuad octoquad, String name, int quadChannel, double angleOffset) { - this.name = name; - this.channel = quadChannel; - this.angleOffset = angleOffset; - this.steerDirMult = INVERT_STEER_ENCODER ? -1 : 1 ; // create a multiplier to flip the steer angle. - - // Set the drive encoder direction. Note the absolute encoder does not have built-in direction inversion. - octoquad.setSingleEncoderDirection(channel, INVERT_DRIVE_ENCODER ? OctoQuad.EncoderDirection.REVERSE : OctoQuad.EncoderDirection.FORWARD); - - // Set the velocity sample interval on both encoders - octoquad.setSingleVelocitySampleInterval(channel, VELOCITY_SAMPLE_INTERVAL_MS); - octoquad.setSingleVelocitySampleInterval(channel + 4, VELOCITY_SAMPLE_INTERVAL_MS); - - // Setup Absolute encoder pulse range to match REV Through Bore encoder. - octoquad.setSingleChannelPulseWidthParams (channel + 4, new OctoQuad.ChannelPulseWidthParams(1,1024)); - } - - /*** - * Calculate the Swerve module's position and velocity values - * @param encoderDataBlock most recent full data block read from OctoQuad. - */ - public void updateModule(OctoQuad.EncoderDataBlock encoderDataBlock) { - driveCounts = encoderDataBlock.positions[channel]; // get Counts. - driveCountsPerSec = encoderDataBlock.velocities[channel] * VELOCITY_SAMPLES_PER_S; // convert counts/interval to counts/sec - - // convert uS to degrees. Add in any possible direction flip. - steerDegrees = AngleUnit.normalizeDegrees((encoderDataBlock.positions[channel+ 4] * DEGREES_PER_US * steerDirMult) - angleOffset); - // convert uS/interval to deg per sec. Add in any possible direction flip. - steerDegreesPerSec = encoderDataBlock.velocities[channel + 4] * DEGREES_PER_US * steerDirMult * VELOCITY_SAMPLES_PER_S; - } - - /** - * Display the Swerve module's state as telemetry - * @param telemetry OpMode Telemetry object - */ - public void show(Telemetry telemetry) { - telemetry.addData(name, "%8.0f %7.0f %7.0f %6.0f", driveCounts, driveCountsPerSec, steerDegrees, steerDegreesPerSec); - } -} diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index aaef29f..4b478ca 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -12,21 +12,22 @@ // Custom definitions may go here // Include common definitions from above. -buildscript { - repositories { - mavenCentral() - maven{ - url = 'https://www.matthewo.tech/maven/' - } - } - dependencies { - classpath 'org.team11260:fast-load-plugin:0.1.2' +//buildscript { +// repositories { +// mavenCentral() +// maven { +// url "https://repo.dairy.foundation/releases" +// } +// } +// dependencies { +// classpath "dev.frozenmilk:Load:0.2.4" +// } +//} - } -} apply from: '../build.common.gradle' apply from: '../build.dependencies.gradle' +//apply plugin: 'dev.frozenmilk.sinister.sloth.load' android { namespace = 'org.firstinspires.ftc.teamcode' @@ -37,9 +38,6 @@ android { } repositories { - maven { - url = 'https://www.matthewo.tech/maven/' - } } dependencies { @@ -48,7 +46,5 @@ dependencies { implementation 'org.apache.commons:commons-math3:3.6.1' implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7' - implementation 'com.acmerobotics.roadrunner:core:0.5.6' implementation files('../libs/PestoCore-release.aar') - implementation 'org.team11260:fast-load:0.1.2' } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Drive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Drive.java deleted file mode 100644 index aed8585..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Drive.java +++ /dev/null @@ -1,118 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.shprobotics.pestocore.devices.GamepadKey; -import com.shprobotics.pestocore.processing.FrontalLobe; -import com.shprobotics.pestocore.processing.MotorCortex; - -import org.firstinspires.ftc.teamcode.subsystems.BaseRobot; -import org.firstinspires.ftc.teamcode.subsystems.FeederSubsystem; -import org.firstinspires.ftc.teamcode.subsystems.HoodSubsystem; -import org.firstinspires.ftc.teamcode.subsystems.IndexerSubsystem; -import org.firstinspires.ftc.teamcode.subsystems.IntakeSubsystem; -import org.firstinspires.ftc.teamcode.subsystems.OuttakeSubsystem; - -@TeleOp(name = "Thanks Andrew") -public class Drive extends BaseRobot { - @Override - public void runOpMode() { - PestoFTCConfig.initializePinpoint = true; - - super.runOpMode(); - - waitForStart(); - - while (opModeIsActive() && !isStopRequested()) { - FrontalLobe.update(); - MotorCortex.update(); - gamepadInterface1.update(); - tracker.update(); - - if (gamepad1.b) { - tracker.reset(); - teleOpController.resetIMU(); - } - - teleOpController.driveFieldCentric(-gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x); - - boolean intaking = gamepad1.right_trigger > 0.05; - boolean outtaking = !intaking && gamepad1.left_trigger > 0.05; - boolean rejecting = !intaking && !outtaking && gamepad1.a; - boolean neutralizing = !intaking && !outtaking && !rejecting; - - if (!outtaking && state == RobotState.OUTTAKE) { - FrontalLobe.removeMacros("outtake"); - } - - if (intaking) { - state = RobotState.INTAKE; - - intakeSubsystem.setState(IntakeSubsystem.IntakeState.INTAKE); - feederSubsystem.setState(FeederSubsystem.FeederState.FORWARD); - outtakeSubsystem.setState(OuttakeSubsystem.OuttakeState.NEUTRAL); - indexerSubsystem.setState(IndexerSubsystem.IndexerState.NEUTRAL); - } - - if (outtaking && state != RobotState.OUTTAKE) { - state = RobotState.OUTTAKE; - - FrontalLobe.useMacro("outtake"); - } - - if (rejecting) { - state = RobotState.REJECT; - - intakeSubsystem.setState(IntakeSubsystem.IntakeState.REJECT); - feederSubsystem.setState(FeederSubsystem.FeederState.REVERSE); - outtakeSubsystem.setState(OuttakeSubsystem.OuttakeState.NEUTRAL); - indexerSubsystem.setState(IndexerSubsystem.IndexerState.NEUTRAL); - } - - if (neutralizing) { - state = RobotState.NEUTRAL; - - intakeSubsystem.setState(IntakeSubsystem.IntakeState.NEUTRAL); - feederSubsystem.setState(FeederSubsystem.FeederState.STOPPED); - outtakeSubsystem.setState(OuttakeSubsystem.OuttakeState.NEUTRAL); - indexerSubsystem.setState(IndexerSubsystem.IndexerState.NEUTRAL); - } - - - - // Nice little LED display on the gamepad - if (hoodSubsystem.getState() == HoodSubsystem.HoodState.CLOSE) - gamepad1.setLedColor(0, 255, 0, Integer.MAX_VALUE); - - if (hoodSubsystem.getState() == HoodSubsystem.HoodState.MID) - gamepad1.setLedColor(0, 0, 255, Integer.MAX_VALUE); - - if (hoodSubsystem.getState() == HoodSubsystem.HoodState.FAR) - gamepad1.setLedColor(255, 0, 0, Integer.MAX_VALUE); - - // Cycle hood modes - if (gamepadInterface1.isKeyDown(GamepadKey.TOUCHPAD)) { - if (hoodSubsystem.getState() == HoodSubsystem.HoodState.CLOSE) { - hoodSubsystem.setState(HoodSubsystem.HoodState.MID); - outtakeSubsystem.setPower(PestoFTCConfig.SHOOTER_MIDDLE); - } else if (hoodSubsystem.getState() == HoodSubsystem.HoodState.MID) { - hoodSubsystem.setState(HoodSubsystem.HoodState.FAR); - outtakeSubsystem.setPower(PestoFTCConfig.SHOOTER_FAR); - } else if (hoodSubsystem.getState() == HoodSubsystem.HoodState.FAR) { - hoodSubsystem.setState(HoodSubsystem.HoodState.CLOSE); - outtakeSubsystem.setPower(PestoFTCConfig.SHOOTER_CLOSE); - } - } - - feederSubsystem.update(); - hoodSubsystem.update(); - intakeSubsystem.update(); - outtakeSubsystem.update(); - indexerSubsystem.update(); - - telemetry.addData("x", tracker.getCurrentPosition().getX()); - telemetry.addData("y", tracker.getCurrentPosition().getY()); - telemetry.addData("r", tracker.getCurrentPosition().getHeadingRadians()); - telemetry.update(); - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PestoFTCConfig.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PestoFTCConfig.java index 555964d..d1b831d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PestoFTCConfig.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PestoFTCConfig.java @@ -4,12 +4,10 @@ import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; -import com.shprobotics.pestocore.drivebases.controllers.DriveController; +import com.shprobotics.pestocore.algorithms.Constants; import com.shprobotics.pestocore.drivebases.controllers.MecanumController; import com.shprobotics.pestocore.drivebases.controllers.TeleOpController; -import com.shprobotics.pestocore.drivebases.trackers.DeterministicTracker; import com.shprobotics.pestocore.drivebases.trackers.ThreeWheelOdometryTracker; -import com.shprobotics.pestocore.hardware.CortexLinkedMotor; import com.shprobotics.pestocore.processing.Cerebrum; import com.shprobotics.pestocore.processing.ConfigInterface; import com.shprobotics.pestocore.processing.FrontalLobe; @@ -20,96 +18,134 @@ @PestoConfig() public class PestoFTCConfig implements ConfigInterface { public static boolean initialized = false; // don't mess with this :O - public static boolean initializePinpoint = false; + public static boolean initializePinpoint = true; + + public static double mass = 23.7; // lbs + public static double MAX_FORCE = 2170; + public static double DECELERATION = 100; + public static double ENDPOINT_KP = 0.3; + public static double HEADING_KP = 4; // ODOMETRY - public static String leftName = "fl"; - public static String centerName = "bl"; - public static String rightName = "fr"; + public static String leftName = "backL"; + public static String centerName = "backR"; + public static String rightName = "frontR"; - public static DcMotorSimple.Direction leftDirection = DcMotorSimple.Direction.REVERSE; - public static DcMotorSimple.Direction centerDirection = DcMotorSimple.Direction.FORWARD; - public static DcMotorSimple.Direction rightDirection = DcMotorSimple.Direction.REVERSE; + public static DcMotorSimple.Direction leftDirection = DcMotorSimple.Direction.FORWARD; + public static DcMotorSimple.Direction centerDirection = DcMotorSimple.Direction.REVERSE; + public static DcMotorSimple.Direction rightDirection = DcMotorSimple.Direction.FORWARD; public static double ODOMETRY_TICKS_PER_INCH = 505.3169; - public static double FORWARD_OFFSET = -10; - public static double ODOMETRY_WIDTH = 9.663; + public static double FORWARD_OFFSET = -1.9; + public static double ODOMETRY_WIDTH = 4.1789; + + public static double STATIC_DRIVE = 0.22; // DROPDOWN - public static double DROPDOWN_DRIVE = 0.54; - public static double DROPDOWN_INTAKE = 0.6; - public static double DROPDOWN_PUSH = 0.420; + public static double DROPDOWN_DRIVE = 0.4; + public static double DROPDOWN_INTAKE = 0.25; + public static double DROPDOWN_PUSH = 0.35; + + // BLOCKER + public static double BLOCKER_BLOCK = 0.4; + public static double BLOCKER_OUTTAKE = 0.7; // INDEXER - public static double INDEXER_OUTTAKE = 0.08; - public static double INDEXER_BLOCK = 0.26; + public static double INDEXER_IN = 0.20; + public static double INDEXER_OUTISH = 0.52; + public static double INDEXER_OUT = 0.589; // HOOD - public static double HOOD_CLOSE = 0.070; - public static double HOOD_MID = 0.1; - public static double HOOD_FAR = 0.1131; + public static double HOOD_CLOSE = 0.33; + public static double HOOD_MID = 0.34; + public static double HOOD_FAR = 0.35; // SHOOTER - public static double SHOOTER_CLOSE = 0.73; - public static double SHOOTER_MIDDLE = 0.9; - public static double SHOOTER_FAR = 1.0; + public static double SHOOTER_KP = 0.3; + + public static double SHOOTER_FF_CLOSE = 0.32; + public static double SHOOTER_FF_MIDDLE = 0.5; + public static double SHOOTER_FF_FAR = 0.5; + public static double SHOOTER_FF_AUTO = 0.5; + + public static double SHOOTER_CLOSE = 1.8; + public static double SHOOTER_MIDDLE = 3.5; + public static double SHOOTER_FAR = 2.6; + public static double SHOOTER_AUTO = 2.6; + + public static double SHOOTER_RPM_TOLERANCE = 0.15; + + // TURRET + public static double TURRET_LEFT = 100; + public static double TURRET_RIGHT = -100; + public static double TURRET_STATIC = 0.12; //increase when its a little off + public static double TURRET_KP = 0.006; + + // BRAKE + public static double BRAKE_DOWN = 0.3; + public static double BRAKE_UP = 0.1; public static void initialize(HardwareMap hardwareMap) { MotorCortex.initialize(hardwareMap); Cerebrum.initialize(); - CortexLinkedMotor frontLeft = MotorCortex.getMotor("fl"); - CortexLinkedMotor frontRight = MotorCortex.getMotor("fr"); - CortexLinkedMotor backLeft = MotorCortex.getMotor("bl"); - CortexLinkedMotor backRight = MotorCortex.getMotor("br"); - - frontLeft.setDirection(DcMotorSimple.Direction.REVERSE); - frontRight.setDirection(DcMotorSimple.Direction.FORWARD); - backLeft.setDirection(DcMotorSimple.Direction.REVERSE); - backRight.setDirection(DcMotorSimple.Direction.FORWARD); - - DriveController driveController = new MecanumController( - MotorCortex.getMotor("fl"), - MotorCortex.getMotor("fr"), - MotorCortex.getMotor("bl"), - MotorCortex.getMotor("br") + MecanumController mecanumController = new MecanumController( + MotorCortex.getMotor("frontLeft"), + MotorCortex.getMotor("frontRight"), + MotorCortex.getMotor("backLeft"), + MotorCortex.getMotor("backRight") ); - driveController.configureMotorDirections(new DcMotorSimple.Direction[]{ + FrontalLobe.driveController = mecanumController; + + mecanumController.configureMotorDirections(new DcMotorSimple.Direction[]{ DcMotorSimple.Direction.REVERSE, DcMotorSimple.Direction.FORWARD, DcMotorSimple.Direction.REVERSE, DcMotorSimple.Direction.FORWARD }); - driveController.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - - if (initializePinpoint) { - DeterministicTracker tracker = new ThreeWheelOdometryTracker.TrackerBuilder( - hardwareMap, - ODOMETRY_TICKS_PER_INCH, - FORWARD_OFFSET, - ODOMETRY_WIDTH, - leftName, - centerName, - rightName, - leftDirection, - centerDirection, - rightDirection + mecanumController.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + ThreeWheelOdometryTracker tracker = new ThreeWheelOdometryTracker.TrackerBuilder( + hardwareMap, + ODOMETRY_TICKS_PER_INCH, + ODOMETRY_TICKS_PER_INCH, + ODOMETRY_TICKS_PER_INCH, + -2.0, + 4.9299, + "backRight", + "frontRight", + "backLeft", + DcMotorSimple.Direction.FORWARD, + DcMotorSimple.Direction.FORWARD, + DcMotorSimple.Direction.FORWARD ) - .build(); - - TeleOpController teleOpController = new TeleOpController(driveController, hardwareMap); - teleOpController.useTrackerIMU(tracker); - - teleOpController.setSpeedController(gamepad -> 1.0); - -// teleOpController.counteractCentripetalForce(); - - FrontalLobe.teleOpController = teleOpController; - FrontalLobe.tracker = tracker; - } - - FrontalLobe.driveController = driveController; + .build(); + + FrontalLobe.tracker = tracker; + + TeleOpController teleOpController = new TeleOpController(mecanumController, hardwareMap); + FrontalLobe.teleOpController = teleOpController; + teleOpController.useTrackerIMU(tracker); + + // TODO: set speed controller lambda + // b is circle + // a is cross + // x is square + // y is triangle + // left_bumper + // right_bumper + // left_trigger > 0.05 + // right_trigger > 0.05 + // dpad_down + // dpad_left + // dpad_right + // dpad_up + // touchpad + teleOpController.setSpeedController(gamepad -> gamepad.x ? 0.6 : 1.0); + + FrontalLobe.driveController = mecanumController; + Constants.mass = mass; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/BaseRobot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/BaseRobot.java index b6f629b..4ce0193 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/BaseRobot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/BaseRobot.java @@ -7,22 +7,19 @@ import com.shprobotics.pestocore.drivebases.trackers.DeterministicTracker; import com.shprobotics.pestocore.processing.FrontalLobe; -import org.firstinspires.ftc.teamcode.PestoFTCConfig; - public class BaseRobot extends LinearOpMode { public MecanumController mecanumController; - public DeterministicTracker tracker; public TeleOpController teleOpController; + public DeterministicTracker tracker; - public FeederSubsystem feederSubsystem; + public BlockerSubsystem blockerSubsystem; public HoodSubsystem hoodSubsystem; - public IntakeSubsystem intakeSubsystem; - public OuttakeSubsystem outtakeSubsystem; - public IndexerSubsystem indexerSubsystem; + public IntakeOuttakeSubsystem intakeOuttakeSubsystem; public GamepadInterface gamepadInterface1; public RobotState state; + public boolean brake; public enum RobotState { INTAKE, @@ -31,28 +28,26 @@ public enum RobotState { NEUTRAL } - @Override - public void runOpMode() { + public void initialize() { FrontalLobe.initialize(hardwareMap); mecanumController = (MecanumController) FrontalLobe.driveController; - if (PestoFTCConfig.initializePinpoint) { - tracker = FrontalLobe.tracker; - tracker.reset(); +// mecanumController.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + + teleOpController = FrontalLobe.teleOpController; - teleOpController = FrontalLobe.teleOpController; - } + tracker = FrontalLobe.tracker; + tracker.reset(); - feederSubsystem = new FeederSubsystem(); + blockerSubsystem = new BlockerSubsystem(); hoodSubsystem = new HoodSubsystem(); - intakeSubsystem = new IntakeSubsystem(); - outtakeSubsystem = new OuttakeSubsystem(); - indexerSubsystem = new IndexerSubsystem(); + intakeOuttakeSubsystem = new IntakeOuttakeSubsystem(); gamepadInterface1 = new GamepadInterface(gamepad1); // State initialization state = RobotState.NEUTRAL; + brake = false; // MACRO initialization @@ -60,21 +55,29 @@ public void runOpMode() { @Override public void start() { FrontalLobe.removeOtherMacros(this); - outtakeSubsystem.setState(OuttakeSubsystem.OuttakeState.OUTTAKE); + intakeOuttakeSubsystem.setState(IntakeOuttakeSubsystem.IntakeOuttakeState.PREREV); } @Override public boolean loop(double v) { // how long (seconds) before starting to move other components - if (v < 1.0) + if (v < 0.3) + return false; + intakeOuttakeSubsystem.setState(IntakeOuttakeSubsystem.IntakeOuttakeState.REV); + if (v < 2) return false; - feederSubsystem.setState(FeederSubsystem.FeederState.FORWARD); - intakeSubsystem.setState(IntakeSubsystem.IntakeState.OUTTAKE); - indexerSubsystem.setState(IndexerSubsystem.IndexerState.OUTTAKE); + blockerSubsystem.setState(BlockerSubsystem.BlockerState.NEUTRAL); + intakeOuttakeSubsystem.setState(IntakeOuttakeSubsystem.IntakeOuttakeState.OUTTAKE); + return true; } }); } + + @Override + public void runOpMode() { + + } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/BlockerSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/BlockerSubsystem.java new file mode 100644 index 0000000..4c98d9a --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/BlockerSubsystem.java @@ -0,0 +1,34 @@ +package org.firstinspires.ftc.teamcode.subsystems; + +import static org.firstinspires.ftc.teamcode.PestoFTCConfig.BLOCKER_BLOCK; +import static org.firstinspires.ftc.teamcode.PestoFTCConfig.BLOCKER_OUTTAKE; + +import com.shprobotics.pestocore.hardware.CortexLinkedServo; +import com.shprobotics.pestocore.processing.MotorCortex; + +public class BlockerSubsystem { + private CortexLinkedServo blocker; + + private BlockerState state; + + public enum BlockerState{ + BLOCK, + NEUTRAL + } + + public BlockerSubsystem(){ + blocker = MotorCortex.getServo("blocker"); + state = BlockerState.BLOCK; + } + + public void setState(BlockerState state){this.state = state;} + + public void update(){ + if (state == BlockerState.BLOCK){ + blocker.setPositionResult(BLOCKER_BLOCK); + } + else{ + blocker.setPositionResult(BLOCKER_OUTTAKE); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/FeederSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/FeederSubsystem.java deleted file mode 100644 index ff2d593..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/FeederSubsystem.java +++ /dev/null @@ -1,37 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems; - -import static org.firstinspires.ftc.teamcode.subsystems.FeederSubsystem.FeederState.FORWARD; -import static org.firstinspires.ftc.teamcode.subsystems.FeederSubsystem.FeederState.REVERSE; -import static org.firstinspires.ftc.teamcode.subsystems.FeederSubsystem.FeederState.STOPPED; - -import com.shprobotics.pestocore.hardware.CortexLinkedCRServo; -import com.shprobotics.pestocore.processing.MotorCortex; - -public class FeederSubsystem { - private final CortexLinkedCRServo feeder; - private FeederState state; - - public enum FeederState { - FORWARD, - REVERSE, - STOPPED - } - - public FeederSubsystem() { - feeder = MotorCortex.getCRServo("feeder"); - state = STOPPED; - } - - public void setState(FeederState state) { - this.state = state; - } - - public void update() { - if (state == FORWARD) - feeder.setPowerResult(0.7); - if (state == REVERSE) - feeder.setPowerResult(-1.0); - if (state == STOPPED) - feeder.setPowerResult(0.0); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HoodSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HoodSubsystem.java index ca43ba4..f7372a6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HoodSubsystem.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HoodSubsystem.java @@ -3,54 +3,36 @@ import static org.firstinspires.ftc.teamcode.PestoFTCConfig.HOOD_CLOSE; import static org.firstinspires.ftc.teamcode.PestoFTCConfig.HOOD_FAR; import static org.firstinspires.ftc.teamcode.PestoFTCConfig.HOOD_MID; -import static org.firstinspires.ftc.teamcode.subsystems.HoodSubsystem.HoodState.CLOSE; -import static org.firstinspires.ftc.teamcode.subsystems.HoodSubsystem.HoodState.FAR; -import static org.firstinspires.ftc.teamcode.subsystems.HoodSubsystem.HoodState.MID; import com.shprobotics.pestocore.hardware.CortexLinkedServo; import com.shprobotics.pestocore.processing.MotorCortex; public class HoodSubsystem { - private final CortexLinkedServo hood; - private final CortexLinkedServo led; - + private CortexLinkedServo hood; private HoodState state; - - public enum HoodState { + public enum HoodState{ CLOSE, MID, FAR } - public HoodSubsystem() { + public HoodSubsystem(){ hood = MotorCortex.getServo("hood"); - led = MotorCortex.getServo("led"); - - state = CLOSE; - } - - public HoodState getState() { - return state; + state = HoodState.CLOSE; } - public void setState(HoodState state) { - this.state = state; - } + public void setState(HoodState state){this.state = state;} - public void update() { - if (state == CLOSE) { + public HoodState getState(){return this.state;} + public void update(){ + if(state == HoodState.CLOSE){ hood.setPositionResult(HOOD_CLOSE); - led.setPositionResult(0.5); // GREEN } - - if (state == MID) { + else if(state == HoodState.MID){ hood.setPositionResult(HOOD_MID); - led.setPositionResult(0.61); // BLUE } - - if (state == FAR) { + else{ hood.setPositionResult(HOOD_FAR); - led.setPositionResult(0.28); // RED } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IndexerSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IndexerSubsystem.java deleted file mode 100644 index f208f3a..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IndexerSubsystem.java +++ /dev/null @@ -1,39 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems; - -import static com.qualcomm.robotcore.hardware.DcMotor.RunMode.RUN_USING_ENCODER; -import static org.firstinspires.ftc.teamcode.PestoFTCConfig.INDEXER_BLOCK; -import static org.firstinspires.ftc.teamcode.PestoFTCConfig.INDEXER_OUTTAKE; -import static org.firstinspires.ftc.teamcode.subsystems.OuttakeSubsystem.OuttakeState.OUTTAKE; - -import com.shprobotics.pestocore.hardware.CortexLinkedMotor; -import com.shprobotics.pestocore.hardware.CortexLinkedServo; -import com.shprobotics.pestocore.processing.MotorCortex; - -public class IndexerSubsystem { - private final CortexLinkedServo indexer; - - private IndexerState state; - - public enum IndexerState { - OUTTAKE, - NEUTRAL - } - - public IndexerSubsystem() { - indexer = MotorCortex.getServo("indexer"); - - state = IndexerState.NEUTRAL; - } - - public void setState(IndexerState state) { - this.state = state; - } - - public void update() { - if (state == IndexerState.OUTTAKE) { - indexer.setPositionResult(INDEXER_OUTTAKE); - } else { - indexer.setPositionResult(INDEXER_BLOCK); - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IntakeOuttakeSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IntakeOuttakeSubsystem.java new file mode 100644 index 0000000..e600f5b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IntakeOuttakeSubsystem.java @@ -0,0 +1,64 @@ +package org.firstinspires.ftc.teamcode.subsystems; + +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.shprobotics.pestocore.hardware.CortexLinkedMotor; +import com.shprobotics.pestocore.processing.MotorCortex; + +public class IntakeOuttakeSubsystem { + private final CortexLinkedMotor theGoatorRight; + //private final CortexLinkedMotor theGoatorLeft; + private final CortexLinkedMotor shooter; + private IntakeOuttakeState state; + + public enum IntakeOuttakeState{ + REJECT, + NEUTRAL, + PREREV, + REV, + INTAKE, + OUTTAKE + } + + public IntakeOuttakeSubsystem(){ + theGoatorRight = MotorCortex.getMotor("theGoatorRight"); + theGoatorRight.setDirection(DcMotorSimple.Direction.REVERSE); + //theGoatorLeft = MotorCortex.getMotor("theGoatorLeft"); + shooter = MotorCortex.getMotor("shooter"); + state = IntakeOuttakeState.NEUTRAL; + } + + public void setState(IntakeOuttakeState state){this.state = state;} + + public void update(){ + if (state == IntakeOuttakeState.INTAKE){ + theGoatorRight.setPowerResult(1); + // theGoatorLeft.setPowerResult(1); + shooter.setPowerResult(0); + } + else if (state == IntakeOuttakeState.REJECT){ + theGoatorRight.setPowerResult(-1); + // theGoatorLeft.setPowerResult(-1); + shooter.setPowerResult(-1); + } + else if (state == IntakeOuttakeState.PREREV){ + theGoatorRight.setPowerResult(0); + // theGoatorLeft.setPowerResult(0); + shooter.setPowerResult(0.5); + } + else if (state == IntakeOuttakeState.REV){ + theGoatorRight.setPowerResult(0); + // theGoatorLeft.setPowerResult(0); + shooter.setPowerResult(1); + } + else if (state == IntakeOuttakeState.OUTTAKE){ + theGoatorRight.setPowerResult(1); + //theGoatorLeft.setPowerResult(1); + shooter.setPowerResult(1); + } + else{ + theGoatorRight.setPowerResult(0); + // theGoatorLeft.setPowerResult(0); + shooter.setPowerResult(0); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IntakeSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IntakeSubsystem.java deleted file mode 100644 index e663faf..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/IntakeSubsystem.java +++ /dev/null @@ -1,63 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems; - -import static com.qualcomm.robotcore.hardware.DcMotorSimple.Direction.REVERSE; -import static org.firstinspires.ftc.teamcode.PestoFTCConfig.DROPDOWN_DRIVE; -import static org.firstinspires.ftc.teamcode.PestoFTCConfig.DROPDOWN_INTAKE; -import static org.firstinspires.ftc.teamcode.PestoFTCConfig.DROPDOWN_PUSH; -import static org.firstinspires.ftc.teamcode.subsystems.IntakeSubsystem.IntakeState.INTAKE; -import static org.firstinspires.ftc.teamcode.subsystems.IntakeSubsystem.IntakeState.NEUTRAL; -import static org.firstinspires.ftc.teamcode.subsystems.IntakeSubsystem.IntakeState.OUTTAKE; -import static org.firstinspires.ftc.teamcode.subsystems.IntakeSubsystem.IntakeState.REJECT; - -import com.shprobotics.pestocore.hardware.CortexLinkedMotor; -import com.shprobotics.pestocore.hardware.CortexLinkedServo; -import com.shprobotics.pestocore.processing.MotorCortex; - -public class IntakeSubsystem { - private final CortexLinkedMotor intake; - private final CortexLinkedServo dropdown; - - private IntakeState state; - - public enum IntakeState { - INTAKE, - NEUTRAL, - REJECT, - OUTTAKE - } - - public IntakeSubsystem() { - intake = MotorCortex.getMotor("intake"); - intake.setDirection(REVERSE); - - dropdown = MotorCortex.getServo("dropdown"); - - state = NEUTRAL; - } - - public void setState(IntakeState state) { - this.state = state; - } - - public void update() { - if (state == INTAKE) { - intake.setPowerResult(1.0); - dropdown.setPositionResult(DROPDOWN_INTAKE); - } - - if (state == NEUTRAL) { - intake.setPowerResult(0.0); - dropdown.setPositionResult(DROPDOWN_DRIVE); - } - - if (state == REJECT) { - intake.setPowerResult(-1.0); - dropdown.setPositionResult(DROPDOWN_INTAKE); - } - - if (state == OUTTAKE) { - intake.setPowerResult(1.0); - dropdown.setPositionResult(DROPDOWN_PUSH); - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OuttakeSubsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OuttakeSubsystem.java deleted file mode 100644 index 807365b..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/OuttakeSubsystem.java +++ /dev/null @@ -1,50 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems; - -import static com.qualcomm.robotcore.hardware.DcMotor.RunMode.RUN_USING_ENCODER; -import static org.firstinspires.ftc.teamcode.PestoFTCConfig.INDEXER_BLOCK; -import static org.firstinspires.ftc.teamcode.PestoFTCConfig.INDEXER_OUTTAKE; -import static org.firstinspires.ftc.teamcode.subsystems.OuttakeSubsystem.OuttakeState.OUTTAKE; - -import com.shprobotics.pestocore.hardware.CortexLinkedMotor; -import com.shprobotics.pestocore.hardware.CortexLinkedServo; -import com.shprobotics.pestocore.processing.MotorCortex; - -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.teamcode.PestoFTCConfig; - -public class OuttakeSubsystem { - private final CortexLinkedMotor shooter; - - private OuttakeState state; - - private double power = PestoFTCConfig.SHOOTER_CLOSE; - - public enum OuttakeState { - OUTTAKE, - NEUTRAL - } - - public OuttakeSubsystem() { - shooter = MotorCortex.getMotor("shooter"); - shooter.setMode(RUN_USING_ENCODER); - - state = OuttakeState.NEUTRAL; - } - - public void setState(OuttakeState state) { - this.state = state; - } - - public void setPower(double power) { - assert 0 <= power && power <= 1; - this.power = power; - } - - public void update() { - if (state == OUTTAKE) { - shooter.setPowerResult(power); - } else { - shooter.setPowerResult(0.0); - } - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleops/Drive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleops/Drive.java new file mode 100644 index 0000000..f68e4a1 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleops/Drive.java @@ -0,0 +1,148 @@ +package org.firstinspires.ftc.teamcode.teleops; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.shprobotics.pestocore.devices.GamepadKey; +import com.shprobotics.pestocore.processing.FrontalLobe; +import com.shprobotics.pestocore.processing.MotorCortex; + +import org.firstinspires.ftc.teamcode.PestoFTCConfig; +import org.firstinspires.ftc.teamcode.subsystems.BaseRobot; +import org.firstinspires.ftc.teamcode.subsystems.BlockerSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.HoodSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.IntakeOuttakeSubsystem; + +@TeleOp(name = "CP?") +public class Drive extends BaseRobot { + @Override + public void runOpMode() { + PestoFTCConfig.initializePinpoint = false; + + super.initialize(); + +// boolean rotationLocked = false; +// double angle = 0.0; + + waitForStart(); + + tracker.reset(); + + while (opModeIsActive() && !isStopRequested()) { + FrontalLobe.update(); + MotorCortex.update(); + gamepadInterface1.update(); + tracker.update(); + + // use lambda + teleOpController.updateSpeed(gamepad1); + + if (gamepad1.b) { + tracker.reset(); + teleOpController.resetIMU(); + } + +// if (gamepadInterface1.isKeyDown(GamepadKey.RIGHT_BUMPER)) { +// rotationLocked = !rotationLocked; +// +// double[] angles = new double[]{0.0, Math.PI / 2, Math.PI, 3 * Math.PI / 2}; +// double bestDist = Double.POSITIVE_INFINITY; +// +// for (double x : angles) { +// double dist = Math.abs(x - MathUtils.normalizeAngle(tracker.getCurrentPosition().getHeadingRadians(), x)); +// +// if (dist < bestDist) { +// bestDist = dist; +// angle = x; +// } +// } +// } + +// if (rotationLocked) { +// double correction = angle - MathUtils.normalizeAngle(tracker.getCurrentPosition().getHeadingRadians(), angle); +// correction = correction * -0.6; // 0.6 == headingKP +// correction += Math.signum(correction) * PestoFTCConfig.STATIC_DRIVE; +// +// teleOpController.driveFieldCentric(-gamepad1.left_stick_y, gamepad1.left_stick_x, correction); +// } else { +// teleOpController.driveFieldCentric(-gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x); +// } + + teleOpController.driveRobotCentric(-gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x); + + boolean intaking = gamepad1.right_trigger > 0.05; + boolean outtaking = !intaking && gamepad1.left_trigger > 0.05; + boolean rejecting = !intaking && !outtaking && gamepad1.a; + boolean neutralizing = !intaking && !outtaking && !rejecting; + + + if (!outtaking && state == RobotState.OUTTAKE) { + FrontalLobe.removeMacros("outtake"); + } + + if (intaking) { + state = RobotState.INTAKE; + + intakeOuttakeSubsystem.setState(IntakeOuttakeSubsystem.IntakeOuttakeState.INTAKE); + blockerSubsystem.setState(BlockerSubsystem.BlockerState.BLOCK); + } + + if (outtaking && state != RobotState.OUTTAKE) { + state = RobotState.OUTTAKE; + + FrontalLobe.useMacro("outtake"); + } + + if (rejecting) { + state = RobotState.REJECT; + + intakeOuttakeSubsystem.setState(IntakeOuttakeSubsystem.IntakeOuttakeState.REJECT); + blockerSubsystem.setState(BlockerSubsystem.BlockerState.NEUTRAL); + } + + if (neutralizing) { + state = RobotState.NEUTRAL; + + intakeOuttakeSubsystem.setState(IntakeOuttakeSubsystem.IntakeOuttakeState.NEUTRAL); + blockerSubsystem.setState(BlockerSubsystem.BlockerState.BLOCK); + } + + + +// // Nice little LED display on the gamepad + if (hoodSubsystem.getState() == HoodSubsystem.HoodState.CLOSE) + gamepad1.setLedColor(0, 255, 0, Integer.MAX_VALUE); + + if (hoodSubsystem.getState() == HoodSubsystem.HoodState.MID) + gamepad1.setLedColor(0, 0, 255, Integer.MAX_VALUE); + + if (hoodSubsystem.getState() == HoodSubsystem.HoodState.FAR) + gamepad1.setLedColor(255, 0, 0, Integer.MAX_VALUE); + +// // Cycle hood modes + if (gamepadInterface1.isKeyDown(GamepadKey.TOUCHPAD)) { + if (hoodSubsystem.getState() == HoodSubsystem.HoodState.CLOSE) { + hoodSubsystem.setState(HoodSubsystem.HoodState.MID); + + } else if (hoodSubsystem.getState() == HoodSubsystem.HoodState.MID) { + hoodSubsystem.setState(HoodSubsystem.HoodState.FAR); + + } else if (hoodSubsystem.getState() == HoodSubsystem.HoodState.FAR) { + hoodSubsystem.setState(HoodSubsystem.HoodState.CLOSE); + } + } + + + blockerSubsystem.update(); + hoodSubsystem.update(); + intakeOuttakeSubsystem.update(); + + +// telemetry.addData("x", tracker.getCurrentPosition().getX()); +// telemetry.addData("y", tracker.getCurrentPosition().getY()); +// telemetry.addData("r", tracker.getCurrentPosition().getHeadingRadians()); +// telemetry.addData("turret", turretSubsystem.getPosition()); +// telemetry.addData("shooter", outtakeSubsystem.getRPM()); +// telemetry.addData("target", outtakeSubsystem.getTargetRPM()); +// telemetry.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleops/Move.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleops/Move.java new file mode 100644 index 0000000..575f9fe --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleops/Move.java @@ -0,0 +1,29 @@ +package org.firstinspires.ftc.teamcode.teleops; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.shprobotics.pestocore.devices.GamepadKey; +import com.shprobotics.pestocore.processing.FrontalLobe; +import com.shprobotics.pestocore.processing.MotorCortex; + +import org.firstinspires.ftc.teamcode.PestoFTCConfig; +import org.firstinspires.ftc.teamcode.subsystems.BaseRobot; +import org.firstinspires.ftc.teamcode.subsystems.BlockerSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.HoodSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.IntakeOuttakeSubsystem; + +@Autonomous(name = "move?!") +public class Move extends BaseRobot { + @Override + public void runOpMode() { + PestoFTCConfig.initializePinpoint = false; + + super.initialize(); + + waitForStart(); + + mecanumController.drive(0.65, 0, 0); + sleep(500); + mecanumController.drive(0, 0, 0); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleops/Trust.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleops/Trust.java new file mode 100644 index 0000000..536bf8d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleops/Trust.java @@ -0,0 +1,40 @@ +package org.firstinspires.ftc.teamcode.teleops; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.shprobotics.pestocore.processing.MotorCortex; + +import org.firstinspires.ftc.teamcode.PestoFTCConfig; +import org.firstinspires.ftc.teamcode.subsystems.BaseRobot; +import org.firstinspires.ftc.teamcode.subsystems.BlockerSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.HoodSubsystem; +import org.firstinspires.ftc.teamcode.subsystems.IntakeOuttakeSubsystem; + +@Autonomous +public class Trust extends BaseRobot { + @Override + public void runOpMode(){ + PestoFTCConfig.initializePinpoint = false; + super.initialize(); + waitForStart(); + intakeOuttakeSubsystem.setState(IntakeOuttakeSubsystem.IntakeOuttakeState.PREREV); + blockerSubsystem.setState(BlockerSubsystem.BlockerState.BLOCK); + hoodSubsystem.setState(HoodSubsystem.HoodState.FAR); + double start = System.nanoTime()/1E9; + while(opModeIsActive() && !isStopRequested() && System.nanoTime()/1E9 - start < 15){ + if((System.nanoTime()/1E9) - start > 0.3 && (System.nanoTime()/1E9) - start < 2){ + intakeOuttakeSubsystem.setState(IntakeOuttakeSubsystem.IntakeOuttakeState.REV); + } + else if((System.nanoTime()/1E9) - start > 2){ + intakeOuttakeSubsystem.setState(IntakeOuttakeSubsystem.IntakeOuttakeState.OUTTAKE); + blockerSubsystem.setState(BlockerSubsystem.BlockerState.NEUTRAL); + } + MotorCortex.update(); + intakeOuttakeSubsystem.update(); + blockerSubsystem.update(); + hoodSubsystem.update(); + } + } + + +} + diff --git a/build.common.gradle b/build.common.gradle index a1d2fa4..0ac4c94 100644 --- a/build.common.gradle +++ b/build.common.gradle @@ -21,7 +21,7 @@ apply plugin: 'com.android.application' android { - compileSdkVersion 30 + compileSdk 34 signingConfigs { release { @@ -114,4 +114,3 @@ android { repositories { } - diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 6221530..afc4534 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -5,6 +5,7 @@ repositories { maven { url = 'https://maven.brott.dev/' } maven { url = "https://repo.dairy.foundation/releases" } + maven { url = "https://mymaven.bylazar.com/releases" } flatDir { dirs rootProject.file('libs') @@ -12,19 +13,27 @@ repositories { } dependencies { - implementation 'org.firstinspires.ftc:Inspection:10.1.1' - implementation 'org.firstinspires.ftc:Blocks:10.1.1' - implementation 'org.firstinspires.ftc:RobotCore:10.1.1' - implementation 'org.firstinspires.ftc:RobotServer:10.1.1' - implementation 'org.firstinspires.ftc:OnBotJava:10.1.1' - implementation 'org.firstinspires.ftc:Hardware:10.1.1' - implementation 'org.firstinspires.ftc:FtcCommon:10.1.1' - implementation 'org.firstinspires.ftc:Vision:10.1.1' - implementation 'androidx.appcompat:appcompat:1.2.0' - implementation "dev.frozenmilk.dairy:CachingHardware:1.0.0" implementation 'io.github.william27b:dashboard:0.15.03' implementation 'io.github.william27b:core:0.15.03' + + implementation 'org.firstinspires.ftc:Inspection:11.0.0' + implementation 'org.firstinspires.ftc:Blocks:11.0.0' + implementation 'org.firstinspires.ftc:RobotCore:11.0.0' + implementation 'org.firstinspires.ftc:RobotServer:11.0.0' + implementation 'org.firstinspires.ftc:OnBotJava:11.0.0' + implementation 'org.firstinspires.ftc:Hardware:11.0.0' + implementation 'org.firstinspires.ftc:FtcCommon:11.0.0' + implementation 'org.firstinspires.ftc:Vision:11.0.0' + implementation 'androidx.appcompat:appcompat:1.2.0' + +// implementation 'com.pedropathing:ftc:2.0.4' +// implementation 'com.pedropathing:telemetry:1.0.0' +// implementation 'com.bylazar:fullpanels:1.0.9' + + implementation 'org.ejml:ejml-all:0.43' + +// implementation "dev.frozenmilk.sinister:Sloth:0.2.4" } diff --git a/libs/PestoCore-release.aar b/libs/PestoCore-release.aar index c94ce0a..7cd1639 100644 Binary files a/libs/PestoCore-release.aar and b/libs/PestoCore-release.aar differ diff --git a/local.properties b/local.properties index 9feb834..f9b6351 100644 --- a/local.properties +++ b/local.properties @@ -4,5 +4,5 @@ # Location of the SDK. This is only used by Gradle. # For customization when using a Version Control System, please read the # header note. -#Fri Nov 14 11:54:08 PST 2025 -sdk.dir=C\:\\Users\\ARadz\\AppData\\Local\\Android\\Sdk +#Fri Jan 23 18:13:04 PST 2026 +sdk.dir=/Users/william/Library/Android/android-sdk