diff --git a/.OutlineViewer/outlineviewer.json b/.OutlineViewer/outlineviewer.json new file mode 100644 index 0000000..629d975 --- /dev/null +++ b/.OutlineViewer/outlineviewer.json @@ -0,0 +1,8 @@ +{ + "Connections": { + "open": true + }, + "Retained Values": { + "open": false + } +} diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..0c9b9de --- /dev/null +++ b/simgui.json @@ -0,0 +1,16 @@ +{ + "HALProvider": { + "Other Devices": { + "SPARK MAX [10]": { + "header": { + "open": true + } + } + } + }, + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo" + } + } +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 85b2fbe..dc7449f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,4 +1,4 @@ -// Copyright (c) FIRST and other WPILib contributors. + // Copyright (c) FIRST and other WPILib contributors. // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. @@ -13,21 +13,18 @@ * constants are needed, to reduce verbosity. */ public final class Constants { - public static final int FrontRightID = 0; - public static final int FrontLeftID = 3; - public static final int BackRightID = 5; - public static final int BackLeftID = 2; - + + + //general xbox pins public static final int XBOX_R_XAXIS = 4; public static final int XBOX_R_YAXIS = 5; public static final int XBOX_L_XAXIS = 0; public static final int XBOX_L_YAXIS = 1; - public static final int XBOX_pin = 0; public static final int Xbox_LT = 2; public static final int Xbox_RT = 3; - - //Controller Button IDs + //We need some flight stick constants I guess + //Controller Button IDs CONTROLLER 1 (port 0) public static final int Xbox_Button_A = 1; public static final int Xbox_Button_B = 2; public static final int Xbox_Button_X = 3; @@ -36,44 +33,58 @@ public final class Constants { public static final int Xbox_Button_RB = 6; public static final int Xbox_Button_LS = 9; public static final int Xbox_Button_RS = 10; - - //Flightstick ID's - public static final int kflightStick = 1; - public static final int Flight_Stick_X = 0; - public static final int Flight_Stick_Y = 1; - public static final int Flight_Stick_Z = 2; - - //We need some flight stick constants I guess + + //Controller Button IDs FOR CONTROLLER 2 (port 1) + public static final int Xbox_Button_AX2 = 1; + public static final int Xbox_Button_BX2 = 2; + public static final int Xbox_Button_XX2 = 3; + public static final int Xbox_Button_YX2 = 4; + public static final int Xbox_Button_LBX2 = 5; + public static final int Xbox_Button_RBX2 = 6; + public static final int Xbox_Button_LSX2 = 9; + public static final int Xbox_Button_RSX2 = 10; + + public static final int XBOX_R_XAXISX2 = 4; + public static final int XBOX_R_YAXISX2 = 5; + public static final int XBOX_L_XAXISX2 = 0; + public static final int XBOX_L_YAXISX2 = 1; + public static final int XBOX_pinX2 = 0; + public static final int Xbox_LTX2 = 2; + public static final int Xbox_RTX2 = 3; + //Also some motor id constant for the intake + public static final int deviceIdIntakeM1 = 9; public static final int deviceIdIntakeM2 = 10; - - public static final double intakeMotorSpeed = .4; + public static final double intakeMotorSpeed = .2; public static final int ampSpike= 8; public static final int logitechPort6 = 6; public static final int logitechPort7 = 7; //Controller Stick Constants - - public static final int kDriverControllerPort = 0; - - - public static class OperatorConstants { - public static final int kDriverControllerPort = 0; - - } - public static final int talonArmPivot = 1; public static final int talonLift1 = 2; public static final int talonLift2 = 3; public static final int extensionMotorID = 0; + //arm constant + public static final int armMotorID = 7; + public static final double armFrontEncoderLimit = 30000; + public static final double armBackEncoderLimit = -0; + public static final double kArmP = 0.00005; + public static final double kArmI = 0.00000001; + public static final double kArmD = 0; + public static final double armGearRatio = 75;// 75:1 + public static final double kArmPID = 30000; //DriveTrain Motor ID's - public static final int FRid = 0; - public static final int BLid = 1; - public static final int FLid = 3; - public static final int BRid = 6; - + public static final int FRid = 2; + public static final int BLid = 3; + public static final int FLid = 0; + public static final int BRid = 5; + // public static final int FRid = 0; + // public static final int BLid = 1; + // public static final int FLid = 3; + // public static final int BRid = 6; //DriveTrain Constants public static final double turnAdjustment = 0.5; public static final double powerAdjustment = 0.5; @@ -85,24 +96,45 @@ public static class OperatorConstants { public static final int xLeftid = 0; public static final int yLeftid = 1; + //Controller 2 ID's + public static final int xBoxControllerid2 = 1; + public static final int xRightidX2 = 4; + public static final int yRightidX2 = 5; + public static final int xLeftidX2 = 0; + public static final int yLeftidX2 = 1; + // turn PID constants - public static final double turnkP = 0.005; - public static final double turnkI = 0.0008; - public static final double turnkD = 0.007; + public static final double turnkP = 0.0034; + public static final double turnkI = 0.0328; + public static final double turnkD = 0; + public static final double maximumDriveError = 1000.0; // balance PID constants - public static final double balancekP = 0.007; + public static final double balancekP = 0.001; public static final double balancekI = 0; public static final double balancekD = 0; + public static final double levelConstant = 2; // auton drive PID constants //public static final int kTimeoutMs = 20; public static final double driveAdjustment = 0.85; public static final int kPIDLoopIdx = 0;//run primary loop - public static final double kF = 0; - public static final double kP = 0.00005; - public static final double kI = 0.0; - public static final double kD = 0; - - public static final double IactZone = 0; -} + public static final double kDriveF = 0; + public static final double kDriveP = 0.00005; + public static final double kDriveI = 0.0; + public static final double kDriveD = 0; + + public static final double IactZone = 0;//was 0.5 + + //pi constants + public static final double CAMERA_HEIGHT_METERS = 0; + public static final double TARGET_HEIGHT_METERS = 0; + public static final double CAMERA_PITCH_RADIANS = 0; + + //limelight constants + public static final double goalHeight = 107; //what are the units???? + public static final double limeLightHeight = 32; + public static final double limeLightInitAngle = 0; + public static double balanceTarget = 0.9; + public static final double cameraOffset = 0; // degrees horizontal angle +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index fb54ca3..74c835a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -31,7 +31,7 @@ public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); - m_robotContainer.LimeLightSubsystem.getAprilId(); + m_robotContainer.armSubsystem.resetArmEncoders(); } /** @@ -49,9 +49,28 @@ public void robotPeriodic() { // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); // SmartDashboard.putBoolean("Has Target", m_robotContainer.rasberryPiCamera.getHasTarget()); - // SmartDashboard.putNumber("Fiducial ID", m_robotContainer.rasberryPiCamera.aprilTagID()); + // SmartDashboard.putNumber("Fiducial ID", m_robotContainer.rasberryPiCamera.getaprilTagID()); + // SmartDashboard.putNumber("Target Area", m_robotContainer.rasberryPiCamera.getTargetArea()); // SmartDashboard.putNumber("Target Pose", m_robotContainer.rasberryPiCamera.getTargetPose()); + // SmartDashboard.putNumber("Target Yaw", m_robotContainer.rasberryPiCamera.getTargetYaw()); + // SmartDashboard.putNumber("Target Skew", m_robotContainer.rasberryPiCamera.getTargetSkew()); + // SmartDashboard.putNumber("Target Pitch", m_robotContainer.rasberryPiCamera.getTargetPitch()); + // SmartDashboard.putNumber("Target Distance", m_robotContainer.rasberryPiCamera.targetDistance()); + + SmartDashboard.putNumber("V Angle", m_robotContainer.LimeLightSubsystem.getV_angle()); + SmartDashboard.putNumber("H Angle", m_robotContainer.LimeLightSubsystem.getH_angle()); + SmartDashboard.putNumber("Z Distance", m_robotContainer.LimeLightSubsystem.calculateXdistance()); + SmartDashboard.putNumber("X Distance", m_robotContainer.LimeLightSubsystem.calculateZdistance()); + SmartDashboard.putNumberArray("Bot Pose", m_robotContainer.LimeLightSubsystem.getBotPose()); + SmartDashboard.putNumber("AprilTag ID", m_robotContainer.LimeLightSubsystem.getaprilTagID()); + SmartDashboard.putNumber("NavX Yaw", m_robotContainer.navX.getYaw()); + SmartDashboard.putNumber("drive output", m_robotContainer.driveTrain.getDriveOutput()); + SmartDashboard.putNumber("getTurn output", m_robotContainer.driveTrain.getTurnOutput()); + SmartDashboard.putNumber("Right Encoder avg", m_robotContainer.driveTrain.getRightEncoders()); + SmartDashboard.putNumber("Left Encoder avg", m_robotContainer.driveTrain.getLeftEncoders()); + SmartDashboard.putBoolean("navx connected", m_robotContainer.navX.isConnected()); + SmartDashboard.putNumber("Arm Position", m_robotContainer.armSubsystem.getArmPosition()); } /** This function is called once each time the robot enters Disabled mode. */ @@ -83,6 +102,11 @@ public void autonomousPeriodic() { SmartDashboard.putNumber("Y-Displacement:", (double)m_robotContainer.navX.getDisplacementY()); SmartDashboard.putNumber("X Position:", m_robotContainer.navX.getXPosition()); SmartDashboard.putNumber("Y Position:", m_robotContainer.navX.getYPosition()); + SmartDashboard.putNumber("FR: Position", m_robotContainer.driveTrain.getFRid()); + SmartDashboard.putNumber("FL: Position", m_robotContainer.driveTrain.getFLid()); + SmartDashboard.putNumber("BR: Position", m_robotContainer.driveTrain.getBRid()); + SmartDashboard.putNumber("BL: Position", m_robotContainer.driveTrain.getBLid()); + SmartDashboard.putNumber("Arm Position", m_robotContainer.armSubsystem.getArmPosition()); } @@ -108,6 +132,11 @@ public void teleopPeriodic() { SmartDashboard.putNumber("Y-Displacement:", (double)m_robotContainer.navX.getDisplacementY()); SmartDashboard.putNumber("X Position:", m_robotContainer.navX.getXPosition()); SmartDashboard.putNumber("Y Position:", m_robotContainer.navX.getYPosition()); + SmartDashboard.putNumber("FR: Position", m_robotContainer.driveTrain.getFRid()); + SmartDashboard.putNumber("FL: Position", m_robotContainer.driveTrain.getFLid()); + SmartDashboard.putNumber("BR: Position", m_robotContainer.driveTrain.getBRid()); + SmartDashboard.putNumber("BL: Position", m_robotContainer.driveTrain.getBLid()); + SmartDashboard.putNumber("Arm Position", m_robotContainer.armSubsystem.getArmPosition()); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 972ac3f..0f3eb4a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,16 +7,15 @@ import frc.robot.subsystems.LimeLightSubsystem; import frc.robot.subsystems.IntakeSubsystem; -import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; -import frc.robot.Constants.OperatorConstants; import frc.robot.subsystems.*; import frc.robot.commands.*; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; -// import frc.robot.subsystems.rasberryPiCamera; +// import frc.robot.subsystems.RasberryPiCamera; /** @@ -28,30 +27,33 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... public LimeLightSubsystem LimeLightSubsystem = new LimeLightSubsystem(); - public XboxController Xbox = new XboxController(0); - // public rasberryPiCamera rasberryPiCamera = new rasberryPiCamera(); - //Intake - private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(); - public final JoystickButton Xbutton = new JoystickButton(Xbox, Constants.Xbox_Button_X); - public final JoystickButton Ybutton = new JoystickButton(Xbox, Constants.Xbox_Button_Y); - public final JoystickButton Abutton = new JoystickButton(Xbox, Constants.Xbox_Button_A); - public final JoystickButton Bbutton = new JoystickButton(Xbox, Constants.Xbox_Button_B); - - // Replace with CommandPS4Controller or CommandJoystick if needed - - private final DriveTrain driveTrain = new DriveTrain(); + public ArmSubsystem armSubsystem = new ArmSubsystem(); + public XboxController Xbox = new XboxController(Constants.xBoxControllerid); + public XboxController Xbox2 = new XboxController(Constants.xBoxControllerid2); + public final DriveTrain driveTrain = new DriveTrain(); public navXSubsystem navX = new navXSubsystem(); - - // Replace with CommandPS4Controller or CommandJoystick if needed - private final XboxController xboxCon = new XboxController(Constants.xBoxControllerid); - private final XboxController flightStick = new XboxController(Constants.kflightStick); + private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(); + //button definitions + private final JoystickButton Xbutton = new JoystickButton(Xbox, Constants.Xbox_Button_X); + private final JoystickButton Ybutton = new JoystickButton(Xbox, Constants.Xbox_Button_Y); + private final JoystickButton Abutton = new JoystickButton(Xbox, Constants.Xbox_Button_A); + private final JoystickButton Bbutton = new JoystickButton(Xbox, Constants.Xbox_Button_B); + private final JoystickButton RBbutton = new JoystickButton(Xbox, Constants.Xbox_Button_RB); + private final JoystickButton LBbutton = new JoystickButton(Xbox, Constants.Xbox_Button_LB); + private final JoystickButton AbuttonX2 = new JoystickButton(Xbox2, Constants.Xbox_Button_A); + private final JoystickButton BbuttonX2 = new JoystickButton(Xbox2, Constants.Xbox_Button_B); + private final JoystickButton YbuttonX2 = new JoystickButton(Xbox2, Constants.Xbox_Button_Y); + private final JoystickButton XbuttonX2 = new JoystickButton(Xbox2, Constants.Xbox_Button_X); + private final JoystickButton RBbuttonX2 = new JoystickButton(Xbox2, Constants.Xbox_Button_RB); + private final JoystickButton LBbuttonX2 = new JoystickButton(Xbox2, Constants.Xbox_Button_LB); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { // Configure the trigger bindings - // driveTrain.setDefaultCommand(new XboxDrive(driveTrain, () -> xboxCon.getRightX(), () -> xboxCon.getLeftY())); - driveTrain.setDefaultCommand(new FlightstickDrive(driveTrain, () -> flightStick.getRawAxis(Constants.Flight_Stick_X), () -> flightStick.getRawAxis(Constants.Flight_Stick_Y), () -> flightStick.getRawAxis(Constants.Flight_Stick_Z))); + driveTrain.setDefaultCommand(new XboxDrive(driveTrain, () -> Xbox.getRightX(), () -> Xbox.getLeftY())); + // armSubsystem.setDefaultCommand(new PIDAssistedArmMovement(armSubsystem, () -> Xbox2.getRawAxis(Constants.XBOX_L_YAXIS))); + armSubsystem.setDefaultCommand(new XboxArm(() -> Xbox2.getRightY(), armSubsystem)); configureBindings(); } @@ -66,13 +68,9 @@ public RobotContainer() { */ private void configureBindings() { // Schedule `ExampleCommand` when `exampleCondition` changes to `true` - - Xbutton.toggleWhenPressed(new IntakeSwitch(intakeSubsystem, false)); - Ybutton.toggleWhenPressed(new IntakeSwitch(intakeSubsystem, true)); - - - // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, - // cancelling on release. + AbuttonX2.onTrue(new ResetEncoders(driveTrain, armSubsystem)); + LBbuttonX2.whileTrue(new IntakeCube(intakeSubsystem, Xbox2)); + RBbuttonX2.whileTrue(new IntakeCone(intakeSubsystem, Xbox2)); } /** @@ -82,7 +80,40 @@ private void configureBindings() { */ public Command getAutonomousCommand() { // An example command will be run in autonomous - return new AutoTurn(driveTrain, navX, 90, 30000); - //new AutoBalancing(navX, driveTrain); + // return new AutoBalancing(navX, driveTrain); + // return new SequentialCommandGroup( + // new SetPipeline(LimeLightSubsystem, 0), + // new Wait(10000), + // new SetPipeline(LimeLightSubsystem, 1), + // new Wait(10000), + // new SetPipeline(LimeLightSubsystem, 2) + // ); + + + // return new AutoBalancin navX, driveTrain); + + // return new SequentialCommandGroup( + // //limelight testing + // new SetPipeline(LimeLightSubsystem, 0), + // new Wait(10000), + // new SetPipeline(LimeLightSubsystem, 1), + // new Wait(10000), + // new SetPipeline(LimeLightSubsystem, 2) + // ); + //auto driving test + return new SequentialCommandGroup( + // new AutoTurn(driveTrain, navX, 90, 3000), + // new AutoTurn(driveTrain, navX, 0, 3000), + // new AutoTurn(driveTrain, navX, -90, 2000) + // new EncoderAutoDrive(driveTrain, 10000, navX) + // new EncoderAutoDrive(driveTrain, 100000, navX) + new AutoBalancing(navX, driveTrain) + ); + //auto turning test + // return new SequentialCommandGroup( + // new AutoTurn(driveTrain, navX, 180, 2000), + // new AutoTurn(driveTrain, navX, 0, 2000), + // new AutoTurn(driveTrain, navX, -90, 2000) + // ); } } diff --git a/src/main/java/frc/robot/commands/AutoBalancing.java b/src/main/java/frc/robot/commands/AutoBalancing.java index ec7374a..77374b9 100644 --- a/src/main/java/frc/robot/commands/AutoBalancing.java +++ b/src/main/java/frc/robot/commands/AutoBalancing.java @@ -33,7 +33,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - driveTrain.PIDBalance(navX.getPitch()); + driveTrain.PIDBalance(navX.getRoll()); driveTrain.PIDturn(navX.getYaw()); driveTrain.setLeftSpeed(driveTrain.getBalanceOutput() - driveTrain.getTurnOutput()); driveTrain.setRightSpeed(driveTrain.getBalanceOutput() + driveTrain.getTurnOutput()); diff --git a/src/main/java/frc/robot/commands/AutoIntake.java b/src/main/java/frc/robot/commands/AutoIntake.java new file mode 100644 index 0000000..cef675e --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoIntake.java @@ -0,0 +1,54 @@ +// 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.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.IntakeSubsystem; + +public class AutoIntake extends CommandBase { + /** Creates a new AutoIntake. */ + private final IntakeSubsystem intakeSubsystem; + private final double speed, timeout; + private int counter; + public AutoIntake(IntakeSubsystem intakeSubsystem, double speed, double timeout) { + this.intakeSubsystem = intakeSubsystem; + this.speed = speed; + this.timeout = timeout; + counter = 0; + + addRequirements(intakeSubsystem); + + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + intakeSubsystem.intakeSetSpeed(speed); + counter++; + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + intakeSubsystem.intakeSetSpeed(0); + + } + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (counter > timeout/20){ + return true; + } else { + return false; + } + } +} diff --git a/src/main/java/frc/robot/commands/AutoTurn.java b/src/main/java/frc/robot/commands/AutoTurn.java index d647852..9565786 100644 --- a/src/main/java/frc/robot/commands/AutoTurn.java +++ b/src/main/java/frc/robot/commands/AutoTurn.java @@ -16,12 +16,14 @@ public class AutoTurn extends CommandBase { private int counter; private final int timeout; private int errorCounter; + private double slew; public AutoTurn(DriveTrain driveTrain, navXSubsystem navX, double turnTarget, int timeout) { this.driveTrain = driveTrain; this.navX = navX; this.turnTarget = turnTarget; counter = 0; errorCounter = 0; + slew = 0; this.timeout = timeout; // Use addRequirements() here to declare subsystem dependencies. addRequirements(driveTrain); @@ -41,14 +43,10 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + driveTrain.PIDturn(navX.getYaw()); - driveTrain.setLeftSpeed(-driveTrain.getTurnOutput()); - driveTrain.setRightSpeed(driveTrain.getTurnOutput()); - if(Math.abs(driveTrain.getTurnError())<3){ - errorCounter++; - }else{ - errorCounter = 0; - } + driveTrain.setLeftSpeed(-(driveTrain.getTurnOutput()-(driveTrain.getTurnOutput() - driveTrain.getTurnPrevOutput()))); + driveTrain.setRightSpeed(driveTrain.getTurnOutput()-(driveTrain.getTurnOutput() - driveTrain.getTurnPrevOutput())); counter++; } @@ -63,7 +61,7 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - if(errorCounter>10 || counter > timeout/20){ + if(errorCounter>20 || counter > timeout/20){ //was counter > timeout/20 return true; }else{ return false; diff --git a/src/main/java/frc/robot/commands/AutoVisionDrive.java b/src/main/java/frc/robot/commands/AutoVisionDrive.java new file mode 100644 index 0000000..22f5386 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoVisionDrive.java @@ -0,0 +1,71 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +import frc.robot.subsystems.DriveTrain; +import frc.robot.subsystems.LimeLightSubsystem; + +import frc.robot.subsystems.navXSubsystem; +import frc.robot.Constants; + +public class AutoVisionDrive extends CommandBase { + /** Creates a new AutoVisionDrive. */ + private final DriveTrain driveTrain; + private final LimeLightSubsystem limelight; + private final navXSubsystem navX; + private final double initialSpeed; + private int counter; + private boolean triggered; + + public AutoVisionDrive(DriveTrain driveTrain, LimeLightSubsystem limelight, navXSubsystem navX, double initialSpeed) { + this.driveTrain = driveTrain; + this.limelight = limelight; + this.navX = navX; + this.initialSpeed = initialSpeed; + this.triggered = false; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(driveTrain); + // addRequirements(rasberryPiCamera); + } + // Called when the command is initially scheduled. + @Override + public void initialize() { + driveTrain.setTurnTarget(0); + driveTrain.setLeftSpeed(initialSpeed); + driveTrain.setRightSpeed(initialSpeed); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + + if(Math.abs(navX.getPitch()) > Constants.levelConstant){ + driveTrain.PIDturn(limelight.getH_angle()); + driveTrain.setLeftSpeed(driveTrain.getTurnOutput()); + driveTrain.setRightSpeed(driveTrain.getTurnOutput()); + triggered = true; + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + driveTrain.setLeftSpeed(0); + driveTrain.setRightSpeed(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if(triggered == true && navX.getPitch() < Constants.levelConstant){ + return true; + }else{ + return false; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/NavXAutoDrive.java b/src/main/java/frc/robot/commands/AutoVisionTurn.java similarity index 87% rename from src/main/java/frc/robot/commands/NavXAutoDrive.java rename to src/main/java/frc/robot/commands/AutoVisionTurn.java index e239da9..26cc51a 100644 --- a/src/main/java/frc/robot/commands/NavXAutoDrive.java +++ b/src/main/java/frc/robot/commands/AutoVisionTurn.java @@ -6,9 +6,9 @@ import edu.wpi.first.wpilibj2.command.CommandBase; -public class NavXAutoDrive extends CommandBase { - /** Creates a new NavXAutoDrive. */ - public NavXAutoDrive() { +public class AutoVisionTurn extends CommandBase { + /** Creates a new AutoVisionTurn. */ + public AutoVisionTurn() { // Use addRequirements() here to declare subsystem dependencies. } diff --git a/src/main/java/frc/robot/commands/AutonomousSequences.java b/src/main/java/frc/robot/commands/AutonomousSequences.java new file mode 100644 index 0000000..089f9f0 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutonomousSequences.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.commands; + +/** Add your docs here. */ +public class AutonomousSequences {} diff --git a/src/main/java/frc/robot/commands/AutoDrive.java b/src/main/java/frc/robot/commands/DriveToBalance.java similarity index 71% rename from src/main/java/frc/robot/commands/AutoDrive.java rename to src/main/java/frc/robot/commands/DriveToBalance.java index e31b96e..5ababf8 100644 --- a/src/main/java/frc/robot/commands/AutoDrive.java +++ b/src/main/java/frc/robot/commands/DriveToBalance.java @@ -5,37 +5,35 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.DriveTrain; import frc.robot.subsystems.navXSubsystem; +import frc.robot.subsystems.DriveTrain; -public class AutoDrive extends CommandBase { - /** Creates a new AutoDrive. */ - private final DriveTrain driveTrain; +public class DriveToBalance extends CommandBase { + /** Creates a new DriveToBalance. */ private final navXSubsystem navX; - private double counter; - private final int millis; - public AutoDrive(DriveTrain driveTrain, navXSubsystem navX, double target, int millis) { - // Use addRequirements() here to declare subsystem dependencies. - this.driveTrain = driveTrain; + private final double initialSpeed; + private final DriveTrain driveTrain; + public DriveToBalance(navXSubsystem navX, DriveTrain driveTrain, double initialSpeed) { + this.initialSpeed = initialSpeed; this.navX = navX; - this.counter = 0; - this.millis = millis; - addRequirements(driveTrain); + this.driveTrain = driveTrain; + // Use addRequirements() here to declare subsystem dependencies. addRequirements(navX); + addRequirements(driveTrain); } // Called when the command is initially scheduled. @Override public void initialize() { - driveTrain.setLeftSpeed(0); - driveTrain.setRightSpeed(0); + driveTrain.setTurnTarget(0); + driveTrain.setLeftSpeed(initialSpeed); + driveTrain.setRightSpeed(initialSpeed); } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() { - // driveTrain.PIDdrive(driveTrain.get, counter); - } + public void execute() {} + // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) {} diff --git a/src/main/java/frc/robot/commands/EncoderAutoDrive.java b/src/main/java/frc/robot/commands/EncoderAutoDrive.java index dd53730..db1fadd 100644 --- a/src/main/java/frc/robot/commands/EncoderAutoDrive.java +++ b/src/main/java/frc/robot/commands/EncoderAutoDrive.java @@ -4,29 +4,74 @@ package frc.robot.commands; +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.DriveTrain; +import frc.robot.subsystems.navXSubsystem; +import frc.robot.Constants; +import frc.robot.subsystems.navXSubsystem; public class EncoderAutoDrive extends CommandBase { /** Creates a new EncoderAutoDrivve. */ - public EncoderAutoDrive() { + private final DriveTrain driveTrain; + private final navXSubsystem navX; + private int counter, cycleCounter; + private final double target, timeout; + public EncoderAutoDrive(DriveTrain driveTrain, double target, navXSubsystem navX, double timeout) { + this.driveTrain = driveTrain; + this.navX = navX; + this.timeout = timeout; + counter = 0; + cycleCounter = 0; + this.target = -target; // Use addRequirements() here to declare subsystem dependencies. + addRequirements(driveTrain); + addRequirements(navX); } // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + driveTrain.resetEncoders(); + driveTrain.resetDrivePID(); + driveTrain.resetTurnPID(); + driveTrain.setDriveTarget(target); + driveTrain.setTurnTarget(navX.getYaw()); + driveTrain.setLeftSpeed(0); + driveTrain.setRightSpeed(0); + } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + driveTrain.PIDdrive(driveTrain.getAveragePosition(), 0.2); + driveTrain.PIDturn(navX.getYaw()); + driveTrain.setRightSpeed(driveTrain.getDriveOutput() + driveTrain.getTurnOutput()); + driveTrain.setLeftSpeed(driveTrain.getDriveOutput() - driveTrain.getTurnOutput()); + if(Math.abs(driveTrain.getDriveError()) < Constants.maximumDriveError){ + counter++; + }else{ + counter = 0; + } + cycleCounter++; + } // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + driveTrain.setLeftSpeed(0); + driveTrain.setRightSpeed(0); + } // Returns true when the command should end. @Override public boolean isFinished() { - return false; + if(counter > 100000 || cycleCounter > timeout/20){ + return true; + }else{ + return false; + } + } } diff --git a/src/main/java/frc/robot/commands/IntakeCone.java b/src/main/java/frc/robot/commands/IntakeCone.java new file mode 100644 index 0000000..a59e666 --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeCone.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; + +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.Constants; + +public class IntakeCone extends CommandBase { + /** Creates a new IntakeCone. */ + private final IntakeSubsystem intake; + private final XboxController xbox2; + public IntakeCone(IntakeSubsystem intake, XboxController xbox2) { + this.intake = intake; + this.xbox2 = xbox2; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(intake); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + intake.intakeSetSpeed(-Constants.intakeMotorSpeed); + xbox2.setRumble(RumbleType.kRightRumble, 1); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + intake.intakeSetSpeed(0); + xbox2.setRumble(RumbleType.kBothRumble, 0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/IntakeSwitch.java b/src/main/java/frc/robot/commands/IntakeCube.java similarity index 66% rename from src/main/java/frc/robot/commands/IntakeSwitch.java rename to src/main/java/frc/robot/commands/IntakeCube.java index b1507fc..1200d02 100644 --- a/src/main/java/frc/robot/commands/IntakeSwitch.java +++ b/src/main/java/frc/robot/commands/IntakeCube.java @@ -6,41 +6,42 @@ import java.util.function.Supplier; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Constants; import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.RobotContainer; -public class IntakeSwitch extends CommandBase { +public class IntakeCube extends CommandBase { /** Creates a new IntakeSwitch. */ private final IntakeSubsystem intakeSubsystem; - private final boolean reverse; - - public IntakeSwitch(IntakeSubsystem intakeSubsystem, boolean reverse) { + private final XboxController xbox2; + public IntakeCube(IntakeSubsystem intakeSubsystem, XboxController xbox2) { // Use addRequirements() here to declare subsystem dependencies. this.intakeSubsystem = intakeSubsystem; - this.reverse = reverse; + this.xbox2 = xbox2; addRequirements(intakeSubsystem); } // Called when the command is initially scheduled. @Override public void initialize() { - // Put xbox input here + intakeSubsystem.intakeSetSpeed(0); + xbox2.setRumble(RumbleType.kLeftRumble, 1); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (reverse == true) { - intakeSubsystem.outputGP(); - } else { - intakeSubsystem.intakeGP(); - } + intakeSubsystem.intakeSetSpeed(Constants.intakeMotorSpeed); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { intakeSubsystem.stopMotor(); + xbox2.setRumble(RumbleType.kBothRumble, 0); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/PIDAssistedArmMovement.java b/src/main/java/frc/robot/commands/PIDAssistedArmMovement.java new file mode 100644 index 0000000..00a33fc --- /dev/null +++ b/src/main/java/frc/robot/commands/PIDAssistedArmMovement.java @@ -0,0 +1,62 @@ +// 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 java.util.function.Supplier; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Constants; +import frc.robot.subsystems.ArmSubsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class PIDAssistedArmMovement extends CommandBase { + /** Creates a new PIDAssistedArmMovement. */ + private final ArmSubsystem arm; + private Supplier xbox; + private double target, value; + public PIDAssistedArmMovement(ArmSubsystem arm, Supplier xbox) { + this.arm = arm; + this.xbox = xbox; + value = 0; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(arm); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + arm.resetArmPID(); + arm.armMove(0); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + value = xbox.get(); + if(Math.abs(value) > 0.04){ + target = value*Constants.kArmPID; + } + SmartDashboard.putNumber("Arm PID Target:", target); + arm.setPIDtarget(target); + arm.moveArmPID(arm.getArmPosition(), 0.5); + if(arm.getArmPosition()Constants.armBackEncoderLimit){ + arm.armMove(arm.getArmMoveOutput()); + }else{ + arm.armMove(0); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + arm.armMove(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ResetEncoders.java b/src/main/java/frc/robot/commands/ResetEncoders.java new file mode 100644 index 0000000..8f0c1c3 --- /dev/null +++ b/src/main/java/frc/robot/commands/ResetEncoders.java @@ -0,0 +1,45 @@ +// 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 java.sql.Driver; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.ArmSubsystem; +import frc.robot.subsystems.DriveTrain; + +public class ResetEncoders extends CommandBase { + /** Creates a new ResetEncoders. */ + private final DriveTrain driveTrain; + private final ArmSubsystem armSubsystem; + + public ResetEncoders(DriveTrain driveTrain, ArmSubsystem armSubsystem) { + this.driveTrain = driveTrain; + this.armSubsystem = armSubsystem; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(driveTrain); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + driveTrain.resetEncoders(); + armSubsystem.resetArmEncoders(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/frc/robot/commands/SetPipeline.java b/src/main/java/frc/robot/commands/SetPipeline.java new file mode 100644 index 0000000..9b48a23 --- /dev/null +++ b/src/main/java/frc/robot/commands/SetPipeline.java @@ -0,0 +1,52 @@ +// 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.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.LimeLightSubsystem; + +public class SetPipeline extends CommandBase { + /** Creates a new AutoSetPipeline. */ + private final LimeLightSubsystem limeLightSubsystem; + private final int pipeline; + private boolean finish; + + public SetPipeline(LimeLightSubsystem limeLightSubsystem, int pipeline) { + this.pipeline = pipeline; + this.limeLightSubsystem = limeLightSubsystem; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(limeLightSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + limeLightSubsystem.setPipeline(pipeline); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + finish = true; + + } + + // 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() { + if (finish == true){ + return true; + } + return false; + } +} diff --git a/src/main/java/frc/robot/commands/VisionAutoDrive.java b/src/main/java/frc/robot/commands/Wait.java similarity index 60% rename from src/main/java/frc/robot/commands/VisionAutoDrive.java rename to src/main/java/frc/robot/commands/Wait.java index db5ac82..4ffd488 100644 --- a/src/main/java/frc/robot/commands/VisionAutoDrive.java +++ b/src/main/java/frc/robot/commands/Wait.java @@ -6,27 +6,42 @@ import edu.wpi.first.wpilibj2.command.CommandBase; -public class VisionAutoDrive extends CommandBase { - /** Creates a new VisionAutoDrive. */ - public VisionAutoDrive() { +public class Wait extends CommandBase { + /** Creates a new Wait. */ + private int millis; + private int counter; + public Wait(int millis) { + this.millis = millis; + counter = 0; // Use addRequirements() here to declare subsystem dependencies. } // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + counter = 0; + } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + counter++; + } // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + counter = 0; + } // Returns true when the command should end. @Override public boolean isFinished() { - return false; + if(counter >= (millis/20)){ + return true; + }else{ + return false; + } + } } diff --git a/src/main/java/frc/robot/commands/XboxArm.java b/src/main/java/frc/robot/commands/XboxArm.java new file mode 100644 index 0000000..6077477 --- /dev/null +++ b/src/main/java/frc/robot/commands/XboxArm.java @@ -0,0 +1,54 @@ +// 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 java.util.function.Supplier; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.Constants; +import frc.robot.subsystems.ArmSubsystem; + +public class XboxArm extends CommandBase { + /** Creates a new XboxArm. */ + private final ArmSubsystem armSubsystem; + private Supplier y_Supplier; + public XboxArm(Supplier y_supplier, ArmSubsystem armSubsystem) { + // Use addRequirements() here to declare subsystem dependencies. + this.armSubsystem = armSubsystem; + this.y_Supplier = y_supplier; + addRequirements(armSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + armSubsystem.armMove(0); + armSubsystem.resetArmEncoders(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (armSubsystem.getArmPosition() > 70000){ + armSubsystem.armMove(0); + } else { + armSubsystem.armMove(y_Supplier.get()); + } + + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + armSubsystem.armMove(0); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/autoDriveApriltag.java b/src/main/java/frc/robot/commands/autoDriveApriltag.java new file mode 100644 index 0000000..872858c --- /dev/null +++ b/src/main/java/frc/robot/commands/autoDriveApriltag.java @@ -0,0 +1,59 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj.AddressableLED; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.DriveTrain; +import frc.robot.subsystems.LimeLightSubsystem; +import frc.robot.subsystems.navXSubsystem; + + +public class AutoDriveApriltag extends CommandBase { + /** Creates a new autoDriveApriltag. */ + private DriveTrain driveTrain; + private LimeLightSubsystem limeLightSubsystem; + private double H_angle; + private double distance; + + public AutoDriveApriltag(DriveTrain driveTrain, LimeLightSubsystem limeLightSubsystem) { + // Use addRequirements() here to declare subsystem dependencies. + this.driveTrain = driveTrain; + this.limeLightSubsystem = limeLightSubsystem; + H_angle = limeLightSubsystem.getH_angle(); + distance = limeLightSubsystem.calculateXdistance();// distance to apriltag + addRequirements(driveTrain); + addRequirements(limeLightSubsystem); + + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + driveTrain.resetDrivePID(); + driveTrain.setDriveTarget(distance); // + driveTrain.setLeftSpeed(0); + driveTrain.setRightSpeed(0); + + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index dead69e..35e20e0 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -4,52 +4,108 @@ package frc.robot.subsystems; +import edu.wpi.first.wpilibj.CAN; import edu.wpi.first.wpilibj2.command.SubsystemBase; import com.ctre.phoenix.motorcontrol.ControlMode; import frc.robot.Constants; import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.TalonFX; +import com.revrobotics.CANSparkMax; + import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; public class ArmSubsystem extends SubsystemBase { /** Creates a new ArmSubsystem. */ - private final TalonFX armPivotMotor = new TalonFX(Constants.talonArmPivot); - private final TalonFX elevatorLiftMotor1 = new TalonFX(Constants.talonLift1); - private final TalonFX elevatorLiftMotor2 = new TalonFX(Constants.talonLift2); - private final CANSparkMax extensionMotor = new CANSparkMax(Constants.extensionMotorID, MotorType.kBrushless); + // private final CANSparkMax armMotor = new CANSparkMax(Constants.armMotorID, MotorType.kBrushless); + private final TalonFX armMotor = new TalonFX(Constants.armMotorID); + private double armTarget; + private double armError; + private double armPrevError; + private double armP; + private double armI; + private double armD; + private double armOutput; + private double prevArmOutput; + public ArmSubsystem() { - armPivotMotor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); - elevatorLiftMotor1.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); - elevatorLiftMotor2.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); + armTarget = 0; + armError = 0; + armPrevError = 0; + armP = 0; + armI = 0; + armD = 0; + armOutput = 0; + prevArmOutput = 0; + armMotor.setNeutralMode(NeutralMode.Coast); } //setters - public void elevatorMove(double speed){ - elevatorLiftMotor1.set(ControlMode.PercentOutput, speed); - elevatorLiftMotor2.set(ControlMode.PercentOutput, speed); + public void armMove(double speed){ + // armMotor.set(speed); + + armMotor.set(ControlMode.PercentOutput, speed); + } - public void armPivotMotor(double speed){ - armPivotMotor.set(ControlMode.PercentOutput, speed); + public void resetArmEncoders(){ + armMotor.setSelectedSensorPosition(0); } - public void extensionMotor (double speed){ - extensionMotor.set(speed); + public double getArmPosition(){ + return armMotor.getSelectedSensorPosition(); } - //getters - public double getElevatorPosition(){//one revolution is 2048 encoder units. - double position1 = elevatorLiftMotor1.getSelectedSensorPosition(); - double position2 = elevatorLiftMotor2.getSelectedSensorPosition(); - if(Math.abs(position1 - position2) < 100){ - return (position1 + position2)/2; - }else{ - return -1;//return '-1' if the two sensor positions vary too much. + // public double getArmSpeed(){ + // return armMotor.get(); + + // } + + public void resetArmPID(){ + armTarget = 0; + armError = 0; + armPrevError = 0; + armP = 0; + armI = 0; + armD = 0; + armOutput = 0; + prevArmOutput = 0; + } + + public void moveArmPID(double sensorInput, double limit){ + armError = armTarget - sensorInput; + armP = armError; + armI += armError; + armD = armError - armPrevError; + + armOutput = Constants.kArmP*armP + Constants.kArmI*armI + Constants.kArmD*armD; + if(armOutput > limit){ + armOutput = limit; } + + if(armOutput < -limit){ + armOutput = -limit; + } + armPrevError = armError; + prevArmOutput = armOutput; + } + + //getters + public double getArmMoveOutput(){ + return armOutput; + } + + public double getArmMoveError(){ + return armError; + } + + //setters + public void setPIDtarget(double target){ + armTarget = target; } @Override public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index 533d07b..e6144d9 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -4,8 +4,15 @@ package frc.robot.subsystems; +import java.lang.annotation.Retention; +import java.lang.annotation.Target; +import java.sql.Blob; + import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.TalonFX; +import com.fasterxml.jackson.annotation.JacksonInject.Value; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; @@ -17,6 +24,8 @@ public class DriveTrain extends SubsystemBase { private final TalonFX FL = new TalonFX(Constants.FLid); private final TalonFX BL = new TalonFX(Constants.BLid); + private double velocity; + //PIDturn variables private double turnTarget; private double turnError; @@ -25,6 +34,7 @@ public class DriveTrain extends SubsystemBase { private double turnI; private double turnD; private double turnOutput; + private double turnPrevOutput; //PIDdrive variables @@ -36,7 +46,7 @@ public class DriveTrain extends SubsystemBase { private double driveD; private double driveOutput; private double potDriveOutput; - private double prevDriveOutput; + private double drivePrevOutput; //BalancePID variables private double balanceTarget; @@ -52,6 +62,10 @@ public DriveTrain() { BR.setInverted(false); FL.setInverted(true); BL.setInverted(true); + FR.setNeutralMode(NeutralMode.Coast); + BR.setNeutralMode(NeutralMode.Coast); + FL.setNeutralMode(NeutralMode.Coast); + BL.setNeutralMode(NeutralMode.Coast); //turn variables turnTarget = 0; turnError = 0; @@ -64,14 +78,14 @@ public DriveTrain() { driveTarget = 0; driveError = 0; drivePrevError = 0; + drivePrevOutput = 0; driveP = 0; driveI = 0; driveD = 0; driveOutput = 0; potDriveOutput = 0; - prevDriveOutput = 0; //BalancePID variables - balanceTarget = 0; + balanceTarget = Constants.balanceTarget; balanceError = 0; balancePrevError = 0; balanceP = 0; @@ -89,7 +103,7 @@ public void setLeftSpeed(double speed){ BL.set(ControlMode.PercentOutput, speed); } - //Turn and Drive PIDs + // PIDs reset public void resetDrivePID(){ driveTarget = 0; driveError = 0; @@ -99,7 +113,7 @@ public void resetDrivePID(){ driveD = 0; driveOutput = 0; potDriveOutput = 0; - prevDriveOutput = 0; + drivePrevOutput = 0; } public void resetTurnPID(){ @@ -112,40 +126,36 @@ public void resetTurnPID(){ turnOutput = 0; } - //setters - public void setTurnTarget(double turnTarget){ - this.turnTarget = turnTarget; - } - - public void setDriveTarget(double driveTarget){ - this.driveTarget = driveTarget; - } - - public void setBalanceTarget(double balanceTarget){ - this.balanceTarget = balanceTarget; - } - //takes in sensor input to turn robot into the correct angle public void PIDturn(double sensorInput){ turnError = turnTarget - sensorInput; turnP = turnError; - if(turnError= 1) output = 1; - // if(output <= -1) output = -1; + turnPrevOutput = turnOutput; + } + + public void resetEncoders(){ + FR.setSelectedSensorPosition(0); + FL.setSelectedSensorPosition(0); + BR.setSelectedSensorPosition(0); + BL.setSelectedSensorPosition(0); + + } + + + public double getEncoderPosition(){ + return (-(FR.getSelectedSensorPosition()+BR.getSelectedSensorPosition()+FL.getSelectedSensorPosition()+BL.getSelectedSensorPosition())/4); + } + + public double getRightEncoders(){ + return ((FR.getSelectedSensorPosition() + BR.getSelectedSensorPosition())/2); + } + public double getLeftEncoders(){ + return ((FL.getSelectedSensorPosition() + BL.getSelectedSensorPosition())/2); } public void PIDdrive(double sensorInput, double limit) { @@ -155,39 +165,42 @@ public void PIDdrive(double sensorInput, double limit) { driveD = driveError - drivePrevError; - driveOutput = Constants.kP*driveP + Constants.kI*driveI + Constants.kD*driveD; + driveOutput = Constants.kDriveP*driveP + Constants.kDriveI*driveI + Constants.kDriveD*driveD; if(driveOutput > limit){ driveOutput = limit; } - if(driveOutput < -limit){ driveOutput = -limit; } drivePrevError = driveError; - prevDriveOutput = driveOutput; - //SmartDashboard.putNumber("PID Drive output:", driveOutput); - + drivePrevOutput = driveOutput; } + // public void PIDdriveAndTurn(double encoderSensorInput, double speedLimit, double targetAngle, double currentAngle) { + // driveError = driveTarget - encoderSensorInput; + // driveP = driveError; + // driveI += driveError; + // driveD = driveError - drivePrevError; + + + // driveOutput = Constants.kP*driveP + Constants.kI*driveI + Constants.kD*driveD; + // if(driveOutput > speedLimit){ + // driveOutput = speedLimit; + // } + // if(driveOutput < -speedLimit){ + // driveOutput = -speedLimit; + // } + + // drivePrevError = driveError; + // prevDriveOutput = driveOutput; + // } + public double getTurnOutput() { return turnOutput; } - //getters - public double getTurnError(){ - return turnError; - } - - public double getDriveError(){ - return driveError; - } - - public double getBalanceError(){ - return balanceError; - } - //Balance PIDs public void PIDBalance(double sensorInput) { @@ -199,11 +212,75 @@ public void PIDBalance(double sensorInput) balancePrevError = balanceError; } - public double getBalanceOutput() - { +//getters + public double getBalanceOutput(){ return balanceOutput; } + public double getVelocity(){ + velocity = (FR.getActiveTrajectoryVelocity() + FL.getActiveTrajectoryVelocity() + BR.getActiveTrajectoryVelocity() + BL.getActiveTrajectoryVelocity()) / 4; + return velocity; + } + public double getDriveOutput(){ + return driveOutput; + } + + public double getDriveError(){ + return driveError; + } + + public double getTurnError(){ + return turnError; + } + + public double getTurnPrevError(){ + return turnPrevError; + } + + public double getTurnPrevOutput(){ + return turnPrevOutput; + } + public double getFRid(){//actual bot is negative since the gear box was mounted upside down. + return FR.getSelectedSensorPosition(); + } + public double getFLid(){ + return FL.getSelectedSensorPosition(); + } + public double getBRid(){ + return BR.getSelectedSensorPosition(); + } + public double getBLid(){ + return BL.getSelectedSensorPosition(); + } + public double getAveragePosition(){ + return (getFRid()+getFLid()+getBRid()+getBLid())/4; + } + public double getDrivePrevOutput(){ + return drivePrevOutput; + } + public void setDriveTarget(double encoderUnit){ + driveTarget = encoderUnit; + } + public double getTurnTarget(){ + return turnTarget; + } + public void setTurnTarget(double degrees){ + turnTarget = degrees; + } + + public void setMotorsCoast(){ + FR.setNeutralMode(NeutralMode.Coast); + BR.setNeutralMode(NeutralMode.Coast); + BL.setNeutralMode(NeutralMode.Coast); + FL.setNeutralMode(NeutralMode.Coast); + } + + public void setMotorsLocked(){ + FR.setNeutralMode(NeutralMode.Brake); + BR.setNeutralMode(NeutralMode.Brake); + FL.setNeutralMode(NeutralMode.Brake); + BL.setNeutralMode(NeutralMode.Brake); + } @Override public void periodic() { // This method will be called once per scheduler run diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 956619b..7416970 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -16,35 +16,31 @@ public class IntakeSubsystem extends SubsystemBase { //IntakeMoterL is the lower intake motor //IntakeMoterh is the Higher intake motor - goes opposite direction of lower intake motor private final CANSparkMax intakeMotorM1; - private final CANSparkMax intakeMotorM2; /* * Instansiates intakeMotor to allow us to control the motor of the intake */ public IntakeSubsystem() { intakeMotorM1 = new CANSparkMax(Constants.deviceIdIntakeM1, CANSparkMax.MotorType.kBrushless); - intakeMotorM2 = new CANSparkMax(Constants.deviceIdIntakeM2, CANSparkMax.MotorType.kBrushless); } - public void intakeGP() + public void intakeSetSpeed(double speed) { - intakeMotorM1.set(Constants.intakeMotorSpeed); - intakeMotorM2.set(Constants.intakeMotorSpeed); + intakeMotorM1.set(speed); } public void stopMotor() { intakeMotorM1.set(0); - intakeMotorM2.set(0); } //This method is kinda useless since we don't really take anything out throught the intake but its here // incase we need to - public void outputGP() - { - intakeMotorM1.set(-Constants.intakeMotorSpeed); - intakeMotorM2.set(-Constants.intakeMotorSpeed); - } + // public void outputGP() + // { + // intakeMotorM1.set(-Constants.intakeMotorSpeed); + // intakeMotorM2.set(-Constants.intakeMotorSpeed); + // } //The current should be the same between motors since they are moving at the same speed. public double getElectricCurrentM1() diff --git a/src/main/java/frc/robot/subsystems/LimeLightSubsystem.java b/src/main/java/frc/robot/subsystems/LimeLightSubsystem.java index 8e882b9..a239c7e 100644 --- a/src/main/java/frc/robot/subsystems/LimeLightSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LimeLightSubsystem.java @@ -5,6 +5,7 @@ package frc.robot.subsystems; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; import edu.wpi.first.cscore.VideoMode; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; @@ -15,33 +16,66 @@ public class LimeLightSubsystem extends SubsystemBase { /** Creates a new LimelightSubSystem. */ private NetworkTable limeLight; - private NetworkTableEntry aprilTagID; - private NetworkTableEntry aprilTagCoords; - private NetworkTableEntry targets; - + private NetworkTableEntry V_angle, H_angle, hasTargets, botPose, aprilID; + // private double[] botPose; + + private double zDistance; + private double xDistance; + public LimeLightSubsystem() { limeLight = NetworkTableInstance.getDefault().getTable("limelight"); - aprilTagID = limeLight.getEntry("tid"); - aprilTagCoords = limeLight.getEntry("camtran"); - targets = limeLight.getEntry("tv"); + + V_angle = limeLight.getEntry("ty"); + H_angle = limeLight.getEntry("tx"); + hasTargets = limeLight.getEntry("tv"); + botPose = limeLight.getEntry("botPose"); + aprilID = limeLight.getEntry("tid"); + + zDistance = -1;//this value is for if there's an error, makes sense that distance will never be negative + xDistance = -1;//the distance in the x direction offset from center of robot. + + } + + public double hasTargets(){ + return hasTargets.getDouble(0); } - public double getAprilId(){ - return aprilTagID.getDouble(0); + public double getV_angle(){ + return V_angle.getDouble(0); } - - public double getAprilCoords(){ - return aprilTagCoords.getDouble(0); + public double getH_angle(){ + return H_angle.getDouble(0); } + public double[] getBotPose(){ + return botPose.getDoubleArray(new double[6]); + + } + public double getaprilTagID(){ + return aprilID.getDouble(0); - public double getTargets(){ - return targets.getDouble(0); + } + public void setPipeline(int pipe){ + limeLight.getEntry("pipeline").setNumber(pipe); + } + //0: AprilTag + //1: Reflective + //2: Zoomed In + + public double calculateZdistance(){//Z direction is foward from the robot + zDistance = ((Constants.goalHeight-Constants.limeLightHeight)/(Math.tan(Math.toRadians(getV_angle()+Constants.limeLightInitAngle)))); + return zDistance; + } - - } + public double calculateXdistance(){//X direction is sideways from the robot + xDistance = calculateZdistance()*Math.tan(Math.toRadians(getH_angle())); + return xDistance; + } + @Override public void periodic() { // This method will be called once per scheduler run + calculateXdistance(); + calculateZdistance(); } } diff --git a/src/main/java/frc/robot/subsystems/navXSubsystem.java b/src/main/java/frc/robot/subsystems/navXSubsystem.java index 04c811c..f32040e 100644 --- a/src/main/java/frc/robot/subsystems/navXSubsystem.java +++ b/src/main/java/frc/robot/subsystems/navXSubsystem.java @@ -43,8 +43,8 @@ public boolean isCalibrating() { return navX.isCalibrating(); } - public float getYaw() { - return navX.getYaw(); + public double getYaw() { + return navX.getAngle(); } public float getPitch() { diff --git a/src/main/java/frc/robot/subsystems/rasberryPiCamera.java b/src/main/java/frc/robot/subsystems/rasberryPiCamera.java index 9181f28..438dae0 100644 --- a/src/main/java/frc/robot/subsystems/rasberryPiCamera.java +++ b/src/main/java/frc/robot/subsystems/rasberryPiCamera.java @@ -4,40 +4,50 @@ // package frc.robot.subsystems; +// import edu.wpi.first.math.util.Units; // import edu.wpi.first.wpilibj2.command.SubsystemBase; +// import frc.robot.Constants; // import java.util.List; // import org.opencv.photo.Photo; // import org.photonvision.PhotonCamera; +// import org.photonvision.PhotonUtils; // import org.photonvision.targeting.PhotonPipelineResult; // import org.photonvision.targeting.PhotonTrackedTarget; -// public class rasberryPiCamera extends SubsystemBase { +// public class RasberryPiCamera extends SubsystemBase { // /** Creates a new rasber ryPiCamera. */ -// private PhotonCamera photonCamera = new PhotonCamera("OV5647"); +// private PhotonCamera photonCamera = new PhotonCamera("698"); //original camera name is "OV5647" // private PhotonPipelineResult result; // private PhotonTrackedTarget target; // private List targets; // private int aprilID; -// public rasberryPiCamera() { +// public RasberryPiCamera() { // // piCam = NetworkTableInstance.getDefault().getTable("photonvision"); // // hasTarget = piCam.getEntry("hasTarget"); // // targetArea = piCam.getEntry("targetArea"); // // targetPose = piCam.getEntry("targetPose"); -// this.result = photonCamera.getLatestResult(); +// this.result = photonCamera.getLatestResult(); + // targets = result.getTargets(); // } -// public double aprilTagID(){ -// aprilID = target.getFiducialId(); -// return aprilID; +// public double getaprilTagID(){ +// if (getHasTarget() == true){ +// aprilID = target.getFiducialId(); +// return aprilID; +// } else { +// return 404; +// } + + // } @@ -46,8 +56,13 @@ // } // public double getTargetArea(){ +// if (getHasTarget() == true) { // target = result.getBestTarget(); // return target.getArea(); +// } else { +// return 404; +// } + // } // public double getTargetPose(){ @@ -55,12 +70,50 @@ // return target.getPoseAmbiguity(); // } +// public double getTargetSkew(){ +// if (getHasTarget() == true){ +// target = result.getBestTarget(); +// return target.getSkew(); +// } else { +// return 404; +// } +// } + +// public double getTargetYaw(){ +// if (getHasTarget() == true){ +// target = result.getBestTarget(); +// return target.getYaw(); +// } else { +// return 404; +// } +// } + +// public double getTargetPitch(){ +// if (getHasTarget() == true){ +// target = result.getBestTarget(); +// return target.getPitch(); +// } else { +// return 404; +// } + +// } + +// public double targetDistance(){ +// if (getHasTarget() == true){ +// target = result.getBestTarget(); +// return PhotonUtils.calculateDistanceToTargetMeters(Constants.CAMERA_HEIGHT_METERS, Constants.TARGET_HEIGHT_METERS, Constants.CAMERA_PITCH_RADIANS, Units.radiansToDegrees(target.getPitch())); +// } else { +// return 404; +// } + +// } + // @Override // public void periodic() { // // This method will be called once per scheduler run // result = photonCamera.getLatestResult(); -// target = result.getBestTarget(); +// // target = result.getBestTarget(); // } -// } +// } \ No newline at end of file