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 deleted file mode 100644 index e1f168a..0000000 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/ClawCmds.java +++ /dev/null @@ -1,18 +0,0 @@ -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/JoystickDriveCommand.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickDriveCommand.java index 1602b2c..74ef7ba 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickDriveCommand.java +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickDriveCommand.java @@ -107,8 +107,8 @@ public void execute() { // The math & signs looks wonky, because this makes things field-relative // (Remember that "3 O'Clock" is zero degrees) - double yvalue = -y.getAsDouble(); - double xvalue = -x.getAsDouble(); + double yvalue = y.getAsDouble(); + double xvalue = x.getAsDouble(); if (driveStraighten != null) { if (driveStraighten.getAsDouble() > 0.7) { if (Math.abs(yvalue) > Math.abs(xvalue)) xvalue = 0; 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 index 568ce55..6d90a26 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickIncDecCmd.java +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/commands/JoystickIncDecCmd.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.ptechnodactyl.commands; +import com.qualcomm.robotcore.util.ElapsedTime; import com.technototes.library.command.Command; import com.technototes.library.control.Stick; import com.technototes.library.logger.Loggable; @@ -10,6 +11,11 @@ public class JoystickIncDecCmd implements Command, Loggable { public ClawSubsystem subsystem; public DoubleSupplier x; + public static ElapsedTime timer; + + public static double time; + + public double interval = 1.5; public JoystickIncDecCmd(ClawSubsystem sub, Stick xStick) { addRequirements(sub); @@ -23,7 +29,10 @@ public JoystickIncDecCmd(ClawSubsystem sub, Stick xStick) { @Override public void execute() { double xvalue = -x.getAsDouble(); + // time = timer.time(); + // if (interval < time) subsystem.increment(xvalue); + // timer.reset(); } @Override diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/DriverController.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/DriverController.java index 0bf7390..2727ddd 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/DriverController.java +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/DriverController.java @@ -18,8 +18,6 @@ public class DriverController { public Stick driveLeftStick, driveRightStick; public CommandButton resetGyroButton, turboButton, snailButton; public CommandButton override; - public CommandAxis driveStraighten; - public CommandAxis drive45; public DriverController(CommandGamepad g, Robot r) { this.robot = r; @@ -36,21 +34,13 @@ private void AssignNamedControllerButton() { resetGyroButton = gamepad.ps_options; driveLeftStick = gamepad.leftStick; driveRightStick = gamepad.rightStick; - driveStraighten = gamepad.rightTrigger; - drive45 = gamepad.leftTrigger; turboButton = gamepad.leftBumper; snailButton = gamepad.rightBumper; } public void bindDriveControls() { CommandScheduler.scheduleJoystick( - new JoystickDriveCommand( - robot.drivebaseSubsystem, - driveLeftStick, - driveRightStick, - driveStraighten, - drive45 - ) + new JoystickDriveCommand(robot.drivebaseSubsystem, driveLeftStick, driveRightStick) ); turboButton.whenPressed(DrivingCommands.TurboDriving(robot.drivebaseSubsystem)); diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OneController.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OneController.java new file mode 100644 index 0000000..2098522 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OneController.java @@ -0,0 +1,77 @@ +package org.firstinspires.ftc.ptechnodactyl.controllers; + +import android.support.v4.app.INotificationSideChannel; +import com.technototes.library.command.CommandScheduler; +import com.technototes.library.control.CommandAxis; +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.DrivingCommands; +import org.firstinspires.ftc.ptechnodactyl.commands.JoystickDriveCommand; + +public class OneController { + + public Robot robot; + public CommandGamepad gamepad; + public CommandButton openClaw; + public CommandButton closeClaw; + public CommandButton ArmHorizontal; + public CommandButton ArmVertical; + public Stick driveLeftStick, driveRightStick; + public CommandButton resetGyroButton, turboButton, snailButton; + public CommandButton increment; + public CommandButton decrement; + + public OneController(CommandGamepad g, Robot r) { + robot = r; + gamepad = g; + AssignNamedControllerButton(); + BindControls(); + } + + private void AssignNamedControllerButton() { + openClaw = gamepad.leftBumper; + closeClaw = gamepad.rightBumper; + ArmHorizontal = gamepad.ps_square; + ArmVertical = gamepad.ps_triangle; + increment = gamepad.ps_circle; + decrement = gamepad.ps_cross; + resetGyroButton = gamepad.ps_options; + driveLeftStick = gamepad.leftStick; + driveRightStick = gamepad.rightStick; + turboButton = gamepad.leftBumper; + snailButton = gamepad.rightBumper; + } + + public void BindControls() { + if (Setup.Connected.CLAWSUBSYSTEM) { + bindClawSubsystemControls(); + } + if (Setup.Connected.DRIVEBASE) { + bindDriveControls(); + } + } + + public void bindClawSubsystemControls() { + openClaw.whenPressed(robot.clawSubsystem::openClaw); + closeClaw.whenPressed(robot.clawSubsystem::closeClaw); + ArmVertical.whenPressed(robot.clawSubsystem::setArmVertical); + ArmHorizontal.whenPressed(robot.clawSubsystem::setArmHorizontal); + increment.whenPressed(robot.clawSubsystem::incrementn); + decrement.whenPressed(robot.clawSubsystem::decrement); + } + + public void bindDriveControls() { + CommandScheduler.scheduleJoystick( + new JoystickDriveCommand(robot.drivebaseSubsystem, driveLeftStick, driveRightStick) + ); + + turboButton.whenPressed(DrivingCommands.TurboDriving(robot.drivebaseSubsystem)); + turboButton.whenReleased(DrivingCommands.NormalDriving(robot.drivebaseSubsystem)); + snailButton.whenPressed(DrivingCommands.SnailDriving(robot.drivebaseSubsystem)); + snailButton.whenReleased(DrivingCommands.NormalDriving(robot.drivebaseSubsystem)); + resetGyroButton.whenPressed(DrivingCommands.ResetGyro(robot.drivebaseSubsystem)); + } +} 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 index db0744f..0cd9495 100644 --- a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OperatorController.java +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/OperatorController.java @@ -1,15 +1,12 @@ 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 { @@ -18,6 +15,9 @@ public class OperatorController { public CommandButton openClaw; public CommandButton closeClaw; public Stick armStick; + public CommandButton ArmHorizontal; + public CommandButton ArmVertical; + public CommandButton intake; public OperatorController(CommandGamepad g, Robot r) { robot = r; @@ -30,6 +30,9 @@ private void AssignNamedControllerButton() { openClaw = gamepad.leftBumper; closeClaw = gamepad.rightBumper; armStick = gamepad.rightStick; + ArmHorizontal = gamepad.ps_circle; + ArmVertical = gamepad.ps_triangle; + intake = gamepad.dpadRight; } public void BindControls() { @@ -39,8 +42,11 @@ public void BindControls() { } public void bindClawSubsystemControls() { - openClaw.whenPressed(ClawCmds.cmds.OpenClaw(robot.clawSubsystem)); - closeClaw.whenPressed(ClawCmds.cmds.CloseClaw(robot.clawSubsystem)); + openClaw.whenPressed(robot.clawSubsystem::openClaw); + closeClaw.whenPressed(robot.clawSubsystem::closeClaw); + ArmVertical.whenPressed(robot.clawSubsystem::setArmVertical); + ArmHorizontal.whenPressed(robot.clawSubsystem::setArmHorizontal); + intake.whenPressed(robot.clawSubsystem::setArmIntake); CommandScheduler.scheduleJoystick(new JoystickIncDecCmd(robot.clawSubsystem, armStick)); } } diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/TestController.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/TestController.java new file mode 100644 index 0000000..e9ddeaf --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/controllers/TestController.java @@ -0,0 +1,42 @@ +package org.firstinspires.ftc.ptechnodactyl.controllers; + +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.commands.JoystickIncDecCmd; + +public class TestController { + + public Robot robot; + public CommandGamepad gamepad; + public CommandButton testPower2; + public CommandButton testPower; + public CommandButton armHorizontal; + public Stick armStick; + + public TestController(CommandGamepad g, Robot r) { + robot = r; + gamepad = g; + AssignNamedControllerButton(); + BindControls(); + } + + private void AssignNamedControllerButton() { + testPower = gamepad.leftBumper; + testPower2 = gamepad.rightBumper; + armStick = gamepad.rightStick; + armHorizontal = gamepad.ps_circle; + } + + public void BindControls() { + bindTestControls(); + } + + public void bindTestControls() { + testPower.whenPressed(robot.clawSubsystem::powIncrement); + testPower2.whenPressed(robot.clawSubsystem::powDecrement); + 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 index 014e007..6302c59 100644 --- 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 @@ -11,11 +11,12 @@ import org.firstinspires.ftc.ptechnodactyl.Robot; import org.firstinspires.ftc.ptechnodactyl.Setup; import org.firstinspires.ftc.ptechnodactyl.commands.EZCmd; +import org.firstinspires.ftc.ptechnodactyl.commands.JoystickIncDecCmd; 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!") +@TeleOp(name = "PT Driving w/Turbo!") @SuppressWarnings("unused") public class JustDrivingTeleOp extends CommandOpMode implements Loggable { diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/MotorTest.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/MotorTest.java new file mode 100644 index 0000000..d65a0b5 --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/MotorTest.java @@ -0,0 +1,29 @@ +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.logger.Loggable; +import com.technototes.library.structure.CommandOpMode; +import org.firstinspires.ftc.ptechnodactyl.Hardware; +import org.firstinspires.ftc.ptechnodactyl.Robot; +import org.firstinspires.ftc.ptechnodactyl.controllers.DriverController; +import org.firstinspires.ftc.ptechnodactyl.controllers.TestController; + +@TeleOp(name = "MotorTest") +@SuppressWarnings("unused") +public class MotorTest extends CommandOpMode implements Loggable { + + public Hardware hardware; + public Robot robot; + + public TestController test; + + @Override + public void uponInit() { + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + hardware = new Hardware(hardwareMap); + robot = new Robot(hardware); + test = new TestController(driverGamepad, robot); + } +} diff --git a/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/oneController.java b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/oneController.java new file mode 100644 index 0000000..69cd1dc --- /dev/null +++ b/Ptechnodactyl/src/main/java/org/firstinspires/ftc/ptechnodactyl/opmodes/tele/oneController.java @@ -0,0 +1,31 @@ +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.logger.Loggable; +import com.technototes.library.structure.CommandOpMode; +import org.firstinspires.ftc.ptechnodactyl.Hardware; +import org.firstinspires.ftc.ptechnodactyl.Robot; +import org.firstinspires.ftc.ptechnodactyl.commands.EZCmd; +import org.firstinspires.ftc.ptechnodactyl.controllers.OneController; + +@TeleOp(name = "OneController") +@SuppressWarnings("unused") +public class oneController extends CommandOpMode implements Loggable { + + public Hardware hardware; + public Robot robot; + + public OneController driver; + + @Override + public void uponInit() { + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + hardware = new Hardware(hardwareMap); + robot = new Robot(hardware); + driver = new OneController(driverGamepad, robot); + CommandScheduler.register(robot.clawSubsystem); + } +} 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 cc09356..8ae3665 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 @@ -4,14 +4,16 @@ 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 { +public class ClawSubsystem implements Subsystem, Loggable { private Servo clawServo; private EncodedMotor arm; @@ -21,14 +23,17 @@ public class ClawSubsystem implements Subsystem { 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; + public static double CLAW_CLOSE_POSITION = 0.9; + public static double MIN_ARM_MOTOR_SPEED = -1; + public static double MAX_ARM_MOTOR_SPEED = 1; + public static int INCREMENT_DECREMENT = 5; + public static int INCREMENT_DECREMENT_BUTTON = 50; + public static double FEEDFORWARD_COEFFICIENT = 0.25; + public static int ARM_VERTICAL = 165; + public static int ARM_HORIZONTAL = 53; + public static int INITIAL_POSITION = 20; + public static int ARM_MAX = 327; + public static int ARM_MIN = 8; @Log(name = "armTarget") public int armTargetPos; @@ -36,6 +41,9 @@ public class ClawSubsystem implements Subsystem { @Log(name = "armPos") public int armPos; + @Log(name = "armPow") + public double armPow; + @Log(name = "armFdFwdVal") public double armFeedFwdValue; @@ -46,7 +54,7 @@ private void setArmPos(int e) { armTargetPos = e; } - public static PIDCoefficients armPID = new PIDCoefficients(0.0005, 0.0, 0.0001); + public static PIDCoefficients armPID = new PIDCoefficients(0.02, 0.0, 0); private void setClawPosition(double d) { if (isHardware) { @@ -55,6 +63,14 @@ private void setClawPosition(double d) { } } + private int getArmUnmodifiedPosition() { + if (isHardware) { + return (int) arm.getSensorValue(); + } else { + return 0; + } + } + public ClawSubsystem(Hardware hw) { isHardware = true; clawServo = hw.clawServo; @@ -93,14 +109,11 @@ The downward torque due to gravity is cos(T) * Gravity (9.8m/s^2) // //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); + setArmPos(ARM_HORIZONTAL); } public ClawSubsystem() { @@ -111,20 +124,38 @@ public ClawSubsystem() { 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; - // } + if (newArmPos > ARM_MAX) { + newArmPos = ARM_MAX; + } else if (newArmPos < ARM_MIN) { + newArmPos = ARM_MIN; + } + setArmPos(newArmPos); + } + + public void increment_button(double value) { + int newArmPos = (int) (armTargetPos + value * INCREMENT_DECREMENT_BUTTON); + if (newArmPos > ARM_MAX) { + newArmPos = ARM_MAX; + } else if (newArmPos < ARM_MIN) { + newArmPos = ARM_MIN; + } setArmPos(newArmPos); } public void incrementn() { - increment(1.0); + increment_button(1); } public void decrement() { - increment(-1.0); + increment_button(-1); + } + + public void powIncrement() { + setArmPos(armTargetPos + 1); + } + + public void powDecrement() { + setArmPos(armTargetPos - 1); } private static double getArmAngle(double ticks) { @@ -140,4 +171,31 @@ public void openClaw() { public void closeClaw() { setClawPosition(CLAW_CLOSE_POSITION); } + + public void setArmVertical() { + setArmPos(ARM_VERTICAL); + } + + public void setArmHorizontal() { + setArmPos(ARM_HORIZONTAL); + } + + public void setArmIntake() { + setArmPos(ARM_MAX); + } + + private void setArmMotorPower(double speed) { + if (isHardware) { + double clippedSpeed = Range.clip(speed, MIN_ARM_MOTOR_SPEED, MAX_ARM_MOTOR_SPEED); + arm.setPower(clippedSpeed); + } + armPow = speed; + } + + @Override + public void periodic() { + armPos = getArmUnmodifiedPosition(); + armPow = armPidController.update(armPos); + setArmMotorPower(armPow); + } }