From ee61f37449dbf1effba7b8169f5d808b2fec05a4 Mon Sep 17 00:00:00 2001 From: Ethan Li Date: Mon, 27 Nov 2023 16:43:40 -0800 Subject: [PATCH] Sasha did everything yeah lol --- .../frc/Subsystems/ElevatorSubsystem.java | 144 ++++++++++++++ .../frc/robot/Commands/ElevatorBaseCommand | 59 ++++++ .../java/frc/robot/Commands/ManualArmCommand | 63 ++++++ src/main/java/frc/robot/Constants.java | 28 ++- src/main/java/frc/robot/Robot.java | 181 ++++++++++-------- src/main/java/frc/robot/RobotContainer.java | 123 +++++++----- 6 files changed, 460 insertions(+), 138 deletions(-) create mode 100644 src/main/java/frc/Subsystems/ElevatorSubsystem.java create mode 100644 src/main/java/frc/robot/Commands/ElevatorBaseCommand create mode 100644 src/main/java/frc/robot/Commands/ManualArmCommand diff --git a/src/main/java/frc/Subsystems/ElevatorSubsystem.java b/src/main/java/frc/Subsystems/ElevatorSubsystem.java new file mode 100644 index 0000000..657d45b --- /dev/null +++ b/src/main/java/frc/Subsystems/ElevatorSubsystem.java @@ -0,0 +1,144 @@ +// 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; + private PIDController pidController; + private final ShuffleboardTab ElevatorTab = Shuffleboard.getTab("Elevator"); + + + private double targetHeight; + private double motorPower; + public ElevatorSubsystem() { + left_motor = new TalonFX(14); + right_motor = new TalonFX(15); + 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); + + + right_motor.follow(left_motor); + + targetHeight = 0; + + + motorPower = 0; + pidController = new PIDController(0.4,0, 0.01); + //pidController.setTolerance(0.7,0.001); + ElevatorTab.addNumber("Current Motor Power", () -> this.motorPower); + ElevatorTab.addNumber("Target Height", () -> this.targetHeight); + 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; + 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; + } + @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{ + left_motor.set(TalonFXControlMode.PercentOutput, -(MathUtil.clamp(motorPower+0.02,-0.5,0.5))); + } + + } + + // 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 b/src/main/java/frc/robot/Commands/ElevatorBaseCommand new file mode 100644 index 0000000..8d920b5 --- /dev/null +++ b/src/main/java/frc/robot/Commands/ElevatorBaseCommand @@ -0,0 +1,59 @@ +// 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 frc.robot.subsystems.ElevatorSubsystem; +import edu.wpi.first.wpilibj2.command.CommandBase; + + +/** An example command that uses an example subsystem. */ +public class ElevatorBaseCommand extends CommandBase { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final ElevatorSubsystem elevatorSubsystem; +public double targetHight; + + + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + public ElevatorBaseCommand(ElevatorSubsystem elevatorSubsystem, double targetHight) { + this.targetHight = targetHight; + 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(targetHight); + + + + + } + + + // 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 diff --git a/src/main/java/frc/robot/Commands/ManualArmCommand b/src/main/java/frc/robot/Commands/ManualArmCommand new file mode 100644 index 0000000..eccfe43 --- /dev/null +++ b/src/main/java/frc/robot/Commands/ManualArmCommand @@ -0,0 +1,63 @@ +// 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 frc.robot.subsystems.ElevatorSubsystem; + + +import java.util.function.DoubleSupplier; + + +import edu.wpi.first.wpilibj2.command.CommandBase; + + +/** An example command that uses an example subsystem. */ +public class ManualArmCommand extends CommandBase { + @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"}) + private final ElevatorSubsystem elevatorSubsystem; +private DoubleSupplier speedSupplier; + /** + * Creates a new ExampleCommand. + * + * @param subsystem The subsystem used by this command. + */ + public ManualArmCommand(ElevatorSubsystem elevatorSubsystem, DoubleSupplier speedSupplier) { + this.elevatorSubsystem = elevatorSubsystem; + this.speedSupplier = speedSupplier; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(elevatorSubsystem); + } + + + // 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() { +double speed =speedSupplier.getAsDouble(); +elevatorSubsystem.setMotorPower(speed); + + + + + } + + + // 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; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 915ef9c..80a3c9d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -2,16 +2,30 @@ // 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; + } +} \ 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 9aaf8a9..a8aa44e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -2,94 +2,111 @@ // 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.wpilibj.livewindow.LiveWindow; +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() {} + - private RobotContainer 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. - robotContainer = new RobotContainer(); - - LiveWindow.disableAllTelemetry(); - } - - /** - * 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() { - // This method intentionally left empty - } - - @Override - public void disabledPeriodic() { - // This method intentionally left empty - } - - /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ - @Override - public void autonomousInit() { - } - - /** This function is called periodically during autonomous. */ - @Override - public void autonomousPeriodic() { - // This method intentionally left empty - } - - @Override - public void teleopInit() { - // do stuff like vibration of controllers at match start - robotContainer.containerTeleopInit(); - } - - /** This function is called periodically during operator control. */ - @Override - public void teleopPeriodic() { - // This method intentionally left empty - } - - @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() { - // This method intentionally left empty - } + /** 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 0d3b8e4..0c882a0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -1,54 +1,79 @@ -// 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. +import frc.robot.subsystems.ElevatorSubsystem; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; -package frc.robot; /** - * 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... - - /** The container for the robot. Contains subsystems, OI devices, and commands. */ - public RobotContainer() { - // Configure the button bindings - configureButtonBindings(); - } - - /** - * Use this method to do things as the drivers gain control of the robot. We use it to vibrate the - * driver b controller to notice accidental swaps. - * - *

Please use this very, very sparingly. It doesn't exist by default for good reason. - */ - public void containerTeleopInit() { - // runs when teleop happens - } - - /** - * Use this method to do things as soon as the robot starts being used. We use it to stop doing - * things that could be harmful or undesirable during game play--rebooting the network switch is a - * good example. Subsystems need to be explicitly wired up to this method. - * - *

Depending on which mode the robot is enabled in, this will either be called before auto or - * before teleop, whichever is first. - * - *

Please use this very, very sparingly. It doesn't exist by default for good reason. - */ - public void containerMatchStarting() { - // runs when the match starts - } - - /** - * 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() { - } + // The robot's subsystems and commands are defined here... + private final ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem(); + + + private final ElevatorBaseCommand ElevatorBaseCommand = new ElevatorBaseCommand(elevatorSubsystem); +private final ManualArmCommand ManualArmCommand = new ManualArmCommand(elevatorSubsystem); + + + private final CommandXboxController driverA = new CommandXboxController(0); + private double targetHight; + + + /** The container for the robot. Contains subsystems, OI devices, and commands. */ + public RobotContainer() { + + + double[] levels={5d,10d,17d,8d,4d,7d,1d}; + + + for(double i :levels){ + targetHight = i; + } + + + // Configure the button bindings + configureButtonBindings(); + + + + + driverA.rightTrigger().onTrue( + new ElevatorBaseCommand(elevatorSubsystem, 5).andThen( + new ElevatorBaseCommand(elevatorSubsystem, 10)).andThen( + new ElevatorBaseCommand(elevatorSubsystem, 17)).andThen( + new ElevatorBaseCommand(elevatorSubsystem, 8)).andThen( + new ElevatorBaseCommand(elevatorSubsystem, 4)).andThen( + new ElevatorBaseCommand(elevatorSubsystem, 7)).andThen( + new ElevatorBaseCommand(elevatorSubsystem, 1))); + + 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(); + } } + +