Skip to content

Commit

Permalink
Merge branch 'main' into GearFox-ReleaseCleanup
Browse files Browse the repository at this point in the history
  • Loading branch information
GearBoxFox authored Jun 12, 2023
2 parents 45c01cb + ce74f6e commit 96a319d
Show file tree
Hide file tree
Showing 7 changed files with 156 additions and 20 deletions.
13 changes: 4 additions & 9 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
import frc.robot.subsystems.LedSubsystem;
import frc.robot.subsystems.arm.ArmAngleSubsystem;
import frc.robot.subsystems.swerve.SwerveDrivetrain;
import frc.robot.subsystems.wrist.WristSubsystem;
Expand All @@ -44,7 +45,7 @@ public class RobotContainer {
private ArmExtSubsystem m_ext;
private ArmSupersystem m_super;
private FootPedal m_foot;
private CANdle m_candle;
private LedSubsystem m_led;

//Controllers
private final CommandXboxController m_driveController = new CommandXboxController(Constants.DRIVER_PORT);
Expand All @@ -60,7 +61,7 @@ public RobotContainer() {
switch (Constants.CURRENT_MODE) {
// Beta robot hardware implementation
case HELIOS_V2:
m_candle = new CANdle(22);
m_led = new LedSubsystem(22);
// m_drive = new SwerveDrivetrain();
// break;
case HELIOS_V1:
Expand Down Expand Up @@ -96,13 +97,7 @@ public RobotContainer() {
* edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {
if (m_candle != null) {
if (DriverStation.getAlliance() == Alliance.Red) {
m_candle.animate(new ColorFlowAnimation(255, 0, 0));
} else {
m_candle.animate(new ColorFlowAnimation(0, 0, 255));
}
}
m_led.setDefaultCommand(new LEDControllerCommand(m_led));

m_drive.setDefaultCommand(new SwerveTeleopDrive(m_drive, m_driveController));
m_arm.setDefaultCommand(new HoldArmAngleCommand(m_arm));
Expand Down
47 changes: 47 additions & 0 deletions src/main/java/frc/robot/commands/LEDControllerCommand.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,47 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

package frc.robot.commands;

import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc.robot.subsystems.LedSubsystem;

public class LEDControllerCommand extends CommandBase {
/** Creates a new LEDControllerCommand. */
LedSubsystem m_led;
Timer m_timer;
public LEDControllerCommand(LedSubsystem led) {
// Use addRequirements() here to declare subsystem dependencies.
m_led = led;
m_timer = new Timer();
addRequirements(led);
}

// Called when the command is initially scheduled.
@Override
public void initialize() {
m_timer.reset();
m_timer.start();
}

// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
if (m_timer.hasElapsed(10)) {
m_led.nextAnimation();
m_timer.restart();
}
}

// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}

// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
3 changes: 2 additions & 1 deletion src/main/java/frc/robot/commands/SwerveTeleopDrive.java
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,8 @@ public void initialize() {}
public void execute() {
double x = -m_driverController.getLeftY();
double y = -m_driverController.getLeftX();
double z = -m_driverController.getRightX() / 1.5;
double z = -m_driverController.getRightX();// / 1.8;


if (m_drive.getSlowmode()) {
x *= 0.5;
Expand Down
95 changes: 95 additions & 0 deletions src/main/java/frc/robot/subsystems/LedSubsystem.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,95 @@
package frc.robot.subsystems;

import com.ctre.phoenix.led.Animation;
import com.ctre.phoenix.led.CANdle;
import com.ctre.phoenix.led.ColorFlowAnimation;
import com.ctre.phoenix.led.FireAnimation;
import com.ctre.phoenix.led.LarsonAnimation;
import com.ctre.phoenix.led.RainbowAnimation;
import com.ctre.phoenix.led.ColorFlowAnimation.Direction;
import com.ctre.phoenix.led.LarsonAnimation.BounceMode;

import edu.wpi.first.wpilibj2.command.SubsystemBase;

public class LedSubsystem extends SubsystemBase {
private final CANdle m_candle;
private final int m_numLED = 400;
private Animations m_currentAnimation = Animations.RAINBOW;

private final Animation m_rainbow = new RainbowAnimation(1.0, 0.5, m_numLED);
private final Animation m_red = new ColorFlowAnimation(255, 0, 0, 0, 0.5, m_numLED, Direction.Forward);
private final Animation m_blue = new ColorFlowAnimation(0, 255, 0, 0, 0.5, m_numLED, Direction.Forward);
private final Animation m_fire = new FireAnimation(1.0, 0.5, m_numLED, 0.25, 0.25);
private final Animation m_larson = new LarsonAnimation(255, 0, 0, 0, 0.5, m_numLED, BounceMode.Front, 20);

public enum Animations {
RAINBOW,
RED,
BLUE,
FIRE,
LARSON
}

public LedSubsystem(int id) {
m_candle = new CANdle(id);
m_candle.animate(m_rainbow);
}

public void nextAnimation() {
switch (m_currentAnimation) {
case BLUE:
m_currentAnimation = Animations.FIRE;
break;
case FIRE:
m_currentAnimation = Animations.LARSON;
break;
case LARSON:
m_currentAnimation = Animations.RAINBOW;
break;
case RAINBOW:
m_currentAnimation = Animations.RED;
break;
case RED:
m_currentAnimation = Animations.BLUE;
break;
default:
m_currentAnimation = Animations.RAINBOW;
break;

}
}

public void setAnimation(Animations animation) {
m_currentAnimation = animation;
}

public Animations getAnimation() {
return m_currentAnimation;
}

@Override
public void periodic() {
switch(m_currentAnimation){
case BLUE:
m_candle.animate(m_blue);
break;
case FIRE:
m_candle.animate(m_fire);
break;
case LARSON:
m_candle.animate(m_larson);
break;
case RAINBOW:
m_candle.animate(m_rainbow);
break;
case RED:
m_candle.animate(m_red);
break;
default:
m_candle.setLEDs(255, 0, 255);
break;

}
}

}
10 changes: 1 addition & 9 deletions src/main/java/frc/robot/subsystems/arm/ArmExtSubsystem.java
Original file line number Diff line number Diff line change
Expand Up @@ -61,15 +61,7 @@ public ArmExtSubsystem() {
m_extPID.setP(Constants.ArmConstants.ARM_EXT_KP.getValue());
m_extPID.setI(Constants.ArmConstants.ARM_EXT_KI.getValue());
m_extPID.setD(Constants.ArmConstants.ARM_EXT_KD.getValue());
m_extPID.setOutputRange(-1, 1
);

// m_extPID = new PIDController(
// Constants.ArmConstants.ARM_EXT_KP.getValue(),
// Constants.ArmConstants.ARM_EXT_KI.getValue(),
// Constants.ArmConstants.ARM_EXT_KD.getValue());
// m_extPID.setTolerance(0.5);
// m_extPID.disableContinuousInput();
m_extPID.setOutputRange(-1, 1);

m_armLimitSwitch = new DigitalInput(Constants.ArmConstants.LIMIT_SWITCH_PORT);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -235,10 +235,16 @@ public void setModuleStates(SwerveModuleState[] states) {
state.speedMetersPerSecond *= m_speedMult;
}


m_flMod.setDesiredState(states[0]);
m_frMod.setDesiredState(states[1]);
m_blMod.setDesiredState(states[2]);
m_brMod.setDesiredState(states[3]);

SmartDashboard.putNumber("FL Desired Speed", states[0].speedMetersPerSecond);
SmartDashboard.putNumber("FR Desired Speed", states[1].speedMetersPerSecond);
SmartDashboard.putNumber("BL Desired Speed", states[2].speedMetersPerSecond);
SmartDashboard.putNumber("BR Desired Speed", states[3].speedMetersPerSecond);
}

public void resetGyro(double heading) {
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/frc/robot/supersystems/ArmSupersystem.java
Original file line number Diff line number Diff line change
Expand Up @@ -190,7 +190,7 @@ public void getDriveSpeed () {
m_swerve.setSpeedMult(range.doubleValue());

if (m_ext.getArmExtension() > 5) {
m_swerve.setRotationMult(0.5);
m_swerve.setRotationMult(0.75);
} else {
m_swerve.setRotationMult(1);
}
Expand Down

0 comments on commit 96a319d

Please sign in to comment.