From 9e65adb829cd4b087e90f741b14081a6a03f0c65 Mon Sep 17 00:00:00 2001 From: Ethan Li Date: Thu, 30 Nov 2023 17:39:42 -0800 Subject: [PATCH 1/4] Hopefully Brandon fixed it (I have no idea what he did) --- .../frc/Subsystems/ElevatorSubsystem.java | 64 +++++++------------ src/main/java/frc/robot/RobotContainer.java | 8 +-- 2 files changed, 23 insertions(+), 49 deletions(-) diff --git a/src/main/java/frc/Subsystems/ElevatorSubsystem.java b/src/main/java/frc/Subsystems/ElevatorSubsystem.java index 69a8ffa..a21fd46 100644 --- a/src/main/java/frc/Subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/Subsystems/ElevatorSubsystem.java @@ -2,17 +2,12 @@ // 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.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.BuiltInLayouts; @@ -22,38 +17,36 @@ import frc.robot.Constants.Elevator; import edu.wpi.first.wpilibj2.command.SubsystemBase; - public class ElevatorSubsystem extends SubsystemBase { /** Creates a new ExampleSubsystem. */ - private TalonFX left_motor; - /** leader */ - private TalonFX right_motor; + private TalonFX left_motor; /** leader */ + private TalonFX right_motor; /** follower */ + private TalonFX wrist_motor; private PIDController pidController; private final ShuffleboardTab ElevatorTab = Shuffleboard.getTab("Elevator"); - + private double currentHeight; private double targetHeight; private double motorPower; public ElevatorSubsystem() { left_motor = new TalonFX(14); right_motor = new TalonFX(15); + wrist_motor = new TalonFX(16); right_motor.configFactoryDefault(); left_motor.configFactoryDefault(); right_motor.clearStickyFaults(); left_motor.clearStickyFaults(); - right_motor.configForwardSoftLimitThreshold( 0, 0); // this is the bottom limit, we stop AT the bottom + // right_motor.configReverseSoftLimitThreshold( // -heightToTicks(24), 0); // this is the top limit, we stop at the very top right_motor.configForwardSoftLimitEnable(true, 0); right_motor.configReverseSoftLimitEnable(true, 0); - right_motor.configOpenloopRamp(.5); - left_motor.setSelectedSensorPosition(0); right_motor.setSelectedSensorPosition(0); @@ -61,18 +54,21 @@ public ElevatorSubsystem() { // make sure we hold our height when we get disabled right_motor.setNeutralMode(NeutralMode.Coast); left_motor.setNeutralMode(NeutralMode.Coast); + wrist_motor.setNeutralMode(NeutralMode.Brake); right_motor.follow(left_motor); targetHeight = 0; - motorPower = 0; + pidController = new PIDController(0.34, 0, 0.02); + //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); @@ -84,16 +80,6 @@ public ElevatorSubsystem() { // ElevatorTab.addNumber("height", () -> this.currentHeight); // ElevatorTab.addNumber("target height", () -> this.targetHeight); // ElevatorTab.addNumber("right motor sensor value", this::getHeight); - - - - - - - - - - } public void setMotorPower(double x){ System.out.println("hello"); @@ -104,37 +90,31 @@ public static double inchesToTicks(double height) { public static double ticksToInches(double ticks) { return (ticks * Elevator.GEAR_CIRCUMFERENCE) / (Elevator.TICKS_PER_REVOLUTION * Elevator.GEAR_RATIO); } - - + public void setTargetHeight(double targetHeight) { this.targetHeight = targetHeight; - pidController.setSetpoint(this.targetHeight); } + } - public double getCurrentHeight(){ - return ticksToInches(-left_motor.getSelectedSensorPosition()); - } public boolean nearTargetHeight(){ - if(targetHeight - 0.5 <= getCurrentHeight() && getCurrentHeight() <= targetHeight + 0.5)return true; - return false; + if(targetHeight -0.5 <= currentHeight && currentHeight <= targetHeight + 0.5){ + return true; + }else{ + return false; + } } + @Override public void periodic() { // This method will be called once per scheduler run - motorPower = pidController.calculate(getCurrentHeight()); - if (!pidController.atSetpoint()){ - if (getCurrentHeight()<5){ - left_motor.set(TalonFXControlMode.PercentOutput, -(MathUtil.clamp(motorPower + 0.02, -0.2,0.2))); - } - else{ + motorPower = pidController.calculate(currentHeight, targetHeight); left_motor.set(TalonFXControlMode.PercentOutput, -(MathUtil.clamp(motorPower + 0.02, -0.5,0.5))); - } - } + currentHeight = ticksToInches(-left_motor.getSelectedSensorPosition()); + } // left_motor.set(TalonFXControlMode.PercentOutput, -(0.1)); - } @Override public void simulationPeriodic() { // This method will be called once per scheduler run during simulation } - } \ No newline at end of file + } \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2ca7234..f0003a1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -29,13 +29,7 @@ public class RobotContainer { public RobotContainer() { - double[] levels = {5d,10d,17d,8d,4d,7d,1d}; - - - for(double i :levels){ - targetHeight = i; - } - + // Configure the button bindings configureButtonBindings(); From 8aeac1c98b5f1e53b5166a8ab4f90b900fc6fbca Mon Sep 17 00:00:00 2001 From: driverstation <5026craptop@gmail.com> Date: Mon, 4 Dec 2023 17:22:28 -0800 Subject: [PATCH 2/4] name changes --- .../frc/Subsystems/ElevatorSubsystem.java | 120 ------------ .../robot/Commands/ElevatorBaseCommand.java | 89 ++++----- .../robot/Commands/SequentialCommands.java | 37 ++++ .../java/frc/robot/Commands/WristCommand.java | 44 +++++ src/main/java/frc/robot/Constants.java | 40 ++-- src/main/java/frc/robot/Robot.java | 181 ++++++++---------- src/main/java/frc/robot/RobotContainer.java | 98 +++++----- .../robot/subsystems/ElevatorSubsystem.java | 123 ++++++++++++ src/main/java/frc/util/ControllerUtil.java | 2 +- 9 files changed, 389 insertions(+), 345 deletions(-) delete mode 100644 src/main/java/frc/Subsystems/ElevatorSubsystem.java create mode 100644 src/main/java/frc/robot/Commands/SequentialCommands.java create mode 100644 src/main/java/frc/robot/Commands/WristCommand.java create mode 100644 src/main/java/frc/robot/subsystems/ElevatorSubsystem.java diff --git a/src/main/java/frc/Subsystems/ElevatorSubsystem.java b/src/main/java/frc/Subsystems/ElevatorSubsystem.java deleted file mode 100644 index a21fd46..0000000 --- a/src/main/java/frc/Subsystems/ElevatorSubsystem.java +++ /dev/null @@ -1,120 +0,0 @@ -// 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.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.BuiltInLayouts; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import frc.robot.Constants; -import frc.robot.Constants.Elevator; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class ElevatorSubsystem extends SubsystemBase { - /** Creates a new ExampleSubsystem. */ - private TalonFX left_motor; /** leader */ - private TalonFX right_motor; /** follower */ - private TalonFX wrist_motor; - private PIDController pidController; - private final ShuffleboardTab ElevatorTab = Shuffleboard.getTab("Elevator"); - - private double currentHeight; - private double targetHeight; - private double motorPower; - public ElevatorSubsystem() { - left_motor = new TalonFX(14); - right_motor = new TalonFX(15); - wrist_motor = new TalonFX(16); - right_motor.configFactoryDefault(); - left_motor.configFactoryDefault(); - right_motor.clearStickyFaults(); - left_motor.clearStickyFaults(); - - right_motor.configForwardSoftLimitThreshold( - 0, 0); // this is the bottom limit, we stop AT the bottom - - // right_motor.configReverseSoftLimitThreshold( - // -heightToTicks(24), 0); // this is the top limit, we stop at the very top - right_motor.configForwardSoftLimitEnable(true, 0); - right_motor.configReverseSoftLimitEnable(true, 0); - - right_motor.configOpenloopRamp(.5); - - left_motor.setSelectedSensorPosition(0); - right_motor.setSelectedSensorPosition(0); - - - // make sure we hold our height when we get disabled - right_motor.setNeutralMode(NeutralMode.Coast); - left_motor.setNeutralMode(NeutralMode.Coast); - wrist_motor.setNeutralMode(NeutralMode.Brake); - - - right_motor.follow(left_motor); - - targetHeight = 0; - - motorPower = 0; - - pidController = new PIDController(0.34, 0, 0.02); - - //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); - ElevatorTab.addNumber("Right Motor Speed", right_motor::getSelectedSensorVelocity); - ElevatorTab.addNumber("Position Error", pidController::getPositionError); - ElevatorTab.addBoolean("If is at target Height", this::nearTargetHeight); -// hi nora - sincerely, evelyn =D - // ElevatorTab.addNumber("height", () -> this.currentHeight); - // ElevatorTab.addNumber("target height", () -> this.targetHeight); - // ElevatorTab.addNumber("right motor sensor value", this::getHeight); - } - public void setMotorPower(double x){ - System.out.println("hello"); - } - public static double inchesToTicks(double height) { - return height * ((Elevator.GEAR_RATIO * Elevator.TICKS_PER_REVOLUTION) / (Elevator.GEAR_CIRCUMFERENCE)); - } - public static double ticksToInches(double ticks) { - return (ticks * Elevator.GEAR_CIRCUMFERENCE) / (Elevator.TICKS_PER_REVOLUTION * Elevator.GEAR_RATIO); - } - - public void setTargetHeight(double targetHeight) { - this.targetHeight = targetHeight; - } - -public boolean nearTargetHeight(){ - if(targetHeight -0.5 <= currentHeight && currentHeight <= targetHeight + 0.5){ - return true; - }else{ - return false; - } - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - motorPower = pidController.calculate(currentHeight, targetHeight); - left_motor.set(TalonFXControlMode.PercentOutput, -(MathUtil.clamp(motorPower + 0.02, -0.5,0.5))); - currentHeight = ticksToInches(-left_motor.getSelectedSensorPosition()); - } - // left_motor.set(TalonFXControlMode.PercentOutput, -(0.1)); - - - @Override - public void simulationPeriodic() { - // This method will be called once per scheduler run during simulation - } - } \ No newline at end of file diff --git a/src/main/java/frc/robot/Commands/ElevatorBaseCommand.java b/src/main/java/frc/robot/Commands/ElevatorBaseCommand.java index 1979f79..5abcf6f 100644 --- a/src/main/java/frc/robot/Commands/ElevatorBaseCommand.java +++ b/src/main/java/frc/robot/Commands/ElevatorBaseCommand.java @@ -2,58 +2,47 @@ // 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; -package frc.robot.Commands; - - -import frc.Subsystems.ElevatorSubsystem; import edu.wpi.first.wpilibj2.command.CommandBase; - +import frc.robot.subsystems.ElevatorSubsystem; /** An example command that uses an example subsystem. */ public class ElevatorBaseCommand extends CommandBase { - @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) - private final ElevatorSubsystem elevatorSubsystem; -public double targetHeight; - - - /** - * Creates a new ExampleCommand. - * - * @param subsystem The subsystem used by this command. - */ - public ElevatorBaseCommand(ElevatorSubsystem elevatorSubsystem, double targetHeight) { - this.targetHeight = targetHeight; - this.elevatorSubsystem = elevatorSubsystem; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(elevatorSubsystem); - } - - - // Called when the command is initially scheduled. - @Override - public void initialize() { - elevatorSubsystem.setTargetHeight(targetHeight); - - - - - } - - - // 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 elevatorSubsystem.nearTargetHeight(); - } -} \ No newline at end of file + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final ElevatorSubsystem elevatorSubsystem; + + public double targetHeight; + + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + public ElevatorBaseCommand(ElevatorSubsystem elevatorSubsystem, double targetHeight) { + this.targetHeight = targetHeight; + this.elevatorSubsystem = elevatorSubsystem; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(elevatorSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + elevatorSubsystem.setTargetHeight(targetHeight); + } + + // 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 elevatorSubsystem.nearTargetHeight(); + } +} diff --git a/src/main/java/frc/robot/Commands/SequentialCommands.java b/src/main/java/frc/robot/Commands/SequentialCommands.java new file mode 100644 index 0000000..cc95aa9 --- /dev/null +++ b/src/main/java/frc/robot/Commands/SequentialCommands.java @@ -0,0 +1,37 @@ +// 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.wpilibj2.command.SequentialCommandGroup; +import frc.robot.subsystems.WristSubsystem; + +public class SequentialCommands extends SequentialCommandGroup { + /** Creates a new TestCommand. */ + public SequentialCommands(WristSubsystem wristSubsystem) { + // Use addRequirements() here to declare subsystem dependencies. + addCommands( + new WristCommand(wristSubsystem, 0), + new WristCommand(wristSubsystem, 20), + new WristCommand(wristSubsystem, 40)); + } + + // // Called when the command is initially scheduled. + // @Override + // public void initialize() {} + + // // 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/Commands/WristCommand.java b/src/main/java/frc/robot/Commands/WristCommand.java new file mode 100644 index 0000000..52587e7 --- /dev/null +++ b/src/main/java/frc/robot/Commands/WristCommand.java @@ -0,0 +1,44 @@ +// 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.wpilibj2.command.CommandBase; +import frc.robot.subsystems.WristSubsystem; + +public class WristCommand extends CommandBase { + /** Creates a new WristCommand. */ + private final WristSubsystem wristSubsystem; + + private double targetAngle; + + public WristCommand(WristSubsystem wristSubsystem, double targetAngle) { + // Use addRequirements() here to declare subsystem dependencies. + this.wristSubsystem = wristSubsystem; + this.targetAngle = targetAngle; + + addRequirements(wristSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + // sets target angle + wristSubsystem.setTargetAngle(targetAngle); + } + + // 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 wristSubsystem.nearTargetAngle(); + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 80a3c9d..d926f83 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -2,30 +2,28 @@ // 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; - @SuppressWarnings("java:S1118") /** -* 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. -*/ + * 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; + } } -} \ 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 f0003a1..b992e09 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -2,62 +2,52 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; -import frc.robot.Commands.ElevatorBaseCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.Subsystems.ElevatorSubsystem; - +import frc.robot.commands.ElevatorBaseCommand; +import frc.robot.subsystems.ElevatorSubsystem; /** -* This class is where the bulk of the robot should be declared. Since Command-based is a -* "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} -* periodic methods (other than the scheduler calls). Instead, the structure of the robot (including -* subsystems, commands, and button mappings) should be declared here. -*/ + * This class is where the bulk of the robot should be declared. Since Command-based is a + * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} + * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including + * subsystems, commands, and button mappings) should be declared here. + */ public class RobotContainer { - // The robot's subsystems and commands are defined here... - private final ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem(); - - -private final ElevatorBaseCommand ElevatorBaseCommand = new ElevatorBaseCommand(elevatorSubsystem, 0); - - - private final CommandXboxController driverA = new CommandXboxController(0); - private double targetHeight; - - - /** The container for the robot. Contains subsystems, OI devices, and commands. */ - public RobotContainer() { - - - - - // Configure the button bindings - configureButtonBindings(); - driverA.y().onTrue(new ElevatorBaseCommand(elevatorSubsystem, 20)); - driverA.x().onTrue(new ElevatorBaseCommand(elevatorSubsystem, 15d)); - driverA.b().onTrue(new ElevatorBaseCommand(elevatorSubsystem, 10d)); - driverA.a().onTrue(new ElevatorBaseCommand(elevatorSubsystem, 3d)); - } - - - /** - * Use this method to define your button->command mappings. Buttons can be created by - * instantiating a {@link GenericHID} or one of its subclasses ({@link - * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link - * edu.wpi.first.wpilibj2.command.button.JoystickButton}. - */ - private void configureButtonBindings() {} - - - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - // An ExampleCommand will run in autonomous - return new InstantCommand(); - } + // The robot's subsystems and commands are defined here... + private final ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem(); + + private final ElevatorBaseCommand ElevatorBaseCommand = + new ElevatorBaseCommand(elevatorSubsystem, 0); + + private final CommandXboxController driverA = new CommandXboxController(0); + private double targetHeight; + + /** The container for the robot. Contains subsystems, OI devices, and commands. */ + public RobotContainer() { + + // Configure the button bindings + configureButtonBindings(); + driverA.y().onTrue(new ElevatorBaseCommand(elevatorSubsystem, 20)); + driverA.x().onTrue(new ElevatorBaseCommand(elevatorSubsystem, 15d)); + driverA.b().onTrue(new ElevatorBaseCommand(elevatorSubsystem, 10d)); + driverA.a().onTrue(new ElevatorBaseCommand(elevatorSubsystem, 3d)); + } + + /** + * Use this method to define your button->command mappings. Buttons can be created by + * instantiating a {@link GenericHID} or one of its subclasses ({@link + * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link + * edu.wpi.first.wpilibj2.command.button.JoystickButton}. + */ + private void configureButtonBindings() {} + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + // An ExampleCommand will run in autonomous + return new InstantCommand(); + } } - - diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java new file mode 100644 index 0000000..2b828d4 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -0,0 +1,123 @@ +// 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.Elevator; + +public class ElevatorSubsystem extends SubsystemBase { + /** Creates a new ExampleSubsystem. */ + private TalonFX left_motor; + /** leader */ + private TalonFX right_motor; + /** follower */ + private TalonFX wrist_motor; + + private PIDController pidController; + private final ShuffleboardTab ElevatorTab = Shuffleboard.getTab("Elevator"); + + private double currentHeight; + private double targetHeight; + private double motorPower; + + public ElevatorSubsystem() { + left_motor = new TalonFX(14); + right_motor = new TalonFX(15); + wrist_motor = new TalonFX(16); + right_motor.configFactoryDefault(); + left_motor.configFactoryDefault(); + right_motor.clearStickyFaults(); + left_motor.clearStickyFaults(); + + right_motor.configForwardSoftLimitThreshold( + 0, 0); // this is the bottom limit, we stop AT the bottom + + // right_motor.configReverseSoftLimitThreshold( + // -heightToTicks(24), 0); // this is the top limit, we stop at the very top + right_motor.configForwardSoftLimitEnable(true, 0); + right_motor.configReverseSoftLimitEnable(true, 0); + + right_motor.configOpenloopRamp(.5); + + left_motor.setSelectedSensorPosition(0); + right_motor.setSelectedSensorPosition(0); + + // make sure we hold our height when we get disabled + right_motor.setNeutralMode(NeutralMode.Coast); + left_motor.setNeutralMode(NeutralMode.Coast); + wrist_motor.setNeutralMode(NeutralMode.Brake); + + right_motor.follow(left_motor); + + targetHeight = 0; + + motorPower = 0; + + pidController = new PIDController(0.34, 0, 0.02); + + // 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); + ElevatorTab.addNumber("Right Motor Speed", right_motor::getSelectedSensorVelocity); + ElevatorTab.addNumber("Position Error", pidController::getPositionError); + ElevatorTab.addBoolean("If is at target Height", this::nearTargetHeight); + // hi nora - sincerely, evelyn =D + // ElevatorTab.addNumber("height", () -> this.currentHeight); + // ElevatorTab.addNumber("target height", () -> this.targetHeight); + // ElevatorTab.addNumber("right motor sensor value", this::getHeight); + } + + public void setMotorPower(double x) { + System.out.println("hello"); + } + + public static double inchesToTicks(double height) { + return height + * ((Elevator.GEAR_RATIO * Elevator.TICKS_PER_REVOLUTION) / (Elevator.GEAR_CIRCUMFERENCE)); + } + + public static double ticksToInches(double ticks) { + return (ticks * Elevator.GEAR_CIRCUMFERENCE) + / (Elevator.TICKS_PER_REVOLUTION * Elevator.GEAR_RATIO); + } + + public void setTargetHeight(double targetHeight) { + this.targetHeight = targetHeight; + } + + public boolean nearTargetHeight() { + if (targetHeight - 0.5 <= currentHeight && currentHeight <= targetHeight + 0.5) { + return true; + } else { + return false; + } + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + motorPower = pidController.calculate(currentHeight, targetHeight); + left_motor.set( + TalonFXControlMode.PercentOutput, -(MathUtil.clamp(motorPower + 0.02, -0.5, 0.5))); + currentHeight = ticksToInches(-left_motor.getSelectedSensorPosition()); + } + // left_motor.set(TalonFXControlMode.PercentOutput, -(0.1)); + + @Override + public void simulationPeriodic() { + // This method will be called once per scheduler run during simulation + } +} 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 +} From d0e466efff2939b9df8f106abc86e590f3560f0f Mon Sep 17 00:00:00 2001 From: driverstation <5026craptop@gmail.com> Date: Mon, 4 Dec 2023 17:30:11 -0800 Subject: [PATCH 3/4] wrist command??? --- .../frc/robot/subsystems/WristSubsystem.java | 86 +++++++++++++++++++ 1 file changed, 86 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/WristSubsystem.java 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..fb70688 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/WristSubsystem.java @@ -0,0 +1,86 @@ +package frc.robot.subsystems; + +public // 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))); + } +} + { + +} From 1c44b3c95556cee7cb80e40779ab20286cfb99a1 Mon Sep 17 00:00:00 2001 From: driverstation <5026craptop@gmail.com> Date: Mon, 4 Dec 2023 17:32:18 -0800 Subject: [PATCH 4/4] cool --- src/main/java/frc/robot/Constants.java | 7 +++++++ src/main/java/frc/robot/subsystems/WristSubsystem.java | 9 ++------- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d926f83..86dfd95 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -26,4 +26,11 @@ public static class Elevator { /** 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; + } } diff --git a/src/main/java/frc/robot/subsystems/WristSubsystem.java b/src/main/java/frc/robot/subsystems/WristSubsystem.java index fb70688..defb30e 100644 --- a/src/main/java/frc/robot/subsystems/WristSubsystem.java +++ b/src/main/java/frc/robot/subsystems/WristSubsystem.java @@ -1,6 +1,4 @@ -package frc.robot.subsystems; - -public // Copyright (c) FIRST and other WPILib contributors. +// 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. @@ -80,7 +78,4 @@ public void periodic() { // 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