Skip to content
This repository has been archived by the owner on Nov 2, 2024. It is now read-only.

Commit

Permalink
Marty lives
Browse files Browse the repository at this point in the history
  • Loading branch information
@owenflatman
owenflatman committed Mar 23, 2024
1 parent e818549 commit ce912fc
Show file tree
Hide file tree
Showing 4 changed files with 40 additions and 61 deletions.
6 changes: 3 additions & 3 deletions src/main/java/frc/lib/util/swerve/SwerveModuleReal.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,9 +31,9 @@ public class SwerveModuleReal implements SwerveModuleIO {
public SwerveModuleReal(int moduleNumber, int driveMotorID, int angleMotorID, int cancoderID,
Rotation2d angleOffset) {

angleEncoder = new CANcoder(cancoderID, "canivore");
mDriveMotor = new TalonFX(driveMotorID, "canivore");
mAngleMotor = new TalonFX(angleMotorID, "canivore");
angleEncoder = new CANcoder(cancoderID);
mDriveMotor = new TalonFX(driveMotorID);
mAngleMotor = new TalonFX(angleMotorID);

configAngleEncoder();
configAngleMotor();
Expand Down
8 changes: 4 additions & 4 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -148,7 +148,7 @@ public static final class Mod0 {
public static final int DRIVE_MOTOR_ID = 6;
public static final int ANGLE_MOTOR_ID = 8;
public static final int CAN_CODER_ID = 4;
public static final Rotation2d ANGLE_OFFSET = Rotation2d.fromDegrees(138.604);
public static final Rotation2d ANGLE_OFFSET = Rotation2d.fromRotations(-0.112549 + 0.5);
}

/**
Expand All @@ -158,7 +158,7 @@ public static final class Mod1 {
public static final int DRIVE_MOTOR_ID = 1;
public static final int ANGLE_MOTOR_ID = 4;
public static final int CAN_CODER_ID = 1;
public static final Rotation2d ANGLE_OFFSET = Rotation2d.fromDegrees(280.107);
public static final Rotation2d ANGLE_OFFSET = Rotation2d.fromRotations(0.277832 + 0.5);
}

/**
Expand All @@ -168,7 +168,7 @@ public static final class Mod2 {
public static final int DRIVE_MOTOR_ID = 3;
public static final int ANGLE_MOTOR_ID = 2;
public static final int CAN_CODER_ID = 2;
public static final Rotation2d ANGLE_OFFSET = Rotation2d.fromDegrees(121.553);
public static final Rotation2d ANGLE_OFFSET = Rotation2d.fromRotations(-0.168213 + 0.5);
}

/**
Expand All @@ -178,7 +178,7 @@ public static final class Mod3 {
public static final int DRIVE_MOTOR_ID = 7;
public static final int ANGLE_MOTOR_ID = 5;
public static final int CAN_CODER_ID = 3;
public static final Rotation2d ANGLE_OFFSET = Rotation2d.fromDegrees(248.027);
public static final Rotation2d ANGLE_OFFSET = Rotation2d.fromRotations(0.213623 + 0.5);
}

public static final HolonomicPathFollowerConfig pathFollowerConfig =
Expand Down
78 changes: 32 additions & 46 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,6 @@
import com.pathplanner.lib.commands.PathPlannerAuto;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.networktables.GenericEntry;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.PneumaticHub;
import edu.wpi.first.wpilibj.XboxController;
Expand All @@ -20,28 +19,15 @@
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.WaitCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.lib.util.DisabledInstantCommand;
import frc.lib.util.Scoring;
import frc.lib.util.Scoring.GamePiece;
import frc.robot.Robot.RobotRunType;
import frc.robot.autos.PPExample;
import frc.robot.commands.arm.ConeIntake;
import frc.robot.commands.arm.ConeUpIntake;
import frc.robot.commands.arm.CubeIntake;
import frc.robot.commands.arm.DockArm;
import frc.robot.commands.arm.ScoreArm;
import frc.robot.commands.arm.VariableArm;
import frc.robot.commands.drive.MoveToEngage;
import frc.robot.commands.drive.MoveToScore;
import frc.robot.commands.drive.TeleopSwerve;
import frc.robot.commands.leds.FlashingLEDColor;
import frc.robot.commands.leds.MovingColorLEDs;
import frc.robot.commands.leds.PoliceLEDs;
import frc.robot.commands.wrist.VariableIntake;
import frc.robot.subsystems.Arm;
import frc.robot.subsystems.LEDs;
import frc.robot.subsystems.WristIntake;
import frc.robot.subsystems.swerve.Swerve;
import frc.robot.subsystems.swerve.SwerveIO;
import frc.robot.subsystems.swerve.SwerveReal;
Expand Down Expand Up @@ -119,8 +105,8 @@ public class RobotContainer {
private LEDs leds = new LEDs(Constants.LEDConstants.LED_COUNT, Constants.LEDConstants.PWM_PORT);
public Swerve s_Swerve;
// private final DropIntake s_dIntake = new DropIntake();
private final Arm s_Arm = new Arm(ph);
private final WristIntake s_wristIntake = new WristIntake();
// private final Arm s_Arm = new Arm(ph);
// private final WristIntake s_wristIntake = new WristIntake();


// public GenericEntry photonSeeing = mainDriverTab
Expand All @@ -147,8 +133,8 @@ public RobotContainer(RobotRunType runtimeType) {

ph.enableCompressorAnalog(90, 120);
s_Swerve.setDefaultCommand(new TeleopSwerve(s_Swerve, driver,
Constants.Swerve.IS_FIELD_RELATIVE, Constants.Swerve.IS_OPEN_LOOP, s_Arm));
s_wristIntake.setDefaultCommand(new VariableIntake(s_wristIntake, operator));
Constants.Swerve.IS_FIELD_RELATIVE, Constants.Swerve.IS_OPEN_LOOP));
// s_wristIntake.setDefaultCommand(new VariableIntake(s_wristIntake, operator));
leds.setDefaultCommand(new MovingColorLEDs(leds, Color.kRed, 8, false));
autoChooser.setDefaultOption("Example Path", "example");
autoChooser.addOption("Example Auto", "auto");
Expand Down Expand Up @@ -183,37 +169,37 @@ public RobotContainer(RobotRunType runtimeType) {
*/
private void configureButtonBindings() {

Trigger armSensor =
new Trigger(() -> this.s_Arm.armSense.get() && DriverStation.isDisabled());
// Trigger armSensor =
// new Trigger(() -> this.s_Arm.armSense.get() && DriverStation.isDisabled());

armSensor.onTrue(new DisabledInstantCommand(() -> s_Arm.setCoastMode()));
// armSensor.onTrue(new DisabledInstantCommand(() -> s_Arm.setCoastMode()));

armSensor.onFalse(new DisabledInstantCommand(() -> s_Arm.setBrakeMode()));
// armSensor.onFalse(new DisabledInstantCommand(() -> s_Arm.setBrakeMode()));

/* Driver Buttons */
driver.start().onTrue(new InstantCommand(() -> s_Swerve.resetInitialized()));

driver.y().onTrue(new DisabledInstantCommand(() -> s_Swerve.resetFieldRelativeOffset()));
driver.rightTrigger().and(driver.leftTrigger()).and(operator.start())
.whileTrue(new MoveToScore(s_Swerve, s_Arm, s_wristIntake));
// driver.rightTrigger().and(driver.leftTrigger()).and(operator.start())
// .whileTrue(new MoveToScore(s_Swerve, s_Arm, s_wristIntake));
driver.rightTrigger().and(driver.leftTrigger())
.onTrue(new InstantCommand(() -> s_Swerve.resetInitialized()));
driver.rightBumper().and(driver.leftBumper())
.whileTrue(new MoveToEngage(s_Swerve, s_Arm, s_wristIntake));
// driver.rightBumper().and(driver.leftBumper())
// .whileTrue(new MoveToEngage(s_Swerve, s_Arm, s_wristIntake));
driver.start()
.whileTrue(new InstantCommand(() -> s_Swerve.wheelsIn(), s_Swerve).repeatedly());

/* Operator Buttons */
operator.leftBumper().onTrue(new FlashingLEDColor(leds, Color.kYellow).withTimeout(15.0));
operator.rightBumper().onTrue(new FlashingLEDColor(leds, Color.kPurple).withTimeout(15.0));

operator.b().onTrue(new ConeIntake(s_Arm)
.alongWith(new InstantCommand(() -> s_wristIntake.setInvert(true))));
operator.a().onTrue(new CubeIntake(s_Arm)
.alongWith(new InstantCommand(() -> s_wristIntake.setInvert(false))));
operator.x().onTrue(new ConeUpIntake(s_Arm)
.alongWith(new InstantCommand(() -> s_wristIntake.setInvert(true))));
operator.y().onTrue(new DockArm(s_Arm, s_wristIntake).withTimeout(.1).repeatedly());
// operator.b().onTrue(new ConeIntake(s_Arm)
// .alongWith(new InstantCommand(() -> s_wristIntake.setInvert(true))));
// operator.a().onTrue(new CubeIntake(s_Arm)
// .alongWith(new InstantCommand(() -> s_wristIntake.setInvert(false))));
// operator.x().onTrue(new ConeUpIntake(s_Arm)
// .alongWith(new InstantCommand(() -> s_wristIntake.setInvert(true))));
// operator.y().onTrue(new DockArm(s_Arm, s_wristIntake).withTimeout(.1).repeatedly());

operator.povUp().onTrue(
new DisabledInstantCommand(() -> Robot.level = MathUtil.clamp(Robot.level + 1, 0, 2)));
Expand All @@ -225,19 +211,19 @@ private void configureButtonBindings() {
Robot.level = MathUtil.clamp(Robot.level - 1, 0, 2);
}
}));
operator.povRight().onTrue(new DisabledInstantCommand(() -> {
Robot.column = MathUtil.clamp(Robot.column + 1, 0, 8);
s_wristIntake.setInvert(Scoring.getGamePiece() == Scoring.GamePiece.CONE);
}));
operator.povLeft().onTrue(new DisabledInstantCommand(() -> {
Robot.column = MathUtil.clamp(Robot.column - 1, 0, 8);
s_wristIntake.setInvert(Scoring.getGamePiece() == Scoring.GamePiece.CONE);
}));
operator.rightTrigger().and(operator.leftTrigger())
.whileTrue(new ScoreArm(s_Arm, s_wristIntake));
operator.back().toggleOnTrue(new PoliceLEDs(leds));
new Trigger(() -> Math.abs(operator.getRightY()) > 0.2)
.whileTrue(new VariableArm(s_Arm, operator));
// operator.povRight().onTrue(new DisabledInstantCommand(() -> {
// Robot.column = MathUtil.clamp(Robot.column + 1, 0, 8);
// s_wristIntake.setInvert(Scoring.getGamePiece() == Scoring.GamePiece.CONE);
// }));
// operator.povLeft().onTrue(new DisabledInstantCommand(() -> {
// Robot.column = MathUtil.clamp(Robot.column - 1, 0, 8);
// s_wristIntake.setInvert(Scoring.getGamePiece() == Scoring.GamePiece.CONE);
// }));
// operator.rightTrigger().and(operator.leftTrigger())
// .whileTrue(new ScoreArm(s_Arm, s_wristIntake));
// operator.back().toggleOnTrue(new PoliceLEDs(leds));
// new Trigger(() -> Math.abs(operator.getRightY()) > 0.2)
// .whileTrue(new VariableArm(s_Arm, operator));

// operator.povUp().whileTrue(new MoveArm(s_Arm, 110, 0));
// operator.povDown().whileTrue(new MoveArm(s_Arm, 45, 0));
Expand Down
9 changes: 1 addition & 8 deletions src/main/java/frc/robot/commands/drive/TeleopSwerve.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,6 @@
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.lib.math.Conversions;
import frc.robot.Constants;
import frc.robot.subsystems.Arm;
import frc.robot.subsystems.swerve.Swerve;

/**
Expand All @@ -18,7 +17,6 @@ public class TeleopSwerve extends Command {
private boolean openLoop;
private Swerve swerveDrive;
private CommandXboxController controller;
private Arm arm;

/**
* Creates an command for driving the swerve drive during tele-op
Expand All @@ -28,13 +26,12 @@ public class TeleopSwerve extends Command {
* @param openLoop Open or closed loop system
*/
public TeleopSwerve(Swerve swerveDrive, CommandXboxController controller, boolean fieldRelative,
boolean openLoop, Arm arm) {
boolean openLoop) {
this.swerveDrive = swerveDrive;
addRequirements(swerveDrive);
this.fieldRelative = fieldRelative;
this.openLoop = openLoop;
this.controller = controller;
this.arm = arm;
}

@Override
Expand All @@ -52,10 +49,6 @@ public void execute() {

double angle_speed = Constants.Swerve.MAX_ANGULAR_VELOCITY;
double speed = Constants.Swerve.MAX_SPEED;
if (arm.getArmAngle() > -70) {
angle_speed /= 3;
speed *= 0.80;
}

Translation2d translation = new Translation2d(yaxis, xaxis).times(speed);
double rotation = raxis * angle_speed;
Expand Down

0 comments on commit ce912fc

Please sign in to comment.