From b5190dbe524a65309c7a20b6131c728fd0b72b4d Mon Sep 17 00:00:00 2001 From: FTC16750 Date: Tue, 4 Mar 2025 17:20:30 -0800 Subject: [PATCH 1/3] harshini 3/4/2025 finished claw subsystem (i think...?) was about to test make sure to plug in the claw to the control hub and to connect the control hub to the battery with a wire that i yank from the robot --- .../subsystems/ClawSubsystem.java | 45 +++++++++++++++++++ 1 file changed, 45 insertions(+) create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ClawSubsystem.java diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ClawSubsystem.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ClawSubsystem.java new file mode 100644 index 00000000..e49c6da5 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ClawSubsystem.java @@ -0,0 +1,45 @@ +package org.firstinspires.ftc.ptechnodactyl.subsystems; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.control.PIDCoefficients; +import com.acmerobotics.roadrunner.control.PIDFController; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.util.Range; +import com.technototes.library.hardware.motor.EncodedMotor; +import com.technototes.library.hardware.servo.Servo; +import com.technototes.library.logger.Log; +import com.technototes.library.logger.Loggable; +import com.technototes.library.subsystem.Subsystem; +import org.firstinspires.ftc.ptechnodactyl.Hardware; + + +@Config +public class ClawSubsystem implements Subsystem, Loggable { + private Servo clawServo, wristServo; + private boolean isHardware; + + @Log(name = "clawPosition") + public double clawPosition = 0; + + + public static double CLAW_OPEN_POSITION = 0.3; + public static double CLAW_CLOSE_POSITION = 0.7; + + private void setClawPosition(double d) { + if (isHardware) { + clawServo.setPosition(d); + clawPosition = d; + } + } + + public void openClaw() { + setClawPosition(CLAW_OPEN_POSITION); + } + + public void closeClaw(){ + setClawPosition(CLAW_CLOSE_POSITION); + } + + + +} \ No newline at end of file From ca5a99ffa19a91a73bdcd167a06a9ee8f40cf4d1 Mon Sep 17 00:00:00 2001 From: Kevin Frei Date: Fri, 2 May 2025 23:51:52 -0700 Subject: [PATCH 2/3] added claw and arm hardware, setup, robot, controls and cmds code --- .../ftc/ptechnodactyl/Hardware.java | 13 ++- .../ftc/ptechnodactyl/Robot.java | 2 + .../ftc/ptechnodactyl/Setup.java | 5 + .../ftc/ptechnodactyl/commands/ClawCmds.java | 18 +++ .../commands/JoystickIncDecCmd.java | 33 ++++++ .../controllers/OperatorController.java | 46 ++++++++ .../subsystems/ClawSubsystem.java | 110 ++++++++++++++++-- 7 files changed, 216 insertions(+), 11 deletions(-) create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/ClawCmds.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickIncDecCmd.java create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OperatorController.java diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Hardware.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Hardware.java index 7b7f025a..9d90993a 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Hardware.java +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Hardware.java @@ -21,16 +21,15 @@ public class Hardware implements Loggable { public List hubs; - public EncodedMotor theMotor, flMotor, frMotor, rlMotor, rrMotor; + public EncodedMotor theMotor, flMotor, frMotor, rlMotor, rrMotor, arm; public Motor placeholder1; public DcMotorEx liftMotor; public Servo placeholder2; - public Servo servo; + public Servo servo, clawServo; public IGyro imu; public Webcam camera; - public Rev2MDistanceSensor distanceSensor; public ColorDistanceSensor colorSensor; @@ -73,6 +72,14 @@ public Hardware(HardwareMap hwmap) { // this.colorSensor = new ColorDistanceSensor(Setup.HardwareNames.COLOR); // } } + if (Setup.Connected.CLAWSUBSYSTEM) { + if (Setup.Connected.CLAWSERVO) { + this.clawServo = new Servo(Setup.HardwareNames.CLAWSERVO); + } + if (Setup.Connected.ARM) { + this.arm = new EncodedMotor<>(Setup.HardwareNames.ARM); + } + } } // We can read the voltage from the different hubs for fun... diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Robot.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Robot.java index 5e4be5e2..f248d563 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Robot.java +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Robot.java @@ -3,6 +3,7 @@ import com.technototes.library.logger.Loggable; import com.technototes.library.util.Alliance; import org.firstinspires.ftc.ptechnodactyl.helpers.StartingPosition; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ClawSubsystem; import org.firstinspires.ftc.ptechnodactyl.subsystems.DrivebaseSubsystem; public class Robot implements Loggable { @@ -11,6 +12,7 @@ public class Robot implements Loggable { public Alliance alliance; public double initialVoltage; public DrivebaseSubsystem drivebaseSubsystem; + public ClawSubsystem clawSubsystem; public Robot(Hardware hw) { this.initialVoltage = hw.voltage(); diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Setup.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Setup.java index 85565241..a8c3203b 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Setup.java +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Setup.java @@ -9,8 +9,11 @@ public static class Connected { public static boolean DRIVEBASE = true; public static boolean TESTSUBSYSTEM = false; + public static boolean CLAWSUBSYSTEM = true; public static boolean MOTOR = false; public static boolean SERVO = false; + public static boolean ARM = true; + public static boolean CLAWSERVO = true; public static boolean DISTANCE_SENSOR = false; public static boolean COLOR_SENSOR = false; public static boolean FLYWHEEL = false; @@ -33,6 +36,8 @@ public static class HardwareNames { public static String DISTANCE = "d"; public static String COLOR = "c"; public static String CAMERA = "camera"; + public static String ARM = "arm"; + public static String CLAWSERVO = "clawServo"; } @Config diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/ClawCmds.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/ClawCmds.java new file mode 100644 index 00000000..e1f168a6 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/ClawCmds.java @@ -0,0 +1,18 @@ +package org.firstinspires.ftc.ptechnodactyl.commands; + +import com.technototes.library.command.Command; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ClawSubsystem; + +public class ClawCmds { + + public static class cmds { + + public static Command OpenClaw(ClawSubsystem CS) { + return Command.create(CS::openClaw); + } + + public static Command CloseClaw(ClawSubsystem CS) { + return Command.create(CS::closeClaw); + } + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickIncDecCmd.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickIncDecCmd.java new file mode 100644 index 00000000..568ce554 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickIncDecCmd.java @@ -0,0 +1,33 @@ +package org.firstinspires.ftc.ptechnodactyl.commands; + +import com.technototes.library.command.Command; +import com.technototes.library.control.Stick; +import com.technototes.library.logger.Loggable; +import java.util.function.DoubleSupplier; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ClawSubsystem; + +public class JoystickIncDecCmd implements Command, Loggable { + + public ClawSubsystem subsystem; + public DoubleSupplier x; + + public JoystickIncDecCmd(ClawSubsystem sub, Stick xStick) { + addRequirements(sub); + subsystem = sub; + x = xStick.getXSupplier(); + } + + // This will make the bot snap to an angle, if the 'straighten' button is pressed + // Otherwise, it just reads the rotation value from the rotation stick + + @Override + public void execute() { + double xvalue = -x.getAsDouble(); + subsystem.increment(xvalue); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OperatorController.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OperatorController.java new file mode 100644 index 00000000..db0744f1 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OperatorController.java @@ -0,0 +1,46 @@ +package org.firstinspires.ftc.ptechnodactyl.controllers; + +import com.technototes.library.command.Command; +import com.technototes.library.command.CommandScheduler; +import com.technototes.library.control.CommandButton; +import com.technototes.library.control.CommandGamepad; +import com.technototes.library.control.Stick; +import org.firstinspires.ftc.ptechnodactyl.Robot; +import org.firstinspires.ftc.ptechnodactyl.Setup; +import org.firstinspires.ftc.ptechnodactyl.commands.ClawCmds; +import org.firstinspires.ftc.ptechnodactyl.commands.JoystickIncDecCmd; +import org.firstinspires.ftc.ptechnodactyl.subsystems.ClawSubsystem; + +public class OperatorController { + + public Robot robot; + public CommandGamepad gamepad; + public CommandButton openClaw; + public CommandButton closeClaw; + public Stick armStick; + + public OperatorController(CommandGamepad g, Robot r) { + robot = r; + gamepad = g; + AssignNamedControllerButton(); + BindControls(); + } + + private void AssignNamedControllerButton() { + openClaw = gamepad.leftBumper; + closeClaw = gamepad.rightBumper; + armStick = gamepad.rightStick; + } + + public void BindControls() { + if (Setup.Connected.CLAWSUBSYSTEM) { + bindClawSubsystemControls(); + } + } + + public void bindClawSubsystemControls() { + openClaw.whenPressed(ClawCmds.cmds.OpenClaw(robot.clawSubsystem)); + closeClaw.whenPressed(ClawCmds.cmds.CloseClaw(robot.clawSubsystem)); + CommandScheduler.scheduleJoystick(new JoystickIncDecCmd(robot.clawSubsystem, armStick)); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ClawSubsystem.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ClawSubsystem.java index e49c6da5..6979af2e 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ClawSubsystem.java +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ClawSubsystem.java @@ -12,18 +12,43 @@ import com.technototes.library.subsystem.Subsystem; import org.firstinspires.ftc.ptechnodactyl.Hardware; - @Config public class ClawSubsystem implements Subsystem, Loggable { - private Servo clawServo, wristServo; + + private Servo clawServo; + private EncodedMotor arm; private boolean isHardware; @Log(name = "clawPosition") public double clawPosition = 0; - public static double CLAW_OPEN_POSITION = 0.3; public static double CLAW_CLOSE_POSITION = 0.7; + public static int INCREMENT_DECREMENT = 200; + public static double FEEDFORWARD_COEFFICIENT = 0.00014; + public static int ARM_VERTICAL = 3100; + public static int ARM_HORIZONTAL = 1000; + public static int INITIAL_POSITION = 150; + public static int ARM_MAX = 0; + public static int ARM_MIN = 0; + + @Log(name = "armTarget") + public int armTargetPos; + + @Log(name = "armPos") + public int armPos; + + @Log(name = "armFdFwdVal") + public double armFeedFwdValue; + + private PIDFController armPidController; + + private void setArmPos(int e) { + armPidController.setTargetPosition(e); + armTargetPos = e; + } + + public static PIDCoefficients armPID = new PIDCoefficients(0.0005, 0.0, 0.0001); private void setClawPosition(double d) { if (isHardware) { @@ -32,14 +57,83 @@ private void setClawPosition(double d) { } } - public void openClaw() { - setClawPosition(CLAW_OPEN_POSITION); + public ClawSubsystem(Hardware hw) { + isHardware = true; + clawServo = hw.clawServo; + arm = hw.arm; + armPidController = new PIDFController( + armPID, + 0, + 0, + 0, + /* + + The function arguments for the Feed Forward function are Position (ticks) and + Velocity (units?). So, for the arm, we want to check to see if which side of + center we're on, and if the velocity is pushing us down, FF should probably be + low (negative?) while if velocity is pushing us *up*, FF should be high (right?) + Someone who's done physics and/or calculus recently should write some real equations + + Braelyn got the math right + + For angle T through this range where we start at zero: + / + / T + 180 _____/_____ 0 + The downward torque due to gravity is cos(T) * Gravity (9.8m/s^2) + + If we're moving from 0 to 180 degrees, then: + While T is less than 90, the "downward torque" is working *against* the motor + When T is greater than 90, the "downward torque" is working *with* the motor + + */ + + (ticks, velocity) -> { + armFeedFwdValue = FEEDFORWARD_COEFFICIENT * Math.cos(getArmAngle(ticks)); + + // if (velocity > MIN_ANGULAR_VELOCITY) { + // //increase armFeedFwdValue to avoid slamming or increase D in PID + // armFeedFwdValue += ARM_SLAM_PREVENTION; + // } + if (Math.abs(armFeedFwdValue) < 0.1) { + armFeedFwdValue = 0.0; + } + + return armFeedFwdValue; + } + ); + setArmPos(INITIAL_POSITION); + } + + public void increment(double value) { + int newArmPos = (int) (armTargetPos + value * INCREMENT_DECREMENT); + // if (newArmPos > ARM_MAX) { + // newArmPos = 3150; + // } else if (newArmPos < ARM_MIN) { + // newArmPos = 0; + // } + setArmPos(newArmPos); } - public void closeClaw(){ - setClawPosition(CLAW_CLOSE_POSITION); + public void incrementn() { + increment(1.0); } + public void decrement() { + increment(-1.0); + } + private static double getArmAngle(double ticks) { + // our horizontal value starts at ARM_HORIZONTAL, so we need to + // subtract it + return ((Math.PI / 2.0) * (ticks - ARM_HORIZONTAL)) / (ARM_VERTICAL - ARM_HORIZONTAL); + } -} \ No newline at end of file + public void openClaw() { + setClawPosition(CLAW_OPEN_POSITION); + } + + public void closeClaw() { + setClawPosition(CLAW_CLOSE_POSITION); + } +} From 2429628df1c36f797dde1f5a976bcbe6801da4b8 Mon Sep 17 00:00:00 2001 From: Kevin Frei Date: Fri, 2 May 2025 23:56:19 -0700 Subject: [PATCH 3/3] added teleop --- .../opmodes/tele/JustDrivingTeleOp.java | 40 +++++++++++++++++++ 1 file changed, 40 insertions(+) create mode 100644 Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/JustDrivingTeleOp.java diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/JustDrivingTeleOp.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/JustDrivingTeleOp.java new file mode 100644 index 00000000..e1a31545 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/JustDrivingTeleOp.java @@ -0,0 +1,40 @@ +package org.firstinspires.ftc.ptechnodactyl.opmodes.tele; + +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.ptechnodactyl.Hardware; +import org.firstinspires.ftc.ptechnodactyl.Robot; +import org.firstinspires.ftc.ptechnodactyl.Setup; +import org.firstinspires.ftc.ptechnodactyl.commands.EZCmd; +import org.firstinspires.ftc.ptechnodactyl.controllers.DriverController; +import org.firstinspires.ftc.ptechnodactyl.controllers.OperatorController; +import org.firstinspires.ftc.ptechnodactyl.helpers.StartingPosition; + +@TeleOp(name = "Driving w/Turbo!") +@SuppressWarnings("unused") +public class JustDrivingTeleOp extends CommandOpMode { + + public Robot robot; + public DriverController controlsDriver; + public OperatorController controlsOperator; + public Hardware hardware; + + @Override + public void uponInit() { + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + hardware = new Hardware(hardwareMap); + robot = new Robot(hardware); + controlsOperator = new OperatorController(codriverGamepad, robot); + if (Setup.Connected.DRIVEBASE) { + controlsDriver = new DriverController(driverGamepad, robot); + CommandScheduler.scheduleForState( + EZCmd.Drive.ResetGyro(robot.drivebaseSubsystem), + OpModeState.INIT + ); + } + } +}