Skip to content
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

Jump to bottom

Marty lives #208

Merged
merged 1 commit into from
Mar 23, 2024
Merged

Marty lives #208

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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 src/main/java/frc/lib/util/swerve/SwerveModuleReal.java
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading