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/.SysId/sysid.json b/ChargedUp/.SysId/sysid.json new file mode 100644 index 0000000..b4b457a --- /dev/null +++ b/ChargedUp/.SysId/sysid.json @@ -0,0 +1,10 @@ +{ + "SysId": { + "Analysis Type": "General Mechanism", + "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/com/projectb/Utilities/Pneumatics.java b/ChargedUp/src/main/java/com/projectb/Utilities/Pneumatics.java deleted file mode 100644 index 58f7f03..0000000 --- a/ChargedUp/src/main/java/com/projectb/Utilities/Pneumatics.java +++ /dev/null @@ -1,5 +0,0 @@ -package com.projectb.Utilities; - -public class Pneumatics { - -} 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..77886f9 --- /dev/null +++ b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmHighPosCommand.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 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); + System.out.println("hellow"); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_verticalExtension.setPosition(Config.kArmHighPosY);//m_verticalExtension.calculateVerticalExtensionGoal(Config.kArmHighPosX, Config.kArmHighPosY)); + m_horizontalExtension.setPosition(Config.kArmHighPosX);//m_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 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 new file mode 100644 index 0000000..40610a2 --- /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.setPosition(Config.kArmHomePosY);//m_verticalExtension.calculateVerticalExtensionGoal(Config.kArmHomePosX, Config.kArmHomePosY)); + m_horizontalExtension.setPosition(Config.kArmHomePosX);//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 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 new file mode 100644 index 0000000..57bdd56 --- /dev/null +++ b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmLowPosCommand.java @@ -0,0 +1,47 @@ +// 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.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. + @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/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 new file mode 100644 index 0000000..f00ca45 --- /dev/null +++ b/ChargedUp/src/main/java/frc/robot/Commands/Arm/ArmShelfPosCommand.java @@ -0,0 +1,47 @@ +// 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.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. + @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/LowerWrist.java b/ChargedUp/src/main/java/frc/robot/Commands/Arm/LowerWrist.java new file mode 100644 index 0000000..edf0a0a --- /dev/null +++ b/ChargedUp/src/main/java/frc/robot/Commands/Arm/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.Arm; + +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/Arm/RaiseWrist.java b/ChargedUp/src/main/java/frc/robot/Commands/Arm/RaiseWrist.java new file mode 100644 index 0000000..0fb30cb --- /dev/null +++ b/ChargedUp/src/main/java/frc/robot/Commands/Arm/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.Arm; + +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/Commands/CommandController.java b/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java new file mode 100644 index 0000000..b8cf8f0 --- /dev/null +++ b/ChargedUp/src/main/java/frc/robot/Commands/CommandController.java @@ -0,0 +1,74 @@ +// 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 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.Commands.Arm.ArmMediumPosCommand; +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. + * + *

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())); + + m_driverHID.y() + .onTrue( + new ArmHighPosCommand(m_wrist, m_vertical, m_horizontal) + ); + m_driverHID.a() + .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 new file mode 100644 index 0000000..0b30179 --- /dev/null +++ b/ChargedUp/src/main/java/frc/robot/Config.java @@ -0,0 +1,111 @@ +package frc.robot; + +import frc.robot.Subsystems.Wrist.WristPosition; + +public class Config { + + /** + * Drive characterisation values //TODO + */ + 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; //preportional + + 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; //preportional term + public static final double kRamseteZeta = 0.7; //dampening + + 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); //gearbox is 11.1:1 + + 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 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 + 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; + + 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; + 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 = 1; + public static final WristPosition kArmMedPosWrist = WristPosition.RAISED; + + 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; + public static final double kArmShelfPosY = 1.5; + public static final WristPosition kArmShelfPosWrist = WristPosition.RAISED; + + /** + * 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)); + + /** + * 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/Constants.java b/ChargedUp/src/main/java/frc/robot/Constants.java new file mode 100644 index 0000000..56ff379 --- /dev/null +++ b/ChargedUp/src/main/java/frc/robot/Constants.java @@ -0,0 +1,39 @@ +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 + + 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; + 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/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 69% rename from ChargedUp/src/main/java/com/projectb/Robot.java rename to ChargedUp/src/main/java/frc/robot/Robot.java index cda212b..22bf78e 100644 --- a/ChargedUp/src/main/java/com/projectb/Robot.java +++ b/ChargedUp/src/main/java/frc/robot/Robot.java @@ -2,9 +2,14 @@ // 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; +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 * each mode, as described in the TimedRobot documentation. If you change the name of this class or @@ -13,6 +18,12 @@ */ 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(); + + /** * This function is run when the robot is first started up and should be used for any @@ -20,6 +31,8 @@ public class Robot extends TimedRobot { */ @Override public void robotInit() { + m_robot.configureBindings(); //setup bindings for drive, mechanisms etc. + Drive.setBrakes(false); //disable brakes so robot can be pushed } @@ -31,7 +44,9 @@ 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 @@ -45,6 +60,8 @@ public void robotPeriodic() {} */ @Override public void autonomousInit() { + Drive.setBrakes(true); //run brakes + m_pneumatics.setPneumatics(true); } @@ -56,16 +73,28 @@ public void autonomousPeriodic() { /** This function is called once when teleop is enabled. */ @Override - public void teleopInit() {} + 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 - public void disabledInit() {} + public void disabledInit() { + Drive.setBrakes(false); //disable brakes so robot is pushable + m_pneumatics.setPneumatics(false); + } /** This function is called periodically when disabled. */ @Override public void disabledPeriodic() {} 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..ffadd48 --- /dev/null +++ b/ChargedUp/src/main/java/frc/robot/Subsystems/Drive.java @@ -0,0 +1,201 @@ +package frc.robot.Subsystems; + +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; + +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{ + + /* + * 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); + 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); + + //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; //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(); //Reset all settings on the drive motors + leftDriveB.configFactoryDefault(); + leftDriveC.configFactoryDefault(); + rightDriveA.configFactoryDefault(); + rightDriveB.configFactoryDefault(); + rightDriveC.configFactoryDefault(); + + 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(); //run the above method to initalise the controllers + setBrakes(false); //Set the falcons to coast mode on robot init + } + + /** + * 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())) //run the WPILIB arcadeDrive method with supplied values + .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); //comment for pegasus config. + } + + /** + * + * @return right encoder distance in metres (m), averaged from all three falcons + */ + public double getRightDriveEncodersDistanceMetres() { + return ((rightDriveA.getSelectedSensorPosition() + + rightDriveB.getSelectedSensorPosition()) /2 ); //+ + // rightDriveC.getSelectedSensorPosition()) / 3); //comment for pegasus config + } + + /** + * 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(); //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()) //check have traveled far enough + >= distanceMeters) + // Stop the drive when the command ends + .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 + * 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; + 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; + } + + /** + * + * @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; //locally updated variable to track brake state + } + +} 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..e87cacd --- /dev/null +++ b/ChargedUp/src/main/java/frc/robot/Subsystems/HorizontalExtension.java @@ -0,0 +1,107 @@ +// 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 com.ctre.phoenix.sensors.SensorInitializationStrategy; + +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Config; +import frc.robot.Constants; + +/** Add your docs here. */ +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; + + //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 + ); + + /** Create a new ArmSubsystem. */ + public HorizontalExtension() { + + 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.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(); + } + + /** + * 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 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; + } + + /** + * + * @return true if arm is within defined position tollerence. + */ + public boolean getArmAtPosition() { + if(getMeasurement() <= armGoal + Config.kHorizontalExtensionPositionTollerenceMetres && getMeasurement() >= armGoal - Config.kHorizontalExtensionPositionTollerenceMetres) { + return true; + } + return false; + } + + + /** + * 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 new file mode 100644 index 0000000..cef10ee --- /dev/null +++ b/ChargedUp/src/main/java/frc/robot/Subsystems/VerticalExtension.java @@ -0,0 +1,107 @@ +// 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 com.ctre.phoenix.sensors.SensorInitializationStrategy; + +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Config; +import frc.robot.Constants; + +/** Add your docs here. */ +public class VerticalExtension extends SubsystemBase { + + private final static 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. + private final ElevatorFeedforward m_feedforward = new ElevatorFeedforward( + 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() { + + initSystem(); + } + + /** + * 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.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(); + } + + /** + * 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 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; + } + + /** + * + * @return true if arm is within defined position tollerence. + */ + public boolean getArmAtPosition() { + if(getMeasurement() <= armGoal + Config.kVerticalExtensionPositionTollerenceMetres && getMeasurement() >= armGoal - Config.kVerticalExtensionPositionTollerenceMetres) { + return true; + } + return false; + } + + + /** + * 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 new file mode 100644 index 0000000..fee98e5 --- /dev/null +++ b/ChargedUp/src/main/java/frc/robot/Subsystems/Wrist.java @@ -0,0 +1,44 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.Subsystems; + +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 SubsystemBase { + + public enum WristPosition { + RAISED, + LOWERED, + } + + private DoubleSolenoid wristSolenoid = new DoubleSolenoid(Constants.kPneumaticsModuleCanId, PneumaticsModuleType.REVPH, Config.kWristFowardPortId, Config.kWristReversePortId); + /** Create a new ArmSubsystem. */ + public Wrist() { + + } + + /** + * 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); + } + } + + + + + +} 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 new file mode 100644 index 0000000..fda8f8e --- /dev/null +++ b/ChargedUp/src/main/java/frc/robot/Utilities/Pneumatics.java @@ -0,0 +1,84 @@ +// 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; + +import edu.wpi.first.wpilibj.Compressor; +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(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; + /** 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; + } + } + } + +} 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