From ca17c68867aa738261e36db8ae6f751181884161 Mon Sep 17 00:00:00 2001 From: LordBBQ <46541720+LordBBQ@users.noreply.github.com> Date: Thu, 12 Jan 2023 21:49:35 -0800 Subject: [PATCH 01/18] 2023 drive code Based off of the WPILib voltage driven DifferentalDrive class. Should be easily compatable with trajectory autos. --- ChargedUp/.SysId/sysid.json | 10 + .../src/main/java/com/projectb/Config.java | 8 - .../src/main/java/com/projectb/Constants.java | 5 - .../java/com/projectb/Subsystems/Drive.java | 5 - .../frc/robot/Commands/CommandController.java | 34 ++ ChargedUp/src/main/java/frc/robot/Config.java | 29 ++ .../src/main/java/frc/robot/Constants.java | 27 ++ .../robot}/DriverInterface.java | 2 +- .../{com/projectb => frc/robot}/Main.java | 2 +- .../{com/projectb => frc/robot}/Robot.java | 14 +- .../main/java/frc/robot/Subsystems/Drive.java | 119 +++++ .../robot}/Utilities/Pneumatics.java | 2 +- ChargedUp/vendordeps/Phoenix.json | 423 ++++++++++++++++++ 13 files changed, 656 insertions(+), 24 deletions(-) create mode 100644 ChargedUp/.SysId/sysid.json delete mode 100644 ChargedUp/src/main/java/com/projectb/Config.java delete mode 100644 ChargedUp/src/main/java/com/projectb/Constants.java delete mode 100644 ChargedUp/src/main/java/com/projectb/Subsystems/Drive.java create mode 100644 ChargedUp/src/main/java/frc/robot/Commands/CommandController.java create mode 100644 ChargedUp/src/main/java/frc/robot/Config.java create mode 100644 ChargedUp/src/main/java/frc/robot/Constants.java rename ChargedUp/src/main/java/{com/projectb => frc/robot}/DriverInterface.java (63%) rename ChargedUp/src/main/java/{com/projectb => frc/robot}/Main.java (94%) rename ChargedUp/src/main/java/{com/projectb => frc/robot}/Robot.java (90%) create mode 100644 ChargedUp/src/main/java/frc/robot/Subsystems/Drive.java rename ChargedUp/src/main/java/{com/projectb => frc/robot}/Utilities/Pneumatics.java (51%) create mode 100644 ChargedUp/vendordeps/Phoenix.json diff --git a/ChargedUp/.SysId/sysid.json b/ChargedUp/.SysId/sysid.json new file mode 100644 index 0000000..9057eca --- /dev/null +++ b/ChargedUp/.SysId/sysid.json @@ -0,0 +1,10 @@ +{ + "SysId": { + "Analysis Type": "Drivetrain", + "NetworkTables Settings": { + "serverTeam": "5985" + }, + "mode": "Client (NT4)", + "serverTeam": "5985" + } +} diff --git a/ChargedUp/src/main/java/com/projectb/Config.java b/ChargedUp/src/main/java/com/projectb/Config.java deleted file mode 100644 index ad9b0ad..0000000 --- a/ChargedUp/src/main/java/com/projectb/Config.java +++ /dev/null @@ -1,8 +0,0 @@ -package com.projectb; - -public class Config { - - - - -} diff --git a/ChargedUp/src/main/java/com/projectb/Constants.java b/ChargedUp/src/main/java/com/projectb/Constants.java deleted file mode 100644 index eb0490f..0000000 --- a/ChargedUp/src/main/java/com/projectb/Constants.java +++ /dev/null @@ -1,5 +0,0 @@ -package com.projectb; - -public class Constants { - -} diff --git a/ChargedUp/src/main/java/com/projectb/Subsystems/Drive.java b/ChargedUp/src/main/java/com/projectb/Subsystems/Drive.java deleted file mode 100644 index 15f0be4..0000000 --- a/ChargedUp/src/main/java/com/projectb/Subsystems/Drive.java +++ /dev/null @@ -1,5 +0,0 @@ -package com.projectb.Subsystems; - -public class Drive { - -} diff --git a/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java b/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java new file mode 100644 index 0000000..18874a1 --- /dev/null +++ b/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java @@ -0,0 +1,34 @@ +// 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.button.CommandJoystick; +import frc.robot.Subsystems.Drive; + +/** Add your docs here. */ +public class CommandController { + + private final Drive m_drive = new Drive(); + CommandJoystick m_driverJoystick = new CommandJoystick(0); + + /** + * Use this method to define bindings between conditions and commands. These are useful for + * automating robot behaviors based on button and sensor input. + * + *

Should be called during {@link Robot#robotInit()}. + * + *

Event binding methods are available on the {@link Trigger} class. + */ + public void configureBindings() { + + // Control the drive with split-stick arcade controls + m_drive.setDefaultCommand( + m_drive.arcadeDriveCommand( + () -> m_driverJoystick.getX(), () -> m_driverJoystick.getY())); + } + + + +} diff --git a/ChargedUp/src/main/java/frc/robot/Config.java b/ChargedUp/src/main/java/frc/robot/Config.java new file mode 100644 index 0000000..e5913dc --- /dev/null +++ b/ChargedUp/src/main/java/frc/robot/Config.java @@ -0,0 +1,29 @@ +package frc.robot; + +public class Config { + + /** + * Drive characterisation values //TODO + */ + public static final double ksVolts = 0.21511; + public static final double kvVoltSecondsPerMeter = 1.1369; + public static final double kaVoltSecondsSquaredPerMeter = 0.3275; + + public static final double kPDriveVel = 0.54175; + + public static final double kTrackwidthMeters = 0.762; + + public static final double kMaxSpeedMetersPerSecond = 2.5915; + public static final double kMaxAccelerationMetersPerSecondSquared = 1; + + // Reasonable baseline values for a RAMSETE follower in units of meters and seconds + public static final double kRamseteB = 2; + public static final double kRamseteZeta = 0.7; + + public static final double wheelDiameterMetres = (6*2.54)/100; + public static final double wheelCircumferenceMetres = wheelDiameterMetres * Math.PI; + public static final double kRatioMotorToWheel = (1/11.1/wheelCircumferenceMetres); + + + +} diff --git a/ChargedUp/src/main/java/frc/robot/Constants.java b/ChargedUp/src/main/java/frc/robot/Constants.java new file mode 100644 index 0000000..baaa19c --- /dev/null +++ b/ChargedUp/src/main/java/frc/robot/Constants.java @@ -0,0 +1,27 @@ +package frc.robot; + +public class Constants { + + /** + * 2023 robot config + */ + // public static final int kLeftDriveACanId = 1; + // public static final int kLeftDriveBCanId = 3; + // public static final int kLeftDriveCCanId = 5; + // public static final int kRightDriveACanId = 2; + // public static final int kRightDriveBCanId = 4; + // public static final int kRightDriveCCanId = 6; + + /** + * Pegasus config + */ + + public static final int kLeftDriveACanId = 1; + public static final int kLeftDriveBCanId = 2; //3 + public static final int kLeftDriveCCanId = 40; //5 + public static final int kRightDriveACanId = 5; //2 + public static final int kRightDriveBCanId = 6; //4 + public static final int kRightDriveCCanId = 41; //6 + + +} diff --git a/ChargedUp/src/main/java/com/projectb/DriverInterface.java b/ChargedUp/src/main/java/frc/robot/DriverInterface.java similarity index 63% rename from ChargedUp/src/main/java/com/projectb/DriverInterface.java rename to ChargedUp/src/main/java/frc/robot/DriverInterface.java index 9e1b623..014ebe6 100644 --- a/ChargedUp/src/main/java/com/projectb/DriverInterface.java +++ b/ChargedUp/src/main/java/frc/robot/DriverInterface.java @@ -1,4 +1,4 @@ -package com.projectb; +package frc.robot; public class DriverInterface { diff --git a/ChargedUp/src/main/java/com/projectb/Main.java b/ChargedUp/src/main/java/frc/robot/Main.java similarity index 94% rename from ChargedUp/src/main/java/com/projectb/Main.java rename to ChargedUp/src/main/java/frc/robot/Main.java index 85a9024..fe215d7 100644 --- a/ChargedUp/src/main/java/com/projectb/Main.java +++ b/ChargedUp/src/main/java/frc/robot/Main.java @@ -2,7 +2,7 @@ // 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 com.projectb; +package frc.robot; import edu.wpi.first.wpilibj.RobotBase; diff --git a/ChargedUp/src/main/java/com/projectb/Robot.java b/ChargedUp/src/main/java/frc/robot/Robot.java similarity index 90% rename from ChargedUp/src/main/java/com/projectb/Robot.java rename to ChargedUp/src/main/java/frc/robot/Robot.java index cda212b..3406aa0 100644 --- a/ChargedUp/src/main/java/com/projectb/Robot.java +++ b/ChargedUp/src/main/java/frc/robot/Robot.java @@ -2,9 +2,11 @@ // 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 com.projectb; +package frc.robot; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.Commands.CommandController; /** * 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 @@ -13,6 +15,9 @@ */ public class Robot extends TimedRobot { + private final CommandController m_robot = new CommandController(); + + /** * This function is run when the robot is first started up and should be used for any @@ -20,7 +25,7 @@ public class Robot extends TimedRobot { */ @Override public void robotInit() { - + m_robot.configureBindings(); } /** @@ -31,7 +36,10 @@ public void robotInit() { * SmartDashboard integrated updating. */ @Override - public void robotPeriodic() {} + public void robotPeriodic() { + CommandScheduler.getInstance().run(); + + } /** * This autonomous (along with the chooser code above) shows how to select between different diff --git a/ChargedUp/src/main/java/frc/robot/Subsystems/Drive.java b/ChargedUp/src/main/java/frc/robot/Subsystems/Drive.java new file mode 100644 index 0000000..d1ed195 --- /dev/null +++ b/ChargedUp/src/main/java/frc/robot/Subsystems/Drive.java @@ -0,0 +1,119 @@ +package frc.robot.Subsystems; + +import java.util.function.DoubleSupplier; + +import com.ctre.phoenix.motorcontrol.TalonFXInvertType; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; + +import frc.robot.Constants; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Drive extends SubsystemBase{ + + private WPI_TalonFX leftDriveA = new WPI_TalonFX(Constants.kLeftDriveACanId); + private WPI_TalonFX leftDriveB = new WPI_TalonFX(Constants.kLeftDriveBCanId); + private WPI_TalonFX leftDriveC = new WPI_TalonFX(Constants.kLeftDriveCCanId); + private WPI_TalonFX rightDriveA = new WPI_TalonFX(Constants.kRightDriveACanId); + private WPI_TalonFX rightDriveB = new WPI_TalonFX(Constants.kRightDriveBCanId); + private WPI_TalonFX rightDriveC = new WPI_TalonFX(Constants.kRightDriveCCanId); + + private final MotorControllerGroup leftDrive = new MotorControllerGroup(leftDriveA, leftDriveB, leftDriveC); + private final MotorControllerGroup rightDrive = new MotorControllerGroup(rightDriveA, rightDriveB, rightDriveC); + + private final DifferentialDrive driveMotors = new DifferentialDrive(leftDrive, rightDrive); + + + public void initSystem() { + leftDriveA.configFactoryDefault(); + leftDriveB.configFactoryDefault(); + leftDriveC.configFactoryDefault(); + rightDriveA.configFactoryDefault(); + rightDriveB.configFactoryDefault(); + rightDriveC.configFactoryDefault(); + + rightDriveA.setInverted(TalonFXInvertType.CounterClockwise); + rightDriveB.setInverted(TalonFXInvertType.CounterClockwise); + rightDriveC.setInverted(TalonFXInvertType.CounterClockwise); + + } + + public Drive() { + initSystem(); + } + + /** + * Returns a command that drives the robot with arcade controls. + * + * @param fwd the commanded forward movement + * @param rot the commanded rotation + */ + public CommandBase arcadeDriveCommand(DoubleSupplier fwd, DoubleSupplier rot) { + // A split-stick arcade command, with forward/backward controlled by the left + // hand, and turning controlled by the right. + return run(() -> driveMotors.arcadeDrive(fwd.getAsDouble(), rot.getAsDouble())) + .withName("arcadeDrive"); + } + + /** + * Resets the drive encoders to a position of zero + */ + public void resetDriveEncoders() { + + leftDriveA.setSelectedSensorPosition(0); + leftDriveB.setSelectedSensorPosition(0); + leftDriveC.setSelectedSensorPosition(0); + rightDriveA.setSelectedSensorPosition(0); + rightDriveB.setSelectedSensorPosition(0); + rightDriveC.setSelectedSensorPosition(0); + + } + + /** + * + * @return left encoder distance in metres (m), averaged from all three falcons + */ + public double getLeftDriveEncodersDistanceMetres() { + return ((leftDriveA.getSelectedSensorPosition() + + leftDriveB.getSelectedSensorPosition())/2 );// + + //leftDriveC.getSelectedSensorPosition()) / 3); + } + + /** + * + * @return right encoder distance in metres (m), averaged from all three falcons + */ + public double getRightDriveEncodersDistanceMetres() { + return ((rightDriveA.getSelectedSensorPosition() + + rightDriveB.getSelectedSensorPosition()) /2 ); //+ + // rightDriveC.getSelectedSensorPosition()) / 3); + } + + /** + * Returns a command that drives the robot forward a specified distance at a specified speed. + * + * @param distanceMeters The distance to drive forward in meters + * @param speed The fraction of max speed at which to drive + */ + public CommandBase driveDistanceCommand(double distanceMeters, double speed) { + return runOnce( + () -> { + resetDriveEncoders(); + }) + // Drive forward at specified speed + .andThen(run(() -> driveMotors.arcadeDrive(speed, 0))) + // End command when we've traveled the specified distance + .until( + () -> + Math.max(getLeftDriveEncodersDistanceMetres(), getRightDriveEncodersDistanceMetres()) + >= distanceMeters) + // Stop the drive when the command ends + .finallyDo(interrupted -> driveMotors.stopMotor()); + } + + + + +} diff --git a/ChargedUp/src/main/java/com/projectb/Utilities/Pneumatics.java b/ChargedUp/src/main/java/frc/robot/Utilities/Pneumatics.java similarity index 51% rename from ChargedUp/src/main/java/com/projectb/Utilities/Pneumatics.java rename to ChargedUp/src/main/java/frc/robot/Utilities/Pneumatics.java index 58f7f03..a0d1ad2 100644 --- a/ChargedUp/src/main/java/com/projectb/Utilities/Pneumatics.java +++ b/ChargedUp/src/main/java/frc/robot/Utilities/Pneumatics.java @@ -1,4 +1,4 @@ -package com.projectb.Utilities; +package frc.robot.Utilities; public class Pneumatics { diff --git a/ChargedUp/vendordeps/Phoenix.json b/ChargedUp/vendordeps/Phoenix.json new file mode 100644 index 0000000..4bd4424 --- /dev/null +++ b/ChargedUp/vendordeps/Phoenix.json @@ -0,0 +1,423 @@ +{ + "fileName": "Phoenix.json", + "name": "CTRE-Phoenix (v5)", + "version": "5.30.2", + "frcYear": 2023, + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2023-latest.json", + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.30.2" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.30.2" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.30.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.30.2", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro", + "artifactId": "tools", + "version": "23.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "tools-sim", + "version": "23.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simTalonSRX", + "version": "23.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simTalonFX", + "version": "23.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simVictorSPX", + "version": "23.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simPigeonIMU", + "version": "23.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simCANCoder", + "version": "23.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProTalonFX", + "version": "23.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProCANcoder", + "version": "23.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProPigeon2", + "version": "23.0.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.30.2", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.30.2", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.30.2", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenixpro", + "artifactId": "tools", + "version": "23.0.1", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.30.2", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.30.2", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.30.2", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "tools-sim", + "version": "23.0.1", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simTalonSRX", + "version": "23.0.1", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simTalonFX", + "version": "23.0.1", + "libName": "CTRE_SimTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simVictorSPX", + "version": "23.0.1", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simPigeonIMU", + "version": "23.0.1", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simCANCoder", + "version": "23.0.1", + "libName": "CTRE_SimCANCoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProTalonFX", + "version": "23.0.1", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProCANcoder", + "version": "23.0.1", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenixpro.sim", + "artifactId": "simProPigeon2", + "version": "23.0.1", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file From e0a3fa11665c2277c0494127574b6e6120a45cf9 Mon Sep 17 00:00:00 2001 From: LordBBQ <46541720+LordBBQ@users.noreply.github.com> Date: Fri, 13 Jan 2023 01:06:37 -0800 Subject: [PATCH 02/18] Written HorizontalExtension subsystem --- ChargedUp/src/main/java/frc/robot/Config.java | 27 +++++++++ .../src/main/java/frc/robot/Constants.java | 6 ++ .../robot/Subsystems/HorizontalExtension.java | 55 +++++++++++++++++++ 3 files changed, 88 insertions(+) create mode 100644 ChargedUp/src/main/java/frc/robot/Subsystems/HorizontalExtension.java diff --git a/ChargedUp/src/main/java/frc/robot/Config.java b/ChargedUp/src/main/java/frc/robot/Config.java index e5913dc..47903b0 100644 --- a/ChargedUp/src/main/java/frc/robot/Config.java +++ b/ChargedUp/src/main/java/frc/robot/Config.java @@ -24,6 +24,33 @@ public class Config { public static final double wheelCircumferenceMetres = wheelDiameterMetres * Math.PI; public static final double kRatioMotorToWheel = (1/11.1/wheelCircumferenceMetres); + public static final double kHorizontalExtensionKS = 0; + public static final double kHorizontalExtensionKV = 0; + public static final double kHorizontalExtensionKG = 0; + public static final double kHorizontalExtensionKA = 0; + public static final double kHorizontalExtensionKP = 0; + public static final double kHorizontalExtensionMaxVelocity = 0; + public static final double kHorizontalExtensionMaxAcceleration = 0; + public static final double kHorizontalExtensionEncoderPPR = 4096; //CANCoder, falcon is 2048 EPR + public static final double kHorizontalExtensionNeutralPosition = 0; + public static final double kHorizontalExtensionEncoderOffset = 0; + + public static final double kVerticalExtensionKS = 0; + public static final double kVerticalExtensionKV = 0; + public static final double kVerticalExtensionKG = 0; + public static final double kVerticalExtensionKA = 0; + public static final double kVerticalExtensionKP = 0; + public static final double kVerticalExtensionMaxVelocity = 0; + public static final double kVerticalExtensionMaxAcceleration = 0; + public static final double kVerticalExtensionEncoderPPR = 4096; //CANCoder, falcon is 2048 EPR + public static final double kVerticalExtensionNeutralPosition = 0; + public static final double kVerticalExtensionEncoderOffset = 0; + + + + + + } diff --git a/ChargedUp/src/main/java/frc/robot/Constants.java b/ChargedUp/src/main/java/frc/robot/Constants.java index baaa19c..6b9f2f2 100644 --- a/ChargedUp/src/main/java/frc/robot/Constants.java +++ b/ChargedUp/src/main/java/frc/robot/Constants.java @@ -23,5 +23,11 @@ public class Constants { public static final int kRightDriveBCanId = 6; //4 public static final int kRightDriveCCanId = 41; //6 + public static final int kVerticalElevatorCanId = 10; + public static final int kHorizontalElevatorCanId = 9; + public static final int kVerticalElevatorEncoderCanId = 21; + public static final int kHorizontalElevatorEncoderCanId = 22; + + } diff --git a/ChargedUp/src/main/java/frc/robot/Subsystems/HorizontalExtension.java b/ChargedUp/src/main/java/frc/robot/Subsystems/HorizontalExtension.java new file mode 100644 index 0000000..0e0683a --- /dev/null +++ b/ChargedUp/src/main/java/frc/robot/Subsystems/HorizontalExtension.java @@ -0,0 +1,55 @@ +// 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.can.WPI_TalonFX; +import com.ctre.phoenix.sensors.CANCoder; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; +import frc.robot.Config; +import frc.robot.Constants; + +/** Add your docs here. */ +public class HorizontalExtension extends ProfiledPIDSubsystem { + private WPI_TalonFX horizontalExtension = new WPI_TalonFX(Constants.kHorizontalElevatorCanId); + private CANCoder horizontalExtensionEncoder = new CANCoder(Constants.kHorizontalElevatorEncoderCanId); + + private final ElevatorFeedforward horizontalExtensionFeedfoward = new ElevatorFeedforward( + Config.kHorizontalExtensionKS, Config.kHorizontalExtensionKG, + Config.kHorizontalExtensionKV, Config.kHorizontalExtensionKA + ); + + public HorizontalExtension() { + super( + new ProfiledPIDController( + Config.kHorizontalExtensionKP, + 0, + 0, + new TrapezoidProfile.Constraints( + Config.kHorizontalExtensionMaxVelocity, + Config.kHorizontalExtensionMaxAcceleration)), + 0); + // Start arm at rest in neutral position + setGoal(Config.kHorizontalExtensionNeutralPosition + Config.kHorizontalExtensionEncoderOffset); + } + + @Override + public void useOutput(double output, TrapezoidProfile.State setpoint) { + // Calculate the feedforward from the sepoint + double feedforward = horizontalExtensionFeedfoward.calculate(setpoint.position, setpoint.velocity); + // Add the feedforward to the PID output to get the motor output + horizontalExtension.setVoltage(output + feedforward); + } + + @Override + public double getMeasurement() { + return horizontalExtensionEncoder.getPosition() / Config.kHorizontalExtensionEncoderPPR + Config.kHorizontalExtensionEncoderOffset; + } + +} From 83bffee17d56966e37d2feeec98895c9c4efc4f9 Mon Sep 17 00:00:00 2001 From: LordBBQ <46541720+LordBBQ@users.noreply.github.com> Date: Fri, 13 Jan 2023 19:14:30 -0800 Subject: [PATCH 03/18] Programmed elevators --- ChargedUp/.OutlineViewer/outlineviewer.json | 46 +++++++++++++++ .../frc/robot/Commands/CommandController.java | 4 ++ ChargedUp/src/main/java/frc/robot/Config.java | 10 ++++ ChargedUp/src/main/java/frc/robot/Robot.java | 2 + .../robot/Subsystems/HorizontalExtension.java | 5 +- .../robot/Subsystems/VerticalExtension.java | 58 +++++++++++++++++++ 6 files changed, 124 insertions(+), 1 deletion(-) create mode 100644 ChargedUp/.OutlineViewer/outlineviewer.json create mode 100644 ChargedUp/src/main/java/frc/robot/Subsystems/VerticalExtension.java diff --git a/ChargedUp/.OutlineViewer/outlineviewer.json b/ChargedUp/.OutlineViewer/outlineviewer.json new file mode 100644 index 0000000..c30396e --- /dev/null +++ b/ChargedUp/.OutlineViewer/outlineviewer.json @@ -0,0 +1,46 @@ +{ + "Clients": { + "open": true + }, + "Connections": { + "open": true + }, + "NetworkTables Settings": { + "mode": "Client (NT4)", + "serverTeam": "5985" + }, + "Server": { + "open": true + }, + "retained": { + "LiveWindow": { + "open": true + } + }, + "shuffleboard": { + "Publishers": { + "open": true + }, + "Subscribers": { + "open": true + }, + "open": true + }, + "transitory": { + "FMSInfo": { + "open": true + }, + "LiveWindow": { + "open": true + }, + "Shuffleboard": { + ".metadata": { + "open": true + }, + ".recording": { + "open": true + }, + "open": true + } + } +} diff --git a/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java b/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java index 18874a1..6514983 100644 --- a/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java +++ b/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java @@ -5,6 +5,7 @@ package frc.robot.Commands; import edu.wpi.first.wpilibj2.command.button.CommandJoystick; +import frc.robot.Config; import frc.robot.Subsystems.Drive; /** Add your docs here. */ @@ -29,6 +30,9 @@ public void configureBindings() { () -> m_driverJoystick.getX(), () -> m_driverJoystick.getY())); } + + + } diff --git a/ChargedUp/src/main/java/frc/robot/Config.java b/ChargedUp/src/main/java/frc/robot/Config.java index 47903b0..4476fba 100644 --- a/ChargedUp/src/main/java/frc/robot/Config.java +++ b/ChargedUp/src/main/java/frc/robot/Config.java @@ -46,6 +46,16 @@ public class Config { public static final double kVerticalExtensionNeutralPosition = 0; public static final double kVerticalExtensionEncoderOffset = 0; + /** + * Elevator constants + * ALL UNITS ARE METRES (M) + */ + public static final double kElevatorBaseWidth = 30*0.0254; //30 inches to m + public static final double kVerticalExtensionPerpendicularHeight = 40*0.0254; + public static final double kElevatorOffestFromFront = 0; + public static final double kElevatorVerticalExtensionLegnth = Math.sqrt(Math.pow(kVerticalExtensionPerpendicularHeight, 2) + Math.pow(kVerticalExtensionPerpendicularHeight, 2)); + + diff --git a/ChargedUp/src/main/java/frc/robot/Robot.java b/ChargedUp/src/main/java/frc/robot/Robot.java index 3406aa0..74b824e 100644 --- a/ChargedUp/src/main/java/frc/robot/Robot.java +++ b/ChargedUp/src/main/java/frc/robot/Robot.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.Commands.CommandController; +import frc.robot.Subsystems.Drive; /** * 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 @@ -38,6 +39,7 @@ public void robotInit() { @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); + //Drive.checkCanDevices(); } diff --git a/ChargedUp/src/main/java/frc/robot/Subsystems/HorizontalExtension.java b/ChargedUp/src/main/java/frc/robot/Subsystems/HorizontalExtension.java index 0e0683a..76e94d7 100644 --- a/ChargedUp/src/main/java/frc/robot/Subsystems/HorizontalExtension.java +++ b/ChargedUp/src/main/java/frc/robot/Subsystems/HorizontalExtension.java @@ -7,7 +7,6 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; -import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; @@ -52,4 +51,8 @@ public double getMeasurement() { return horizontalExtensionEncoder.getPosition() / Config.kHorizontalExtensionEncoderPPR + Config.kHorizontalExtensionEncoderOffset; } + public double calculateHorizontalExtensionGoal(double x, double y) { + return Config.kVerticalExtensionPerpendicularHeight * Math.cos(Math.atan(Config.kElevatorBaseWidth / Config.kVerticalExtensionPerpendicularHeight)); + } + } diff --git a/ChargedUp/src/main/java/frc/robot/Subsystems/VerticalExtension.java b/ChargedUp/src/main/java/frc/robot/Subsystems/VerticalExtension.java new file mode 100644 index 0000000..0d3ec34 --- /dev/null +++ b/ChargedUp/src/main/java/frc/robot/Subsystems/VerticalExtension.java @@ -0,0 +1,58 @@ +// 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.can.WPI_TalonFX; +import com.ctre.phoenix.sensors.CANCoder; + +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; +import frc.robot.Config; +import frc.robot.Constants; + +/** Add your docs here. */ +public class VerticalExtension extends ProfiledPIDSubsystem { + private WPI_TalonFX verticalExtension = new WPI_TalonFX(Constants.kVerticalElevatorCanId); + private CANCoder verticalExtensionEncoder = new CANCoder(Constants.kVerticalElevatorEncoderCanId); + + private final ElevatorFeedforward verticalExtensionFeedfoward = new ElevatorFeedforward( + Config.kVerticalExtensionKS, Config.kVerticalExtensionKG, + Config.kVerticalExtensionKV, Config.kVerticalExtensionKA + ); + + public VerticalExtension() { + super( + new ProfiledPIDController( + Config.kVerticalExtensionKP, + 0, + 0, + new TrapezoidProfile.Constraints( + Config.kVerticalExtensionMaxVelocity, + Config.kVerticalExtensionMaxAcceleration)), + 0); + // Start arm at rest in neutral position + setGoal(Config.kVerticalExtensionNeutralPosition + Config.kVerticalExtensionEncoderOffset); + } + + @Override + public void useOutput(double output, TrapezoidProfile.State setpoint) { + // Calculate the feedforward from the sepoint + double feedforward = verticalExtensionFeedfoward.calculate(setpoint.position, setpoint.velocity); + // Add the feedforward to the PID output to get the motor output + verticalExtension.setVoltage(output + feedforward); + } + + @Override + public double getMeasurement() { + return verticalExtensionEncoder.getPosition() / Config.kVerticalExtensionEncoderPPR + Config.kVerticalExtensionEncoderOffset; + } + + public double calculateVerticalExtensionGoal(double x, double y) { + return x * Math.cos(180 - Math.atan(Config.kElevatorBaseWidth / Config.kVerticalExtensionPerpendicularHeight)) - y * Math.sin(180 - Math.atan(Config.kElevatorBaseWidth / Config.kVerticalExtensionPerpendicularHeight)); + } + +} From d0309759b1c0b10848e7979fc307f59913929400 Mon Sep 17 00:00:00 2001 From: LordBBQ <46541720+LordBBQ@users.noreply.github.com> Date: Fri, 13 Jan 2023 19:15:03 -0800 Subject: [PATCH 04/18] made basic can errorchecking --- .../main/java/frc/robot/Subsystems/Drive.java | 41 +++++++++++++++---- 1 file changed, 33 insertions(+), 8 deletions(-) diff --git a/ChargedUp/src/main/java/frc/robot/Subsystems/Drive.java b/ChargedUp/src/main/java/frc/robot/Subsystems/Drive.java index d1ed195..bb18451 100644 --- a/ChargedUp/src/main/java/frc/robot/Subsystems/Drive.java +++ b/ChargedUp/src/main/java/frc/robot/Subsystems/Drive.java @@ -13,12 +13,12 @@ public class Drive extends SubsystemBase{ - private WPI_TalonFX leftDriveA = new WPI_TalonFX(Constants.kLeftDriveACanId); - private WPI_TalonFX leftDriveB = new WPI_TalonFX(Constants.kLeftDriveBCanId); - private WPI_TalonFX leftDriveC = new WPI_TalonFX(Constants.kLeftDriveCCanId); - private WPI_TalonFX rightDriveA = new WPI_TalonFX(Constants.kRightDriveACanId); - private WPI_TalonFX rightDriveB = new WPI_TalonFX(Constants.kRightDriveBCanId); - private WPI_TalonFX rightDriveC = new WPI_TalonFX(Constants.kRightDriveCCanId); + private static WPI_TalonFX leftDriveA = new WPI_TalonFX(Constants.kLeftDriveACanId); + private static WPI_TalonFX leftDriveB = new WPI_TalonFX(Constants.kLeftDriveBCanId); + private static WPI_TalonFX leftDriveC = new WPI_TalonFX(Constants.kLeftDriveCCanId); + private static WPI_TalonFX rightDriveA = new WPI_TalonFX(Constants.kRightDriveACanId); + private static WPI_TalonFX rightDriveB = new WPI_TalonFX(Constants.kRightDriveBCanId); + private static WPI_TalonFX rightDriveC = new WPI_TalonFX(Constants.kRightDriveCCanId); private final MotorControllerGroup leftDrive = new MotorControllerGroup(leftDriveA, leftDriveB, leftDriveC); private final MotorControllerGroup rightDrive = new MotorControllerGroup(rightDriveA, rightDriveB, rightDriveC); @@ -113,7 +113,32 @@ public CommandBase driveDistanceCommand(double distanceMeters, double speed) { .finallyDo(interrupted -> driveMotors.stopMotor()); } - - + /** + * Tests if any motors are disconnected from CANBus. Will print and return if so! + * @return true if any of the drive assosciated devices are disconnected from CAN + */ + public static boolean checkCanDevices() { + boolean canOk = true; + if(leftDriveA.getFirmwareVersion() == -1) { + System.out.println("WARNING leftDriveA missing from CANBus!"); + canOk = false; + } else if(leftDriveB.getFirmwareVersion() == -1) { + System.out.println("WARNING leftDriveB missing from CANBus!"); + canOk = false; + } else if(leftDriveC.getFirmwareVersion() == -1) { + System.out.println("WARNING leftDriveC missing from CANBus!"); + canOk = false; + } else if(rightDriveA.getFirmwareVersion() == -1) { + System.out.println("WARNING rightDriveA missing from CANBus!"); + canOk = false; + } else if(rightDriveB.getFirmwareVersion() == -1) { + System.out.println("WARNING rightDriveB missing from CANBus!"); + canOk = false; + } else if(rightDriveC.getFirmwareVersion() == -1) { + System.out.println("WARNING rightDriveC missing from CANBus!"); + canOk = false; + } + return canOk; + } } From c9ef0063fab9b8438f7c90c63f0878a14d31ba0d Mon Sep 17 00:00:00 2001 From: LordBBQ <46541720+LordBBQ@users.noreply.github.com> Date: Fri, 13 Jan 2023 19:16:02 -0800 Subject: [PATCH 05/18] fixed bug --- ChargedUp/.OutlineViewer/outlineviewer.json | 46 ++++++++++++++++++++ ChargedUp/src/main/java/frc/robot/Robot.java | 1 - 2 files changed, 46 insertions(+), 1 deletion(-) create mode 100644 ChargedUp/.OutlineViewer/outlineviewer.json diff --git a/ChargedUp/.OutlineViewer/outlineviewer.json b/ChargedUp/.OutlineViewer/outlineviewer.json new file mode 100644 index 0000000..c30396e --- /dev/null +++ b/ChargedUp/.OutlineViewer/outlineviewer.json @@ -0,0 +1,46 @@ +{ + "Clients": { + "open": true + }, + "Connections": { + "open": true + }, + "NetworkTables Settings": { + "mode": "Client (NT4)", + "serverTeam": "5985" + }, + "Server": { + "open": true + }, + "retained": { + "LiveWindow": { + "open": true + } + }, + "shuffleboard": { + "Publishers": { + "open": true + }, + "Subscribers": { + "open": true + }, + "open": true + }, + "transitory": { + "FMSInfo": { + "open": true + }, + "LiveWindow": { + "open": true + }, + "Shuffleboard": { + ".metadata": { + "open": true + }, + ".recording": { + "open": true + }, + "open": true + } + } +} diff --git a/ChargedUp/src/main/java/frc/robot/Robot.java b/ChargedUp/src/main/java/frc/robot/Robot.java index 3406aa0..bc957d2 100644 --- a/ChargedUp/src/main/java/frc/robot/Robot.java +++ b/ChargedUp/src/main/java/frc/robot/Robot.java @@ -38,7 +38,6 @@ public void robotInit() { @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); - } /** From 1c31ee254794fd37983a26af4b3c935505e80eec Mon Sep 17 00:00:00 2001 From: LordBBQ <46541720+LordBBQ@users.noreply.github.com> Date: Sun, 15 Jan 2023 20:36:17 -0800 Subject: [PATCH 06/18] Updated to allow proper control of brakes --- ChargedUp/src/main/java/frc/robot/Robot.java | 14 ++++++-- .../main/java/frc/robot/Subsystems/Drive.java | 36 +++++++++++++++++++ 2 files changed, 48 insertions(+), 2 deletions(-) diff --git a/ChargedUp/src/main/java/frc/robot/Robot.java b/ChargedUp/src/main/java/frc/robot/Robot.java index bc957d2..dd5888a 100644 --- a/ChargedUp/src/main/java/frc/robot/Robot.java +++ b/ChargedUp/src/main/java/frc/robot/Robot.java @@ -7,6 +7,7 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.Commands.CommandController; +import frc.robot.Subsystems.Drive; /** * 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 @@ -26,6 +27,8 @@ public class Robot extends TimedRobot { @Override public void robotInit() { m_robot.configureBindings(); + Drive.setBrakes(false); + } /** @@ -52,6 +55,7 @@ public void robotPeriodic() { */ @Override public void autonomousInit() { + Drive.setBrakes(true); } @@ -63,7 +67,10 @@ public void autonomousPeriodic() { /** This function is called once when teleop is enabled. */ @Override - public void teleopInit() {} + public void teleopInit() { + Drive.setBrakes(true); + + } /** This function is called periodically during operator control. */ @Override @@ -71,7 +78,10 @@ public void teleopPeriodic() {} /** This function is called once when the robot is disabled. */ @Override - public void disabledInit() {} + public void disabledInit() { + Drive.setBrakes(false); + + } /** This function is called periodically when disabled. */ @Override diff --git a/ChargedUp/src/main/java/frc/robot/Subsystems/Drive.java b/ChargedUp/src/main/java/frc/robot/Subsystems/Drive.java index bb18451..ccfe3ae 100644 --- a/ChargedUp/src/main/java/frc/robot/Subsystems/Drive.java +++ b/ChargedUp/src/main/java/frc/robot/Subsystems/Drive.java @@ -2,6 +2,7 @@ import java.util.function.DoubleSupplier; +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonFXInvertType; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; @@ -25,6 +26,8 @@ public class Drive extends SubsystemBase{ private final DifferentialDrive driveMotors = new DifferentialDrive(leftDrive, rightDrive); + private static boolean brakeState = false; + public void initSystem() { leftDriveA.configFactoryDefault(); @@ -42,6 +45,7 @@ public void initSystem() { public Drive() { initSystem(); + setBrakes(false); } /** @@ -141,4 +145,36 @@ public static boolean checkCanDevices() { return canOk; } + /** + * + * @param brakes true for brakes enabled + */ + public static void setBrakes(boolean brakes) { + if(brakes) { + leftDriveA.setNeutralMode(NeutralMode.Brake); + leftDriveB.setNeutralMode(NeutralMode.Brake); + leftDriveC.setNeutralMode(NeutralMode.Brake); + rightDriveA.setNeutralMode(NeutralMode.Brake); + rightDriveB.setNeutralMode(NeutralMode.Brake); + rightDriveC.setNeutralMode(NeutralMode.Brake); + brakeState = true; + } else { + leftDriveA.setNeutralMode(NeutralMode.Coast); + leftDriveB.setNeutralMode(NeutralMode.Coast); + leftDriveC.setNeutralMode(NeutralMode.Coast); + rightDriveA.setNeutralMode(NeutralMode.Coast); + rightDriveB.setNeutralMode(NeutralMode.Coast); + rightDriveC.setNeutralMode(NeutralMode.Coast); + brakeState = false; + } + } + + /** + * + * @return true if brakes are enabled + */ + public boolean getBrakes() { + return brakeState; + } + } From c7ad6be48a41453eb9b34c6b561ac4e89ea2b1bc Mon Sep 17 00:00:00 2001 From: LordBBQ <46541720+LordBBQ@users.noreply.github.com> Date: Mon, 16 Jan 2023 00:46:08 -0800 Subject: [PATCH 07/18] Wrist code based of ArmFeedForward --- .../frc/robot/Commands/CommandController.java | 12 +++ ChargedUp/src/main/java/frc/robot/Config.java | 12 +++ .../src/main/java/frc/robot/Constants.java | 3 + .../robot/Subsystems/HorizontalExtension.java | 81 ++++++++++--------- .../robot/Subsystems/VerticalExtension.java | 77 +++++++++--------- .../main/java/frc/robot/Subsystems/Wrist.java | 58 +++++++++++++ 6 files changed, 167 insertions(+), 76 deletions(-) create mode 100644 ChargedUp/src/main/java/frc/robot/Subsystems/Wrist.java diff --git a/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java b/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java index 6514983..379db22 100644 --- a/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java +++ b/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java @@ -4,15 +4,23 @@ package frc.robot.Commands; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandJoystick; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Config; import frc.robot.Subsystems.Drive; +import frc.robot.Subsystems.VerticalExtension; +import frc.robot.Subsystems.HorizontalExtension; /** Add your docs here. */ public class CommandController { private final Drive m_drive = new Drive(); + private final HorizontalExtension m_horizontalEtension = new HorizontalExtension(); + private final VerticalExtension m_verticalExtension = new VerticalExtension(); CommandJoystick m_driverJoystick = new CommandJoystick(0); + CommandXboxController m_driverHID = new CommandXboxController(1); + /** * Use this method to define bindings between conditions and commands. These are useful for @@ -28,6 +36,10 @@ public void configureBindings() { m_drive.setDefaultCommand( m_drive.arcadeDriveCommand( () -> m_driverJoystick.getX(), () -> m_driverJoystick.getY())); + + m_driverHID.a().onTrue(m_horizontalEtension.setArmGoalCommand(HorizontalExtension.calculateHorizontalExtensionGoal(1, 1))); + m_driverHID.a().onTrue(m_verticalExtension.setArmGoalCommand(VerticalExtension.calculateVerticalExtensionGoal(1, 1))); + } diff --git a/ChargedUp/src/main/java/frc/robot/Config.java b/ChargedUp/src/main/java/frc/robot/Config.java index 4476fba..c029446 100644 --- a/ChargedUp/src/main/java/frc/robot/Config.java +++ b/ChargedUp/src/main/java/frc/robot/Config.java @@ -46,6 +46,18 @@ public class Config { public static final double kVerticalExtensionNeutralPosition = 0; public static final double kVerticalExtensionEncoderOffset = 0; + public static final double kWristKS = 0; + public static final double kWristKV = 0; + public static final double kWristKG = 0; + public static final double kWristKA = 0; + public static final double kWristKP = 0; + public static final double kWristMaxVelocity = 0; + public static final double kWristMaxAcceleration = 0; + public static final double kWristEncoderPPR = 4096; //CANCoder, falcon is 2048 EPR + public static final double kWristNeutralPosition = 0; + public static final double kWristEncoderOffset = 0; + + /** * Elevator constants * ALL UNITS ARE METRES (M) diff --git a/ChargedUp/src/main/java/frc/robot/Constants.java b/ChargedUp/src/main/java/frc/robot/Constants.java index 6b9f2f2..7b04897 100644 --- a/ChargedUp/src/main/java/frc/robot/Constants.java +++ b/ChargedUp/src/main/java/frc/robot/Constants.java @@ -25,8 +25,11 @@ public class Constants { public static final int kVerticalElevatorCanId = 10; public static final int kHorizontalElevatorCanId = 9; + public static final int kWristMotorCanId = 11; public static final int kVerticalElevatorEncoderCanId = 21; public static final int kHorizontalElevatorEncoderCanId = 22; + public static final int kWristEncoderCanId = 23; + diff --git a/ChargedUp/src/main/java/frc/robot/Subsystems/HorizontalExtension.java b/ChargedUp/src/main/java/frc/robot/Subsystems/HorizontalExtension.java index 76e94d7..0b47ab8 100644 --- a/ChargedUp/src/main/java/frc/robot/Subsystems/HorizontalExtension.java +++ b/ChargedUp/src/main/java/frc/robot/Subsystems/HorizontalExtension.java @@ -4,54 +4,57 @@ package frc.robot.Subsystems; +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import edu.wpi.first.math.controller.ElevatorFeedforward; -import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.TrapezoidProfileSubsystem; import frc.robot.Config; import frc.robot.Constants; /** Add your docs here. */ -public class HorizontalExtension extends ProfiledPIDSubsystem { - private WPI_TalonFX horizontalExtension = new WPI_TalonFX(Constants.kHorizontalElevatorCanId); - private CANCoder horizontalExtensionEncoder = new CANCoder(Constants.kHorizontalElevatorEncoderCanId); - - private final ElevatorFeedforward horizontalExtensionFeedfoward = new ElevatorFeedforward( - Config.kHorizontalExtensionKS, Config.kHorizontalExtensionKG, - Config.kHorizontalExtensionKV, Config.kHorizontalExtensionKA - ); - - public HorizontalExtension() { - super( - new ProfiledPIDController( - Config.kHorizontalExtensionKP, - 0, - 0, - new TrapezoidProfile.Constraints( - Config.kHorizontalExtensionMaxVelocity, - Config.kHorizontalExtensionMaxAcceleration)), - 0); - // Start arm at rest in neutral position - setGoal(Config.kHorizontalExtensionNeutralPosition + Config.kHorizontalExtensionEncoderOffset); - } - - @Override - public void useOutput(double output, TrapezoidProfile.State setpoint) { - // Calculate the feedforward from the sepoint - double feedforward = horizontalExtensionFeedfoward.calculate(setpoint.position, setpoint.velocity); - // Add the feedforward to the PID output to get the motor output - horizontalExtension.setVoltage(output + feedforward); - } - - @Override - public double getMeasurement() { - return horizontalExtensionEncoder.getPosition() / Config.kHorizontalExtensionEncoderPPR + Config.kHorizontalExtensionEncoderOffset; - } - - public double calculateHorizontalExtensionGoal(double x, double y) { +public class HorizontalExtension extends TrapezoidProfileSubsystem { + + private final WPI_TalonFX horizontalExtensionMotor = new WPI_TalonFX(Constants.kHorizontalElevatorCanId); + private final CANCoder horizontalExtensionEncoder = new CANCoder(Constants.kHorizontalElevatorEncoderCanId); + private final ElevatorFeedforward m_feedforward = new ElevatorFeedforward( + Config.kVerticalExtensionKS, Config.kVerticalExtensionKG, + Config.kVerticalExtensionKV, Config.kVerticalExtensionKA + ); + + /** Create a new ArmSubsystem. */ + public HorizontalExtension() { + super( + new TrapezoidProfile.Constraints( + Config.kMaxSpeedMetersPerSecond, Config.kMaxAccelerationMetersPerSecondSquared), + Config.kHorizontalExtensionEncoderOffset); + horizontalExtensionMotor.config_kP(0, Config.kHorizontalExtensionKP); + horizontalExtensionMotor.configRemoteFeedbackFilter(horizontalExtensionEncoder, 0); + horizontalExtensionMotor.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor0); + + } + + @Override + public void useState(TrapezoidProfile.State setpoint) { + // Calculate the feedforward from the sepoint + double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity); + + // Add the feedforward to the PID output to get the motor output + horizontalExtensionMotor.config_kF(0, feedforward); + horizontalExtensionMotor.set(ControlMode.Position, setpoint.position); + + } + + public Command setArmGoalCommand(double kArmOffsetRads) { + return Commands.runOnce(() -> setGoal(kArmOffsetRads), this); + } + + public static double calculateHorizontalExtensionGoal(double x, double y) { return Config.kVerticalExtensionPerpendicularHeight * Math.cos(Math.atan(Config.kElevatorBaseWidth / Config.kVerticalExtensionPerpendicularHeight)); } diff --git a/ChargedUp/src/main/java/frc/robot/Subsystems/VerticalExtension.java b/ChargedUp/src/main/java/frc/robot/Subsystems/VerticalExtension.java index 0d3ec34..859978c 100644 --- a/ChargedUp/src/main/java/frc/robot/Subsystems/VerticalExtension.java +++ b/ChargedUp/src/main/java/frc/robot/Subsystems/VerticalExtension.java @@ -4,52 +4,55 @@ package frc.robot.Subsystems; +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import edu.wpi.first.math.controller.ElevatorFeedforward; -import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.TrapezoidProfileSubsystem; import frc.robot.Config; import frc.robot.Constants; /** Add your docs here. */ -public class VerticalExtension extends ProfiledPIDSubsystem { - private WPI_TalonFX verticalExtension = new WPI_TalonFX(Constants.kVerticalElevatorCanId); - private CANCoder verticalExtensionEncoder = new CANCoder(Constants.kVerticalElevatorEncoderCanId); - - private final ElevatorFeedforward verticalExtensionFeedfoward = new ElevatorFeedforward( - Config.kVerticalExtensionKS, Config.kVerticalExtensionKG, - Config.kVerticalExtensionKV, Config.kVerticalExtensionKA - ); - - public VerticalExtension() { - super( - new ProfiledPIDController( - Config.kVerticalExtensionKP, - 0, - 0, - new TrapezoidProfile.Constraints( - Config.kVerticalExtensionMaxVelocity, - Config.kVerticalExtensionMaxAcceleration)), - 0); - // Start arm at rest in neutral position - setGoal(Config.kVerticalExtensionNeutralPosition + Config.kVerticalExtensionEncoderOffset); - } - - @Override - public void useOutput(double output, TrapezoidProfile.State setpoint) { - // Calculate the feedforward from the sepoint - double feedforward = verticalExtensionFeedfoward.calculate(setpoint.position, setpoint.velocity); - // Add the feedforward to the PID output to get the motor output - verticalExtension.setVoltage(output + feedforward); - } - - @Override - public double getMeasurement() { - return verticalExtensionEncoder.getPosition() / Config.kVerticalExtensionEncoderPPR + Config.kVerticalExtensionEncoderOffset; - } +public class VerticalExtension extends TrapezoidProfileSubsystem { + + private final WPI_TalonFX verticalExtensionMotor = new WPI_TalonFX(Constants.kHorizontalElevatorCanId); + private final CANCoder verticalExtensionEncoder = new CANCoder(Constants.kHorizontalElevatorEncoderCanId); + private final ElevatorFeedforward m_feedforward = new ElevatorFeedforward( + Config.kVerticalExtensionKS, Config.kVerticalExtensionKG, + Config.kVerticalExtensionKV, Config.kVerticalExtensionKA + ); + + /** Create a new ArmSubsystem. */ + public VerticalExtension() { + super( + new TrapezoidProfile.Constraints( + Config.kMaxSpeedMetersPerSecond, Config.kMaxAccelerationMetersPerSecondSquared), + Config.kVerticalExtensionEncoderOffset); + verticalExtensionMotor.config_kP(0, Config.kVerticalExtensionKP); + verticalExtensionMotor.configRemoteFeedbackFilter(verticalExtensionEncoder, 0); + verticalExtensionMotor.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor0); + + } + + @Override + public void useState(TrapezoidProfile.State setpoint) { + // Calculate the feedforward from the sepoint + double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity); + + // Add the feedforward to the PID output to get the motor output + verticalExtensionMotor.config_kF(0, feedforward); + verticalExtensionMotor.set(ControlMode.Position, setpoint.position); + + } + + public Command setArmGoalCommand(double kArmOffsetRads) { + return Commands.runOnce(() -> setGoal(kArmOffsetRads), this); + } public double calculateVerticalExtensionGoal(double x, double y) { return x * Math.cos(180 - Math.atan(Config.kElevatorBaseWidth / Config.kVerticalExtensionPerpendicularHeight)) - y * Math.sin(180 - Math.atan(Config.kElevatorBaseWidth / Config.kVerticalExtensionPerpendicularHeight)); diff --git a/ChargedUp/src/main/java/frc/robot/Subsystems/Wrist.java b/ChargedUp/src/main/java/frc/robot/Subsystems/Wrist.java new file mode 100644 index 0000000..a27e98d --- /dev/null +++ b/ChargedUp/src/main/java/frc/robot/Subsystems/Wrist.java @@ -0,0 +1,58 @@ +// 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.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import com.ctre.phoenix.sensors.CANCoder; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.TrapezoidProfileSubsystem; +import frc.robot.Config; +import frc.robot.Constants; + +/** Add your docs here. */ +public class Wrist extends TrapezoidProfileSubsystem { + + private final WPI_TalonFX wristMotor = new WPI_TalonFX(Constants.kWristMotorCanId); + private final CANCoder wristEncoder = new CANCoder(Constants.kWristEncoderCanId); + private final ArmFeedforward m_feedforward = new ArmFeedforward( + Config.kWristKS, Config.kWristKG, + Config.kWristKV, Config.kWristKA + ); + + /** Create a new ArmSubsystem. */ + public Wrist() { + super( + new TrapezoidProfile.Constraints( + Config.kWristMaxVelocity, Config.kWristMaxAcceleration), + Config.kWristEncoderOffset); + wristMotor.config_kP(0, Config.kHorizontalExtensionKP); + wristMotor.configRemoteFeedbackFilter(wristEncoder, 0); + wristMotor.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor0); + + } + + @Override + public void useState(TrapezoidProfile.State setpoint) { + // Calculate the feedforward from the sepoint + double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity); + + // Add the feedforward to the PID output to get the motor output + wristMotor.config_kF(0, feedforward); + wristMotor.set(ControlMode.Position, setpoint.position); + + } + + public Command setWristGoalCommand(double kWristEncoderOffset) { + return Commands.runOnce(() -> setGoal(kWristEncoderOffset), this); + } + + +} From 59be55c4b2a5d51a76bc49c42c1ae846848c1cc9 Mon Sep 17 00:00:00 2001 From: LordBBQ <46541720+LordBBQ@users.noreply.github.com> Date: Mon, 16 Jan 2023 19:08:32 -0800 Subject: [PATCH 08/18] removed warnings --- .../src/main/java/frc/robot/Commands/CommandController.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java b/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java index 379db22..bc3a4e1 100644 --- a/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java +++ b/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java @@ -4,10 +4,8 @@ package frc.robot.Commands; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.Config; import frc.robot.Subsystems.Drive; import frc.robot.Subsystems.VerticalExtension; import frc.robot.Subsystems.HorizontalExtension; From c6abdade9822b2eb949d8ee2ca042d21f0a0723d Mon Sep 17 00:00:00 2001 From: LordBBQ <46541720+LordBBQ@users.noreply.github.com> Date: Mon, 16 Jan 2023 19:49:25 -0800 Subject: [PATCH 09/18] added a bunch of comments --- .../frc/robot/Commands/CommandController.java | 2 +- ChargedUp/src/main/java/frc/robot/Config.java | 16 +++---- ChargedUp/src/main/java/frc/robot/Robot.java | 10 ++--- .../main/java/frc/robot/Subsystems/Drive.java | 45 ++++++++++++++----- 4 files changed, 47 insertions(+), 26 deletions(-) diff --git a/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java b/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java index 18874a1..d86c672 100644 --- a/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java +++ b/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java @@ -11,7 +11,7 @@ public class CommandController { private final Drive m_drive = new Drive(); - CommandJoystick m_driverJoystick = new CommandJoystick(0); + CommandJoystick m_driverJoystick = new CommandJoystick(0); //Setup driver joystick in port 0 on DS /** * Use this method to define bindings between conditions and commands. These are useful for diff --git a/ChargedUp/src/main/java/frc/robot/Config.java b/ChargedUp/src/main/java/frc/robot/Config.java index e5913dc..ae2f4ae 100644 --- a/ChargedUp/src/main/java/frc/robot/Config.java +++ b/ChargedUp/src/main/java/frc/robot/Config.java @@ -5,24 +5,24 @@ public class Config { /** * Drive characterisation values //TODO */ - public static final double ksVolts = 0.21511; - public static final double kvVoltSecondsPerMeter = 1.1369; + public static final double ksVolts = 0.21511; //constant of voltage needed to overcome static friction etc. + public static final double kvVoltSecondsPerMeter = 1.1369; //velocity constant public static final double kaVoltSecondsSquaredPerMeter = 0.3275; - public static final double kPDriveVel = 0.54175; + public static final double kPDriveVel = 0.54175; //preportional - public static final double kTrackwidthMeters = 0.762; + public static final double kTrackwidthMeters = 0.762; //distance in metres between the centre of the each set of wheels on the sides public static final double kMaxSpeedMetersPerSecond = 2.5915; public static final double kMaxAccelerationMetersPerSecondSquared = 1; // Reasonable baseline values for a RAMSETE follower in units of meters and seconds - public static final double kRamseteB = 2; - public static final double kRamseteZeta = 0.7; + public static final double kRamseteB = 2; //preportional term + public static final double kRamseteZeta = 0.7; //dampening - public static final double wheelDiameterMetres = (6*2.54)/100; + public static final double wheelDiameterMetres = (6*2.54)/100; //6 inches to m public static final double wheelCircumferenceMetres = wheelDiameterMetres * Math.PI; - public static final double kRatioMotorToWheel = (1/11.1/wheelCircumferenceMetres); + public static final double kRatioMotorToWheel = (1/11.1/wheelCircumferenceMetres); //gearbox is 11.1:1 diff --git a/ChargedUp/src/main/java/frc/robot/Robot.java b/ChargedUp/src/main/java/frc/robot/Robot.java index dd5888a..cb526d6 100644 --- a/ChargedUp/src/main/java/frc/robot/Robot.java +++ b/ChargedUp/src/main/java/frc/robot/Robot.java @@ -26,8 +26,8 @@ public class Robot extends TimedRobot { */ @Override public void robotInit() { - m_robot.configureBindings(); - Drive.setBrakes(false); + m_robot.configureBindings(); //setup bindings for drive, mechanisms etc. + Drive.setBrakes(false); //disable brakes so robot can be pushed } @@ -55,7 +55,7 @@ public void robotPeriodic() { */ @Override public void autonomousInit() { - Drive.setBrakes(true); + Drive.setBrakes(true); //run brakes } @@ -68,7 +68,7 @@ public void autonomousPeriodic() { /** This function is called once when teleop is enabled. */ @Override public void teleopInit() { - Drive.setBrakes(true); + Drive.setBrakes(true); //run brakes } @@ -79,7 +79,7 @@ public void teleopPeriodic() {} /** This function is called once when the robot is disabled. */ @Override public void disabledInit() { - Drive.setBrakes(false); + Drive.setBrakes(false); //disable brakes so robot is pushable } diff --git a/ChargedUp/src/main/java/frc/robot/Subsystems/Drive.java b/ChargedUp/src/main/java/frc/robot/Subsystems/Drive.java index ccfe3ae..ffadd48 100644 --- a/ChargedUp/src/main/java/frc/robot/Subsystems/Drive.java +++ b/ChargedUp/src/main/java/frc/robot/Subsystems/Drive.java @@ -14,6 +14,19 @@ public class Drive extends SubsystemBase{ + /* + * Setup drive motors + * + * Front + * ====| |==== + * ||LA RA|| + * ||LB RB|| + * ||LC RC|| + * || || + * || || + * ============ + */ + private static WPI_TalonFX leftDriveA = new WPI_TalonFX(Constants.kLeftDriveACanId); private static WPI_TalonFX leftDriveB = new WPI_TalonFX(Constants.kLeftDriveBCanId); private static WPI_TalonFX leftDriveC = new WPI_TalonFX(Constants.kLeftDriveCCanId); @@ -21,31 +34,37 @@ public class Drive extends SubsystemBase{ private static WPI_TalonFX rightDriveB = new WPI_TalonFX(Constants.kRightDriveBCanId); private static WPI_TalonFX rightDriveC = new WPI_TalonFX(Constants.kRightDriveCCanId); + //Setup objects for use with the DifferentialDrive private final MotorControllerGroup leftDrive = new MotorControllerGroup(leftDriveA, leftDriveB, leftDriveC); private final MotorControllerGroup rightDrive = new MotorControllerGroup(rightDriveA, rightDriveB, rightDriveC); private final DifferentialDrive driveMotors = new DifferentialDrive(leftDrive, rightDrive); - private static boolean brakeState = false; + private static boolean brakeState = false; //Define default state for the brakes + /** + * Configure all motor controllers, sensors, etc. of this subsystem + * This means that in theory, on a controller fail, all that is needed to reconfigure + * the new ones is to change the CAN ID and it will pick up its config. + */ public void initSystem() { - leftDriveA.configFactoryDefault(); + leftDriveA.configFactoryDefault(); //Reset all settings on the drive motors leftDriveB.configFactoryDefault(); leftDriveC.configFactoryDefault(); rightDriveA.configFactoryDefault(); rightDriveB.configFactoryDefault(); rightDriveC.configFactoryDefault(); - rightDriveA.setInverted(TalonFXInvertType.CounterClockwise); - rightDriveB.setInverted(TalonFXInvertType.CounterClockwise); + rightDriveA.setInverted(TalonFXInvertType.CounterClockwise); //Invert the right side meaning that a foward command + rightDriveB.setInverted(TalonFXInvertType.CounterClockwise); //will result in all motors flashing green rightDriveC.setInverted(TalonFXInvertType.CounterClockwise); } public Drive() { - initSystem(); - setBrakes(false); + initSystem(); //run the above method to initalise the controllers + setBrakes(false); //Set the falcons to coast mode on robot init } /** @@ -57,7 +76,7 @@ public Drive() { public CommandBase arcadeDriveCommand(DoubleSupplier fwd, DoubleSupplier rot) { // A split-stick arcade command, with forward/backward controlled by the left // hand, and turning controlled by the right. - return run(() -> driveMotors.arcadeDrive(fwd.getAsDouble(), rot.getAsDouble())) + return run(() -> driveMotors.arcadeDrive(fwd.getAsDouble(), rot.getAsDouble())) //run the WPILIB arcadeDrive method with supplied values .withName("arcadeDrive"); } @@ -82,7 +101,7 @@ public void resetDriveEncoders() { public double getLeftDriveEncodersDistanceMetres() { return ((leftDriveA.getSelectedSensorPosition() + leftDriveB.getSelectedSensorPosition())/2 );// + - //leftDriveC.getSelectedSensorPosition()) / 3); + //leftDriveC.getSelectedSensorPosition()) / 3); //comment for pegasus config. } /** @@ -92,7 +111,7 @@ public double getLeftDriveEncodersDistanceMetres() { public double getRightDriveEncodersDistanceMetres() { return ((rightDriveA.getSelectedSensorPosition() + rightDriveB.getSelectedSensorPosition()) /2 ); //+ - // rightDriveC.getSelectedSensorPosition()) / 3); + // rightDriveC.getSelectedSensorPosition()) / 3); //comment for pegasus config } /** @@ -104,14 +123,14 @@ public double getRightDriveEncodersDistanceMetres() { public CommandBase driveDistanceCommand(double distanceMeters, double speed) { return runOnce( () -> { - resetDriveEncoders(); + resetDriveEncoders(); //check encoders are zeroed }) // Drive forward at specified speed .andThen(run(() -> driveMotors.arcadeDrive(speed, 0))) // End command when we've traveled the specified distance .until( () -> - Math.max(getLeftDriveEncodersDistanceMetres(), getRightDriveEncodersDistanceMetres()) + Math.max(getLeftDriveEncodersDistanceMetres(), getRightDriveEncodersDistanceMetres()) //check have traveled far enough >= distanceMeters) // Stop the drive when the command ends .finallyDo(interrupted -> driveMotors.stopMotor()); @@ -120,6 +139,8 @@ public CommandBase driveDistanceCommand(double distanceMeters, double speed) { /** * Tests if any motors are disconnected from CANBus. Will print and return if so! * @return true if any of the drive assosciated devices are disconnected from CAN + * This works by requesting the falcon's firmware verison, which will return -1 if + * the device is not on the bus */ public static boolean checkCanDevices() { boolean canOk = true; @@ -174,7 +195,7 @@ public static void setBrakes(boolean brakes) { * @return true if brakes are enabled */ public boolean getBrakes() { - return brakeState; + return brakeState; //locally updated variable to track brake state } } From e050c147e6199140c12ee8e604cb7c7c2bd9de55 Mon Sep 17 00:00:00 2001 From: LordBBQ <46541720+LordBBQ@users.noreply.github.com> Date: Mon, 16 Jan 2023 19:50:15 -0800 Subject: [PATCH 10/18] removed errors --- ChargedUp/src/main/java/frc/robot/Robot.java | 1 - .../src/main/java/frc/robot/Subsystems/VerticalExtension.java | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/ChargedUp/src/main/java/frc/robot/Robot.java b/ChargedUp/src/main/java/frc/robot/Robot.java index 74b824e..a8208dd 100644 --- a/ChargedUp/src/main/java/frc/robot/Robot.java +++ b/ChargedUp/src/main/java/frc/robot/Robot.java @@ -7,7 +7,6 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.Commands.CommandController; -import frc.robot.Subsystems.Drive; /** * 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 diff --git a/ChargedUp/src/main/java/frc/robot/Subsystems/VerticalExtension.java b/ChargedUp/src/main/java/frc/robot/Subsystems/VerticalExtension.java index 859978c..252c405 100644 --- a/ChargedUp/src/main/java/frc/robot/Subsystems/VerticalExtension.java +++ b/ChargedUp/src/main/java/frc/robot/Subsystems/VerticalExtension.java @@ -54,7 +54,7 @@ public Command setArmGoalCommand(double kArmOffsetRads) { return Commands.runOnce(() -> setGoal(kArmOffsetRads), this); } - public double calculateVerticalExtensionGoal(double x, double y) { + public static double calculateVerticalExtensionGoal(double x, double y) { return x * Math.cos(180 - Math.atan(Config.kElevatorBaseWidth / Config.kVerticalExtensionPerpendicularHeight)) - y * Math.sin(180 - Math.atan(Config.kElevatorBaseWidth / Config.kVerticalExtensionPerpendicularHeight)); } From ba411b33b9b621deed2d79d6f55567b81ac7cc4c Mon Sep 17 00:00:00 2001 From: LordBBQ <46541720+LordBBQ@users.noreply.github.com> Date: Mon, 16 Jan 2023 19:53:34 -0800 Subject: [PATCH 11/18] commented code --- .../src/main/java/frc/robot/Commands/CommandController.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java b/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java index bc3a4e1..d9ef360 100644 --- a/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java +++ b/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java @@ -16,8 +16,8 @@ public class CommandController { private final Drive m_drive = new Drive(); private final HorizontalExtension m_horizontalEtension = new HorizontalExtension(); private final VerticalExtension m_verticalExtension = new VerticalExtension(); - CommandJoystick m_driverJoystick = new CommandJoystick(0); - CommandXboxController m_driverHID = new CommandXboxController(1); + CommandJoystick m_driverJoystick = new CommandJoystick(0); //declare joystick on ds port 0 + CommandXboxController m_driverHID = new CommandXboxController(1); //declare xbox on ds port 1 /** From a84af5d4f6331791c6cec3107fe7ab951b0595d4 Mon Sep 17 00:00:00 2001 From: LordBBQ <46541720+LordBBQ@users.noreply.github.com> Date: Mon, 16 Jan 2023 20:09:02 -0800 Subject: [PATCH 12/18] documented stuff, fixed bug where there were still mentions of the wrong motors --- .../robot/Subsystems/HorizontalExtension.java | 44 +++++++++++++++---- .../robot/Subsystems/VerticalExtension.java | 43 +++++++++++++++--- 2 files changed, 71 insertions(+), 16 deletions(-) diff --git a/ChargedUp/src/main/java/frc/robot/Subsystems/HorizontalExtension.java b/ChargedUp/src/main/java/frc/robot/Subsystems/HorizontalExtension.java index 0b47ab8..7d85483 100644 --- a/ChargedUp/src/main/java/frc/robot/Subsystems/HorizontalExtension.java +++ b/ChargedUp/src/main/java/frc/robot/Subsystems/HorizontalExtension.java @@ -8,6 +8,7 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix.sensors.SensorInitializationStrategy; import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.trajectory.TrapezoidProfile; @@ -21,24 +22,39 @@ public class HorizontalExtension extends TrapezoidProfileSubsystem { private final WPI_TalonFX horizontalExtensionMotor = new WPI_TalonFX(Constants.kHorizontalElevatorCanId); - private final CANCoder horizontalExtensionEncoder = new CANCoder(Constants.kHorizontalElevatorEncoderCanId); - private final ElevatorFeedforward m_feedforward = new ElevatorFeedforward( - Config.kVerticalExtensionKS, Config.kVerticalExtensionKG, - Config.kVerticalExtensionKV, Config.kVerticalExtensionKA + private final CANCoder horizontalExtensionEncoder = new CANCoder(Constants.kHorizontalElevatorEncoderCanId); //we want an absolute encoder for reliabality + private final ElevatorFeedforward m_feedforward = new ElevatorFeedforward( //define feedFoward control for the elevator + Config.kHorizontalExtensionKS, Config.kHorizontalExtensionKG, //kS is the static friction constant, kG is the gravity constant + Config.kHorizontalExtensionKV, Config.kHorizontalExtensionKA //kV is the velocity constant, kA is the acceleration constant ); + //this subsystem uses a combination of a feedfoward and feedback control. + //this gives us the advantage of better profiling from feedfoward and better precision from feedback. + /** Create a new ArmSubsystem. */ public HorizontalExtension() { + //define a new trapezoid profile using wpi libraries. Configure the motor. super( new TrapezoidProfile.Constraints( Config.kMaxSpeedMetersPerSecond, Config.kMaxAccelerationMetersPerSecondSquared), Config.kHorizontalExtensionEncoderOffset); + initSystem(); + } + + /** + * Reset and configure sensors, motors, and encoders. + */ + public void initSystem() { + horizontalExtensionMotor.configFactoryDefault(); //reset and configure the motor so we know it is correctly configured horizontalExtensionMotor.config_kP(0, Config.kHorizontalExtensionKP); horizontalExtensionMotor.configRemoteFeedbackFilter(horizontalExtensionEncoder, 0); horizontalExtensionMotor.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor0); + horizontalExtensionEncoder.configFactoryDefault(); //reset and configure the encoder + horizontalExtensionEncoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition); } + @Override public void useState(TrapezoidProfile.State setpoint) { // Calculate the feedforward from the sepoint @@ -46,16 +62,26 @@ public void useState(TrapezoidProfile.State setpoint) { // Add the feedforward to the PID output to get the motor output horizontalExtensionMotor.config_kF(0, feedforward); - horizontalExtensionMotor.set(ControlMode.Position, setpoint.position); - + horizontalExtensionMotor.set(ControlMode.Position, setpoint.position); } + /** + * Run arm to position + * @param kArmOffsetRads + * @return when arm is at desired position + */ public Command setArmGoalCommand(double kArmOffsetRads) { return Commands.runOnce(() -> setGoal(kArmOffsetRads), this); } - public static double calculateHorizontalExtensionGoal(double x, double y) { - return Config.kVerticalExtensionPerpendicularHeight * Math.cos(Math.atan(Config.kElevatorBaseWidth / Config.kVerticalExtensionPerpendicularHeight)); - } + /** + * Method to calculate the desired porition of the motor based off a target x and y position. + * @param x desired x position + * @param y desired y position + * @return the desired setpoint for the extension + */ + public static double calculateHorizontalExtensionGoal(double x, double y) { + return Config.kVerticalExtensionPerpendicularHeight * Math.cos(Math.atan(Config.kElevatorBaseWidth / Config.kVerticalExtensionPerpendicularHeight)); + } } diff --git a/ChargedUp/src/main/java/frc/robot/Subsystems/VerticalExtension.java b/ChargedUp/src/main/java/frc/robot/Subsystems/VerticalExtension.java index 252c405..fdb4b84 100644 --- a/ChargedUp/src/main/java/frc/robot/Subsystems/VerticalExtension.java +++ b/ChargedUp/src/main/java/frc/robot/Subsystems/VerticalExtension.java @@ -8,6 +8,7 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; +import com.ctre.phoenix.sensors.SensorInitializationStrategy; import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.trajectory.TrapezoidProfile; @@ -20,15 +21,19 @@ /** Add your docs here. */ public class VerticalExtension extends TrapezoidProfileSubsystem { - private final WPI_TalonFX verticalExtensionMotor = new WPI_TalonFX(Constants.kHorizontalElevatorCanId); - private final CANCoder verticalExtensionEncoder = new CANCoder(Constants.kHorizontalElevatorEncoderCanId); + private final WPI_TalonFX verticalExtensionMotor = new WPI_TalonFX(Constants.kVerticalElevatorCanId); + private final CANCoder verticalExtensionEncoder = new CANCoder(Constants.kVerticalElevatorEncoderCanId); + + //this subsystem uses a combination of a feedfoward and feedback control. + //this gives us the advantage of better profiling from feedfoward and better precision from feedback. private final ElevatorFeedforward m_feedforward = new ElevatorFeedforward( - Config.kVerticalExtensionKS, Config.kVerticalExtensionKG, - Config.kVerticalExtensionKV, Config.kVerticalExtensionKA + Config.kVerticalExtensionKS, Config.kVerticalExtensionKG, //kS is the static friction constant, kG is the gravity constant + Config.kVerticalExtensionKV, Config.kVerticalExtensionKA //kV is the velocity constant, kA is the acceleration constant ); /** Create a new ArmSubsystem. */ public VerticalExtension() { + //define a new trapezoid profile using wpi libraries. Configure the motor. super( new TrapezoidProfile.Constraints( Config.kMaxSpeedMetersPerSecond, Config.kMaxAccelerationMetersPerSecondSquared), @@ -39,6 +44,19 @@ public VerticalExtension() { } + /** + * Reset and configure sensors, motors, and encoders. + */ + public void initSystem() { + verticalExtensionMotor.configFactoryDefault(); //reset and configure the motor so we know it is correctly configured + verticalExtensionMotor.config_kP(0, Config.kVerticalExtensionKP); + verticalExtensionMotor.configRemoteFeedbackFilter(verticalExtensionEncoder, 0); + verticalExtensionMotor.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor0); + + verticalExtensionEncoder.configFactoryDefault(); //reset and configure the encoder + verticalExtensionEncoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition); + } + @Override public void useState(TrapezoidProfile.State setpoint) { // Calculate the feedforward from the sepoint @@ -50,12 +68,23 @@ public void useState(TrapezoidProfile.State setpoint) { } + /** + * Run arm to position + * @param kArmOffsetRads + * @return when arm is at desired position + */ public Command setArmGoalCommand(double kArmOffsetRads) { return Commands.runOnce(() -> setGoal(kArmOffsetRads), this); } - public static double calculateVerticalExtensionGoal(double x, double y) { - return x * Math.cos(180 - Math.atan(Config.kElevatorBaseWidth / Config.kVerticalExtensionPerpendicularHeight)) - y * Math.sin(180 - Math.atan(Config.kElevatorBaseWidth / Config.kVerticalExtensionPerpendicularHeight)); - } + /** + * Method to calculate the desired porition of the motor based off a target x and y position. + * @param x desired x position + * @param y desired y position + * @return the desired setpoint for the extension + */ + public static double calculateVerticalExtensionGoal(double x, double y) { + return x * Math.cos(180 - Math.atan(Config.kElevatorBaseWidth / Config.kVerticalExtensionPerpendicularHeight)) - y * Math.sin(180 - Math.atan(Config.kElevatorBaseWidth / Config.kVerticalExtensionPerpendicularHeight)); + } } From cb010526fb0316875fb10adabde7b770e7269a30 Mon Sep 17 00:00:00 2001 From: LordBBQ <46541720+LordBBQ@users.noreply.github.com> Date: Thu, 19 Jan 2023 18:11:45 -0800 Subject: [PATCH 13/18] Create Pneumatics.java --- .../java/frc/robot/Subsystems/Pneumatics.java | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) create mode 100644 ChargedUp/src/main/java/frc/robot/Subsystems/Pneumatics.java diff --git a/ChargedUp/src/main/java/frc/robot/Subsystems/Pneumatics.java b/ChargedUp/src/main/java/frc/robot/Subsystems/Pneumatics.java new file mode 100644 index 0000000..c962d80 --- /dev/null +++ b/ChargedUp/src/main/java/frc/robot/Subsystems/Pneumatics.java @@ -0,0 +1,19 @@ +// 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 edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Pneumatics extends SubsystemBase { + /** Creates a new Pneumatics. */ + public Pneumatics() { + + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} From a1221e0ea98dc430745528d5a40e0afc61c00bc2 Mon Sep 17 00:00:00 2001 From: LordBBQ <46541720+LordBBQ@users.noreply.github.com> Date: Thu, 19 Jan 2023 19:42:44 -0800 Subject: [PATCH 14/18] Updated wrist code to use pneumatics --- .../java/frc/robot/Commands/LowerWrist.java | 30 +++++++ .../java/frc/robot/Commands/RaiseWrist.java | 30 +++++++ ChargedUp/src/main/java/frc/robot/Config.java | 20 ++--- ChargedUp/src/main/java/frc/robot/Robot.java | 6 ++ .../java/frc/robot/Subsystems/Pneumatics.java | 19 ----- .../main/java/frc/robot/Subsystems/Wrist.java | 59 +++++-------- .../java/frc/robot/Utilities/Diagnostics.java | 8 ++ .../java/frc/robot/Utilities/Pneumatics.java | 82 ++++++++++++++++++- 8 files changed, 185 insertions(+), 69 deletions(-) create mode 100644 ChargedUp/src/main/java/frc/robot/Commands/LowerWrist.java create mode 100644 ChargedUp/src/main/java/frc/robot/Commands/RaiseWrist.java delete mode 100644 ChargedUp/src/main/java/frc/robot/Subsystems/Pneumatics.java create mode 100644 ChargedUp/src/main/java/frc/robot/Utilities/Diagnostics.java diff --git a/ChargedUp/src/main/java/frc/robot/Commands/LowerWrist.java b/ChargedUp/src/main/java/frc/robot/Commands/LowerWrist.java new file mode 100644 index 0000000..890f96d --- /dev/null +++ b/ChargedUp/src/main/java/frc/robot/Commands/LowerWrist.java @@ -0,0 +1,30 @@ +// 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.Wrist; +import frc.robot.Subsystems.Wrist.WristPosition; + +public class LowerWrist extends CommandBase { + private final Wrist m_wrist; + /** Creates a new RaiseWrist. */ + public LowerWrist(Wrist subsystem) { + m_wrist = subsystem; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_wrist.setWristPosition(WristPosition.LOWERED); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; //should investigate delay here TODO + } +} diff --git a/ChargedUp/src/main/java/frc/robot/Commands/RaiseWrist.java b/ChargedUp/src/main/java/frc/robot/Commands/RaiseWrist.java new file mode 100644 index 0000000..8fa48e4 --- /dev/null +++ b/ChargedUp/src/main/java/frc/robot/Commands/RaiseWrist.java @@ -0,0 +1,30 @@ +// 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.Wrist; +import frc.robot.Subsystems.Wrist.WristPosition; + +public class RaiseWrist extends CommandBase { + private final Wrist m_wrist; + /** Creates a new RaiseWrist. */ + public RaiseWrist(Wrist subsystem) { + m_wrist = subsystem; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_wrist.setWristPosition(WristPosition.RAISED); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; //should investigate delay here TODO + } +} diff --git a/ChargedUp/src/main/java/frc/robot/Config.java b/ChargedUp/src/main/java/frc/robot/Config.java index 7bcabd2..715d1c1 100644 --- a/ChargedUp/src/main/java/frc/robot/Config.java +++ b/ChargedUp/src/main/java/frc/robot/Config.java @@ -46,17 +46,8 @@ public class Config { public static final double kVerticalExtensionNeutralPosition = 0; public static final double kVerticalExtensionEncoderOffset = 0; - public static final double kWristKS = 0; - public static final double kWristKV = 0; - public static final double kWristKG = 0; - public static final double kWristKA = 0; - public static final double kWristKP = 0; - public static final double kWristMaxVelocity = 0; - public static final double kWristMaxAcceleration = 0; - public static final double kWristEncoderPPR = 4096; //CANCoder, falcon is 2048 EPR - public static final double kWristNeutralPosition = 0; - public static final double kWristEncoderOffset = 0; - + public static final int kWristFowardPortId = 0; + public static final int kWristReversePortId = 1; /** * Elevator constants @@ -67,7 +58,14 @@ public class Config { public static final double kElevatorOffestFromFront = 0; public static final double kElevatorVerticalExtensionLegnth = Math.sqrt(Math.pow(kVerticalExtensionPerpendicularHeight, 2) + Math.pow(kVerticalExtensionPerpendicularHeight, 2)); + /** + * Pneumatics constants + */ + public static final double kPneumaticsMinPressure = 110; //The pressure the pneumatics will start charging at + public static final double kPneumaticsMaxPressure = 120; //The pressure the pneumatics will stop charging at + public static final double kCompressorRunCurrent = 10;//Ampres. Used for error checking //TODO + public static final byte kCompressorCheckIterations = 50;//50 cycles of code. Checks that the compressor still does not run after x cycles diff --git a/ChargedUp/src/main/java/frc/robot/Robot.java b/ChargedUp/src/main/java/frc/robot/Robot.java index 94ea4f4..61a5edc 100644 --- a/ChargedUp/src/main/java/frc/robot/Robot.java +++ b/ChargedUp/src/main/java/frc/robot/Robot.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.Commands.CommandController; import frc.robot.Subsystems.Drive; +import frc.robot.Utilities.Pneumatics; /** * 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 @@ -16,6 +17,8 @@ */ public class Robot extends TimedRobot { + private Pneumatics m_pneumatics = Pneumatics.getInstance(); + private final CommandController m_robot = new CommandController(); @@ -56,6 +59,7 @@ public void robotPeriodic() { @Override public void autonomousInit() { Drive.setBrakes(true); //run brakes + m_pneumatics.setPneumatics(true); } @@ -69,6 +73,7 @@ public void autonomousPeriodic() { @Override public void teleopInit() { Drive.setBrakes(true); //run brakes + m_pneumatics.setPneumatics(true); } @@ -80,6 +85,7 @@ public void teleopPeriodic() {} @Override public void disabledInit() { Drive.setBrakes(false); //disable brakes so robot is pushable + m_pneumatics.setPneumatics(false); } /** This function is called periodically when disabled. */ diff --git a/ChargedUp/src/main/java/frc/robot/Subsystems/Pneumatics.java b/ChargedUp/src/main/java/frc/robot/Subsystems/Pneumatics.java deleted file mode 100644 index c962d80..0000000 --- a/ChargedUp/src/main/java/frc/robot/Subsystems/Pneumatics.java +++ /dev/null @@ -1,19 +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.robot.Subsystems; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class Pneumatics extends SubsystemBase { - /** Creates a new Pneumatics. */ - public Pneumatics() { - - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } -} diff --git a/ChargedUp/src/main/java/frc/robot/Subsystems/Wrist.java b/ChargedUp/src/main/java/frc/robot/Subsystems/Wrist.java index a27e98d..caa2d6a 100644 --- a/ChargedUp/src/main/java/frc/robot/Subsystems/Wrist.java +++ b/ChargedUp/src/main/java/frc/robot/Subsystems/Wrist.java @@ -4,55 +4,40 @@ package frc.robot.Subsystems; -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import com.ctre.phoenix.sensors.CANCoder; - -import edu.wpi.first.math.controller.ArmFeedforward; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.TrapezoidProfileSubsystem; +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.PneumaticsModuleType; +import edu.wpi.first.wpilibj.DoubleSolenoid.Value; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Config; -import frc.robot.Constants; /** Add your docs here. */ -public class Wrist extends TrapezoidProfileSubsystem { +public class Wrist extends SubsystemBase { - private final WPI_TalonFX wristMotor = new WPI_TalonFX(Constants.kWristMotorCanId); - private final CANCoder wristEncoder = new CANCoder(Constants.kWristEncoderCanId); - private final ArmFeedforward m_feedforward = new ArmFeedforward( - Config.kWristKS, Config.kWristKG, - Config.kWristKV, Config.kWristKA - ); + public enum WristPosition { + RAISED, + LOWERED, + } + private DoubleSolenoid wristSolenoid = new DoubleSolenoid(PneumaticsModuleType.REVPH, Config.kWristFowardPortId, Config.kWristReversePortId); /** Create a new ArmSubsystem. */ public Wrist() { - super( - new TrapezoidProfile.Constraints( - Config.kWristMaxVelocity, Config.kWristMaxAcceleration), - Config.kWristEncoderOffset); - wristMotor.config_kP(0, Config.kHorizontalExtensionKP); - wristMotor.configRemoteFeedbackFilter(wristEncoder, 0); - wristMotor.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor0); + + } + /** + * Set wrist to desired position + * @param wristPosition desired wrist position + */ + public void setWristPosition(WristPosition wristPosition) { + if(wristPosition == WristPosition.LOWERED) { + wristSolenoid.set(Value.kReverse); + } else if(wristPosition == WristPosition.RAISED) { + wristSolenoid.set(Value.kForward); + } } - @Override - public void useState(TrapezoidProfile.State setpoint) { - // Calculate the feedforward from the sepoint - double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity); - // Add the feedforward to the PID output to get the motor output - wristMotor.config_kF(0, feedforward); - wristMotor.set(ControlMode.Position, setpoint.position); - - } - public Command setWristGoalCommand(double kWristEncoderOffset) { - return Commands.runOnce(() -> setGoal(kWristEncoderOffset), this); - } } diff --git a/ChargedUp/src/main/java/frc/robot/Utilities/Diagnostics.java b/ChargedUp/src/main/java/frc/robot/Utilities/Diagnostics.java new file mode 100644 index 0000000..fe3066f --- /dev/null +++ b/ChargedUp/src/main/java/frc/robot/Utilities/Diagnostics.java @@ -0,0 +1,8 @@ +// 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.Utilities; + +/** Add your docs here. */ +public class Diagnostics {} diff --git a/ChargedUp/src/main/java/frc/robot/Utilities/Pneumatics.java b/ChargedUp/src/main/java/frc/robot/Utilities/Pneumatics.java index a0d1ad2..f02a5eb 100644 --- a/ChargedUp/src/main/java/frc/robot/Utilities/Pneumatics.java +++ b/ChargedUp/src/main/java/frc/robot/Utilities/Pneumatics.java @@ -1,5 +1,83 @@ +// 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.Utilities; -public class Pneumatics { - +import edu.wpi.first.wpilibj.Compressor; +import edu.wpi.first.wpilibj.PneumaticsModuleType; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Config; + +public class Pneumatics extends SubsystemBase { + + private Compressor comp = new Compressor(PneumaticsModuleType.REVPH); + private boolean compEnabled = false; //we can change this when we enable and disable the compressor. This means we can keep track of faults! + private byte compCheckIteration = 0; //iterated once per cycle of suspected compressor fault. + private static Pneumatics m_instance; + /** Creates a new Pneumatics. */ + public Pneumatics() { + + } + + public static Pneumatics getInstance() { + if(m_instance == null) { + m_instance = new Pneumatics(); + } + return m_instance; + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + /** + * Enable or disable pneumatics with pressure values set from config file + * @param boolean enable pneumatics + */ + public void setPneumatics(boolean enabled) { + if(enabled) { + compEnabled = true; + comp.enableAnalog(Config.kPneumaticsMinPressure, Config.kPneumaticsMaxPressure); + } else { + compEnabled = false; + comp.disable(); + } + } + + /** + * Checks for potential compressor fault. Checks the following: + * - Compressor running + * - no current fault + * - Compressor stopped + * - current drawn fault + * @return if fault detected with compressor + */ + public boolean getCompressorCurrentFault() { + if(compEnabled) { + if(comp.getCurrent() <= Config.kCompressorRunCurrent) { + compCheckIteration ++; + if(compCheckIteration >= Config.kCompressorCheckIterations) { + return true; + } else { + return false; + } + } else { + return false; + } + } else { + if(comp.getCurrent() >= Config.kCompressorRunCurrent) { + compCheckIteration ++; + if(compCheckIteration >= Config.kCompressorCheckIterations) { + return true; + } else { + return false; + } + } else { + return false; + } + } + } + } From 1310e22919c4251de5dad22353707d3fffc2a001 Mon Sep 17 00:00:00 2001 From: LordBBQ <46541720+LordBBQ@users.noreply.github.com> Date: Thu, 19 Jan 2023 21:03:12 -0800 Subject: [PATCH 15/18] Added more command stuff --- .../robot/Commands/Arm/ArmHomePosCommand.java | 48 +++++++++++++++++++ .../robot/Commands/{ => Arm}/LowerWrist.java | 2 +- .../robot/Commands/{ => Arm}/RaiseWrist.java | 2 +- ChargedUp/src/main/java/frc/robot/Config.java | 33 +++++++++++++ .../robot/Subsystems/VerticalExtension.java | 21 +++++++- 5 files changed, 102 insertions(+), 4 deletions(-) create mode 100644 ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHomePosCommand.java rename ChargedUp/src/main/java/frc/robot/Commands/{ => Arm}/LowerWrist.java (96%) rename ChargedUp/src/main/java/frc/robot/Commands/{ => Arm}/RaiseWrist.java (96%) diff --git a/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHomePosCommand.java b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHomePosCommand.java new file mode 100644 index 0000000..c7f791b --- /dev/null +++ b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHomePosCommand.java @@ -0,0 +1,48 @@ +// 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.Arm; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Config; +import frc.robot.Subsystems.HorizontalExtension; +import frc.robot.Subsystems.VerticalExtension; +import frc.robot.Subsystems.Wrist; + +public class ArmHomePosCommand extends CommandBase { + private final Wrist m_wrist; + private final HorizontalExtension m_horizontalExtension; + private final VerticalExtension m_verticalExtension; + /** Creates a new ArmHomePosCommand. */ + public ArmHomePosCommand(Wrist wristSubsystem, VerticalExtension verticalExtensionSubsystem, HorizontalExtension horizontalExtensionSubsystem) { + m_wrist = wristSubsystem; + m_verticalExtension = verticalExtensionSubsystem; + m_horizontalExtension = horizontalExtensionSubsystem; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_wrist.setWristPosition(Config.kArmHomePosWrist); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_verticalExtension.setArmGoalCommand(m_verticalExtension.calculateVerticalExtensionGoal(Config.kArmHomePosX, Config.kArmHomePosY)); + m_horizontalExtension.setArmGoalCommand(m_horizontalExtension.calculateHorizontalExtensionGoal(Config.kArmHomePosX, Config.kArmHomePosY)); + + } + + // 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/ChargedUp/src/main/java/frc/robot/Commands/LowerWrist.java b/ChargedUp/src/main/java/frc/robot/Commands/Arm/LowerWrist.java similarity index 96% rename from ChargedUp/src/main/java/frc/robot/Commands/LowerWrist.java rename to ChargedUp/src/main/java/frc/robot/Commands/Arm/LowerWrist.java index 890f96d..edf0a0a 100644 --- a/ChargedUp/src/main/java/frc/robot/Commands/LowerWrist.java +++ b/ChargedUp/src/main/java/frc/robot/Commands/Arm/LowerWrist.java @@ -2,7 +2,7 @@ // 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.Arm; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.Subsystems.Wrist; diff --git a/ChargedUp/src/main/java/frc/robot/Commands/RaiseWrist.java b/ChargedUp/src/main/java/frc/robot/Commands/Arm/RaiseWrist.java similarity index 96% rename from ChargedUp/src/main/java/frc/robot/Commands/RaiseWrist.java rename to ChargedUp/src/main/java/frc/robot/Commands/Arm/RaiseWrist.java index 8fa48e4..0fb30cb 100644 --- a/ChargedUp/src/main/java/frc/robot/Commands/RaiseWrist.java +++ b/ChargedUp/src/main/java/frc/robot/Commands/Arm/RaiseWrist.java @@ -2,7 +2,7 @@ // 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.Arm; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.Subsystems.Wrist; diff --git a/ChargedUp/src/main/java/frc/robot/Config.java b/ChargedUp/src/main/java/frc/robot/Config.java index 715d1c1..800f20f 100644 --- a/ChargedUp/src/main/java/frc/robot/Config.java +++ b/ChargedUp/src/main/java/frc/robot/Config.java @@ -1,5 +1,7 @@ package frc.robot; +import frc.robot.Subsystems.Wrist.WristPosition; + public class Config { /** @@ -32,6 +34,8 @@ public class Config { public static final double kHorizontalExtensionMaxVelocity = 0; public static final double kHorizontalExtensionMaxAcceleration = 0; public static final double kHorizontalExtensionEncoderPPR = 4096; //CANCoder, falcon is 2048 EPR + public static final double kHorizontalExtensionMetresPerRotation = 0.2; + public static final double kHorizontalExtensionPositionTollerenceMetres = 0.1; public static final double kHorizontalExtensionNeutralPosition = 0; public static final double kHorizontalExtensionEncoderOffset = 0; @@ -43,12 +47,41 @@ public class Config { public static final double kVerticalExtensionMaxVelocity = 0; public static final double kVerticalExtensionMaxAcceleration = 0; public static final double kVerticalExtensionEncoderPPR = 4096; //CANCoder, falcon is 2048 EPR + public static final double kVerticalExtensionMetresPerRotation = 0.2; + public static final double kVerticalExtensionPositionTollerenceMetres = 0.1; + public static final double kVerticalExtensionNeutralPosition = 0; public static final double kVerticalExtensionEncoderOffset = 0; public static final int kWristFowardPortId = 0; public static final int kWristReversePortId = 1; + /** + * Constants for arm positions for different elements of gameplay + * ALL MEASUREMENTS IN METRES!!! + */ + + public static final double kArmHomePosX = 0; + public static final double kArmHomePosY = 0; + public static final WristPosition kArmHomePosWrist = WristPosition.LOWERED; + + public static final double kArmLowPosX = 0.1; + public static final double kArmLowPosY = 0.5; + public static final WristPosition kArmLowPosWrist = WristPosition.LOWERED; + + + public static final double kArmMedPosX = 0.5; + public static final double kArmMedPosY = 0.7; + public static final WristPosition kArmMedPosWrist = WristPosition.RAISED; + + public static final double kArmHighPosX = 1; + public static final double kArmHighPosY = 1.2; + public static final WristPosition kArmHighPosWrist = WristPosition.RAISED; + + public static final double kArmShelfPosX = 1; + public static final double kArmShelfPosY = 1.5; + public static final WristPosition kArmShelfPosWrist = WristPosition.RAISED; + /** * Elevator constants * ALL UNITS ARE METRES (M) diff --git a/ChargedUp/src/main/java/frc/robot/Subsystems/VerticalExtension.java b/ChargedUp/src/main/java/frc/robot/Subsystems/VerticalExtension.java index fdb4b84..e9d47b1 100644 --- a/ChargedUp/src/main/java/frc/robot/Subsystems/VerticalExtension.java +++ b/ChargedUp/src/main/java/frc/robot/Subsystems/VerticalExtension.java @@ -23,6 +23,7 @@ public class VerticalExtension extends TrapezoidProfileSubsystem { private final WPI_TalonFX verticalExtensionMotor = new WPI_TalonFX(Constants.kVerticalElevatorCanId); private final CANCoder verticalExtensionEncoder = new CANCoder(Constants.kVerticalElevatorEncoderCanId); + private double armGoal = 0; //this subsystem uses a combination of a feedfoward and feedback control. //this gives us the advantage of better profiling from feedfoward and better precision from feedback. @@ -74,6 +75,7 @@ public void useState(TrapezoidProfile.State setpoint) { * @return when arm is at desired position */ public Command setArmGoalCommand(double kArmOffsetRads) { + armGoal = kArmOffsetRads; return Commands.runOnce(() -> setGoal(kArmOffsetRads), this); } @@ -81,10 +83,25 @@ public Command setArmGoalCommand(double kArmOffsetRads) { * Method to calculate the desired porition of the motor based off a target x and y position. * @param x desired x position * @param y desired y position - * @return the desired setpoint for the extension + * @return the desired setpoint for the extension in encoder units */ public static double calculateVerticalExtensionGoal(double x, double y) { - return x * Math.cos(180 - Math.atan(Config.kElevatorBaseWidth / Config.kVerticalExtensionPerpendicularHeight)) - y * Math.sin(180 - Math.atan(Config.kElevatorBaseWidth / Config.kVerticalExtensionPerpendicularHeight)); + return x * Math.cos(180 - Math.atan(Config.kElevatorBaseWidth / Config.kVerticalExtensionPerpendicularHeight)) - y * Math.sin(180 - Math.atan(Config.kElevatorBaseWidth / Config.kVerticalExtensionPerpendicularHeight)) * Config.kVerticalExtensionMetresPerRotation; + } + + /** + * + * @return true if arm is within defined position tollerence. + */ + public boolean getArmAtPosition() { + if(getArmPosition() <= armGoal + Config.kVerticalExtensionPositionTollerenceMetres && getArmPosition() >= armGoal - Config.kVerticalExtensionPositionTollerenceMetres) { + return true; + } + return false; + } + + public double getArmPosition() { + return verticalExtensionEncoder.getPosition() / Config.kVerticalExtensionEncoderPPR * Config.kVerticalExtensionMetresPerRotation; } } From ca3ba2735d1f8dd932b9cc52d6f4e455909521c4 Mon Sep 17 00:00:00 2001 From: LordBBQ <46541720+LordBBQ@users.noreply.github.com> Date: Fri, 20 Jan 2023 00:13:57 -0800 Subject: [PATCH 16/18] commands for arm positions --- .../robot/Commands/Arm/ArmHighPosCommand.java | 48 +++++++++++++++++++ .../robot/Commands/Arm/ArmHomePosCommand.java | 4 +- .../robot/Commands/Arm/ArmLowPosCommand.java | 48 +++++++++++++++++++ .../Commands/Arm/ArmShelfPosCommand.java | 48 +++++++++++++++++++ .../frc/robot/Commands/CommandController.java | 7 --- ChargedUp/src/main/java/frc/robot/Config.java | 1 + 6 files changed, 147 insertions(+), 9 deletions(-) create mode 100644 ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHighPosCommand.java create mode 100644 ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmLowPosCommand.java create mode 100644 ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmShelfPosCommand.java diff --git a/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHighPosCommand.java b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHighPosCommand.java new file mode 100644 index 0000000..09f615f --- /dev/null +++ b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHighPosCommand.java @@ -0,0 +1,48 @@ +// 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.Arm; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Config; +import frc.robot.Subsystems.HorizontalExtension; +import frc.robot.Subsystems.VerticalExtension; +import frc.robot.Subsystems.Wrist; + +public class ArmHighPosCommand extends CommandBase { + private final Wrist m_wrist; + private final HorizontalExtension m_horizontalExtension; + private final VerticalExtension m_verticalExtension; + /** Creates a new ArmHighPosCommand. */ + public ArmHighPosCommand(Wrist wristSubsystem, VerticalExtension verticalExtensionSubsystem, HorizontalExtension horizontalExtensionSubsystem) { + m_wrist = wristSubsystem; + m_verticalExtension = verticalExtensionSubsystem; + m_horizontalExtension = horizontalExtensionSubsystem; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_wrist.setWristPosition(Config.kArmHighPosWrist); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_verticalExtension.setArmGoalCommand(VerticalExtension.calculateVerticalExtensionGoal(Config.kArmHighPosX, Config.kArmHighPosY)); + m_horizontalExtension.setArmGoalCommand(HorizontalExtension.calculateHorizontalExtensionGoal(Config.kArmHighPosX, Config.kArmHighPosY)); + + } + + // 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/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHomePosCommand.java b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHomePosCommand.java index c7f791b..418e754 100644 --- a/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHomePosCommand.java +++ b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHomePosCommand.java @@ -31,8 +31,8 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_verticalExtension.setArmGoalCommand(m_verticalExtension.calculateVerticalExtensionGoal(Config.kArmHomePosX, Config.kArmHomePosY)); - m_horizontalExtension.setArmGoalCommand(m_horizontalExtension.calculateHorizontalExtensionGoal(Config.kArmHomePosX, Config.kArmHomePosY)); + m_verticalExtension.setArmGoalCommand(VerticalExtension.calculateVerticalExtensionGoal(Config.kArmHomePosX, Config.kArmHomePosY)); + m_horizontalExtension.setArmGoalCommand(HorizontalExtension.calculateHorizontalExtensionGoal(Config.kArmHomePosX, Config.kArmHomePosY)); } diff --git a/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmLowPosCommand.java b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmLowPosCommand.java new file mode 100644 index 0000000..8d3076d --- /dev/null +++ b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmLowPosCommand.java @@ -0,0 +1,48 @@ +// 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.Arm; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Config; +import frc.robot.Subsystems.HorizontalExtension; +import frc.robot.Subsystems.VerticalExtension; +import frc.robot.Subsystems.Wrist; + +public class ArmLowPosCommand extends CommandBase { + private final Wrist m_wrist; + private final HorizontalExtension m_horizontalExtension; + private final VerticalExtension m_verticalExtension; + /** Creates a new ArmLowPosCommand. */ + public ArmLowPosCommand(Wrist wristSubsystem, VerticalExtension verticalExtensionSubsystem, HorizontalExtension horizontalExtensionSubsystem) { + m_wrist = wristSubsystem; + m_verticalExtension = verticalExtensionSubsystem; + m_horizontalExtension = horizontalExtensionSubsystem; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_wrist.setWristPosition(Config.kArmLowPosWrist); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_verticalExtension.setArmGoalCommand(VerticalExtension.calculateVerticalExtensionGoal(Config.kArmLowPosX, Config.kArmLowPosY)); + m_horizontalExtension.setArmGoalCommand(HorizontalExtension.calculateHorizontalExtensionGoal(Config.kArmLowPosX, Config.kArmLowPosY)); + + } + + // 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/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmShelfPosCommand.java b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmShelfPosCommand.java new file mode 100644 index 0000000..14d0dd3 --- /dev/null +++ b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmShelfPosCommand.java @@ -0,0 +1,48 @@ +// 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.Arm; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Config; +import frc.robot.Subsystems.HorizontalExtension; +import frc.robot.Subsystems.VerticalExtension; +import frc.robot.Subsystems.Wrist; + +public class ArmShelfPosCommand extends CommandBase { + private final Wrist m_wrist; + private final HorizontalExtension m_horizontalExtension; + private final VerticalExtension m_verticalExtension; + /** Creates a new ArmShelfPosCommand. */ + public ArmShelfPosCommand(Wrist wristSubsystem, VerticalExtension verticalExtensionSubsystem, HorizontalExtension horizontalExtensionSubsystem) { + m_wrist = wristSubsystem; + m_verticalExtension = verticalExtensionSubsystem; + m_horizontalExtension = horizontalExtensionSubsystem; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_wrist.setWristPosition(Config.kArmShelfPosWrist); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_verticalExtension.setArmGoalCommand(VerticalExtension.calculateVerticalExtensionGoal(Config.kArmShelfPosX, Config.kArmShelfPosY)); + m_horizontalExtension.setArmGoalCommand(HorizontalExtension.calculateHorizontalExtensionGoal(Config.kArmShelfPosX, Config.kArmShelfPosY)); + + } + + // 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/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java b/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java index d9ef360..6b39a7f 100644 --- a/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java +++ b/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java @@ -7,15 +7,11 @@ import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Subsystems.Drive; -import frc.robot.Subsystems.VerticalExtension; -import frc.robot.Subsystems.HorizontalExtension; /** Add your docs here. */ public class CommandController { private final Drive m_drive = new Drive(); - private final HorizontalExtension m_horizontalEtension = new HorizontalExtension(); - private final VerticalExtension m_verticalExtension = new VerticalExtension(); CommandJoystick m_driverJoystick = new CommandJoystick(0); //declare joystick on ds port 0 CommandXboxController m_driverHID = new CommandXboxController(1); //declare xbox on ds port 1 @@ -34,9 +30,6 @@ public void configureBindings() { m_drive.setDefaultCommand( m_drive.arcadeDriveCommand( () -> m_driverJoystick.getX(), () -> m_driverJoystick.getY())); - - m_driverHID.a().onTrue(m_horizontalEtension.setArmGoalCommand(HorizontalExtension.calculateHorizontalExtensionGoal(1, 1))); - m_driverHID.a().onTrue(m_verticalExtension.setArmGoalCommand(VerticalExtension.calculateVerticalExtensionGoal(1, 1))); } diff --git a/ChargedUp/src/main/java/frc/robot/Config.java b/ChargedUp/src/main/java/frc/robot/Config.java index 800f20f..90577bd 100644 --- a/ChargedUp/src/main/java/frc/robot/Config.java +++ b/ChargedUp/src/main/java/frc/robot/Config.java @@ -95,6 +95,7 @@ public class Config { * Pneumatics constants */ + public static final int kPneumaticsModuleCanId = 51; public static final double kPneumaticsMinPressure = 110; //The pressure the pneumatics will start charging at public static final double kPneumaticsMaxPressure = 120; //The pressure the pneumatics will stop charging at public static final double kCompressorRunCurrent = 10;//Ampres. Used for error checking //TODO From 40491e412e6df2e5889dd387e7b0208cd4915290 Mon Sep 17 00:00:00 2001 From: LordBBQ <46541720+LordBBQ@users.noreply.github.com> Date: Mon, 23 Jan 2023 22:23:59 -0800 Subject: [PATCH 17/18] Ditched profiledPID and trapezoidalProfile for MotionMagic had a lot of issues with the afformentioned methods and poor docs, so switched to inbuilt motion profiling. Needs tuning and has warnings still. --- ChargedUp/.SysId/sysid.json | 2 +- .../robot/Commands/Arm/ArmHighPosCommand.java | 12 +- .../robot/Commands/Arm/ArmHomePosCommand.java | 6 +- .../robot/Commands/Arm/ArmLowPosCommand.java | 3 +- .../Commands/Arm/ArmShelfPosCommand.java | 2 +- .../frc/robot/Commands/CommandController.java | 23 ++++ ChargedUp/src/main/java/frc/robot/Config.java | 19 ++-- .../src/main/java/frc/robot/Constants.java | 2 +- ChargedUp/src/main/java/frc/robot/Robot.java | 9 +- .../robot/Subsystems/HorizontalExtension.java | 18 +++ .../robot/Subsystems/VerticalExtension.java | 107 ++++++++++++------ 11 files changed, 149 insertions(+), 54 deletions(-) diff --git a/ChargedUp/.SysId/sysid.json b/ChargedUp/.SysId/sysid.json index 9057eca..b4b457a 100644 --- a/ChargedUp/.SysId/sysid.json +++ b/ChargedUp/.SysId/sysid.json @@ -1,6 +1,6 @@ { "SysId": { - "Analysis Type": "Drivetrain", + "Analysis Type": "General Mechanism", "NetworkTables Settings": { "serverTeam": "5985" }, diff --git a/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHighPosCommand.java b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHighPosCommand.java index 09f615f..7aa7619 100644 --- a/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHighPosCommand.java +++ b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHighPosCommand.java @@ -26,13 +26,18 @@ public ArmHighPosCommand(Wrist wristSubsystem, VerticalExtension verticalExtensi @Override public void initialize() { m_wrist.setWristPosition(Config.kArmHighPosWrist); + System.out.println("hellow"); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_verticalExtension.setArmGoalCommand(VerticalExtension.calculateVerticalExtensionGoal(Config.kArmHighPosX, Config.kArmHighPosY)); - m_horizontalExtension.setArmGoalCommand(HorizontalExtension.calculateHorizontalExtensionGoal(Config.kArmHighPosX, Config.kArmHighPosY)); + m_verticalExtension.setPosition(2); + //m_verticalExtension.useOutput(0, null); + //m_verticalExtension.setGoal(200); + //m_verticalExtension.enable(); + System.out.println(m_verticalExtension.getArmAtPosition()); + } @@ -43,6 +48,7 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return false; + System.out.println(m_verticalExtension.getArmAtPosition()); + return true;//(m_verticalExtension.getArmAtPosition()); } } diff --git a/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHomePosCommand.java b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHomePosCommand.java index 418e754..dfc9064 100644 --- a/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHomePosCommand.java +++ b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHomePosCommand.java @@ -31,7 +31,9 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_verticalExtension.setArmGoalCommand(VerticalExtension.calculateVerticalExtensionGoal(Config.kArmHomePosX, Config.kArmHomePosY)); + //m_verticalExtension.setArmGoalCommand(VerticalExtension.calculateVerticalExtensionGoal(Config.kArmHomePosX, Config.kArmHomePosY)); + m_verticalExtension.setPosition(0); + m_horizontalExtension.setArmGoalCommand(HorizontalExtension.calculateHorizontalExtensionGoal(Config.kArmHomePosX, Config.kArmHomePosY)); } @@ -43,6 +45,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return true; } } diff --git a/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmLowPosCommand.java b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmLowPosCommand.java index 8d3076d..81e7a47 100644 --- a/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmLowPosCommand.java +++ b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmLowPosCommand.java @@ -31,7 +31,8 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_verticalExtension.setArmGoalCommand(VerticalExtension.calculateVerticalExtensionGoal(Config.kArmLowPosX, Config.kArmLowPosY)); + //m_verticalExtension.setGoal(1); + //m_verticalExtension.enable(); m_horizontalExtension.setArmGoalCommand(HorizontalExtension.calculateHorizontalExtensionGoal(Config.kArmLowPosX, Config.kArmLowPosY)); } diff --git a/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmShelfPosCommand.java b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmShelfPosCommand.java index 14d0dd3..5b17278 100644 --- a/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmShelfPosCommand.java +++ b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmShelfPosCommand.java @@ -31,7 +31,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_verticalExtension.setArmGoalCommand(VerticalExtension.calculateVerticalExtensionGoal(Config.kArmShelfPosX, Config.kArmShelfPosY)); + //m_verticalExtension.setArmGoalCommand(VerticalExtension.calculateVerticalExtensionGoal(Config.kArmShelfPosX, Config.kArmShelfPosY)); m_horizontalExtension.setArmGoalCommand(HorizontalExtension.calculateHorizontalExtensionGoal(Config.kArmShelfPosX, Config.kArmShelfPosY)); } diff --git a/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java b/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java index 6b39a7f..c70a9c6 100644 --- a/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java +++ b/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java @@ -4,18 +4,31 @@ package frc.robot.Commands; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Robot; +import frc.robot.Commands.Arm.ArmHighPosCommand; +import frc.robot.Commands.Arm.ArmHomePosCommand; +import frc.robot.Commands.Arm.ArmLowPosCommand; import frc.robot.Subsystems.Drive; +import frc.robot.Subsystems.HorizontalExtension; +import frc.robot.Subsystems.VerticalExtension; +import frc.robot.Subsystems.Wrist; /** Add your docs here. */ public class CommandController { private final Drive m_drive = new Drive(); + private final Wrist m_wrist = new Wrist(); + private final HorizontalExtension m_horizontal = new HorizontalExtension(); + private final VerticalExtension m_vertical = new VerticalExtension(); CommandJoystick m_driverJoystick = new CommandJoystick(0); //declare joystick on ds port 0 CommandXboxController m_driverHID = new CommandXboxController(1); //declare xbox on ds port 1 + /** * Use this method to define bindings between conditions and commands. These are useful for * automating robot behaviors based on button and sensor input. @@ -30,6 +43,16 @@ public void configureBindings() { m_drive.setDefaultCommand( m_drive.arcadeDriveCommand( () -> m_driverJoystick.getX(), () -> m_driverJoystick.getY())); + + m_driverHID.y() + .onTrue( + new ArmHighPosCommand(m_wrist, m_vertical, m_horizontal) + ); + m_driverHID.a() + .onTrue( + new ArmHomePosCommand(m_wrist, m_vertical, m_horizontal) + ); + } diff --git a/ChargedUp/src/main/java/frc/robot/Config.java b/ChargedUp/src/main/java/frc/robot/Config.java index 90577bd..3bdbf96 100644 --- a/ChargedUp/src/main/java/frc/robot/Config.java +++ b/ChargedUp/src/main/java/frc/robot/Config.java @@ -39,15 +39,16 @@ public class Config { public static final double kHorizontalExtensionNeutralPosition = 0; public static final double kHorizontalExtensionEncoderOffset = 0; - public static final double kVerticalExtensionKS = 0; - public static final double kVerticalExtensionKV = 0; - public static final double kVerticalExtensionKG = 0; - public static final double kVerticalExtensionKA = 0; - public static final double kVerticalExtensionKP = 0; - public static final double kVerticalExtensionMaxVelocity = 0; - public static final double kVerticalExtensionMaxAcceleration = 0; - public static final double kVerticalExtensionEncoderPPR = 4096; //CANCoder, falcon is 2048 EPR - public static final double kVerticalExtensionMetresPerRotation = 0.2; + public static final double kVerticalExtensionKS = -0.0029118; + public static final double kVerticalExtensionKV = 25.116; + public static final double kVerticalExtensionKG = 0.0013625; + public static final double kVerticalExtensionKA = 0.60171; + public static final double kVerticalExtensionKP = 0.3; + public static final double kVerticalExtensionKD = 0; + public static final double kVerticalExtensionMaxVelocity = 2; + public static final double kVerticalExtensionMaxAcceleration = 1; + public static final double kVerticalExtensionEncoderPPR = 2048; //CANCoder, falcon is 2048 EPR + public static final double kVerticalExtensionMetresPerRotation = 0.1196; public static final double kVerticalExtensionPositionTollerenceMetres = 0.1; public static final double kVerticalExtensionNeutralPosition = 0; diff --git a/ChargedUp/src/main/java/frc/robot/Constants.java b/ChargedUp/src/main/java/frc/robot/Constants.java index 7b04897..f59e8f9 100644 --- a/ChargedUp/src/main/java/frc/robot/Constants.java +++ b/ChargedUp/src/main/java/frc/robot/Constants.java @@ -23,7 +23,7 @@ public class Constants { public static final int kRightDriveBCanId = 6; //4 public static final int kRightDriveCCanId = 41; //6 - public static final int kVerticalElevatorCanId = 10; + public static final int kVerticalElevatorCanId = 58;//10; public static final int kHorizontalElevatorCanId = 9; public static final int kWristMotorCanId = 11; public static final int kVerticalElevatorEncoderCanId = 21; diff --git a/ChargedUp/src/main/java/frc/robot/Robot.java b/ChargedUp/src/main/java/frc/robot/Robot.java index 61a5edc..22bf78e 100644 --- a/ChargedUp/src/main/java/frc/robot/Robot.java +++ b/ChargedUp/src/main/java/frc/robot/Robot.java @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.Commands.CommandController; import frc.robot.Subsystems.Drive; +import frc.robot.Subsystems.VerticalExtension; import frc.robot.Utilities.Pneumatics; /** * The VM is configured to automatically run this class, and to call the functions corresponding to @@ -18,6 +19,7 @@ public class Robot extends TimedRobot { private Pneumatics m_pneumatics = Pneumatics.getInstance(); + private static VerticalExtension m_verticalExtension = new VerticalExtension(); private final CommandController m_robot = new CommandController(); @@ -74,12 +76,17 @@ public void autonomousPeriodic() { public void teleopInit() { Drive.setBrakes(true); //run brakes m_pneumatics.setPneumatics(true); + m_verticalExtension.initSystem(); + m_verticalExtension.getMeasurement(); } /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() {} + public void teleopPeriodic() { + System.out.println(m_verticalExtension.getMeasurement()); + + } /** This function is called once when the robot is disabled. */ @Override diff --git a/ChargedUp/src/main/java/frc/robot/Subsystems/HorizontalExtension.java b/ChargedUp/src/main/java/frc/robot/Subsystems/HorizontalExtension.java index 7d85483..6b6727a 100644 --- a/ChargedUp/src/main/java/frc/robot/Subsystems/HorizontalExtension.java +++ b/ChargedUp/src/main/java/frc/robot/Subsystems/HorizontalExtension.java @@ -21,6 +21,8 @@ /** Add your docs here. */ public class HorizontalExtension extends TrapezoidProfileSubsystem { + + private double armGoal = 0; private final WPI_TalonFX horizontalExtensionMotor = new WPI_TalonFX(Constants.kHorizontalElevatorCanId); private final CANCoder horizontalExtensionEncoder = new CANCoder(Constants.kHorizontalElevatorEncoderCanId); //we want an absolute encoder for reliabality private final ElevatorFeedforward m_feedforward = new ElevatorFeedforward( //define feedFoward control for the elevator @@ -71,6 +73,7 @@ public void useState(TrapezoidProfile.State setpoint) { * @return when arm is at desired position */ public Command setArmGoalCommand(double kArmOffsetRads) { + armGoal = kArmOffsetRads; return Commands.runOnce(() -> setGoal(kArmOffsetRads), this); } @@ -84,4 +87,19 @@ public static double calculateHorizontalExtensionGoal(double x, double y) { return Config.kVerticalExtensionPerpendicularHeight * Math.cos(Math.atan(Config.kElevatorBaseWidth / Config.kVerticalExtensionPerpendicularHeight)); } + /** + * + * @return true if arm is within defined position tollerence. + */ + public boolean getArmAtPosition() { + if(getArmPosition() <= armGoal + Config.kHorizontalExtensionPositionTollerenceMetres && getArmPosition() >= armGoal - Config.kHorizontalExtensionPositionTollerenceMetres) { + return true; + } + return false; + } + + public double getArmPosition() { + return horizontalExtensionEncoder.getPosition() / Config.kHorizontalExtensionEncoderPPR * Config.kHorizontalExtensionMetresPerRotation; + } + } diff --git a/ChargedUp/src/main/java/frc/robot/Subsystems/VerticalExtension.java b/ChargedUp/src/main/java/frc/robot/Subsystems/VerticalExtension.java index e9d47b1..1ebaf44 100644 --- a/ChargedUp/src/main/java/frc/robot/Subsystems/VerticalExtension.java +++ b/ChargedUp/src/main/java/frc/robot/Subsystems/VerticalExtension.java @@ -11,17 +11,22 @@ import com.ctre.phoenix.sensors.SensorInitializationStrategy; import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.trajectory.Trajectory.State; +import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.TrapezoidProfileSubsystem; import frc.robot.Config; import frc.robot.Constants; /** Add your docs here. */ -public class VerticalExtension extends TrapezoidProfileSubsystem { +public class VerticalExtension extends SubsystemBase { - private final WPI_TalonFX verticalExtensionMotor = new WPI_TalonFX(Constants.kVerticalElevatorCanId); + private final static WPI_TalonFX verticalExtensionMotor = new WPI_TalonFX(Constants.kVerticalElevatorCanId); private final CANCoder verticalExtensionEncoder = new CANCoder(Constants.kVerticalElevatorEncoderCanId); private double armGoal = 0; @@ -35,13 +40,19 @@ public class VerticalExtension extends TrapezoidProfileSubsystem { /** Create a new ArmSubsystem. */ public VerticalExtension() { //define a new trapezoid profile using wpi libraries. Configure the motor. - super( - new TrapezoidProfile.Constraints( - Config.kMaxSpeedMetersPerSecond, Config.kMaxAccelerationMetersPerSecondSquared), - Config.kVerticalExtensionEncoderOffset); - verticalExtensionMotor.config_kP(0, Config.kVerticalExtensionKP); - verticalExtensionMotor.configRemoteFeedbackFilter(verticalExtensionEncoder, 0); - verticalExtensionMotor.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor0); + // super( + // new ProfiledPIDController( + // Config.kVerticalExtensionKP, + // 0.0, + // Config.kVerticalExtensionKD, + // new TrapezoidProfile.Constraints( + // Config.kVerticalExtensionMaxVelocity, + // Config.kVerticalExtensionMaxAcceleration)), + // 0); + // initSystem(); + // setGoal(0); + //verticalExtensionMotor.configRemoteFeedbackFilter(verticalExtensionEncoder, 0); + //verticalExtensionMotor.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor0); } @@ -51,33 +62,49 @@ public VerticalExtension() { public void initSystem() { verticalExtensionMotor.configFactoryDefault(); //reset and configure the motor so we know it is correctly configured verticalExtensionMotor.config_kP(0, Config.kVerticalExtensionKP); - verticalExtensionMotor.configRemoteFeedbackFilter(verticalExtensionEncoder, 0); - verticalExtensionMotor.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor0); + verticalExtensionMotor.config_kD(0, Config.kVerticalExtensionKD); + verticalExtensionMotor.config_kF(0, m_feedforward.calculate(Config.kVerticalExtensionMaxVelocity) / Config.kVerticalExtensionEncoderPPR); + verticalExtensionMotor.config_kP(1, Config.kVerticalExtensionKP); + verticalExtensionMotor.config_kD(1, Config.kVerticalExtensionKD); + verticalExtensionMotor.config_kF(1, m_feedforward.calculate(Config.kVerticalExtensionMaxVelocity) / Config.kVerticalExtensionEncoderPPR); + verticalExtensionMotor.configMotionAcceleration(Config.kVerticalExtensionMaxAcceleration / (Config.kVerticalExtensionMetresPerRotation / Config.kVerticalExtensionEncoderPPR)); + verticalExtensionMotor.configMotionCruiseVelocity(Config.kVerticalExtensionMaxVelocity / (Config.kVerticalExtensionMetresPerRotation / Config.kVerticalExtensionEncoderPPR)); + + verticalExtensionMotor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor, 0, 0); + //verticalExtensionMotor.configRemoteFeedbackFilter(verticalExtensionEncoder, 0); + //verticalExtensionMotor.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor0); verticalExtensionEncoder.configFactoryDefault(); //reset and configure the encoder verticalExtensionEncoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition); + resetSensors(); } - @Override - public void useState(TrapezoidProfile.State setpoint) { - // Calculate the feedforward from the sepoint - double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity); + // @Override + // public void useOutput(double output, edu.wpi.first.math.trajectory.TrapezoidProfile.State setpoint) { + // // //System.out.println(setpoint.position); + // // // Calculate the feedforward from the sepoint + // double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity); - // Add the feedforward to the PID output to get the motor output - verticalExtensionMotor.config_kF(0, feedforward); - verticalExtensionMotor.set(ControlMode.Position, setpoint.position); + // // // Add the feedforward to the PID output to get the motor output + // // //System.out.println(setpoint.position); - } - - /** - * Run arm to position - * @param kArmOffsetRads - * @return when arm is at desired position - */ - public Command setArmGoalCommand(double kArmOffsetRads) { - armGoal = kArmOffsetRads; - return Commands.runOnce(() -> setGoal(kArmOffsetRads), this); - } + // // verticalExtensionMotor.setVoltage(output + feedforward); + // verticalExtensionMotor.set(getController().calculate(output) + feedforward); + // } + + // /** + // * + // * Run arm to position + // * @param kArmOffsetRads + // * @return when arm is at desired position + // */ + // public Command setArmGoalCommand(double kArmOffsetRads) { + // armGoal = kArmOffsetRads; + // return Commands.runOnce(() -> { + // this.setGoal(armGoal); + // this.enable(); + // }, this); + // } /** * Method to calculate the desired porition of the motor based off a target x and y position. @@ -94,14 +121,24 @@ public static double calculateVerticalExtensionGoal(double x, double y) { * @return true if arm is within defined position tollerence. */ public boolean getArmAtPosition() { - if(getArmPosition() <= armGoal + Config.kVerticalExtensionPositionTollerenceMetres && getArmPosition() >= armGoal - Config.kVerticalExtensionPositionTollerenceMetres) { - return true; - } - return false; + return false;//this.getController().atGoal(); + // if(getMeasurement() <= armGoal + Config.kVerticalExtensionPositionTollerenceMetres && getMeasurement() >= armGoal - Config.kVerticalExtensionPositionTollerenceMetres) { + // return true; + // } + // return false; + } + + //@Override + public double getMeasurement() { + return verticalExtensionMotor.getSelectedSensorPosition() * (Config.kVerticalExtensionMetresPerRotation / Config.kVerticalExtensionEncoderPPR); + } + + public static void resetSensors() { + verticalExtensionMotor.setSelectedSensorPosition(0); } - public double getArmPosition() { - return verticalExtensionEncoder.getPosition() / Config.kVerticalExtensionEncoderPPR * Config.kVerticalExtensionMetresPerRotation; + public void setPosition(double position) { + verticalExtensionMotor.set(ControlMode.MotionMagic, position / (Config.kVerticalExtensionMetresPerRotation / Config.kVerticalExtensionEncoderPPR)); } } From 0f17beb4e57c694e271ddb626bfe9a214983dada Mon Sep 17 00:00:00 2001 From: LordBBQ <46541720+LordBBQ@users.noreply.github.com> Date: Tue, 24 Jan 2023 20:45:15 -0800 Subject: [PATCH 18/18] tested code --- .../robot/Commands/Arm/ArmHighPosCommand.java | 11 +-- .../robot/Commands/Arm/ArmHomePosCommand.java | 8 +- .../robot/Commands/Arm/ArmLowPosCommand.java | 8 +- .../Commands/Arm/ArmMediumPosCommand.java | 49 ++++++++++ .../Commands/Arm/ArmShelfPosCommand.java | 7 +- .../frc/robot/Commands/CommandController.java | 12 ++- .../main/java/frc/robot/Commands/Score.java | 34 +++++++ ChargedUp/src/main/java/frc/robot/Config.java | 8 +- .../src/main/java/frc/robot/Constants.java | 3 + .../robot/Subsystems/HorizontalExtension.java | 96 ++++++++++--------- .../robot/Subsystems/VerticalExtension.java | 77 ++++----------- .../main/java/frc/robot/Subsystems/Wrist.java | 3 +- .../java/frc/robot/Utilities/Pneumatics.java | 3 +- 13 files changed, 186 insertions(+), 133 deletions(-) create mode 100644 ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmMediumPosCommand.java create mode 100644 ChargedUp/src/main/java/frc/robot/Commands/Score.java diff --git a/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHighPosCommand.java b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHighPosCommand.java index 7aa7619..77886f9 100644 --- a/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHighPosCommand.java +++ b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHighPosCommand.java @@ -32,12 +32,8 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_verticalExtension.setPosition(2); - //m_verticalExtension.useOutput(0, null); - //m_verticalExtension.setGoal(200); - //m_verticalExtension.enable(); - System.out.println(m_verticalExtension.getArmAtPosition()); - + m_verticalExtension.setPosition(Config.kArmHighPosY);//m_verticalExtension.calculateVerticalExtensionGoal(Config.kArmHighPosX, Config.kArmHighPosY)); + m_horizontalExtension.setPosition(Config.kArmHighPosX);//m_horizontalExtension.calculateHorizontalExtensionGoal(Config.kArmHighPosX, Config.kArmHighPosY)); } @@ -48,7 +44,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - System.out.println(m_verticalExtension.getArmAtPosition()); - return true;//(m_verticalExtension.getArmAtPosition()); + return m_verticalExtension.getArmAtPosition() && m_horizontalExtension.getArmAtPosition(); } } diff --git a/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHomePosCommand.java b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHomePosCommand.java index dfc9064..40610a2 100644 --- a/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHomePosCommand.java +++ b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHomePosCommand.java @@ -31,10 +31,8 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - //m_verticalExtension.setArmGoalCommand(VerticalExtension.calculateVerticalExtensionGoal(Config.kArmHomePosX, Config.kArmHomePosY)); - m_verticalExtension.setPosition(0); - - m_horizontalExtension.setArmGoalCommand(HorizontalExtension.calculateHorizontalExtensionGoal(Config.kArmHomePosX, Config.kArmHomePosY)); + m_verticalExtension.setPosition(Config.kArmHomePosY);//m_verticalExtension.calculateVerticalExtensionGoal(Config.kArmHomePosX, Config.kArmHomePosY)); + m_horizontalExtension.setPosition(Config.kArmHomePosX);//m_horizontalExtension.calculateHorizontalExtensionGoal(Config.kArmHomePosX, Config.kArmHomePosY)); } @@ -45,6 +43,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return true; + return m_verticalExtension.getArmAtPosition() && m_horizontalExtension.getArmAtPosition(); } } diff --git a/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmLowPosCommand.java b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmLowPosCommand.java index 81e7a47..57bdd56 100644 --- a/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmLowPosCommand.java +++ b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmLowPosCommand.java @@ -31,10 +31,8 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - //m_verticalExtension.setGoal(1); - //m_verticalExtension.enable(); - m_horizontalExtension.setArmGoalCommand(HorizontalExtension.calculateHorizontalExtensionGoal(Config.kArmLowPosX, Config.kArmLowPosY)); - + m_verticalExtension.setPosition(Config.kArmLowPosY);//m_verticalExtension.calculateVerticalExtensionGoal(Config.kArmLowPosX, Config.kArmLowPosY)); + m_horizontalExtension.setPosition(Config.kArmHighPosX);//m_horizontalExtension.calculateHorizontalExtensionGoal(Config.kArmLowPosX, Config.kArmLowPosY)); } // Called once the command ends or is interrupted. @@ -44,6 +42,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return m_verticalExtension.getArmAtPosition() && m_horizontalExtension.getArmAtPosition(); } } diff --git a/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmMediumPosCommand.java b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmMediumPosCommand.java new file mode 100644 index 0000000..21ccbd8 --- /dev/null +++ b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmMediumPosCommand.java @@ -0,0 +1,49 @@ +// 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.Arm; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Config; +import frc.robot.Subsystems.HorizontalExtension; +import frc.robot.Subsystems.VerticalExtension; +import frc.robot.Subsystems.Wrist; + +public class ArmMediumPosCommand extends CommandBase { + private final Wrist m_wrist; + private final HorizontalExtension m_horizontalExtension; + private final VerticalExtension m_verticalExtension; + /** Creates a new ArmsumPosCommand. */ + public ArmMediumPosCommand(Wrist wristSubsystem, VerticalExtension verticalExtensionSubsystem, HorizontalExtension horizontalExtensionSubsystem) { + m_wrist = wristSubsystem; + m_verticalExtension = verticalExtensionSubsystem; + m_horizontalExtension = horizontalExtensionSubsystem; + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_wrist.setWristPosition(Config.kArmMedPosWrist); + System.out.println("hellow"); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_verticalExtension.setPosition(Config.kArmMedPosY);//m_verticalExtension.calculateVerticalExtensionGoal(Config.kArmMedPosX, Config.kArmMedPosY)); + m_horizontalExtension.setPosition(Config.kArmMedPosX);//m_horizontalExtension.calculateHorizontalExtensionGoal(Config.kArmMedPosX, Config.kArmMedPosY)); + + } + + // 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 m_verticalExtension.getArmAtPosition() && m_horizontalExtension.getArmAtPosition(); + } +} diff --git a/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmShelfPosCommand.java b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmShelfPosCommand.java index 5b17278..f00ca45 100644 --- a/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmShelfPosCommand.java +++ b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmShelfPosCommand.java @@ -31,9 +31,8 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - //m_verticalExtension.setArmGoalCommand(VerticalExtension.calculateVerticalExtensionGoal(Config.kArmShelfPosX, Config.kArmShelfPosY)); - m_horizontalExtension.setArmGoalCommand(HorizontalExtension.calculateHorizontalExtensionGoal(Config.kArmShelfPosX, Config.kArmShelfPosY)); - + m_verticalExtension.setPosition(Config.kArmShelfPosY);//m_verticalExtension.calculateVerticalExtensionGoal(Config.kArmShelfPosX, Config.kArmShelfPosY)); + m_horizontalExtension.setPosition(Config.kArmShelfPosX);//m_horizontalExtension.calculateHorizontalExtensionGoal(Config.kArmShelfPosX, Config.kArmShelfPosY)); } // Called once the command ends or is interrupted. @@ -43,6 +42,6 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return m_verticalExtension.getArmAtPosition() && m_horizontalExtension.getArmAtPosition(); } } diff --git a/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java b/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java index c70a9c6..b8cf8f0 100644 --- a/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java +++ b/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java @@ -4,7 +4,6 @@ package frc.robot.Commands; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -12,6 +11,7 @@ import frc.robot.Commands.Arm.ArmHighPosCommand; import frc.robot.Commands.Arm.ArmHomePosCommand; import frc.robot.Commands.Arm.ArmLowPosCommand; +import frc.robot.Commands.Arm.ArmMediumPosCommand; import frc.robot.Subsystems.Drive; import frc.robot.Subsystems.HorizontalExtension; import frc.robot.Subsystems.VerticalExtension; @@ -52,6 +52,16 @@ public void configureBindings() { .onTrue( new ArmHomePosCommand(m_wrist, m_vertical, m_horizontal) ); + + m_driverHID.x() + .onTrue( + new ArmLowPosCommand(m_wrist, m_vertical, m_horizontal) + ); + + m_driverHID.b() + .onTrue( + new ArmMediumPosCommand(m_wrist, m_vertical, m_horizontal) + ); } diff --git a/ChargedUp/src/main/java/frc/robot/Commands/Score.java b/ChargedUp/src/main/java/frc/robot/Commands/Score.java new file mode 100644 index 0000000..0a32b78 --- /dev/null +++ b/ChargedUp/src/main/java/frc/robot/Commands/Score.java @@ -0,0 +1,34 @@ +// 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.HorizontalExtension; +import frc.robot.Subsystems.VerticalExtension; +import frc.robot.Subsystems.Wrist; + +public class Score extends SequentialCommandGroup { + + public enum ScorePos { + LOW, + MEDIUM, + HIGH, + } + /** Creates a new Score. */ + public Score(ScorePos desiredpos, HorizontalExtension horizontalExtensionSubsystem, VerticalExtension verticalExtensionSubsystem, Wrist wristSubsystem) { + + switch(desiredpos) { + case LOW: + //TODO + case HIGH: + break; + case MEDIUM: + break; + default: + break; + } + } + +} diff --git a/ChargedUp/src/main/java/frc/robot/Config.java b/ChargedUp/src/main/java/frc/robot/Config.java index 3bdbf96..0b30179 100644 --- a/ChargedUp/src/main/java/frc/robot/Config.java +++ b/ChargedUp/src/main/java/frc/robot/Config.java @@ -31,6 +31,7 @@ public class Config { public static final double kHorizontalExtensionKG = 0; public static final double kHorizontalExtensionKA = 0; public static final double kHorizontalExtensionKP = 0; + public static final double kHorizontalExtensionKD = 0; public static final double kHorizontalExtensionMaxVelocity = 0; public static final double kHorizontalExtensionMaxAcceleration = 0; public static final double kHorizontalExtensionEncoderPPR = 4096; //CANCoder, falcon is 2048 EPR @@ -72,11 +73,11 @@ public class Config { public static final double kArmMedPosX = 0.5; - public static final double kArmMedPosY = 0.7; + public static final double kArmMedPosY = 1; public static final WristPosition kArmMedPosWrist = WristPosition.RAISED; - public static final double kArmHighPosX = 1; - public static final double kArmHighPosY = 1.2; + public static final double kArmHighPosX = 1.2; + public static final double kArmHighPosY = 2; public static final WristPosition kArmHighPosWrist = WristPosition.RAISED; public static final double kArmShelfPosX = 1; @@ -96,7 +97,6 @@ public class Config { * Pneumatics constants */ - public static final int kPneumaticsModuleCanId = 51; public static final double kPneumaticsMinPressure = 110; //The pressure the pneumatics will start charging at public static final double kPneumaticsMaxPressure = 120; //The pressure the pneumatics will stop charging at public static final double kCompressorRunCurrent = 10;//Ampres. Used for error checking //TODO diff --git a/ChargedUp/src/main/java/frc/robot/Constants.java b/ChargedUp/src/main/java/frc/robot/Constants.java index f59e8f9..56ff379 100644 --- a/ChargedUp/src/main/java/frc/robot/Constants.java +++ b/ChargedUp/src/main/java/frc/robot/Constants.java @@ -30,6 +30,9 @@ public class Constants { public static final int kHorizontalElevatorEncoderCanId = 22; public static final int kWristEncoderCanId = 23; + public static final int kPneumaticsModuleCanId = 50; + + diff --git a/ChargedUp/src/main/java/frc/robot/Subsystems/HorizontalExtension.java b/ChargedUp/src/main/java/frc/robot/Subsystems/HorizontalExtension.java index 6b6727a..e87cacd 100644 --- a/ChargedUp/src/main/java/frc/robot/Subsystems/HorizontalExtension.java +++ b/ChargedUp/src/main/java/frc/robot/Subsystems/HorizontalExtension.java @@ -11,35 +11,27 @@ import com.ctre.phoenix.sensors.SensorInitializationStrategy; import edu.wpi.first.math.controller.ElevatorFeedforward; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.TrapezoidProfileSubsystem; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Config; import frc.robot.Constants; /** Add your docs here. */ -public class HorizontalExtension extends TrapezoidProfileSubsystem { - +public class HorizontalExtension extends SubsystemBase { + private final static WPI_TalonFX horizontalExtensionMotor = new WPI_TalonFX(Constants.kHorizontalElevatorCanId); + private final CANCoder horizontalExtensionEncoder = new CANCoder(Constants.kHorizontalElevatorEncoderCanId); private double armGoal = 0; - private final WPI_TalonFX horizontalExtensionMotor = new WPI_TalonFX(Constants.kHorizontalElevatorCanId); - private final CANCoder horizontalExtensionEncoder = new CANCoder(Constants.kHorizontalElevatorEncoderCanId); //we want an absolute encoder for reliabality - private final ElevatorFeedforward m_feedforward = new ElevatorFeedforward( //define feedFoward control for the elevator + + //this subsystem uses a combination of a feedfoward and feedback control. + //this gives us the advantage of better profiling from feedfoward and better precision from feedback. + private final ElevatorFeedforward m_feedforward = new ElevatorFeedforward( Config.kHorizontalExtensionKS, Config.kHorizontalExtensionKG, //kS is the static friction constant, kG is the gravity constant Config.kHorizontalExtensionKV, Config.kHorizontalExtensionKA //kV is the velocity constant, kA is the acceleration constant ); - //this subsystem uses a combination of a feedfoward and feedback control. - //this gives us the advantage of better profiling from feedfoward and better precision from feedback. - /** Create a new ArmSubsystem. */ public HorizontalExtension() { - //define a new trapezoid profile using wpi libraries. Configure the motor. - super( - new TrapezoidProfile.Constraints( - Config.kMaxSpeedMetersPerSecond, Config.kMaxAccelerationMetersPerSecondSquared), - Config.kHorizontalExtensionEncoderOffset); + initSystem(); } @@ -49,42 +41,31 @@ public HorizontalExtension() { public void initSystem() { horizontalExtensionMotor.configFactoryDefault(); //reset and configure the motor so we know it is correctly configured horizontalExtensionMotor.config_kP(0, Config.kHorizontalExtensionKP); - horizontalExtensionMotor.configRemoteFeedbackFilter(horizontalExtensionEncoder, 0); - horizontalExtensionMotor.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor0); + horizontalExtensionMotor.config_kD(0, Config.kHorizontalExtensionKD); + horizontalExtensionMotor.config_kF(0, m_feedforward.calculate(Config.kHorizontalExtensionMaxVelocity) / Config.kHorizontalExtensionEncoderPPR); + horizontalExtensionMotor.config_kP(1, Config.kHorizontalExtensionKP); + horizontalExtensionMotor.config_kD(1, Config.kHorizontalExtensionKD); + horizontalExtensionMotor.config_kF(1, m_feedforward.calculate(Config.kHorizontalExtensionMaxVelocity) / Config.kHorizontalExtensionEncoderPPR); + horizontalExtensionMotor.configMotionAcceleration(Config.kHorizontalExtensionMaxAcceleration / (Config.kHorizontalExtensionMetresPerRotation / Config.kHorizontalExtensionEncoderPPR)); + horizontalExtensionMotor.configMotionCruiseVelocity(Config.kHorizontalExtensionMaxVelocity / (Config.kHorizontalExtensionMetresPerRotation / Config.kHorizontalExtensionEncoderPPR)); + + horizontalExtensionMotor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor, 0, 0); + //horizontalExtensionMotor.configRemoteFeedbackFilter(horizontalExtensionEncoder, 0); + //horizontalExtensionMotor.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor0); horizontalExtensionEncoder.configFactoryDefault(); //reset and configure the encoder horizontalExtensionEncoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition); + resetSensors(); } - - @Override - public void useState(TrapezoidProfile.State setpoint) { - // Calculate the feedforward from the sepoint - double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity); - - // Add the feedforward to the PID output to get the motor output - horizontalExtensionMotor.config_kF(0, feedforward); - horizontalExtensionMotor.set(ControlMode.Position, setpoint.position); - } - - /** - * Run arm to position - * @param kArmOffsetRads - * @return when arm is at desired position - */ - public Command setArmGoalCommand(double kArmOffsetRads) { - armGoal = kArmOffsetRads; - return Commands.runOnce(() -> setGoal(kArmOffsetRads), this); - } - - /** + /** * Method to calculate the desired porition of the motor based off a target x and y position. * @param x desired x position * @param y desired y position - * @return the desired setpoint for the extension + * @return the desired setpoint for the extension in encoder units */ - public static double calculateHorizontalExtensionGoal(double x, double y) { - return Config.kVerticalExtensionPerpendicularHeight * Math.cos(Math.atan(Config.kElevatorBaseWidth / Config.kVerticalExtensionPerpendicularHeight)); + public double calculateHorizontalExtensionGoal(double x, double y) { + return x * Math.cos(180 - Math.atan(Config.kElevatorBaseWidth / Config.kVerticalExtensionPerpendicularHeight)) - y * Math.sin(180 - Math.atan(Config.kElevatorBaseWidth / Config.kVerticalExtensionPerpendicularHeight)) * Config.kHorizontalExtensionMetresPerRotation; } /** @@ -92,14 +73,35 @@ public static double calculateHorizontalExtensionGoal(double x, double y) { * @return true if arm is within defined position tollerence. */ public boolean getArmAtPosition() { - if(getArmPosition() <= armGoal + Config.kHorizontalExtensionPositionTollerenceMetres && getArmPosition() >= armGoal - Config.kHorizontalExtensionPositionTollerenceMetres) { + if(getMeasurement() <= armGoal + Config.kHorizontalExtensionPositionTollerenceMetres && getMeasurement() >= armGoal - Config.kHorizontalExtensionPositionTollerenceMetres) { return true; } return false; } - public double getArmPosition() { - return horizontalExtensionEncoder.getPosition() / Config.kHorizontalExtensionEncoderPPR * Config.kHorizontalExtensionMetresPerRotation; + + /** + * Get position of arm + * @return horizontal extension position in metres + */ + public double getMeasurement() { + return horizontalExtensionMotor.getSelectedSensorPosition() * (Config.kHorizontalExtensionMetresPerRotation / Config.kHorizontalExtensionEncoderPPR); + } + + /** + * Reset all sensors + */ + public static void resetSensors() { + horizontalExtensionMotor.setSelectedSensorPosition(0); + } + + /** + * Move arm to desired position using motionMagic + * @param position in metres + */ + public void setPosition(double position) { + armGoal = position; + horizontalExtensionMotor.set(ControlMode.MotionMagic, position / (Config.kHorizontalExtensionMetresPerRotation / Config.kHorizontalExtensionEncoderPPR)); } } diff --git a/ChargedUp/src/main/java/frc/robot/Subsystems/VerticalExtension.java b/ChargedUp/src/main/java/frc/robot/Subsystems/VerticalExtension.java index 1ebaf44..cef10ee 100644 --- a/ChargedUp/src/main/java/frc/robot/Subsystems/VerticalExtension.java +++ b/ChargedUp/src/main/java/frc/robot/Subsystems/VerticalExtension.java @@ -11,15 +11,7 @@ import com.ctre.phoenix.sensors.SensorInitializationStrategy; import edu.wpi.first.math.controller.ElevatorFeedforward; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.trajectory.Trajectory.State; -import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.TrapezoidProfileSubsystem; import frc.robot.Config; import frc.robot.Constants; @@ -39,21 +31,8 @@ public class VerticalExtension extends SubsystemBase { /** Create a new ArmSubsystem. */ public VerticalExtension() { - //define a new trapezoid profile using wpi libraries. Configure the motor. - // super( - // new ProfiledPIDController( - // Config.kVerticalExtensionKP, - // 0.0, - // Config.kVerticalExtensionKD, - // new TrapezoidProfile.Constraints( - // Config.kVerticalExtensionMaxVelocity, - // Config.kVerticalExtensionMaxAcceleration)), - // 0); - // initSystem(); - // setGoal(0); - //verticalExtensionMotor.configRemoteFeedbackFilter(verticalExtensionEncoder, 0); - //verticalExtensionMotor.configSelectedFeedbackSensor(FeedbackDevice.RemoteSensor0); + initSystem(); } /** @@ -79,41 +58,14 @@ public void initSystem() { resetSensors(); } - // @Override - // public void useOutput(double output, edu.wpi.first.math.trajectory.TrapezoidProfile.State setpoint) { - // // //System.out.println(setpoint.position); - // // // Calculate the feedforward from the sepoint - // double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity); - - // // // Add the feedforward to the PID output to get the motor output - // // //System.out.println(setpoint.position); - - // // verticalExtensionMotor.setVoltage(output + feedforward); - // verticalExtensionMotor.set(getController().calculate(output) + feedforward); - // } - - // /** - // * - // * Run arm to position - // * @param kArmOffsetRads - // * @return when arm is at desired position - // */ - // public Command setArmGoalCommand(double kArmOffsetRads) { - // armGoal = kArmOffsetRads; - // return Commands.runOnce(() -> { - // this.setGoal(armGoal); - // this.enable(); - // }, this); - // } - /** * Method to calculate the desired porition of the motor based off a target x and y position. * @param x desired x position * @param y desired y position * @return the desired setpoint for the extension in encoder units */ - public static double calculateVerticalExtensionGoal(double x, double y) { - return x * Math.cos(180 - Math.atan(Config.kElevatorBaseWidth / Config.kVerticalExtensionPerpendicularHeight)) - y * Math.sin(180 - Math.atan(Config.kElevatorBaseWidth / Config.kVerticalExtensionPerpendicularHeight)) * Config.kVerticalExtensionMetresPerRotation; + public double calculateVerticalExtensionGoal(double x, double y) { + return Config.kElevatorBaseWidth * Math.sin(Math.atan(Config.kElevatorBaseWidth / Config.kVerticalExtensionPerpendicularHeight)) + Config.kVerticalExtensionPerpendicularHeight * Math.cos(Math.atan(Config.kElevatorBaseWidth / Config.kVerticalExtensionPerpendicularHeight));//x * Math.cos(180 - Math.atan(Config.kElevatorBaseWidth / Config.kVerticalExtensionPerpendicularHeight)) - y * Math.sin(180 - Math.atan(Config.kElevatorBaseWidth / Config.kVerticalExtensionPerpendicularHeight)) * Config.kVerticalExtensionMetresPerRotation; } /** @@ -121,23 +73,34 @@ public static double calculateVerticalExtensionGoal(double x, double y) { * @return true if arm is within defined position tollerence. */ public boolean getArmAtPosition() { - return false;//this.getController().atGoal(); - // if(getMeasurement() <= armGoal + Config.kVerticalExtensionPositionTollerenceMetres && getMeasurement() >= armGoal - Config.kVerticalExtensionPositionTollerenceMetres) { - // return true; - // } - // return false; + if(getMeasurement() <= armGoal + Config.kVerticalExtensionPositionTollerenceMetres && getMeasurement() >= armGoal - Config.kVerticalExtensionPositionTollerenceMetres) { + return true; + } + return false; } - //@Override + + /** + * Get position of arm + * @return vertical extension position in metres + */ public double getMeasurement() { return verticalExtensionMotor.getSelectedSensorPosition() * (Config.kVerticalExtensionMetresPerRotation / Config.kVerticalExtensionEncoderPPR); } + /** + * Reset all sensors + */ public static void resetSensors() { verticalExtensionMotor.setSelectedSensorPosition(0); } + /** + * Move arm to desired position using motionMagic + * @param position in metres + */ public void setPosition(double position) { + armGoal = position; verticalExtensionMotor.set(ControlMode.MotionMagic, position / (Config.kVerticalExtensionMetresPerRotation / Config.kVerticalExtensionEncoderPPR)); } diff --git a/ChargedUp/src/main/java/frc/robot/Subsystems/Wrist.java b/ChargedUp/src/main/java/frc/robot/Subsystems/Wrist.java index caa2d6a..fee98e5 100644 --- a/ChargedUp/src/main/java/frc/robot/Subsystems/Wrist.java +++ b/ChargedUp/src/main/java/frc/robot/Subsystems/Wrist.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.DoubleSolenoid.Value; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Config; +import frc.robot.Constants; /** Add your docs here. */ public class Wrist extends SubsystemBase { @@ -18,7 +19,7 @@ public enum WristPosition { LOWERED, } - private DoubleSolenoid wristSolenoid = new DoubleSolenoid(PneumaticsModuleType.REVPH, Config.kWristFowardPortId, Config.kWristReversePortId); + private DoubleSolenoid wristSolenoid = new DoubleSolenoid(Constants.kPneumaticsModuleCanId, PneumaticsModuleType.REVPH, Config.kWristFowardPortId, Config.kWristReversePortId); /** Create a new ArmSubsystem. */ public Wrist() { diff --git a/ChargedUp/src/main/java/frc/robot/Utilities/Pneumatics.java b/ChargedUp/src/main/java/frc/robot/Utilities/Pneumatics.java index f02a5eb..fda8f8e 100644 --- a/ChargedUp/src/main/java/frc/robot/Utilities/Pneumatics.java +++ b/ChargedUp/src/main/java/frc/robot/Utilities/Pneumatics.java @@ -8,10 +8,11 @@ import edu.wpi.first.wpilibj.PneumaticsModuleType; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Config; +import frc.robot.Constants; public class Pneumatics extends SubsystemBase { - private Compressor comp = new Compressor(PneumaticsModuleType.REVPH); + private Compressor comp = new Compressor(Constants.kPneumaticsModuleCanId, PneumaticsModuleType.REVPH); private boolean compEnabled = false; //we can change this when we enable and disable the compressor. This means we can keep track of faults! private byte compCheckIteration = 0; //iterated once per cycle of suspected compressor fault. private static Pneumatics m_instance;