From 770ce6d36fb5a18ea5b083db6b0cfd390d6963e5 Mon Sep 17 00:00:00 2001 From: Kevin Frei Date: Thu, 21 Nov 2024 15:32:49 -0800 Subject: [PATCH] Harshini drivebase i love drivebases (#80) * 11/18/24 - Harshini i made a forward backward side opmode and tested it (it goes in a square) for kevin :D * 11/18/24 - Harshini after consulting with kevin on monday, yesterday, odopods are actually messed up and we(I) need to test to see which ones need to be reversed teehee yay yippee yahooooooo * 11/19/24 - Harshini methods for slide buttons * 11/19/24 - Harshini changed PID constants and feed fwd constant (not fully tested yet, will finish on friday hopefullyyy) * 11/21/24 - Harshini updated kid shampoo constants, should be all correct now (tested on bot) :DDD --------- Co-authored-by: FTC16750 --- .../ftc/twenty403/AutoConstants.java | 6 +- .../firstinspires/ftc/twenty403/Hardware.java | 2 + .../firstinspires/ftc/twenty403/Setup.java | 2 + .../ftc/twenty403/commands/auto/Paths.java | 27 +++++ .../controls/KidShamTestController.java | 100 ++++++++++++++++++ .../controls/OperatorController.java | 16 ++- .../twenty403/opmodes/JustDrivingTeleOp.java | 1 + .../ftc/twenty403/opmodes/KidShamTest.java | 42 ++++++++ .../opmodes/auto/ForwardBackwardSide.java | 51 +++++++++ .../twenty403/subsystems/ArmSubsystem.java | 24 ++++- .../subsystems/KidShampooSubsystem.java | 16 +-- 11 files changed, 267 insertions(+), 20 deletions(-) create mode 100644 Twenty403/src/main/java/org/firstinspires/ftc/twenty403/controls/KidShamTestController.java create mode 100644 Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmodes/KidShamTest.java create mode 100644 Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmodes/auto/ForwardBackwardSide.java diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/AutoConstants.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/AutoConstants.java index 4284a6f0..55d84d6f 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/AutoConstants.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/AutoConstants.java @@ -68,9 +68,9 @@ public class AutoConstants { //forward backward constants - public static ConfigurablePoseD FORWARD = new ConfigurablePoseD(0, 48, (90)); - public static ConfigurablePoseD REST = new ConfigurablePoseD(0, 0, (90)); - public static ConfigurablePoseD SIDE = new ConfigurablePoseD(48, 0, (90)); + public static ConfigurablePoseD FORWARD = new ConfigurablePoseD(0, 48, (0)); + public static ConfigurablePoseD REST = new ConfigurablePoseD(0, 0, (0)); + public static ConfigurablePoseD SIDE = new ConfigurablePoseD(48, 0, (0)); //public static ConfigurablePoseD OBSERVATION_ZONE = new ConfigurablePoseD(-60, 55, (135)); diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Hardware.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Hardware.java index c197c56c..f7ac84c9 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Hardware.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Hardware.java @@ -81,6 +81,8 @@ public Hardware(HardwareMap hwmap) { if (Setup.Connected.ODOSUBSYSTEM) { odoR = new OctoquadEncoder(octoquad, Setup.OctoQuadPorts.ODOR); odoF = new OctoquadEncoder(octoquad, Setup.OctoQuadPorts.ODOF); + odoR.setDirection(Setup.OctoQuadPorts.ODOR_REVERSE); + odoF.setDirection(Setup.OctoQuadPorts.ODOF_REVERSE); } armEncoder = new OctoquadEncoder(octoquad, Setup.OctoQuadPorts.ARMENCODER); } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Setup.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Setup.java index 1f0fa79c..6d8f065d 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Setup.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/Setup.java @@ -47,6 +47,8 @@ public static class OctoQuadPorts { public static int ARMENCODER = 2; public static int ODOR = 1; //TODO: verify with robot, r & l may be swapped public static int ODOF = 0; + public static boolean ODOR_REVERSE = true; + public static boolean ODOF_REVERSE = false; } @Config diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/commands/auto/Paths.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/commands/auto/Paths.java index 84d29372..8ab7bdc0 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/commands/auto/Paths.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/commands/auto/Paths.java @@ -1,6 +1,7 @@ package org.firstinspires.ftc.twenty403.commands.auto; import com.technototes.library.command.Command; +import com.technototes.library.command.WaitCommand; import com.technototes.path.command.TrajectorySequenceCommand; import org.firstinspires.ftc.twenty403.AutoConstants; import org.firstinspires.ftc.twenty403.Robot; @@ -110,4 +111,30 @@ public static Command SampleScoring(Robot r) { ) ); } + + + public static Command ForwardBackwardSide(Robot r) { + return new TrajectorySequenceCommand( + r.drivebaseSubsystem, + AutoConstants.FORWARD_BACKWARD1 + ) + .andThen( + new TrajectorySequenceCommand( + r.drivebaseSubsystem, + AutoConstants.FORWARD_BACKWARD2 + ) + ) + .andThen( + new TrajectorySequenceCommand( + r.drivebaseSubsystem, + AutoConstants.FORWARD_BACKWARD3 + ) + ) + .andThen( + new TrajectorySequenceCommand( + r.drivebaseSubsystem, + AutoConstants.FORWARD_BACKWARD4 + ) + ); + } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/controls/KidShamTestController.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/controls/KidShamTestController.java new file mode 100644 index 00000000..e9a1ecdc --- /dev/null +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/controls/KidShamTestController.java @@ -0,0 +1,100 @@ +package org.firstinspires.ftc.twenty403.controls; + +import com.technototes.library.command.Command; +import com.technototes.library.control.CommandButton; +import com.technototes.library.control.CommandGamepad; + +import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.Setup; + +public class KidShamTestController { + + public Robot robot; + public CommandGamepad gamepad; + public CommandButton openRetainer; + public CommandButton closeRetainer; + public CommandButton eatRetainer; + public CommandButton biteJaw; + public CommandButton releaseJaw; + public CommandButton slurpIntake; + public CommandButton spitIntake; + public CommandButton stopIntake; + public CommandButton suspend; + public CommandButton SuspendReverse; + public CommandButton armIntake; + public CommandButton armLowNet; + public CommandButton armLowSpecimen; + public CommandButton armHighSpecimen; + public CommandButton armHorizontal; + public CommandButton armVertical; + public CommandButton armIncrement; + public CommandButton armDecrement; + public CommandButton dumpWrist; + public CommandButton scoopWrist; + public CommandButton slideIn; + public CommandButton slideOut; + public CommandButton slideMax; + public CommandButton slideMin; + + + public KidShamTestController(CommandGamepad g, Robot r) { + robot = r; + gamepad = g; + AssignNamedControllerButton(); + BindControls(); + } + + private void AssignNamedControllerButton() { + openRetainer = gamepad.dpadUp; + closeRetainer = gamepad.dpadRight; + slurpIntake = gamepad.leftBumper; + spitIntake = gamepad.rightBumper; + //temp changing the button below from biteJaw to intake :DD + biteJaw = gamepad.dpadLeft; + releaseJaw = gamepad.dpadDown; + dumpWrist = gamepad.ps_circle; + scoopWrist = gamepad.ps_square; + + } + + public void BindControls() { + if (Setup.Connected.KIDSSHAMPOOSUBSYSTEM) { + bindKidShampooControls(); + } + } + + public void bindKidShampooControls() { + openRetainer.whenPressed( + Command.create(robot.kidShampooSubsystem::openRetainer, robot.kidShampooSubsystem) + ); + closeRetainer.whenPressed( + Command.create(robot.kidShampooSubsystem::closeRetainer, robot.kidShampooSubsystem) + ); + biteJaw.whenPressed( + Command.create(robot.kidShampooSubsystem::biteJaw, robot.kidShampooSubsystem) + ); + releaseJaw.whenPressed( + Command.create(robot.kidShampooSubsystem::releaseJaw, robot.kidShampooSubsystem) + ); + slurpIntake.whenPressed( + Command.create(robot.kidShampooSubsystem::slurpIntake, robot.kidShampooSubsystem) + ); + spitIntake.whenPressed( + Command.create(robot.kidShampooSubsystem::spitIntake, robot.kidShampooSubsystem) + ); + slurpIntake.whenReleased( + Command.create(robot.kidShampooSubsystem::stopIntake, robot.kidShampooSubsystem) + ); + spitIntake.whenReleased( + Command.create(robot.kidShampooSubsystem::stopIntake, robot.kidShampooSubsystem) + ); + dumpWrist.whenPressed( + Command.create(robot.kidShampooSubsystem::dumpWrist, robot.kidShampooSubsystem) + ); + scoopWrist.whenPressed( + Command.create(robot.kidShampooSubsystem::scoopWrist, robot.kidShampooSubsystem) + ); + } + } + + diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/controls/OperatorController.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/controls/OperatorController.java index 95847928..f830f18d 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/controls/OperatorController.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/controls/OperatorController.java @@ -35,6 +35,9 @@ public class OperatorController { public CommandButton scoopWrist; public CommandButton slideIn; public CommandButton slideOut; + public CommandButton slideMax; + public CommandButton slideMin; + public OperatorController(CommandGamepad g, Robot r) { robot = r; @@ -52,8 +55,10 @@ private void AssignNamedControllerButton() { //temp changing the button below from biteJaw to intake :DD biteJaw = gamepad.ps_cross; releaseJaw = gamepad.ps_triangle; - dumpWrist = gamepad.ps_share; - scoopWrist = gamepad.ps_options; + //dumpWrist = gamepad.ps_share; + //scoopWrist = gamepad.ps_options; + slideMax = gamepad.ps_share; + slideMin = gamepad.ps_options; // suspend = gamepad.ps_circle; armIntake = gamepad.dpadUp; // armLowNet = gamepad.dpadLeft; @@ -112,6 +117,7 @@ public void bindKidShampooControls() { scoopWrist.whenPressed( Command.create(robot.kidShampooSubsystem::stopIntake, robot.kidShampooSubsystem) ); + } public void bindArmControls() { @@ -128,9 +134,9 @@ public void bindArmControls() { armIncrement.whenPressed(Command.create(robot.armSubsystem::increment, robot.armSubsystem)); armDecrement.whenPressed(Command.create(robot.armSubsystem::decrement, robot.armSubsystem)); slideIn.whenPressed(Command.create(robot.armSubsystem::slideDecrement, robot.armSubsystem)); - slideOut.whenPressed( - Command.create(robot.armSubsystem::slideIncrement, robot.armSubsystem) - ); + slideOut.whenPressed(Command.create(robot.armSubsystem::slideIncrement, robot.armSubsystem)); + slideMin.whenPressed(Command.create(robot.armSubsystem::setSlideToZero, robot.armSubsystem)); + slideMax.whenPressed(Command.create(robot.armSubsystem::specimenSlides, robot.armSubsystem)); } public void bindHangControls() { diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmodes/JustDrivingTeleOp.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmodes/JustDrivingTeleOp.java index f4760521..c02ecbdf 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmodes/JustDrivingTeleOp.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmodes/JustDrivingTeleOp.java @@ -12,6 +12,7 @@ import org.firstinspires.ftc.twenty403.Setup; import org.firstinspires.ftc.twenty403.commands.EZCmd; import org.firstinspires.ftc.twenty403.controls.DriverController; +import org.firstinspires.ftc.twenty403.controls.KidShamTestController; import org.firstinspires.ftc.twenty403.controls.OperatorController; import org.firstinspires.ftc.twenty403.helpers.StartingPosition; diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmodes/KidShamTest.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmodes/KidShamTest.java new file mode 100644 index 00000000..c7e673a1 --- /dev/null +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmodes/KidShamTest.java @@ -0,0 +1,42 @@ +package org.firstinspires.ftc.twenty403.opmodes; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.technototes.library.command.CommandScheduler; +import com.technototes.library.structure.CommandOpMode; +import com.technototes.library.util.Alliance; + +import org.firstinspires.ftc.twenty403.AutoConstants; +import org.firstinspires.ftc.twenty403.Hardware; +import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.Setup; +import org.firstinspires.ftc.twenty403.commands.EZCmd; +import org.firstinspires.ftc.twenty403.controls.DriverController; +import org.firstinspires.ftc.twenty403.controls.KidShamTestController; +import org.firstinspires.ftc.twenty403.controls.OperatorController; +import org.firstinspires.ftc.twenty403.helpers.StartingPosition; + +@TeleOp(name = "KidShamTest") +@SuppressWarnings("unused") +public class KidShamTest extends CommandOpMode { + + public Robot robot; + public KidShamTestController kidShamTestController; + public Hardware hardware; + + @Override + public void uponInit() { + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + hardware = new Hardware(hardwareMap); + robot = new Robot(hardware, Alliance.BLUE, StartingPosition.Unspecified); + kidShamTestController = new KidShamTestController(codriverGamepad, robot); + if (Setup.Connected.DRIVEBASE) { + + CommandScheduler.scheduleForState( + EZCmd.Drive.ResetGyro(robot.drivebaseSubsystem), + OpModeState.INIT + ); + } + } +} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmodes/auto/ForwardBackwardSide.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmodes/auto/ForwardBackwardSide.java new file mode 100644 index 00000000..4f39ee0b --- /dev/null +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/opmodes/auto/ForwardBackwardSide.java @@ -0,0 +1,51 @@ +package org.firstinspires.ftc.twenty403.opmodes.auto; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.technototes.library.command.CommandScheduler; +import com.technototes.library.command.SequentialCommandGroup; +import com.technototes.library.structure.CommandOpMode; +import com.technototes.library.util.Alliance; + +import org.firstinspires.ftc.twenty403.AutoConstants; +import org.firstinspires.ftc.twenty403.Hardware; +import org.firstinspires.ftc.twenty403.Robot; +import org.firstinspires.ftc.twenty403.commands.EZCmd; +import org.firstinspires.ftc.twenty403.commands.auto.Paths; +import org.firstinspires.ftc.twenty403.controls.DriverController; +import org.firstinspires.ftc.twenty403.controls.SafetyTestController; +import org.firstinspires.ftc.twenty403.helpers.StartingPosition; + +//TODO: figure out where the other splineTest is coming from (duplicate spline error) +@Autonomous(name = "ForwardBackwardSide") +@SuppressWarnings("unused") +public class ForwardBackwardSide extends CommandOpMode { + + public Robot robot; + public DriverController controls; + public SafetyTestController safety; + public Hardware hardware; + + @Override + public void uponInit() { + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + hardware = new Hardware(hardwareMap); + robot = new Robot(hardware, Alliance.RED, StartingPosition.Observation); + robot.drivebaseSubsystem.setPoseEstimate(AutoConstants.REST.toPose()); + // safety = new SafetyTestController(driverGamepad, robot); + //robot.safetySubsystem.startMonitoring(); + CommandScheduler.scheduleForState( + new SequentialCommandGroup( + Paths.ForwardBackwardSide(robot), + EZCmd.Drive.RecordHeading(robot.drivebaseSubsystem), + CommandScheduler::terminateOpMode + ), + OpModeState.RUN + ); + // CommandScheduler.scheduleForState( + // new SafetyStartCommand(robot.safetySubsystem), + // OpModeState.RUN + // ); + } +} diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/ArmSubsystem.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/ArmSubsystem.java index 6f3fd7d1..d3b07850 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/ArmSubsystem.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/ArmSubsystem.java @@ -19,7 +19,7 @@ public class ArmSubsystem implements Subsystem, Loggable { private IEncoder armEncoder; private boolean isHardware; public int slideResetPos; - public static double FEEDFORWARD_COEFFICIENT = 0.0003; //0.7 + public static double FEEDFORWARD_COEFFICIENT = 0.00014; //11-19-24 public static int ROTATE_MOTOR_LOW_BASKET_SCORING_POSITION = 100; public static int ROTATE_MOTOR_HIGH_BASKET_SCORING_POSITION = 200; public static int ROTATE_MOTOR_SPECIMEN_SCORING_POSITION_LOW = 300; @@ -29,14 +29,15 @@ public class ArmSubsystem implements Subsystem, Loggable { public static int ROTATE_MOTOR_INTAKE_POSITION = 400; public static int SLIDES_MOTOR_LOW_BASKET_SCORING_POSITION = 500; public static int SLIDES_MOTOR_HIGH_BASKET_SCORING_POSITION = 600; - public static int SLIDES_MOTOR_SPECIMEN_SCORING_POSITION = 700; + public static int SLIDES_MOTOR_SPECIMEN_SCORING_POSITION = 2500; public static int SLIDES_MOTOR_INTAKE_POSITION = 800; public static int ARM_VERTICAL = 3100; public static int ARM_HORIZONTAL = 1000; public static int INITIAL_POSITION = 150; public static int INCREMENT_DECREMENT = 120; public static int SLIDE_INC_DEC = 100; - public static int SLIDE_MAX_POS = 3100; + public static int SLIDE_MAX_POS = 850; + public static int SLIDE_MIN_POS = -150; public static int SLIDE_OFFSET = 2000; public static double MIN_SLIDE_MOTOR_POWER = -0.3; public static double MAX_SLIDE_MOTOR_POWER = 0.5; @@ -47,7 +48,7 @@ public class ArmSubsystem implements Subsystem, Loggable { public static int ARM_POS_CLOSE_ENOUGH = Math.abs(ARM_HORIZONTAL - ARM_VERTICAL) / 18; // as of now, we arent having a D - public static PIDCoefficients armPID = new PIDCoefficients(0.0002, 0.0, 0.000); + public static PIDCoefficients armPID = new PIDCoefficients(0.0007, 0.0, 0.000); public static PIDCoefficients slidePID = new PIDCoefficients(0.001, 0.0, 0.000); @Log(name = "armPow") @@ -189,6 +190,12 @@ public void decrement() { } } + public void setSlideToZero() { + setSlidePos(SLIDE_MIN_POS); + } + + + public void resetSlideZero() { slideResetPos = getSlideUnmodifiedPosition(); slideTargetPos = slideResetPos; @@ -196,10 +203,19 @@ public void resetSlideZero() { public void slideIncrement() { setSlidePos(slideTargetPos + SLIDE_INC_DEC); + if (slidePos > SLIDE_MAX_POS){ + setSlidePos(SLIDE_MAX_POS); + } } public void slideDecrement() { setSlidePos(slideTargetPos - SLIDE_INC_DEC); + if (slidePos < SLIDE_MIN_POS){ + setSlidePos(SLIDE_MIN_POS); + } + } + public void slideSpecimen() { + setSlidePos(SLIDES_MOTOR_SPECIMEN_SCORING_POSITION); } private int getCurrentSlidePos() { diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/KidShampooSubsystem.java b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/KidShampooSubsystem.java index 785f63d5..67d5aceb 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/KidShampooSubsystem.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/twenty403/subsystems/KidShampooSubsystem.java @@ -26,19 +26,19 @@ public class KidShampooSubsystem implements Subsystem, Loggable { private ColorSensor colorSensor; private Rev2MDistanceSensor rev2MDistanceSensor; - public static double RETAINER_OPEN_POSITION = -.2; + public static double RETAINER_OPEN_POSITION = .78; - public static double RETAINER_CLOSE_POSITION = .1; + public static double RETAINER_CLOSE_POSITION = .52; - public static double JAW_BITE_POSITION = .1; + public static double JAW_BITE_POSITION = .4; - public static double JAW_RELEASE_POSITION = -.1; - public static double INTAKE_SLURP = -.1; + public static double JAW_RELEASE_POSITION = .2; + public static double INTAKE_SLURP = -.6; - public static double INTAKE_SPIT = .1; + public static double INTAKE_SPIT = .6; - public static double WRIST_SCOOP = .1; - public static double WRIST_DUMP = -.1; + public static double WRIST_SCOOP = .25; + public static double WRIST_DUMP = 0; @Log(name = "distance value ") public double distance_value;