Skip to content

Commit

Permalink
troubleshooting
Browse files Browse the repository at this point in the history
  • Loading branch information
@sigalrmp
sigalrmp committed Jan 25, 2025
1 parent 07a2ccb commit cba9ec1
Showing 1 changed file with 82 additions and 105 deletions.
187 changes: 82 additions & 105 deletions src/main/java/org/sciborgs1155/robot/Robot.java
View file
Original file line number Diff line number Diff line change
@@ -1,19 +1,7 @@
package org.sciborgs1155.robot;

import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.Second;
import static edu.wpi.first.units.Units.Seconds;
import static edu.wpi.first.wpilibj2.command.button.RobotModeTriggers.autonomous;
import static edu.wpi.first.wpilibj2.command.button.RobotModeTriggers.test;
import static org.sciborgs1155.robot.Constants.DEADBAND;
import static org.sciborgs1155.robot.Constants.PERIOD;
import static org.sciborgs1155.robot.arm.ArmConstants.INTAKE_ANGLE;
import static org.sciborgs1155.robot.arm.ArmConstants.PROCESSOR_OUTTAKE_ANGLE;
import static org.sciborgs1155.robot.arm.ArmConstants.TROUGH_OUTTAKE_ANGLE;
import static org.sciborgs1155.robot.drive.DriveConstants.MAX_ANGULAR_ACCEL;
import static org.sciborgs1155.robot.drive.DriveConstants.MAX_SPEED;
import static org.sciborgs1155.robot.drive.DriveConstants.TELEOP_ANGULAR_SPEED;

import edu.wpi.first.wpilibj.DataLogManager;
import edu.wpi.first.wpilibj.DriverStation;
Expand All @@ -26,14 +14,12 @@
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import java.util.Set;
import monologue.Annotations.Log;
import monologue.Logged;
import monologue.Monologue;
import org.littletonrobotics.urcl.URCL;
import org.sciborgs1155.lib.CommandRobot;
import org.sciborgs1155.lib.FaultLogger;
import org.sciborgs1155.lib.InputStream;
import org.sciborgs1155.lib.Test;
import org.sciborgs1155.robot.Constants.Field.Level;
import org.sciborgs1155.robot.Ports.OI;
Expand Down Expand Up @@ -113,88 +99,88 @@ private void configureGameBehavior() {

/** Configures trigger -> command bindings. */
private void configureBindings() {
operator
.povDown()
.onTrue(
elevator
.scoreLevel(Level.L1)
.until(() -> elevator.atGoal())
.andThen(scoral.outtake().until(() -> scoral.beambreak())));
operator
.povLeft()
.onTrue(
elevator
.scoreLevel(Level.L2)
.until(() -> elevator.atGoal())
.andThen(scoral.outtake().until(() -> scoral.beambreak())));
operator
.povRight()
.onTrue(
elevator
.scoreLevel(Level.L3)
.until(() -> elevator.atGoal())
.andThen(scoral.outtake().until(() -> scoral.beambreak())));
operator
.povUp()
.onTrue(
elevator
.scoreLevel(Level.L4)
.until(() -> elevator.atGoal())
.andThen(scoral.outtake().until(() -> scoral.beambreak())));
operator.leftBumper().onTrue(arm.climbSetup());
operator.leftTrigger().onTrue(arm.climbExecute());
operator.rightBumper().onTrue(arm.goTo(TROUGH_OUTTAKE_ANGLE).alongWith(coroller.outtake()));
operator.rightTrigger().onTrue(arm.goTo(INTAKE_ANGLE).alongWith(coroller.intake()));
operator.a().onTrue(arm.goTo(PROCESSOR_OUTTAKE_ANGLE).alongWith(coroller.outtake()));

// x and y are switched: we use joystick Y axis to control field x motion
InputStream x = InputStream.of(driver::getLeftY).negate();
InputStream y = InputStream.of(driver::getLeftX).negate();

// Apply speed multiplier, deadband, square inputs, and scale translation to max speed
InputStream r =
InputStream.hypot(x, y)
.log("Robot/raw joystick")
.scale(() -> speedMultiplier)
.clamp(1.0)
.deadband(Constants.DEADBAND, 1.0)
.signedPow(2.0)
.log("Robot/processed joystick")
.scale(MAX_SPEED.in(MetersPerSecond));

InputStream theta = InputStream.atan(x, y);

// Split x and y components of translation input
x = r.scale(theta.map(Math::cos)); // .rateLimit(MAX_ACCEL.in(MetersPerSecondPerSecond));
y = r.scale(theta.map(Math::sin)); // .rateLimit(MAX_ACCEL.in(MetersPerSecondPerSecond));

// Apply speed multiplier, deadband, square inputs, and scale rotation to max teleop speed
InputStream omega =
InputStream.of(driver::getRightX)
.negate()
.scale(() -> speedMultiplier)
.clamp(1.0)
.deadband(DEADBAND, 1.0)
.signedPow(2.0)
.scale(TELEOP_ANGULAR_SPEED.in(RadiansPerSecond))
.rateLimit(MAX_ANGULAR_ACCEL.in(RadiansPerSecond.per(Second)));

drive.setDefaultCommand(drive.drive(x, y, omega));
led.setDefaultCommand(led.scrolling());

autonomous().whileTrue(Commands.defer(autos::getSelected, Set.of(drive)).asProxy());
autonomous().whileTrue(led.autos());

test().whileTrue(systemsCheck());

driver.b().whileTrue(drive.zeroHeading());
driver
.leftBumper()
.or(driver.rightBumper())
.onTrue(Commands.runOnce(() -> speedMultiplier = Constants.SLOW_SPEED_MULTIPLIER))
.onFalse(Commands.runOnce(() -> speedMultiplier = Constants.FULL_SPEED_MULTIPLIER));

driver.leftTrigger().onTrue(hopper.intake().alongWith(scoral.intake()));
// operator
// .povDown()
// .onTrue(
// elevator
// .scoreLevel(Level.L1)
// .until(() -> elevator.atGoal())
// .andThen(scoral.outtake().until(() -> scoral.beambreak())));
// operator
// .povLeft()
// .onTrue(
// elevator
// .scoreLevel(Level.L2)
// .until(() -> elevator.atGoal())
// .andThen(scoral.outtake().until(() -> scoral.beambreak())));
// operator
// .povRight()
// .onTrue(
// elevator
// .scoreLevel(Level.L3)
// .until(() -> elevator.atGoal())
// .andThen(scoral.outtake().until(() -> scoral.beambreak())));
// operator
// .povUp()
// .onTrue(
// elevator
// .scoreLevel(Level.L4)
// .until(() -> elevator.atGoal())
// .andThen(scoral.outtake().until(() -> scoral.beambreak())));
// operator.leftBumper().onTrue(arm.climbSetup());
// operator.leftTrigger().onTrue(arm.climbExecute());
// operator.rightBumper().onTrue(arm.goTo(TROUGH_OUTTAKE_ANGLE).alongWith(coroller.outtake()));
// operator.rightTrigger().onTrue(arm.goTo(INTAKE_ANGLE).alongWith(coroller.intake()));
// operator.a().onTrue(arm.goTo(PROCESSOR_OUTTAKE_ANGLE).alongWith(coroller.outtake()));

// // x and y are switched: we use joystick Y axis to control field x motion
// InputStream x = InputStream.of(driver::getLeftY).negate();
// InputStream y = InputStream.of(driver::getLeftX).negate();

// // Apply speed multiplier, deadband, square inputs, and scale translation to max speed
// InputStream r =
// InputStream.hypot(x, y)
// .log("Robot/raw joystick")
// .scale(() -> speedMultiplier)
// .clamp(1.0)
// .deadband(Constants.DEADBAND, 1.0)
// .signedPow(2.0)
// .log("Robot/processed joystick")
// .scale(MAX_SPEED.in(MetersPerSecond));

// InputStream theta = InputStream.atan(x, y);

// // Split x and y components of translation input
// x = r.scale(theta.map(Math::cos)); // .rateLimit(MAX_ACCEL.in(MetersPerSecondPerSecond));
// y = r.scale(theta.map(Math::sin)); // .rateLimit(MAX_ACCEL.in(MetersPerSecondPerSecond));

// // Apply speed multiplier, deadband, square inputs, and scale rotation to max teleop speed
// InputStream omega =
// InputStream.of(driver::getRightX)
// .negate()
// .scale(() -> speedMultiplier)
// .clamp(1.0)
// .deadband(DEADBAND, 1.0)
// .signedPow(2.0)
// .scale(TELEOP_ANGULAR_SPEED.in(RadiansPerSecond))
// .rateLimit(MAX_ANGULAR_ACCEL.in(RadiansPerSecond.per(Second)));

// drive.setDefaultCommand(drive.drive(x, y, omega));
// led.setDefaultCommand(led.scrolling());

// autonomous().whileTrue(Commands.defer(autos::getSelected, Set.of(drive)).asProxy());
// autonomous().whileTrue(led.autos());

// test().whileTrue(systemsCheck());

// driver.b().whileTrue(drive.zeroHeading());
// driver
// .leftBumper()
// .or(driver.rightBumper())
// .onTrue(Commands.runOnce(() -> speedMultiplier = Constants.SLOW_SPEED_MULTIPLIER))
// .onFalse(Commands.runOnce(() -> speedMultiplier = Constants.FULL_SPEED_MULTIPLIER));

// driver.leftTrigger().onTrue(hopper.intake().alongWith(scoral.intake()));
}

/**
Expand Down Expand Up @@ -226,15 +212,6 @@ public Command systemsCheck() {
.withName("Test Mechanisms");
}

// drive = Drive.create();
// private final Vision vision = Vision.create();
// private final LEDStrip led = new LEDStrip();
// private final Elevator elevator = Elevator.create();
// private final Scoral scoral = Scoral.create();
// private final Hopper hopper = Hopper.create();
// private final Arm arm = Arm.create();
// private final Coroller coroller = Coroller

@Override
public void close() {
super.close();
Expand Down

0 comments on commit cba9ec1

Please sign in to comment.