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/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 + ); + } + } +} 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..6979af2e --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/subsystems/ClawSubsystem.java @@ -0,0 +1,139 @@ +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; + 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) { + clawServo.setPosition(d); + clawPosition = d; + } + } + + 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 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); + } + + public void openClaw() { + setClawPosition(CLAW_OPEN_POSITION); + } + + public void closeClaw() { + setClawPosition(CLAW_CLOSE_POSITION); + } +}