It is advised to statically import this class (or one of its inner classes) wherever the -* constants are needed, to reduce verbosity. -*/ + * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean + * constants. This class should not be used for any other purpose. All constants should be declared + * globally (i.e. public static). Do not put anything functional in this class. + * + *
It is advised to statically import this class (or one of its inner classes) wherever the + * constants are needed, to reduce verbosity. + */ public final class Constants { - public static class OperatorConstants { - public static final int kDriverControllerPort = 0; - } - public static class Elevator{ - /**the gear ratio of the motor to the final gear revolutions */ - public static final double GEAR_RATIO = 0.1008; - /**The number of ticks per motor revolution */ - public static final double TICKS_PER_REVOLUTION = 2048; - /**The gear circumferance for distance */ - public static final double GEAR_CIRCUMFERENCE = 1.432*Math.PI; - + public static class OperatorConstants { + public static final int kDriverControllerPort = 0; + } + + public static class Elevator { + /** the gear ratio of the motor to the final gear revolutions */ + public static final double GEAR_RATIO = 0.1008; + /** The number of ticks per motor revolution */ + public static final double TICKS_PER_REVOLUTION = 2048; + /** The gear circumferance for distance */ + public static final double GEAR_CIRCUMFERENCE = 1.432 * Math.PI; + } + public final class Wrist { + public static final int DRIVER_CONTROLLER_PORT = 0; + public static final int WRIST_MOTOR_DEVICE_NUMBER = 16; + public static final double TICKS = 2048; + public static final double DEGREES = 360; + public static final double GEAR_RATIO = 0.061; + } } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a8aa44e..b31bfdc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -2,111 +2,94 @@ // 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; - import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; - /** -* The VM is configured to automatically run this class, and to call the functions corresponding to -* each mode, as described in the TimedRobot documentation. If you change the name of this class or -* the package after creating this project, you must also update the build.gradle file in the -* project. -*/ + * The VM is configured to automatically run this class, and to call the functions corresponding to + * each mode, as described in the TimedRobot documentation. If you change the name of this class or + * the package after creating this project, you must also update the build.gradle file in the + * project. + */ public class Robot extends TimedRobot { - private Command m_autonomousCommand; - - - private RobotContainer m_robotContainer; - - - /** - * This function is run when the robot is first started up and should be used for any - * initialization code. - */ - @Override - public void robotInit() { - // Instantiate our RobotContainer. This will perform all our button bindings, and put our - // autonomous chooser on the dashboard. - m_robotContainer = new RobotContainer(); - } - - - /** - * This function is called every robot packet, no matter the mode. Use this for items like - * diagnostics that you want ran during disabled, autonomous, teleoperated and test. - * - *
This runs after the mode specific periodic functions, but before LiveWindow and - * SmartDashboard integrated updating. - */ - @Override - public void robotPeriodic() { - // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled - // commands, running already-scheduled commands, removing finished or interrupted commands, - // and running subsystem periodic() methods. This must be called from the robot's periodic - // block in order for anything in the Command-based framework to work. - CommandScheduler.getInstance().run(); - } - - - /** This function is called once each time the robot enters Disabled mode. */ - @Override - public void disabledInit() {} - - - @Override - public void disabledPeriodic() {} - - - /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ - @Override - public void autonomousInit() { - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - - - // schedule the autonomous command (example) - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); - } - } - - - /** This function is called periodically during autonomous. */ - @Override - public void autonomousPeriodic() {} - - - @Override - public void teleopInit() { - // This makes sure that the autonomous stops running when - // teleop starts running. If you want the autonomous to - // continue until interrupted by another command, remove - // this line or comment it out. - if (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); - } - } - - - /** This function is called periodically during operator control. */ - @Override - public void teleopPeriodic() {} - - - @Override - public void testInit() { - // Cancels all running commands at the start of test mode. - CommandScheduler.getInstance().cancelAll(); - } - - - /** This function is called periodically during test mode. */ - @Override - public void testPeriodic() {} + private Command m_autonomousCommand; + + private RobotContainer m_robotContainer; + + /** + * This function is run when the robot is first started up and should be used for any + * initialization code. + */ + @Override + public void robotInit() { + // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // autonomous chooser on the dashboard. + m_robotContainer = new RobotContainer(); + } + + /** + * This function is called every robot packet, no matter the mode. Use this for items like + * diagnostics that you want ran during disabled, autonomous, teleoperated and test. + * + *
This runs after the mode specific periodic functions, but before LiveWindow and + * SmartDashboard integrated updating. + */ + @Override + public void robotPeriodic() { + // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled + // commands, running already-scheduled commands, removing finished or interrupted commands, + // and running subsystem periodic() methods. This must be called from the robot's periodic + // block in order for anything in the Command-based framework to work. + CommandScheduler.getInstance().run(); + } + + /** This function is called once each time the robot enters Disabled mode. */ + @Override + public void disabledInit() {} + + @Override + public void disabledPeriodic() {} + + /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ + @Override + public void autonomousInit() { + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + + // schedule the autonomous command (example) + if (m_autonomousCommand != null) { + m_autonomousCommand.schedule(); + } + } + + /** This function is called periodically during autonomous. */ + @Override + public void autonomousPeriodic() {} + + @Override + public void teleopInit() { + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (m_autonomousCommand != null) { + m_autonomousCommand.cancel(); + } + } + + /** This function is called periodically during operator control. */ + @Override + public void teleopPeriodic() {} + + @Override + public void testInit() { + // Cancels all running commands at the start of test mode. + CommandScheduler.getInstance().cancelAll(); + } + + /** This function is called periodically during test mode. */ + @Override + public void testPeriodic() {} } - - diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b02f3a5..f8f50a4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -16,7 +16,8 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... private final ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem(); - private ElevatorBaseCommand ElevatorBaseCommand = new ElevatorBaseCommand(elevatorSubsystem, 0); + private final ElevatorBaseCommand ElevatorBaseCommand = + new ElevatorBaseCommand(elevatorSubsystem, 0); private final CommandXboxController driverA = new CommandXboxController(0); private double targetHeight; @@ -45,7 +46,7 @@ private void configureButtonBindings() {} * @return the command to run in autonomous */ public Command getAutonomousCommand() { - // An ExampleCommand will run in autonomous return new InstantCommand(); + // An ExampleCommand will run in autonomous } } diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index 3b8e12c..2b828d4 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -19,7 +19,7 @@ public class ElevatorSubsystem extends SubsystemBase { private TalonFX left_motor; /** leader */ private TalonFX right_motor; - /** leader */ + /** follower */ private TalonFX wrist_motor; private PIDController pidController; @@ -67,6 +67,7 @@ public ElevatorSubsystem() { // pidController.setTolerance(0.7,0.001); ElevatorTab.addNumber("Current Motor Power", () -> this.motorPower); ElevatorTab.addNumber("Target Height", () -> this.targetHeight); + ElevatorTab.addNumber("Current Height", () -> currentHeight); ElevatorTab.add(pidController); ElevatorTab.addNumber("Left Motor Speed", left_motor::getSelectedSensorVelocity); @@ -95,7 +96,6 @@ public static double ticksToInches(double ticks) { public void setTargetHeight(double targetHeight) { this.targetHeight = targetHeight; - pidController.setSetpoint(targetHeight); } public boolean nearTargetHeight() { @@ -109,7 +109,7 @@ public boolean nearTargetHeight() { @Override public void periodic() { // This method will be called once per scheduler run - motorPower = pidController.calculate(currentHeight); + motorPower = pidController.calculate(currentHeight, targetHeight); left_motor.set( TalonFXControlMode.PercentOutput, -(MathUtil.clamp(motorPower + 0.02, -0.5, 0.5))); currentHeight = ticksToInches(-left_motor.getSelectedSensorPosition()); diff --git a/src/main/java/frc/robot/subsystems/WristSubsystem.java b/src/main/java/frc/robot/subsystems/WristSubsystem.java new file mode 100644 index 0000000..defb30e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/WristSubsystem.java @@ -0,0 +1,81 @@ +// 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.subsystems; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonFX; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import frc.robot.Constants.Wrist; + +public class WristSubsystem extends SubsystemBase { + /** Creates a new WristSubsystem. */ + private double targetAngle; + + private final ShuffleboardTab WristTab = Shuffleboard.getTab("Wrist"); + + private double currentAngle; + private TalonFX wrist_motor; + private PIDController pidController; + private double motorPower; + + public WristSubsystem() { + + this.targetAngle = targetAngle; + this.wrist_motor = new TalonFX(Constants.Wrist.WRIST_MOTOR_DEVICE_NUMBER); + this.wrist_motor.configFactoryDefault(); + this.wrist_motor.clearStickyFaults(); + this.wrist_motor.configForwardSoftLimitThreshold(0); + this.wrist_motor.configReverseSoftLimitThreshold(degreesToTicks(90)); + this.wrist_motor.configReverseSoftLimitEnable(true, 0); + this.wrist_motor.configForwardSoftLimitEnable(true, 0); + this.wrist_motor.setNeutralMode(NeutralMode.Coast); + this.wrist_motor.setSelectedSensorPosition(0); + this.wrist_motor.configOpenloopRamp(.5); // can't go from 0 to 1 instantly + + pidController = new PIDController(0.1, 0, 0); + WristTab.add(pidController); + WristTab.addNumber("Current Motor Position", wrist_motor::getSelectedSensorPosition); + WristTab.addBoolean("Is at target", this::nearTargetAngle); + WristTab.addNumber("Target Angle", () -> this.targetAngle); +} + + public static double degreesToTicks(double angle) { + return (angle*360d)/((Wrist.GEAR_RATIO * Wrist.TICKS)); + } + + public void setTargetAngle(double targetAngle) { + this.targetAngle = targetAngle; + pidController.setSetpoint(targetAngle); + } + + public static double ticksToDegrees(double ticks) { + return ((ticks / Wrist.TICKS) * Wrist.GEAR_RATIO*360); + } + + private double getCurrentAngle() { + currentAngle = ticksToDegrees(wrist_motor.getSelectedSensorPosition()); + return currentAngle; + } + + public boolean nearTargetAngle() { + if (targetAngle - 0.5 <= getCurrentAngle() && getCurrentAngle() <= targetAngle + 0.5) + return true; + return false; + } + + @Override + public void periodic() { + // calculates motor power + motorPower = pidController.calculate(getCurrentAngle()); + // This method will be called once per scheduler run + wrist_motor.set(TalonFXControlMode.PercentOutput, -(MathUtil.clamp(motorPower, -0.5, 0.5))); + } +} \ No newline at end of file diff --git a/src/main/java/frc/util/ControllerUtil.java b/src/main/java/frc/util/ControllerUtil.java index 4ef37d0..dbf30ed 100644 --- a/src/main/java/frc/util/ControllerUtil.java +++ b/src/main/java/frc/util/ControllerUtil.java @@ -37,4 +37,4 @@ public static BooleanSupplier cumBooleanSupplier( BooleanSupplier layer, boolean layerState, BooleanSupplier button) { return () -> layer.getAsBoolean() == layerState && button.getAsBoolean(); } -} \ No newline at end of file +}