Ptechnodactyl

Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/Hardware.java

@@ -21,16 +21,15 @@ public class Hardware implements Loggable {
 
     public List<LynxModule> hubs;
 
-    public EncodedMotor<DcMotorEx> theMotor, flMotor, frMotor, rlMotor, rrMotor;
+    public EncodedMotor<DcMotorEx> theMotor, flMotor, frMotor, rlMotor, rrMotor, arm;
     public Motor<DcMotorEx> 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...

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();

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

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);
+        }
+    }
+}

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;
+    }
+}

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));
+    }
+}

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
+            );
+        }
+    }
+}

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<DcMotorEx> 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);
+    }
+}