Skip to content
Open

Large diffs are not rendered by default.

Original file line number Diff line number Diff line change
@@ -0,0 +1,76 @@
package org.firstinspires.ftc.learnbot.opmodes;

import com.bylazar.configurables.annotations.Configurable;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.technototes.library.hardware.motor.EncodedMotor;
import java.util.List;
import java.util.function.DoubleSupplier;
import org.firstinspires.ftc.learnbot.component.LauncherComponent;
import org.firstinspires.ftc.robotcore.external.navigation.VoltageUnit;

@Configurable
@SuppressWarnings("unused")
@TeleOp(name = "Launcher Cfg")
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sure would be cool if this were available as part of the component (I should try it)

public class LauncherValidator extends LinearOpMode {

public static String MotorName = "rr";

public boolean anyDpad() {
return (
gamepad1.dpadUpWasReleased() ||
gamepad1.dpadDownWasReleased() ||
gamepad1.dpadLeftWasReleased() ||
gamepad1.dpadRightWasReleased()
);
}

public boolean anyButtons() {
return (
gamepad1.aWasReleased() ||
gamepad1.bWasReleased() ||
gamepad1.xWasReleased() ||
gamepad1.yWasReleased()
);
}

@Override
public void runOpMode() throws InterruptedException {
// We need to get the voltage for
List<LynxModule> hubs = hardwareMap.getAll(LynxModule.class);
DoubleSupplier getVoltage = () -> {
double volts = 0;
for (LynxModule lm : hubs) {
volts += lm.getInputVoltage(VoltageUnit.VOLTS);
}
return volts / hubs.size();
};
EncodedMotor<DcMotorEx> m = new EncodedMotor<>(
hardwareMap.get(DcMotorEx.class, MotorName),
MotorName
);
LauncherComponent lc = new LauncherComponent(m, null, getVoltage);
waitForStart();
telemetry.addLine(">>> Press the dpad for feedfwd <<<");
telemetry.addLine(">>> Press any button for validation <<<");
telemetry.update();
while (opModeIsActive()) {
if (anyDpad()) {
lc.feedFwdHelper(telemetry, gamepad1, this::opModeIsActive);
break;
} else if (anyButtons()) {
while (opModeIsActive() && !anyDpad()) {
telemetry.addLine(">>> Press left trigger for Launcher1 control");
telemetry.addLine(">>> Press right trigger for Launcher2 control");
telemetry.addLine(">>> Hit the dpad to stop");
telemetry.addLine(
lc.hardwareValidation(gamepad1.left_trigger, gamepad1.right_trigger)
);
telemetry.update();
}
}
}
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@ public Hardware(HardwareMap hwmap) {
if (Setup.Connected.INTAKESUBSYSTEM) {
intake = this.map.get(DcMotorEx.class, Setup.HardwareNames.INTAKE_MOTOR);
}
if (Setup.Connected.LAUNCHERSUBSYSTEM) {
if (Setup.Connected.LAUNCHERCOMPONENT) {
launcher1 = new EncodedMotor(Setup.HardwareNames.LAUNCHER_MOTOR1);
launcher2 = new EncodedMotor(Setup.HardwareNames.LAUNCHER_MOTOR2);
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -3,11 +3,11 @@
import com.pedropathing.follower.Follower;
import com.technototes.library.logger.Loggable;
import com.technototes.library.util.Alliance;
import org.firstinspires.ftc.sixteen750.component.LauncherComponent;
import org.firstinspires.ftc.sixteen750.helpers.StartingPosition;
import org.firstinspires.ftc.sixteen750.subsystems.AimingSubsystem;
import org.firstinspires.ftc.sixteen750.subsystems.BrakeSubsystem;
import org.firstinspires.ftc.sixteen750.subsystems.IntakeSubsystem;
import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem;
import org.firstinspires.ftc.sixteen750.subsystems.LimelightSubsystem;
import org.firstinspires.ftc.sixteen750.subsystems.SafetySubsystem;
import org.firstinspires.ftc.sixteen750.subsystems.TestSubsystem;
Expand All @@ -20,7 +20,7 @@ public class Robot implements Loggable {
public double initialVoltage;

public SafetySubsystem safetySubsystem;
public LauncherSubsystem launcherSubsystem;
public LauncherComponent launcherComponent;
public IntakeSubsystem intakeSubsystem;
public BrakeSubsystem brakeSubsystem;
public AimingSubsystem aimingSubsystem;
Expand All @@ -41,8 +41,23 @@ public Robot(Hardware hw, Alliance team, StartingPosition pos) {
if (Setup.Connected.INTAKESUBSYSTEM) {
this.intakeSubsystem = new IntakeSubsystem(hw);
}
if (Setup.Connected.LAUNCHERSUBSYSTEM) {
this.launcherSubsystem = new LauncherSubsystem(hw);
if (Setup.Connected.LAUNCHERCOMPONENT) {
// For Review:
// The limelightSubsystem now implements the "TargetAcquisition" interface. This let's
// us completely decouple the LauncherComponent from the LimelightSubsystem, and instead
// just requires that we provide *anything* that will tell us the distance to target.
// We could build out a little thing that only uses Odometry to decide where the target
// is, instead, and wouldn't need to change anything in the LauncherComponent.
this.launcherComponent = new LauncherComponent(
hw.launcher1,
hw.launcher2,
limelightSubsystem,
hw::voltage
);
} else {
// This should let us run code (scheduled commands, for instance...) without any
// hardware in the launcher subsystem
this.launcherComponent = new LauncherComponent();
}
if (Setup.Connected.BRAKESUBSYSTEM) {
this.brakeSubsystem = new BrakeSubsystem(hw);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@ public static class Connected {
public static boolean EXTERNAL_IMU = true;
public static boolean OTOS = false;
public static boolean INTAKESUBSYSTEM = true;
public static boolean LAUNCHERSUBSYSTEM = true;
public static boolean LAUNCHERCOMPONENT = true;
public static boolean AIMINGSUBSYSTEM = true;
public static boolean BRAKESUBSYSTEM = true;
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,6 @@
import com.technototes.library.util.PIDFController;
import org.firstinspires.ftc.sixteen750.Robot;
import org.firstinspires.ftc.sixteen750.Setup;
import org.firstinspires.ftc.sixteen750.subsystems.LauncherSubsystem;
import org.firstinspires.ftc.sixteen750.subsystems.LimelightSubsystem;

@Configurable
Expand Down Expand Up @@ -73,7 +72,7 @@ public void execute() {
// Math.toRadians(robot.limelightSubsystem.getTX()) //.getTX .getLimelightRotation()
// );
// robot.follower.holdPoint(new BezierPoint(wantedPose), wantedPose.getHeading(), false);
// LauncherSubsystem.targetPower = 1;
// LauncherComponent.targetPower = 1;
// firsttime = false;
//}
currentPose = robot.follower.getPose();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,7 @@ public void execute() {
// Math.toRadians(robot.limelightSubsystem.getTX()) //.getTX .getLimelightRotation()
// );
// robot.follower.holdPoint(new BezierPoint(wantedPose), wantedPose.getHeading(), false);
// LauncherSubsystem.targetPower = 1;
// LauncherComponent.targetPower = 1;
// firsttime = false;
//}
currentPose = robot.follower.getPose();
Expand Down

This file was deleted.

Original file line number Diff line number Diff line change
Expand Up @@ -14,34 +14,6 @@ public static Command TurnTo90(Robot r) {
); //pose might need to be current pose?
}

public static Command Launch(Robot r) {
return Command.create(r.launcherSubsystem::Launch);
}

public static Command SetFarShoot(Robot r) {
return Command.create(r.launcherSubsystem::FarShoot);
}

public static Command SetCloseShoot(Robot r) {
return Command.create(r.launcherSubsystem::CloseShoot);
}

public static Command AutoLaunch1(Robot r) {
return Command.create(r.launcherSubsystem::AutoLaunch1);
}

public static Command AutoLaunch2(Robot r) {
return Command.create(r.launcherSubsystem::AutoLaunch2);
}

public static Command FarAutoLaunch(Robot r) {
return Command.create(r.launcherSubsystem::FarAutoLaunch);
}

public static Command StopLaunch(Robot r) {
return Command.create(r.launcherSubsystem::Stop);
}

public static Command Rumble(Robot r) {
return Command.create(r.intakeSubsystem::setRumble);
}
Expand All @@ -50,14 +22,6 @@ public static Command RumbleOff(Robot r) {
return Command.create(r.intakeSubsystem::setRumbleOff);
}

public static Command IncreaseMotor(Robot r) {
return Command.create(r.launcherSubsystem::IncreaseMotorVelocity);
}

public static Command DecreaseMotor(Robot r) {
return Command.create(r.launcherSubsystem::DecreaseMotorVelocity);
}

public static Command Intake(Robot r) {
return Command.create(r.intakeSubsystem::Intake);
}
Expand Down Expand Up @@ -117,28 +81,4 @@ public static Command MotorPowerTest(Robot r) {
public static Command MotorVelocityTest(Robot r) {
return Command.create(r.testSubsystem::setMotorVelocityTest);
}

public static Command ReadVelocity(Robot r) {
return Command.create(r.launcherSubsystem::readVelocity);
}

public static Command SetRegressionCAuto(Robot r) {
return Command.create(r.launcherSubsystem::setRegressionCAuto);
}

public static Command SetRegressionDAuto(Robot r) {
return Command.create(r.launcherSubsystem::setRegressionDAuto);
}

public static Command SetRegressionCTeleop(Robot r) {
return Command.create(r.launcherSubsystem::setRegressionCTeleop);
}

public static Command SetRegressionDTeleop(Robot r) {
return Command.create(r.launcherSubsystem::setRegressionDTeleop);
}

public static Command IncreaseRegressionDTeleop(Robot r) {
return Command.create(r.launcherSubsystem::increaseRegressionDTeleop);
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
import org.firstinspires.ftc.sixteen750.commands.AltAutoOrient;
import org.firstinspires.ftc.sixteen750.commands.AltAutoOrientFar;
import org.firstinspires.ftc.sixteen750.commands.TeleCommands;
import org.firstinspires.ftc.sixteen750.component.LauncherComponent.LauncherCommand;

@Configurable
public class Paths {
Expand All @@ -26,7 +27,7 @@ public static Command Launching3Balls(Robot r) {
new ParallelCommandGroup(
TeleCommands.Intake(r),
TeleCommands.GateUp(r),
TeleCommands.AutoLaunch1(r)
LauncherCommand.AutoLaunch1()
),
new WaitCommand(3),
TeleCommands.GateDown(r),
Expand Down
Loading