Skip to content

Commit

Permalink
Add flywheel to robot container
Browse files Browse the repository at this point in the history
  • Loading branch information
@MichaelLesirge
MichaelLesirge committed Mar 2, 2024
1 parent 62815f9 commit 66498d5
Show file tree
Hide file tree
Showing 4 changed files with 76 additions and 32 deletions.
16 changes: 11 additions & 5 deletions src/main/java/frc/robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,7 +89,7 @@ public static class ArmConstants {
public static final double ARM_INTAKE_DEGREES = -40;

public static final int LEFT_MOTOR_ID = 5;
public static final int RIGHT_MOTOR_ID = 19;
public static final int RIGHT_MOTOR_ID = 19;
public static final int RIGHT_ENCODER_ID = 6;

public static final boolean ARE_MOTORS_REVERSED = false;
Expand All @@ -102,8 +102,13 @@ public static class ArmConstants {
}

public static class IntakeShooterConstants {
public static final boolean INTAKE_REVERSE = false;
public static final boolean FLYWHEEL_REVERSE = false;
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 @@ -255,8 +260,9 @@ public static class SwerveDrivetrainConstants {
}

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;
// 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 {
Expand Down
50 changes: 38 additions & 12 deletions src/main/java/frc/robot/RobotContainer.java
View file
Original file line number Diff line number Diff line change
@@ -1,17 +1,21 @@
package frc.robot;

import frc.robot.Constants.DriverConstants;
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.SetLightstripColor;
import frc.robot.Constants.VisionConstants;
import frc.robot.subsystems.IntakeShooter;
import frc.robot.subsystems.LightStrip;
import frc.robot.subsystems.SwerveDrivetrain;
import frc.robot.subsystems.SwerveModule;
Expand Down Expand Up @@ -87,10 +91,16 @@ public class RobotContainer {
ArmConstants.RIGHT_ENCODER_ID,
ArmConstants.ARE_MOTORS_REVERSED) : new DummyArm();

private final IntakeShooter intakeShooter = new IntakeShooter(
IntakeShooterConstants.FLYWHEEL_MOTOR_LEFT_ID,
IntakeShooterConstants.FLYWHEEL_MOTOR_RIGHT_ID,
IntakeShooterConstants.INTAKE_MOTOR_LEFT_ID,
IntakeShooterConstants.INTAKE_MOTOR_RIGHT_ID);

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

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

Expand Down Expand Up @@ -122,6 +132,8 @@ public void setUpDriveController() {
final GenericHID genericHID = new GenericHID(DriverConstants.DRIVER_JOYSTICK_PORT);
final HIDType genericHIDType = genericHID.getType();

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

drivetrain.removeDefaultCommand();

if (genericHIDType == null) {
Expand Down Expand Up @@ -166,12 +178,14 @@ 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.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));
}
Expand All @@ -186,7 +200,13 @@ public void setUpOperatorController() {
final Command armToAmp = new ArmRotateTo(arm, ArmConstants.ARM_AMP_SHOOTING_DEGREES);
final Command armToSpeaker = new ArmRotateTo(arm, ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES);

final Command cancelCommand = new CancelCommands(drivetrain, arm);
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);

if (genericHIDType == null) {
SmartDashboard.putString("Operator Ctrl", "No Connection");
Expand All @@ -199,15 +219,22 @@ public void setUpOperatorController() {
joystick.button(5).onTrue(armToAmp);
joystick.button(6).onTrue(armToSpeaker);

joystick.button(7).onTrue(new SetLightstripColor(lightStrip, LightConstants.LED_COLOR_BLUE));
joystick.button(8).onTrue(new SetLightstripColor(lightStrip, LightConstants.LED_COLOR_RED));
joystick.button(7).onTrue(amplifyLightSignal);
joystick.button(8).onTrue(coopLightSignal);
} else {
SmartDashboard.putString("Operator Ctrl", "GamePad");
final CommandXboxController xbox = new CommandXboxController(genericHID.getPort());

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

xbox.rightTrigger().onTrue(armToIntake);
xbox.leftTrigger(5).onTrue(armToSpeaker);
xbox.povDown().onTrue(armToAmp);
xbox.rightBumper().onTrue(intake);

xbox.a().onTrue(armToAmp);

xbox.povLeft().onTrue(amplifyLightSignal);
xbox.povRight().onTrue(coopLightSignal);

xbox.b().onTrue(cancelCommand);
}
Expand All @@ -225,5 +252,4 @@ private void configureBindings() {
public Command getAutonomousCommand() {
return autoChooser.getSelected();
}
}
;
};
17 changes: 12 additions & 5 deletions src/main/java/frc/robot/commands/FollowTag.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@ public class FollowTag extends Command {
* @param loseTagAfterSeconds how long to wait before giving up on rediscover
* tag, set to null to never finish
*/
public FollowTag(SwerveDrivetrain drivetrain, Vision vision, Transform2d targetDistanceToTag, int tagID) {
public FollowTag(SwerveDrivetrain drivetrain, Vision vision, int tagID, Transform2d targetDistanceToTag) {
this.drivetrain = drivetrain;

this.vision = vision;
Expand All @@ -53,12 +53,12 @@ public FollowTag(SwerveDrivetrain drivetrain, Vision vision, Transform2d targetD
addRequirements(drivetrain);
}

public FollowTag(SwerveDrivetrain drivetrain, Vision vision, Translation2d targetDistanceToTag, int tagID) {
this(drivetrain, vision, new Transform2d(targetDistanceToTag, new Rotation2d()), tagID);
public FollowTag(SwerveDrivetrain drivetrain, Vision vision, int tagID, Translation2d targetDistanceToTag) {
this(drivetrain, vision, tagID, new Transform2d(targetDistanceToTag, new Rotation2d()));
}

public FollowTag(SwerveDrivetrain drivetrain, Vision vision, double targetDistanceToTag, int tagID) {
this(drivetrain, vision, new Transform2d(new Translation2d(targetDistanceToTag, 0), new Rotation2d()), tagID);
public FollowTag(SwerveDrivetrain drivetrain, Vision vision, int tagID, double targetDistanceToTag) {
this(drivetrain, vision, tagID, new Transform2d(new Translation2d(targetDistanceToTag, 0), new Rotation2d()));
}

@Override
Expand All @@ -84,6 +84,13 @@ public void execute() {
rotationSpeed = -rotationController.calculate(rotation.getRadians());
}

double speedLimit = RobotMovementConstants.MAX_TRANSLATION_SPEED;
double maxSpeed = Math.max(Math.abs(forwardSpeed), Math.abs(leftSpeed));
if (maxSpeed > speedLimit) {
forwardSpeed = (forwardSpeed / maxSpeed) * speedLimit;
leftSpeed = (leftSpeed / maxSpeed) * speedLimit;
}

drivetrain.setDesiredState(new ChassisSpeeds(forwardSpeed, leftSpeed, rotationSpeed));
drivetrain.updateSmartDashboard();
}
Expand Down
25 changes: 15 additions & 10 deletions src/main/java/frc/robot/subsystems/IntakeShooter.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,29 +43,34 @@ public IntakeShooter(int flywheel1id, int flywheel2id, int intake1id, int intake
intake2.setInverted(IntakeShooterConstants.INTAKE_REVERSE);
}

public void setFlyWheelSpeed(double speed) {
flywheel1.set(speed);
flywheel2.set(speed);
}

public void startFlyWheels() {
flywheel1.set(1);
flywheel2.set(1);
setFlyWheelSpeed(1);
}

public void stopFlywheels() {
flywheel1.stopMotor();
flywheel2.stopMotor();
setFlyWheelSpeed(0);
}

public void setIntakeSpeed(double speed) {
intake1.set(speed);
intake2.set(speed);
}

public void intake() {
intake1.set(1);
intake1.set(1);
setIntakeSpeed(1);
}

public void intakeReverse() {
intake1.set(-1);
intake1.set(-1);
setIntakeSpeed(-1);
}

public void stopIntake() {
intake1.stopMotor();
intake1.stopMotor();
setIntakeSpeed(0);
}

}

0 comments on commit 66498d5

Please sign in to comment.