diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/Hardware.java b/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/Hardware.java index 53be136..72e2963 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/Hardware.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/Hardware.java @@ -47,7 +47,7 @@ public Hardware(HardwareMap hwmap) { if (Setup.Connected.INTAKE) { leftIntakeServo = new CRServo(Setup.HardwareNames.LEFTINTAKESERVO); rightIntakeServo = new CRServo(Setup.HardwareNames.RIGHTINTAKESERVO); - intake = new Motor<>(Setup.HardwareNames.LAUNCHER); + intake = new Motor<>(Setup.HardwareNames.INTAKEMOTOR); } if (Setup.Connected.LIMELIGHT) { limelight = hwmap.get(Limelight3A.class, Setup.HardwareNames.LIMELIGHT); diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/subsystems/IntakeSubsystem.java b/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/subsystems/IntakeSubsystem.java index 380ed29..d8b5c94 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/subsystems/IntakeSubsystem.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/subsystems/IntakeSubsystem.java @@ -65,7 +65,7 @@ public void Stop() { private void setIntakeServo(double power) { if (hasHardware) { - _leftintake.setPower(power); + _leftintake.setPower(-power); _rightintake.setPower(power); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/subsystems/LauncherSubsystem.java b/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/subsystems/LauncherSubsystem.java index b3ce372..b31a117 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/subsystems/LauncherSubsystem.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/subsystems/LauncherSubsystem.java @@ -2,6 +2,7 @@ import com.bylazar.configurables.annotations.Configurable; import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.PIDFCoefficients; import com.technototes.library.hardware.motor.EncodedMotor; import com.technototes.library.logger.Log; @@ -17,13 +18,14 @@ public class LauncherSubsystem implements Loggable, Subsystem { public static double TARGET_MOTOR_VELOCITY = 1530; //.58; // 0.5 // /1.0 boolean hasHardware; - public static EncodedMotor top; + public static EncodedMotor launchMotor; // this doesn't really matter as much since f calculation is just overriding it :( public static PIDFCoefficients launcherP = new PIDFCoefficients(0.006, 0.0, 0.0, 0); public static double SPIN_F_SCALE = 0.00016; public static double SPIN_VOLT_COMP = 0.0216; public static double DIFFERENCE = 0.0046; public static double PEAK_VOLTAGE = 13; + public static boolean ISLAUNCHMOTORREVERSED = true; private static PIDFController launcherPID; private double voltage; @@ -45,8 +47,14 @@ public LauncherSubsystem(Hardware h) { hasHardware = Setup.Connected.LAUNCHER; // Do stuff in here if (hasHardware) { - top = h.launcher; - top.coast(); + launchMotor = h.launcher; + launchMotor.coast(); + if (ISLAUNCHMOTORREVERSED) { + launchMotor.setDirection(DcMotorSimple.Direction.REVERSE); + } else { + launchMotor.setDirection(DcMotorSimple.Direction.FORWARD); + } + double ADDITION = PEAK_VOLTAGE - h.voltage(); if (ADDITION == 0) { SPIN_VOLT_COMP = SPIN_VOLT_COMP + 0.001; @@ -61,7 +69,7 @@ public LauncherSubsystem(Hardware h) { // top.setPIDFCoefficients(launcherP); setTargetSpeed(0); } else { - top = null; + launchMotor = null; } } @@ -98,7 +106,7 @@ public void DecreaseVelocity() { } public boolean GetCurrentTargetVelocity() { - return top.getVelocity() >= 1300; + return launchMotor.getVelocity() >= 1300; } public void Stop() { @@ -120,14 +128,14 @@ public double getTargetSpeed() { private void setMotorPower(double pow) { double power = Math.clamp(pow, -1, 1); - if (top != null) { - top.setPower(power); + if (launchMotor != null) { + launchMotor.setPower(power); } } public double getMotorSpeed() { - if (top != null) { - return top.getVelocity(); + if (launchMotor != null) { + return launchMotor.getVelocity(); } return -1; } @@ -137,7 +145,7 @@ public void periodic() { setMotorPower(launcherPID.update(getMotorSpeed())); err = launcherPID.getLastError(); MOTOR_VELOCITY = getMotorSpeed(); - power = top.getPower(); + power = launchMotor.getPower(); // launcherP.f = SPIN_F_SCALE * target + SPIN_VOLT_COMP * Math.min(PEAK_VOLTAGE, Hardware.voltage()); } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/validation/IntakeValidator.java b/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/validation/IntakeValidator.java index fc38d52..a8cb505 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/validation/IntakeValidator.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/validation/IntakeValidator.java @@ -16,7 +16,7 @@ public class IntakeValidator extends ValidationOpMode { private CRServo right; public static double intakeVelocity = 2000; public static double intakeVelocity2 = -200; - public static double spinLeftFlower = -1; + public static double spinLeftFlower = 1; public static double spinRightFlower = 1; @Override @@ -36,16 +36,28 @@ public void loop() { intakeMotor.setVelocity(intakeVelocity); } else if (this.gamepad1.dpad_down) { intakeMotor.setVelocity(intakeVelocity2); + } else { + intakeMotor.setVelocity(0); } - addLine("Press left on d-pad to intake left"); - if (this.gamepad1.dpad_left) { + addLine("Press left bumper to intake left side (positive)"); + addLine("Press left trigger to intake left side (negative)"); + if (this.gamepad1.left_bumper) { left.setPower(spinLeftFlower); + } else if (this.gamepad1.left_trigger_pressed) { + left.setPower(-spinLeftFlower); + } else { + left.setPower(0); } - addLine("Press right on d-pad to intake right"); - if (this.gamepad1.dpad_right) { + addLine("Press right bumper to intake right side (positive)"); + addLine("Press right trigger to intake right side (negative)"); + if (this.gamepad1.right_bumper) { right.setPower(spinRightFlower); + } else if (this.gamepad1.right_trigger_pressed) { + right.setPower(-spinRightFlower); + } else { + right.setPower(0); } } } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/validation/LaunchValidator.java b/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/validation/LaunchValidator.java index 6a57534..3dffd60 100644 --- a/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/validation/LaunchValidator.java +++ b/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/validation/LaunchValidator.java @@ -15,9 +15,7 @@ public class LaunchValidator extends ValidationOpMode { private DcMotorEx launchmotor; private CRServo spinnything; public static double launchvelocity = 2000; - public static double intakeVelocity2 = -200; - public static double spinspeed = -1; - public static double spinRightFlower = 1; + public static double spinspeed = 1; @Override public void init() { @@ -30,16 +28,22 @@ public void init() { @Override public void loop() { super.loop(); - addLine("Press up on d-pad to launch"); + addLine("Press up on d-pad to spin launcher (positive)"); + addLine("Press down on d-pad to spin launch (negative)"); if (this.gamepad1.dpad_up) { launchmotor.setVelocity(launchvelocity); + } else if (this.gamepad1.dpad_down) { + launchmotor.setVelocity(-launchvelocity); } else { launchmotor.setVelocity(0); } - addLine("Press right bumper to spin"); + addLine("Press right bumper to spin transfer (positive)"); + addLine("Press left bumper to spin transfer (negative)"); if (this.gamepad1.right_bumper) { spinnything.setPower(spinspeed); + } else if (this.gamepad1.left_bumper) { + spinnything.setPower(-spinspeed); } else { spinnything.setPower(0); } diff --git a/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/validation/TransferValidator.java b/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/validation/TransferValidator.java deleted file mode 100644 index b39d401..0000000 --- a/Twenty403/src/main/java/org/firstinspires/ftc/teamcode/twenty403/validation/TransferValidator.java +++ /dev/null @@ -1,29 +0,0 @@ -package org.firstinspires.ftc.teamcode.twenty403.validation; - -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.CRServo; -import com.technototes.library.structure.ValidationOpMode; -import org.firstinspires.ftc.teamcode.twenty403.Setup; - -@TeleOp(name = "Windmill Test", group = "validators") -public class TransferValidator extends ValidationOpMode { - - private CRServo transferMotor; - public static double intakeVelocity = 0.75; - - @Override - public void init() { - super.init(); - transferMotor = this.hardwareMap.get(CRServo.class, Setup.HardwareNames.SPIN); - } - - @Override - public void loop() { - super.loop(); - - addLine("Press right bumper to spin intake"); - if (this.gamepad1.right_bumper) { - transferMotor.setPower(intakeVelocity); - } - } -}