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 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 2/5] 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 3/5] 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 4/5] 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 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 5/5] 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 } }