Skip to content

Velocity-formula-and-hood-comp #20

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
kevinfrei merged 5 commits into from
Oct 9, 2026
Merged

Velocity-formula-and-hood-comp #20

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
df6e892
ok i did the thing code isnt great didnt get a chance to clean up som…
TechnototesLaptop Oct 3, 2026
bdb04f1
ok did all the hood comp auto hood angle and auto velocity math the h…
explosivegamer25 Oct 5, 2026
a975319
ok shouldve fixed all of the stuff aswell as added a small bit to the…
explosivegamer25 Oct 6, 2026
b1d4086
Auto-formatted code
kevinfrei Oct 5, 2026
d8d62f7
Evandro and Kyan , we added a reset position command, kevin said its …
evandrospenceralves-star Oct 7, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 3 additions & 3 deletions Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/PedroConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,14 +42,14 @@ public static Follower create(HardwareMap h) {
c.xPodPort.set(1);
c.yPodPort.set(0);
c.ticksPerUnit.set(505.316944406);
c.xPodOffset.set(2.92913);
c.yPodOffset.set(2.77717);
c.xPodOffset.set(2.598); // from cad but i trust it more
c.yPodOffset.set(2.264); // from cad but i trust it more
c.xPodDirection.set(OctoQuad.EncoderDirection.FORWARD);
c.yPodDirection.set(OctoQuad.EncoderDirection.REVERSE);
c.globalDistanceUnit.set(DistanceUnit.INCH);
c.offsetUnits.set(DistanceUnit.INCH);
c.i2cRecoveryMode.set(OctoQuad.I2cRecoveryMode.MODE_1_PERIPH_RST_ON_FRAME_ERR);
c.headingScalar.set(1.0348099609138208);
c.headingScalar.set(1.01828);
});
public static ForesightConfig foresightConfig = new ForesightConfig(c -> {
Controller primaryTranslationalForward = Controller.proportional(0.28921569321161206);
Expand Down
4 changes: 4 additions & 0 deletions Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/commands/PedroDriver.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,10 @@ public void ResetGyro() {
headingOffset = follower.pose().heading();
}

public void ResetPosition() {
follower.setPose(new Pose(6.75, 6.75, 0));
}

public void SetSnailSpeed() {
// follower.setMaxPowerScaling(OtherSettings.SNAIL_SPEED);
turnSpeed = OtherSettings.SNAIL_TURN;
Expand Down
18 changes: 15 additions & 3 deletions ...een750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/commands/TeleCommands.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@ public static Command Feed(Robot r) {
}

public static Command IntakeStop(Robot r) {
return Command.create(r.intakeSubsystem::StopIntake);
return Command.create(r.intakeSubsystem::StopIntake);
}

public static Command Reject(Robot r) {
Expand All @@ -46,9 +46,21 @@ public static Command GateClose(Robot r) {
return Command.create(r.intakeSubsystem::GateClose);
}

public static Command Track(Robot r) {
public static Command TrackHive(Robot r) {
return Command.create(r.turretsubsystem::setTurretTarget);
}
public static Command IncreaseFlywheel (Robot r) {
return Command.create(r.turretsubsystem::increaseVelocity);
}
public static Command DecreaseFlywheel (Robot r) {
return Command.create(r.turretsubsystem::decreaseVelocity);
}

public static Command Launch (Robot r) {return Command.create(r.turretsubsystem::Launch);}
public static Command AutoHood(Robot r) {
Comment thread
explosivegamer25 marked this conversation as resolved.
return Command.create(r.turretsubsystem::setHoodAutoPos);
}

public static Command Launch(Robot r) {
return Command.create(r.turretsubsystem::Launch);
}
}
4 changes: 4 additions & 0 deletions ...main/java/org/firstinspires/ftc/teamcode/sixteen750/commands/driving/DrivingCommands.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,10 @@ public static Command SnailDriving(PedroDriver pd) {
return pd::SetSnailSpeed;
}

public static Command ResetPosition(PedroDriver pd) {
return pd::ResetPosition;
}

public static Command TurboDriving(PedroDriver pd) {
return pd::SetTurboSpeed;
}
Expand Down
71 changes: 19 additions & 52 deletions ...50/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/controls/DriverController.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
package org.firstinspires.ftc.teamcode.sixteen750.controls;

import com.technototes.library.command.Command;
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;
Expand All @@ -26,14 +26,15 @@ public class DriverController implements Loggable {
public CommandButton spitButton;
public CommandButton gateButton;
public CommandButton override;
public CommandButton RelocButton;
public CommandButton relocButton;
public CommandButton holdButton;
public CommandButton turretlockButton;
public CommandButton trackButton;

public CommandButton flywheelButton;
public CommandButton intakeTrigger;
public CommandAxis autoAim;
public CommandButton upButton;
public CommandButton downButton;
public PedroDriver pedroDriver;
public CommandButton hoodButton;

public static double triggerThreshold = 0.1;

Expand All @@ -53,10 +54,6 @@ public DriverController(CommandGamepad g, Robot r) {
if (Setup.Connected.INTAKESUBSYSTEM) {
bindIntakeControls();
}

if (Setup.Connected.AIMINGSUBSYSTEM) {
bindAimControls();
}
}

public void AssignNamedControllerButton() {
Expand All @@ -65,14 +62,17 @@ public void AssignNamedControllerButton() {
driveRightStick = gamepad.rightStick;
intakeTrigger = gamepad.rightTrigger.getAsButton();

snailButton = gamepad.ps_triangle;
upButton = gamepad.dpadUp;
downButton = gamepad.dpadDown;
snailButton = gamepad.leftBumper;
launchButton = gamepad.rightBumper;
flywheelButton = gamepad.dpadLeft;
spitButton = gamepad.ps_square;
gateButton = gamepad.ps_cross;
holdButton = gamepad.ps_circle;
RelocButton = gamepad.ps_share;
turretlockButton = gamepad.dpadUp;
trackButton = gamepad.leftBumper;
relocButton = gamepad.ps_share;
trackButton = gamepad.ps_triangle;
hoodButton = gamepad.dpadRight;
}

public void bindDriveControls() {
Expand All @@ -84,29 +84,21 @@ public void bindDriveControls() {
);
CommandScheduler.scheduleJoystick(pedroDriver);

// turboButton.whenPressed(DrivingCommands.TurboDriving(robot.drivebase));
// turboButton.whenReleased(DrivingCommands.NormalDriving(robot.drivebase));
snailButton.whenPressedReleased(
DrivingCommands.SnailDriving(pedroDriver),
DrivingCommands.NormalDriving(pedroDriver)
);

resetGyroButton.whenPressed(DrivingCommands.ResetGyro(pedroDriver));
//MotorDecrease.whenPressed(TeleCommands.DecreaseMotor(robot));
//MotorIncrease.whenPressed(TeleCommands.IncreaseMotor(robot));

// if (Setup.Connected.LIMELIGHTSUBSYSTEM) {
// autoAim.whenPressed(DrivingCommands.AutoOrient(pedroDriver));
// autoAim.whenReleased(DrivingCommands.NoAutoOrient(pedroDriver));
// RelocButton.whenPressed(TeleCommands.LLRelocCommand(robot));
//AltAutoAlign.whenPressed(new AltAutoOrient(robot));
//AltAutoAlign.whenReleased(DrivingCommands.NormalDriving(pedroDriver));
// }
// autoAim.whilePressed(new LLPipelineChangeCommand(hardware.limelight, Setup.HardwareNames.AprilTag_Pipeline));
relocButton.whenPressed(DrivingCommands.ResetPosition(pedroDriver));
}

public void bindLaunchControls() {
trackButton.whilePressed(TeleCommands.Track(robot));
trackButton.whileInverseToggled(TeleCommands.TrackHive(robot));
flywheelButton.whileInverseToggled(TeleCommands.Launch(robot));
hoodButton.whileInverseToggled(TeleCommands.AutoHood(robot));
upButton.whenPressed(TeleCommands.IncreaseFlywheel(robot));
downButton.whenPressed(TeleCommands.DecreaseFlywheel(robot));
}

public void bindIntakeControls() {
Expand All @@ -120,29 +112,4 @@ public void bindIntakeControls() {
launchButton.whenReleased(TeleCommands.Hold(robot));
gateButton.whenPressedReleased(TeleCommands.GateOpen(robot), TeleCommands.GateClose(robot));
}

// spitTrigger.whilePressed(TeleCommands.Spit(robot.intakeSubsystem));
// spitTrigger.whileReleased(TeleCommands.Intake(robot.intakeSubsystem));

public void bindAimControls() {
// if(yippee) {
// leverButton.whenPressed(
// TeleCommands.LeverStop(robot.aimingSubsystem));
// yippee = false;
// } else {
// leverButton.whenPressed(
// TeleCommands.LeverGo(robot.aimingSubsystem));
// yippee = true;
// }
// gateButton.whenPressed(TeleCommands.GateDown(robot));
gateButton.whenPressed(TeleCommands.Feed(robot));
gateButton.whenReleased(TeleCommands.IntakeStop(robot));
// gateButton.whenReleased(TeleCommands.GateUp(robot));

//
holdButton.whilePressed(TeleCommands.Intake(robot));
// holdButton.whenPressed(TeleCommands.GateDown(robot));
holdButton.whenReleased(TeleCommands.IntakeStop(robot));
//holdButton.whenReleased(TeleCommands.GateUp(robot));
}
}
10 changes: 7 additions & 3 deletions Sixteen750/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/opmodes/MainTele.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
import static org.firstinspires.ftc.teamcode.sixteen750.Setup.HardwareNames.AprilTag_Pipeline;

import com.bylazar.telemetry.PanelsTelemetry;
import com.bylazar.telemetry.TelemetryManager;
import com.pedropathing.math.Pose;
import com.qualcomm.hardware.limelightvision.Limelight3A;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
Expand All @@ -20,6 +21,7 @@
import org.firstinspires.ftc.teamcode.sixteen750.controls.DriverController;
import org.firstinspires.ftc.teamcode.sixteen750.controls.OperatorController;
import org.firstinspires.ftc.teamcode.sixteen750.helpers.StartingPosition;
import org.firstinspires.ftc.teamcode.sixteen750.subsystems.TurretSubsystem;

@TeleOp(name = "MainTele")
@SuppressWarnings("unused")
Expand All @@ -31,23 +33,23 @@ public class MainTele extends CommandOpMode implements Loggable {
public Hardware hardware;
private Limelight3A limelight;
private PanelsTelemetry panelsTelemetry;
TelemetryManager ptel;

@Override
public void uponInit() {
hardware = new Hardware(hardwareMap);
robot = new Robot(hardware, Alliance.RED, StartingPosition.Unspecified);
// controlsOperator = new OperatorController(codriverGamepad, robot);
panelsTelemetry = PanelsTelemetry.INSTANCE;
robot.follower.setPose(new Pose(6.75, 6.75, 0));
robot.follower.setPose(new Pose(6.75, 6.75, 0)); // temporary starting pos for now
// limelight = hardwareMap.get(Limelight3A.class, Setup.HardwareNames.LIMELIGHT);
controlsDriver = new DriverController(driverGamepad, robot);
if (Setup.Connected.DRIVEBASE) {
// Just pick a starting point
CommandScheduler.scheduleForState(
new SequentialCommandGroup(
HeadingHelper.RestorePreviousPosition(robot.follower),
DrivingCommands.ResetGyro(controlsDriver.pedroDriver),
Command.create(robot.turretsubsystem::Launch)
DrivingCommands.ResetGyro(controlsDriver.pedroDriver)
),
OpModeState.INIT
);
Expand All @@ -69,6 +71,8 @@ public void uponInit() {
* Starts polling for data. If you neglect to call start(), getLatestResult() will return null.
*/
limelight.start();

ptel = PanelsTelemetry.INSTANCE.getTelemetry();
}
if (Setup.Connected.TURRETSUBSYSTEM) {
// CommandScheduler.register(robot.launcherSubsystem);
Expand Down
21 changes: 14 additions & 7 deletions ...c/main/java/org/firstinspires/ftc/teamcode/sixteen750/opmodes/auto/PartnerPark12Park.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.technototes.library.command.Command;
import com.technototes.library.command.CommandScheduler;
import com.technototes.library.command.ParallelCommandGroup;
import com.technototes.library.command.SequentialCommandGroup;
import com.technototes.library.command.WaitCommand;
import com.technototes.library.structure.CommandOpMode;
Expand All @@ -20,7 +21,7 @@
import org.firstinspires.ftc.teamcode.sixteen750.pedro.meepmeep.Paths;
import org.firstinspires.ftc.teamcode.sixteen750.pedro.meepmeep.Poses;

@Autonomous(name = "PartnerPark12Park", preselectTeleOp = "BlueTele")
@Autonomous(name = "PartnerPark12Park", preselectTeleOp = "MainTele")
@SuppressWarnings("unused")
public class PartnerPark12Park extends CommandOpMode {

Expand All @@ -41,22 +42,28 @@ public void uponInit() {
panelsTelemetry = PanelsTelemetry.INSTANCE;
robot.follower.setPose(Poses.StartPoses.getStart());
CommandScheduler.scheduleForState(
new ParallelCommandGroup(
t.Launch(robot),
t.TrackHive(robot),
t.AutoHood(robot)
).alongWith(
new SequentialCommandGroup(
//t.Launch(robot) .alongWith(
new PedroPathCommand(robot.follower, p.StartToPartnerPark()), //)
new PedroPathCommand(robot.follower, p.PartnerParkToLaunch1()),
//t.Feed(robot),
t.Feed(robot),
new WaitCommand(0.6),
new PedroPathCommand(robot.follower, p.Launch1ToGardenPreInt()),
//.alongWith( t.Intake(robot) )
new PedroPathCommand(robot.follower, p.Launch1ToGardenPreInt())
.alongWith(
t.Intake(robot)),
new PedroPathCommand(robot.follower, p.GardenPreIntToGardenInt()),
new WaitCommand(1),
new PedroPathCommand(robot.follower, p.GardenIntToLaunch2()),
//t.Feed(robot),
t.Feed(robot),
new WaitCommand(0.6),
new PedroPathCommand(robot.follower, p.Launch2ToPark()),
CommandScheduler::terminateOpMode
),
)
),
OpModeState.RUN
);
}
Expand Down
22 changes: 18 additions & 4 deletions ...0/src/main/java/org/firstinspires/ftc/teamcode/sixteen750/subsystems/IntakeSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,11 +18,12 @@ public class IntakeSubsystem implements Loggable, Subsystem {

public static double REJECT_VELOCITY = -0.4;

public static double HOLD_VELOCITY = 0.4;
public static double HOLD_VELOCITY1 = 0.45;
public static double HOLD_VELOCITY2 = 0.55;

public static double FEED_VELOCITY = 0.95;
public static double FEED_VELOCITY = 1;

public static double BLOCK_POSITION = 0.5;
public static double BLOCK_POSITION = 0.53;

public static double FIRE_POSITION = 0.3;

Expand Down Expand Up @@ -72,7 +73,8 @@ public void Spit() {
}

public void Hold() {
setTransferPower(HOLD_VELOCITY);
setTransfer1Power(HOLD_VELOCITY2);
setTransfer2Power(HOLD_VELOCITY1);
setGatePosition(BLOCK_POSITION);
}

Expand Down Expand Up @@ -107,6 +109,18 @@ private void setTransferPower(double power) {
}
}

private void setTransfer1Power(double power) {
if (hasHardware) {
_transfer1.setPower(power);
}
}

private void setTransfer2Power(double power) {
if (hasHardware) {
_transfer2.setPower(power);
}
}

private void setGatePosition(double pos) {
if (hasHardware) {
_gate.setPosition(pos);
Expand Down
Loading
Loading