diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d35d451..307216d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -17,6 +17,17 @@ */ public final class Constants { + public static final double kElevatorP = 0; + public static final double kElevatorI = 0; + public static final double kElevatorD = 0; + public static final int kElevatorHoldVoltage = 0; + public static final double kClimbThreshold = 100; + + public static final double kClimbHeight = 0; + public static final double kClimbZero = 0; + public static final double kLevelP = 0; + public static final int kClimbTicks = 0; + public static double kFlywheelP = 0.07; public static double kFlywheelD = 0; public static double kFlywheelFF = .0465; diff --git a/src/main/java/frc/robot/Hardware.java b/src/main/java/frc/robot/Hardware.java index 7329613..c471ddb 100644 --- a/src/main/java/frc/robot/Hardware.java +++ b/src/main/java/frc/robot/Hardware.java @@ -8,15 +8,17 @@ package frc.robot; import com.ctre.phoenix.motorcontrol.can.TalonFX; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.kauailabs.navx.frc.AHRS; - import frc.robot.util.Limelight; /** * Add your docs here. */ public class Hardware { - + public static TalonFX elevatorMaster; + public static TalonSRX levelMotor; + public static TalonFX flywheelMaster; public static TalonFX flywheelFollower; diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 9b5a8e1..221e82b 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -9,6 +9,9 @@ import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import frc.robot.commands.ClimbAndLevelCommand; +import frc.robot.commands.RaiseElevatorCommand; +import frc.robot.commands.ZeroElevator; import frc.robot.commands.SetFlywheelRPM; import frc.robot.commands.AutoAlign; import frc.robot.commands.DriveWithJoystick; @@ -19,6 +22,24 @@ */ public class OI { + + private Joystick operatorJoystick; + private JoystickButton raiseElevatorButton; + private JoystickButton climbButton; + private JoystickButton zeroElevatorButton; + + public OI(){ + operatorJoystick = new Joystick(1); + + raiseElevatorButton = new JoystickButton(operatorJoystick, 1); + climbButton = new JoystickButton(operatorJoystick, 2); + zeroElevatorButton = new JoystickButton(operatorJoystick, 3); + + raiseElevatorButton.whenPressed(new RaiseElevatorCommand()); + climbButton.whenPressed(new ClimbAndLevelCommand()); + zeroElevatorButton.whenActive(new ZeroElevator()); + } + private Joystick driverJoystick; private JoystickButton shootBall; private JoystickButton stopFlywheel; @@ -27,6 +48,11 @@ public class OI { private JoystickButton align; private JoystickButton quickTurnButton; + private JoystickButton raiseElevatorButton; + private JoystickButton climbButton; + private JoystickButton zeroElevatorButton; + + public OI(){ driverJoystick = new Joystick(0); shootBall = new JoystickButton(driverJoystick, 1); @@ -41,6 +67,14 @@ public OI(){ shootBall.whenPressed(new SetFlywheelRPM(3250)); stopFlywheel.whenPressed(new SetFlywheelRPM(0)); + + raiseElevatorButton = new JoystickButton(operatorJoystick, 1); + climbButton = new JoystickButton(operatorJoystick, 2); + zeroElevatorButton = new JoystickButton(operatorJoystick, 3); + + raiseElevatorButton.whenPressed(new RaiseElevatorCommand()); + climbButton.whenPressed(new ClimbAndLevelCommand()); + zeroElevatorButton.whenActive(new ZeroElevator()); } public double getWheel() diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index abbae21..0ac951f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.subsystems.Climber; import frc.robot.subsystems.Flywheel; import java.util.ArrayList; @@ -49,6 +50,10 @@ public class Robot extends TimedRobot { //private RobotContainer m_robotContainer; + + public static Climber climber; + public static OI oi; + public enum RobotState{ DISABLED, TELEOP, AUTON }; @@ -72,6 +77,8 @@ public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. + climber = new Climber(); + oi = new OI(); //m_robotContainer = new RobotContainer(); } @@ -114,8 +121,6 @@ public void disabledPeriodic() { */ @Override public void autonomousInit() { - //m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - drivetrain.setPathDirection(false); Trajectory traj1 = TrajectoryGenerator.generateTrajectory(List.of( new Pose2d(), diff --git a/src/main/java/frc/robot/commands/ClimbAndLevelCommand.java b/src/main/java/frc/robot/commands/ClimbAndLevelCommand.java new file mode 100644 index 0000000..2d14cfc --- /dev/null +++ b/src/main/java/frc/robot/commands/ClimbAndLevelCommand.java @@ -0,0 +1,44 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Robot; +import frc.robot.subsystems.Climber.ElevatorState; + +public class ClimbAndLevelCommand extends CommandBase { + /** + * Creates a new ClimbAndLevelCommand. + */ + public ClimbAndLevelCommand() { + addRequirements(Robot.climber); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + Robot.climber.setElevatorState(ElevatorState.PULLING); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // 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 true; + } +} diff --git a/src/main/java/frc/robot/commands/RaiseElevatorCommand.java b/src/main/java/frc/robot/commands/RaiseElevatorCommand.java new file mode 100644 index 0000000..9be4a79 --- /dev/null +++ b/src/main/java/frc/robot/commands/RaiseElevatorCommand.java @@ -0,0 +1,44 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Robot; +import frc.robot.subsystems.Climber.ElevatorState; + +public class RaiseElevatorCommand extends CommandBase { + /** + * Creates a new RaiseElevator. + */ + public RaiseElevatorCommand() { + addRequirements(Robot.climber); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + Robot.climber.setElevatorState(ElevatorState.SETPOINT); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // 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 true; + } +} diff --git a/src/main/java/frc/robot/commands/ZeroElevator.java b/src/main/java/frc/robot/commands/ZeroElevator.java new file mode 100644 index 0000000..54aea1a --- /dev/null +++ b/src/main/java/frc/robot/commands/ZeroElevator.java @@ -0,0 +1,43 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Robot; +import frc.robot.subsystems.Climber.ElevatorState; + +public class ZeroElevator extends CommandBase { + /** + * Creates a new ZeroElevator. + */ + public ZeroElevator() { + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + Robot.climber.setElevatorState(ElevatorState.ZEROING); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // 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; + } +} diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java new file mode 100644 index 0000000..588569c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -0,0 +1,107 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.DemandType; +import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice; +import com.ctre.phoenix.motorcontrol.can.TalonFX; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.Hardware; + +public class Climber extends SubsystemBase { + /** + * Creates a new Elevator. + */ + + private ElevatorState currState; + private double angleError; + + public enum ElevatorState { + PULLING, SETPOINT, HOLD, ZEROED, LEVEL, ZEROING + }; + + public Climber() { + + Hardware.elevatorMaster = new TalonFX(0); + Hardware.levelMotor = new TalonSRX(1); + + Hardware.elevatorMaster.configFactoryDefault(); + Hardware.levelMotor.configFactoryDefault(); + + Hardware.elevatorMaster.setInverted(false); + Hardware.levelMotor.setInverted(false); + + Hardware.elevatorMaster.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, Constants.kPIDIdx, + Constants.kTimeoutMs); + + Hardware.elevatorMaster.setSelectedSensorPosition(0); + + currState = ElevatorState.ZEROED; + + Hardware.elevatorMaster.configPeakOutputForward(1); + Hardware.elevatorMaster.configPeakOutputReverse(-1); + + Hardware.elevatorMaster.config_kP(Constants.kPIDIdx, Constants.kElevatorP); + Hardware.elevatorMaster.config_kI(Constants.kPIDIdx, Constants.kElevatorI); + Hardware.elevatorMaster.config_kD(Constants.kPIDIdx, Constants.kElevatorD); + + } + + public void periodic() { + + switch (currState) { + + case SETPOINT: + Hardware.elevatorMaster.set(ControlMode.Position, Constants.kClimbHeight, DemandType.ArbitraryFeedForward, Constants.kElevatorHoldVoltage/12); + if(Math.abs(Hardware.elevatorMaster.getSelectedSensorPosition() - Constants.kClimbTicks) < Constants.kClimbThreshold){ + currState = ElevatorState.HOLD; + } + break; + + case HOLD: + break; + + case PULLING: + Hardware.elevatorMaster.set(ControlMode.Position, Constants.kClimbZero, DemandType.ArbitraryFeedForward, Constants.kElevatorHoldVoltage/12); + if(Math.abs(Hardware.elevatorMaster.getSelectedSensorPosition() - Constants.kClimbZero) < Constants.kClimbThreshold){ + currState = ElevatorState.LEVEL; + } + break; + + case LEVEL: + Hardware.levelMotor.set(ControlMode.PercentOutput, angleError * Constants.kLevelP); + break; + + case ZEROING: + Hardware.elevatorMaster.set(ControlMode.Position, Constants.kClimbZero, DemandType.ArbitraryFeedForward, Constants.kElevatorHoldVoltage/12); + if(Math.abs(Hardware.elevatorMaster.getSelectedSensorPosition() - Constants.kClimbZero) < Constants.kClimbThreshold){ + currState = ElevatorState.ZEROED; + } + break; + + case ZEROED: + stop(); + break; + } + } + + public void stop() { + Hardware.elevatorMaster.set(ControlMode.PercentOutput, 0); + } + + public void setElevatorState(ElevatorState desiredState){ + currState = desiredState; + } + + public ElevatorState getElevatorState(){ + return currState; + } +} diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json index 8b25850..77ce29b 100644 --- a/vendordeps/Phoenix.json +++ b/vendordeps/Phoenix.json @@ -153,6 +153,7 @@ "groupId": "com.ctre.phoenix", "artifactId": "platform-stub", "version": "5.17.6", + "libName": "CTRE_PhoenixPlatform", "headerClassifier": "headers", "sharedLibrary": false, @@ -166,6 +167,7 @@ "groupId": "com.ctre.phoenix", "artifactId": "core", "version": "5.17.6", + "libName": "CTRE_PhoenixCore", "headerClassifier": "headers", "sharedLibrary": false,