Skip to content

Commit

Permalink
Merge branch 'updated-hang' of https://github.com/redshiftrobotics/cr…
Browse files Browse the repository at this point in the history 
…escendo-2024 into updated-hang
  • Loading branch information
@AceiusRedshift
AceiusRedshift committed Mar 2, 2024
2 parents eaf1f38 + 97db7af commit 6f251e1
Show file tree
Hide file tree
Showing 25 changed files with 662 additions and 242 deletions.
56 changes: 55 additions & 1 deletion shuffleboard.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -140,7 +140,7 @@
},
"6,2": {
"size": [
3,
2,
1
],
"content": {
Expand Down Expand Up @@ -208,6 +208,60 @@
"Visuals/Orientation": "HORIZONTAL"
}
},
"7,3": {
"size": [
1,
1
],
"content": {
"_type": "Text View",
"_source0": "network_table:///SmartDashboard/tPoseX",
"_title": "tPoseX",
"_glyph": 110,
"_showGlyph": true
}
},
"6,3": {
"size": [
1,
1
],
"content": {
"_type": "Text View",
"_source0": "network_table:///SmartDashboard/tPoseY",
"_title": "tPoseY",
"_glyph": 111,
"_showGlyph": true
}
},
"8,3": {
"size": [
1,
1
],
"content": {
"_type": "Text View",
"_source0": "network_table:///SmartDashboard/tPoseDegrees",
"_title": "tPoseDegrees",
"_glyph": 419,
"_showGlyph": true
}
},
"8,2": {
"size": [
1,
1
],
"content": {
"_type": "Boolean Box",
"_source0": "network_table:///SmartDashboard/tPoseActive",
"_title": "tPoseActive",
"_glyph": 288,
"_showGlyph": true,
"Colors/Color when true": "#7CFC00FF",
"Colors/Color when false": "#8B0000FF"
}
},
"1,0": {
"size": [
2,
Expand Down
7 changes: 6 additions & 1 deletion simgui-ds.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,7 +91,12 @@
],
"robotJoysticks": [
{
"guid": "Keyboard0"
"guid": "78696e70757401000000000000000000",
"useGamepad": true
},
{
"guid": "78696e70757401000000000000000000",
"useGamepad": true
}
]
}
40 changes: 37 additions & 3 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,6 @@ public static class HangConstants {

public static class DriverConstants {
public static final int DRIVER_JOYSTICK_PORT = 0;

public static final int OPERATOR_JOYSTICK_PORT = 1;

public static final double DEAD_ZONE = 0.25;
Expand Down Expand Up @@ -130,8 +129,28 @@ public static class ArmConstants {
}

public static class IntakeShooterConstants {
static {
switch (currentBot) {
case WOOD_BOT:
HAS_INTAKE = false;
break;

case COMP_BOT:
default:
HAS_INTAKE = true;
break;
}
}

public static final boolean HAS_INTAKE;

public static final boolean INTAKE_REVERSE = false;
public static final boolean FLYWHEEL_REVERSE = false;

public static final int FLYWHEEL_MOTOR_LEFT_ID = 1;
public static final int FLYWHEEL_MOTOR_RIGHT_ID = 2;
public static final int INTAKE_MOTOR_LEFT_ID = 3;
public static final int INTAKE_MOTOR_RIGHT_ID = 4;
}

public static class RobotMovementConstants {
Expand Down Expand Up @@ -282,14 +301,29 @@ public static class SwerveDrivetrainConstants {
public static final double MODULE_LOCATION_X;
}

public static class AutoConstants {
// preffered distance to tag, specifically for autopositioning the robot to in
// front of the tag
public static final double PREFERRED_TAG_DISTANCE = 2;
}

public static class VisionConstants {

public static final Transform3d CAMERA_POSE = new Transform3d(0.5, 0, 0.25, new Rotation3d());
public static final Transform3d CAMERA_POSE = new Transform3d(0, 0, 0, new Rotation3d());
public static final String CAMERA_NAME = "Arducam_OV9281_USB_Camera";
}

public static class LightConstants {
public static final int LED_CONTROLLER_PWM_SLOT = 0;
public static final int LED_CONTROLLER_PWM_SLOT = 1;
public static final int LED_QUANTITY = 60;

public static final double LED_COLOR_RED = 0.61;
public static final double LED_COLOR_ORANGE = 0.65;
public static final double LED_COLOR_YELLOW = 0.69;
public static final double LED_COLOR_GREEN = 0.77;
public static final double LED_COLOR_BLUE = 0.87;
public static final double LED_COLOR_PURPLE = 0.91;
public static final double LED_COLOR_WHITE = 0.93;
public static final double LED_COLOR_RAINBOW = -0.99;
}
}
94 changes: 72 additions & 22 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,11 +2,16 @@

import frc.robot.Constants.DriverConstants;
import frc.robot.Constants.HangConstants;
import frc.robot.Constants.IntakeShooterConstants;
import frc.robot.Constants.LightConstants;
import frc.robot.Constants.SwerveDrivetrainConstants;
import frc.robot.Constants.SwerveModuleConstants;
import frc.robot.Constants.ArmConstants;
import frc.robot.Constants.AutoConstants;
import frc.robot.commands.Autos;
import frc.robot.commands.CancelCommands;
import frc.robot.commands.ChassisRemoteControl;
import frc.robot.commands.FollowTag;
import frc.robot.commands.AimAtTag;
import frc.robot.commands.ArmRotateTo;
import frc.robot.commands.ChassisRemoteControl;
Expand All @@ -17,17 +22,19 @@
import frc.robot.subsystems.SwerveDrivetrain;
import frc.robot.subsystems.SwerveModule;
import frc.robot.subsystems.Vision;
import frc.robot.subsystems.arm.ArmInterface;
import frc.robot.subsystems.arm.Arm;
import frc.robot.subsystems.arm.DummyArm;
import frc.robot.subsystems.arm.RealArm;
import frc.robot.subsystems.hang.DummyHang;
import frc.robot.subsystems.hang.Hang;
import frc.robot.subsystems.hang.RealHang;
import frc.robot.subsystems.intake.DummyShooter;
import frc.robot.subsystems.intake.IntakeShooter;
import frc.robot.subsystems.intake.RealShooter;
import frc.robot.inputs.ChassisDriveInputs;

import com.kauailabs.navx.frc.AHRS;

import edu.wpi.first.wpilibj.AddressableLED;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.GenericHID.HIDType;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
Expand Down Expand Up @@ -86,7 +93,7 @@ public class RobotContainer {
* if ArmConstants.HAS_ARM is false, a dummy class implementing the arm's API is
* created instead to prevent errors.
*/
private final ArmInterface arm = Constants.ArmConstants.HAS_ARM ? new RealArm(
private final Arm arm = Constants.ArmConstants.HAS_ARM ? new RealArm(
ArmConstants.LEFT_MOTOR_ID,
ArmConstants.RIGHT_MOTOR_ID,
ArmConstants.RIGHT_ENCODER_ID,
Expand All @@ -98,14 +105,22 @@ public class RobotContainer {
HangConstants.LIMIT_SWITCH_ID)
: new DummyHang();

private final SwerveDrivetrain drivetrain = new SwerveDrivetrain(gyro, swerveModuleFL, swerveModuleFR,
private final IntakeShooter intakeShooter = Constants.IntakeShooterConstants.HAS_INTAKE ? new RealShooter(
IntakeShooterConstants.FLYWHEEL_MOTOR_LEFT_ID,
IntakeShooterConstants.FLYWHEEL_MOTOR_RIGHT_ID,
IntakeShooterConstants.INTAKE_MOTOR_LEFT_ID,
IntakeShooterConstants.INTAKE_MOTOR_RIGHT_ID) : new DummyShooter();

private final SwerveDrivetrain drivetrain = new SwerveDrivetrain(
gyro,
swerveModuleFL, swerveModuleFR,
swerveModuleBL, swerveModuleBR);

private final SendableChooser<Command> autoChooser = new SendableChooser<Command>();

private final Vision vision = new Vision(VisionConstants.CAMERA_NAME, VisionConstants.CAMERA_POSE);

private final LightStrip lightStrip = new LightStrip(new AddressableLED(LightConstants.LED_CONTROLLER_PWM_SLOT));
private final LightStrip lightStrip = new LightStrip(LightConstants.LED_CONTROLLER_PWM_SLOT);

/**
* The container for the robot. Contains subsystems, OI devices, and commands.
Expand All @@ -114,14 +129,14 @@ public RobotContainer() {
autoChooser.addOption("Rotate by 90", Autos.rotateTestAuto(drivetrain, 90, false));
autoChooser.addOption("Forward", Autos.driveAuto(drivetrain, +1));
autoChooser.addOption("Backward", Autos.driveAuto(drivetrain, -1));
autoChooser.addOption("Make LEDs blue", new SetLightstripColor(lightStrip, 0, 0, 200));
SmartDashboard.putData("Auto Chooser", autoChooser);

SmartDashboard.putString("Bot Name", Constants.currentBot.toString() + " - " + Constants.serialNumber);

configureBindings();

setUpDriveController();
setUpOperatorController();

PortForwarder.add(5800, "photonvision.local", 5800);
}
Expand All @@ -131,16 +146,18 @@ public void setUpDriveController() {
final GenericHID genericHID = new GenericHID(DriverConstants.DRIVER_JOYSTICK_PORT);
final HIDType genericHIDType = genericHID.getType();

SmartDashboard.putString("Drive Controller", genericHIDType.toString());
final Command cancelCommand = new CancelCommands(drivetrain, arm);

drivetrain.removeDefaultCommand();

ChassisDriveInputs inputs;
if (genericHIDType == null) {
SmartDashboard.putString("Drive Ctrl", "No Connection");
} else if (genericHIDType.equals(GenericHID.HIDType.kHIDJoystick)) {
SmartDashboard.putString("Drive Ctrl", "Joystick");

if (genericHIDType.equals(GenericHID.HIDType.kHIDJoystick)) {
final CommandJoystick joystick = new CommandJoystick(genericHID.getPort());

inputs = new ChassisDriveInputs(
ChassisDriveInputs inputs = new ChassisDriveInputs(
joystick::getX, -1,
joystick::getY, -1,
joystick::getTwist, -1,
Expand All @@ -157,10 +174,15 @@ public void setUpDriveController() {
// joystick.button(9).onTrue(Commands.run(drivetrain::brakeMode, drivetrain));
// joystick.button(10).onTrue(Commands.run(drivetrain::toDefaultStates,
// drivetrain));

drivetrain.setDefaultCommand(new ChassisRemoteControl(drivetrain, inputs));

} else {
SmartDashboard.putString("Drive Ctrl", "GamePad");

final CommandXboxController xbox = new CommandXboxController(genericHID.getPort());

inputs = new ChassisDriveInputs(
ChassisDriveInputs inputs = new ChassisDriveInputs(
xbox::getLeftX, -1,
xbox::getLeftY, -1,
xbox::getRightX, -1,
Expand All @@ -170,32 +192,45 @@ public void setUpDriveController() {
// xbox.povLeft().whileTrue(Commands.run(drivetrain::toDefaultStates,
// drivetrain));

xbox.b().onTrue(Commands.runOnce(inputs::decreaseSpeedLevel));
xbox.povDown().onTrue(Commands.runOnce(inputs::decreaseSpeedLevel));
xbox.povUp().onTrue(Commands.runOnce(inputs::increaseSpeedLevel));
xbox.button(3).onTrue(Commands.runOnce(inputs::toggleFieldRelative));
xbox.y().onTrue(Commands.runOnce(inputs::toggleFieldRelative));

xbox.b().onTrue(cancelCommand);

xbox.a().whileTrue(new AimAtTag(drivetrain, vision, 1, inputs));
}
xbox.x().whileTrue(new FollowTag(drivetrain, vision, 1, AutoConstants.PREFERRED_TAG_DISTANCE));

drivetrain.setDefaultCommand(new ChassisRemoteControl(drivetrain, inputs));
drivetrain.setDefaultCommand(new ChassisRemoteControl(drivetrain, inputs));
}
}

public void setUpOperatorController() {
// Create joysticks
final GenericHID genericHID = new GenericHID(DriverConstants.DRIVER_JOYSTICK_PORT);
final GenericHID genericHID = new GenericHID(DriverConstants.OPERATOR_JOYSTICK_PORT);
final HIDType genericHIDType = genericHID.getType();

final ArmRotateTo armToIntake = new ArmRotateTo(arm, ArmConstants.ARM_INTAKE_DEGREES);
final ArmRotateTo armToAmp = new ArmRotateTo(arm, ArmConstants.ARM_AMP_SHOOTING_DEGREES);
final ArmRotateTo armToSpeaker = new ArmRotateTo(arm, ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES);
final Command armToIntake = new ArmRotateTo(arm, ArmConstants.ARM_INTAKE_DEGREES);
final Command armToAmp = new ArmRotateTo(arm, ArmConstants.ARM_AMP_SHOOTING_DEGREES);
final Command armToSpeaker = new ArmRotateTo(arm, ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES);

final Command intake = Commands.runOnce(intakeShooter::intake, intakeShooter);
final Command startFlyWheel = Commands.runOnce(intakeShooter::startFlyWheels, intakeShooter);

final Command amplifyLightSignal = new SetLightstripColor(lightStrip, LightConstants.LED_COLOR_BLUE);
final Command coopLightSignal = new SetLightstripColor(lightStrip, LightConstants.LED_COLOR_RED);

final Command cancelCommand = new CancelCommands(drivetrain, arm, intakeShooter);

final SetHangSpeed hangStop = new SetHangSpeed(hang, 0);
final SetHangSpeed hangForwarSpeed = new SetHangSpeed(hang, HangConstants.speed);
final SetHangSpeed hangBackwardSpeed = new SetHangSpeed(hang, -HangConstants.speed);

SmartDashboard.putString("Operator Controller", genericHIDType.toString());

if (genericHIDType.equals(GenericHID.HIDType.kHIDJoystick)) {
if (genericHIDType == null) {
SmartDashboard.putString("Operator Ctrl", "No Connection");
} else if (genericHIDType.equals(GenericHID.HIDType.kHIDJoystick)) {
SmartDashboard.putString("Operator Ctrl", "Joystick");
final CommandJoystick joystick = new CommandJoystick(genericHID.getPort());

joystick.button(4).onTrue(armToIntake);
Expand All @@ -205,7 +240,11 @@ public void setUpOperatorController() {
joystick.button(7).or(joystick.button(8)).whileFalse(hangStop);
joystick.button(7).onTrue(hangForwarSpeed);
joystick.button(8).onTrue(hangBackwardSpeed);

joystick.button(9).onTrue(amplifyLightSignal);
joystick.button(10).onTrue(coopLightSignal);
} else {
SmartDashboard.putString("Operator Ctrl", "GamePad");
final CommandXboxController xbox = new CommandXboxController(genericHID.getPort());

xbox.povDown().onTrue(armToIntake);
Expand All @@ -216,6 +255,17 @@ public void setUpOperatorController() {
xbox.rightBumper().onTrue(hangForwarSpeed);
xbox.leftBumper().onTrue(hangBackwardSpeed);

xbox.leftTrigger().onTrue(armToSpeaker);
xbox.leftBumper().onTrue(startFlyWheel);

xbox.rightTrigger().onTrue(armToIntake);
xbox.rightBumper().onTrue(intake);

xbox.a().onTrue(armToAmp);

xbox.povLeft().onTrue(amplifyLightSignal);
xbox.povRight().onTrue(coopLightSignal);
xbox.b().onTrue(cancelCommand);
}
}

Expand All @@ -231,4 +281,4 @@ private void configureBindings() {
public Command getAutonomousCommand() {
return autoChooser.getSelected();
}
}
};
Loading

0 comments on commit 6f251e1

Please sign in to comment.