From 760844de105ccecfb283fcc00cf9cae2e3c21986 Mon Sep 17 00:00:00 2001 From: Rewand727 Date: Wed, 8 Mar 2023 16:52:00 -0700 Subject: [PATCH 01/41] Commands need fixing --- src/main/java/frc/robot/Constants.java | 45 ++++++++++- src/main/java/frc/robot/Robot.java | 13 +++- src/main/java/frc/robot/RobotContainer.java | 16 ++-- src/main/java/frc/robot/commands/XboxArm.java | 47 +++++++++++ ...ckDriveCommand.java => autoArmPickup.java} | 10 ++- .../frc/robot/commands/autoDriveApriltag.java | 78 +++++++++++++++++++ .../java/frc/robot/commands/autoPlace.java | 6 +- .../frc/robot/subsystems/ArmSubsystem.java | 77 ++++++++++++------ .../java/frc/robot/subsystems/DriveTrain.java | 24 ++++++ .../robot/subsystems/LimeLightSubsystem.java | 48 ++++++++---- .../robot/subsystems/rasberryPiCamera.java | 23 +++++- 11 files changed, 333 insertions(+), 54 deletions(-) create mode 100644 src/main/java/frc/robot/commands/XboxArm.java rename src/main/java/frc/robot/commands/{joyStickDriveCommand.java => autoArmPickup.java} (80%) create mode 100644 src/main/java/frc/robot/commands/autoDriveApriltag.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b3fdc3f..64b3ece 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -27,7 +27,9 @@ public final class Constants { 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,7 +38,24 @@ 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; - //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; @@ -60,6 +79,7 @@ public static class OperatorConstants { public static final int talonLift1 = 2; public static final int talonLift2 = 3; public static final int extensionMotorID = 0; + public static final int armMotor = 0; //DriveTrain Motor ID's public static final int FRid = 0; @@ -78,6 +98,13 @@ 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.1; public static final double turnkI = 0.00; @@ -97,5 +124,17 @@ public static class OperatorConstants { public static final double kI = 0.0; public static final double kD = 0; - public static final double IactZone = 0; + public static final double IactZone = 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; + + } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d554d52..23bdef7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -31,7 +31,6 @@ 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(); } /** @@ -53,6 +52,18 @@ public void robotPeriodic() { 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()); + + + } /** This function is called once each time the robot enters Disabled mode. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index abb5d22..76667a7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,7 +5,6 @@ package frc.robot; -import frc.robot.commands.IntakeSwitch; import frc.robot.subsystems.LimeLightSubsystem; import frc.robot.subsystems.IntakeSubsystem; import edu.wpi.first.wpilibj.Joystick; @@ -29,7 +28,10 @@ public class RobotContainer { // The robot's subsystems and commands are defined here... public LimeLightSubsystem LimeLightSubsystem = new LimeLightSubsystem(); - public XboxController Xbox = new XboxController(0); + public ArmSubsystem armSubsystem = new ArmSubsystem(); + public XboxController Xbox = new XboxController(Constants.xBoxControllerid); + public XboxController Xbox2 = new XboxController(Constants.xBoxControllerid2); + public rasberryPiCamera rasberryPiCamera = new rasberryPiCamera(); //Intake private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(); @@ -39,19 +41,21 @@ public class RobotContainer { public final JoystickButton Abutton = new JoystickButton(Xbox, Constants.Xbox_Button_A); public final JoystickButton Bbutton = new JoystickButton(Xbox, Constants.Xbox_Button_B); + public final JoystickButton AButtonX2 = new JoystickButton(Xbox, Constants.Xbox_Button_AX2); + // Replace with CommandPS4Controller or CommandJoystick if needed private 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); + /** 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 XboxDrive(driveTrain, () -> Xbox.getRightX(), () -> Xbox.getLeftY())); + armSubsystem.setDefaultCommand(new XboxArm(() -> Xbox2.getRightX(), armSubsystem)); configureBindings(); } @@ -69,6 +73,8 @@ private void configureBindings() { Xbutton.toggleWhenPressed(new IntakeSwitch(intakeSubsystem, false)); Ybutton.toggleWhenPressed(new IntakeSwitch(intakeSubsystem, true)); + Abutton.toggleWhenPressed(new autoDriveApriltag(driveTrain, rasberryPiCamera, LimeLightSubsystem)); + // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, 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..0c2efd7 --- /dev/null +++ b/src/main/java/frc/robot/commands/XboxArm.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 java.util.function.Supplier; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.ArmSubsystem; + +public class XboxArm extends CommandBase { + /** Creates a new XboxArm. */ + private final ArmSubsystem armSubsystem; + private final Supplier x_Supplier; + public XboxArm(Supplier x_supplier,ArmSubsystem armSubsystem) { + // Use addRequirements() here to declare subsystem dependencies. + this.armSubsystem = armSubsystem; + this.x_Supplier = x_supplier; + addRequirements(armSubsystem); + } + + // 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() { + if (x_Supplier.get() <= 0.5){ + armSubsystem.elevatorMove(x_Supplier.get()); + } else { + armSubsystem.elevatorMove(0.5); + } + + } + + // 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/commands/joyStickDriveCommand.java b/src/main/java/frc/robot/commands/autoArmPickup.java similarity index 80% rename from src/main/java/frc/robot/commands/joyStickDriveCommand.java rename to src/main/java/frc/robot/commands/autoArmPickup.java index 85e41c6..5b1de09 100644 --- a/src/main/java/frc/robot/commands/joyStickDriveCommand.java +++ b/src/main/java/frc/robot/commands/autoArmPickup.java @@ -5,10 +5,12 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.driveTrain; -public class joyStickDriveCommand extends CommandBase { - /** Creates a new joyStickDrive. */ - public joyStickDriveCommand() { +import frc.robot.subsystems.ArmSubsystem; + +public class autoArmPickup extends CommandBase { + /** Creates a new autoArmPickup. */ + private ArmSubsystem armSubsystem; + public autoArmPickup() { // Use addRequirements() here to declare subsystem dependencies. } 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..8cfaea6 --- /dev/null +++ b/src/main/java/frc/robot/commands/autoDriveApriltag.java @@ -0,0 +1,78 @@ +// 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.rasberryPiCamera; +import frc.robot.subsystems.navXSubsystem; + + +public class autoDriveApriltag extends CommandBase { + /** Creates a new autoDriveApriltag. */ + private DriveTrain driveTrain; + private LimeLightSubsystem limeLightSubsystem; + private rasberryPiCamera rasberryPiCamera; + private double piAngle = 0; + private double H_angle; + private double distance; + + public autoDriveApriltag(DriveTrain driveTrain, rasberryPiCamera rasberryPiCamera, LimeLightSubsystem limeLightSubsystem) { + // Use addRequirements() here to declare subsystem dependencies. + this.driveTrain = driveTrain; + this.rasberryPiCamera = rasberryPiCamera; + this.limeLightSubsystem = limeLightSubsystem; + addRequirements(driveTrain); + addRequirements(rasberryPiCamera); + addRequirements(limeLightSubsystem); + H_angle = limeLightSubsystem.getH_angle(); + distance = limeLightSubsystem.calculateXdistance();// distance to apriltag + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + driveTrain.resetDrivePID(); + driveTrain.setLeftSpeed(0); + driveTrain.setRightSpeed(0); + + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + for (int i = 0; i < 3; i++){ + H_angle = H_angle / 2; + distance = distance / 2; + driveTrain.PIDdrive(distance, 0.5); + + driveTrain.PIDturn(H_angle); + } + + driveTrain.PIDturn(H_angle); + + // piAngle = rasberryPiCamera.getTargetPitch(); // or yaw or whatever is the right angle for turning + + // driveTrain.PIDturn(piAngle); + + // driveTrain.PIDdrive(rasberryPiCamera.targetDistance(), rasberryPiCamera.targetDistance() + 10); + + } + + // 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/commands/autoPlace.java b/src/main/java/frc/robot/commands/autoPlace.java index baeda5d..5cd3ada 100644 --- a/src/main/java/frc/robot/commands/autoPlace.java +++ b/src/main/java/frc/robot/commands/autoPlace.java @@ -7,18 +7,22 @@ import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.LimeLightSubsystem; +import frc.robot.subsystems.rasberryPiCamera; public class autoPlace extends CommandBase { /** Creates a new autoPlace. */ private final LimeLightSubsystem limeLightSubsystem; + private final rasberryPiCamera rasberryPiCamera; - public autoPlace(LimeLightSubsystem limeLightSubsystem) { + public autoPlace(LimeLightSubsystem limeLightSubsystem, rasberryPiCamera rasberryPiCamera) { // Use addRequirements() here to declare subsystem dependencies. this.limeLightSubsystem = limeLightSubsystem; + this.rasberryPiCamera = rasberryPiCamera; addRequirements(limeLightSubsystem); + addRequirements(rasberryPiCamera); } // Called when the command is initially scheduled. diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index dead69e..0c519ff 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -15,42 +15,75 @@ 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 TalonFX armMotor = new TalonFX(Constants.armMotor); + 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); + armMotor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); + armTarget = 0; + armError = 0; + armPrevError = 0; + armP = 0; + armI = 0; + armD = 0; + armOutput = 0; + prevArmOutput = 0; } //setters public void elevatorMove(double speed){ - elevatorLiftMotor1.set(ControlMode.PercentOutput, speed); - elevatorLiftMotor2.set(ControlMode.PercentOutput, speed); + armMotor.set(ControlMode.PercentOutput, speed); } - public void armPivotMotor(double speed){ - armPivotMotor.set(ControlMode.PercentOutput, speed); + public void resetArmPID(){ + armTarget = 0; + armError = 0; + armPrevError = 0; + armP = 0; + armI = 0; + armD = 0; + armOutput = 0; + prevArmOutput = 0; } - public void extensionMotor (double speed){ - extensionMotor.set(speed); - } + public void moveArmPID(double sensorInput, double limit){ + armError = armTarget - sensorInput; + armP = armError; + armI += armError; + armD = armError - armPrevError; + + + armOutput = Constants.kP*armP + Constants.kI*armI + Constants.kD*armD; + if(armOutput > limit){ + armOutput = limit; + } - //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. + if(armOutput < -limit){ + armOutput = -limit; } + + armPrevError = armError; + prevArmOutput = armOutput; } + //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. + // } + // } @Override public void periodic() { // This method will be called once per scheduler run diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index 7bacfb0..7ca4fad 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -135,6 +135,21 @@ public void PIDturn(double sensorInput){ } + public void resetEncoders(){ + FR.setSelectedSensorPosition(0); + FL.setSelectedSensorPosition(0); + BR.setSelectedSensorPosition(0); + BL.setSelectedSensorPosition(0); + + } + + + public void getEncoderPosition(){ + FL.getActiveTrajectoryVelocity(); + } + + + public void PIDdrive(double sensorInput, double limit) { driveError = driveTarget - sensorInput; driveP = driveError; @@ -173,11 +188,20 @@ public void PIDBalance(double sensorInput) balancePrevError = balanceError; } + public double getBalanceOutput() { return balanceOutput; } + public double getDriveOutput(){ + return driveOutput; + } + + public void setDriveTarget(double encoderUnit){ + driveTarget = encoderUnit; + } + @Override public void periodic() { // This method will be called once per scheduler run diff --git a/src/main/java/frc/robot/subsystems/LimeLightSubsystem.java b/src/main/java/frc/robot/subsystems/LimeLightSubsystem.java index 8e882b9..e58c7f8 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,46 @@ 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; + private NetworkTableEntry H_angle; + private double zDistance; + private double xDistance; + + private NetworkTableEntry hasTargets; 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"); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("").getDoubleArray(new double[6]); + + 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 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 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 getTargets(){ - return targets.getDouble(0); - - - - } + 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/rasberryPiCamera.java b/src/main/java/frc/robot/subsystems/rasberryPiCamera.java index e03adf9..309d9f9 100644 --- a/src/main/java/frc/robot/subsystems/rasberryPiCamera.java +++ b/src/main/java/frc/robot/subsystems/rasberryPiCamera.java @@ -4,12 +4,15 @@ 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; @@ -17,7 +20,7 @@ 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; @@ -55,8 +58,26 @@ public double getTargetPose(){ return target.getPoseAmbiguity(); } + public double getTargetSkew(){ + target = result.getBestTarget(); + return target.getSkew(); + } + + public double getTargetYaw(){ + target = result.getBestTarget(); + return target.getYaw(); + } + public double getTargetPitch(){ + target = result.getBestTarget(); + return target.getPitch(); + } + public double targetDistance(){ + target = result.getBestTarget(); + return PhotonUtils.calculateDistanceToTargetMeters(Constants.CAMERA_HEIGHT_METERS, Constants.TARGET_HEIGHT_METERS, Constants.CAMERA_PITCH_RADIANS, Units.radiansToDegrees(target.getPitch())); + } + @Override public void periodic() { // This method will be called once per scheduler run From ab4de4cd8fafe745d098ad2bdfea06d6e66695ed Mon Sep 17 00:00:00 2001 From: Rewand727 Date: Wed, 8 Mar 2023 16:55:38 -0700 Subject: [PATCH 02/41] Removed visionautodrive --- .../frc/robot/commands/VisionAutoDrive.java | 32 --------- .../robot/subsystems/rasberryPiCamera.java | 66 ------------------- 2 files changed, 98 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/VisionAutoDrive.java delete mode 100644 src/main/java/frc/robot/subsystems/rasberryPiCamera.java diff --git a/src/main/java/frc/robot/commands/VisionAutoDrive.java b/src/main/java/frc/robot/commands/VisionAutoDrive.java deleted file mode 100644 index db5ac82..0000000 --- a/src/main/java/frc/robot/commands/VisionAutoDrive.java +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -public class VisionAutoDrive extends CommandBase { - /** Creates a new VisionAutoDrive. */ - public VisionAutoDrive() { - // 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() {} - - // 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/rasberryPiCamera.java b/src/main/java/frc/robot/subsystems/rasberryPiCamera.java deleted file mode 100644 index 9181f28..0000000 --- a/src/main/java/frc/robot/subsystems/rasberryPiCamera.java +++ /dev/null @@ -1,66 +0,0 @@ -// // Copyright (c) FIRST and other WPILib contributors. -// // Open Source Software; you can modify and/or share it under the terms of -// // the WPILib BSD license file in the root directory of this project. - -// package frc.robot.subsystems; - -// import edu.wpi.first.wpilibj2.command.SubsystemBase; - -// import java.util.List; - -// import org.opencv.photo.Photo; -// import org.photonvision.PhotonCamera; -// import org.photonvision.targeting.PhotonPipelineResult; -// import org.photonvision.targeting.PhotonTrackedTarget; - - - -// public class rasberryPiCamera extends SubsystemBase { -// /** Creates a new rasber ryPiCamera. */ -// private PhotonCamera photonCamera = new PhotonCamera("OV5647"); -// private PhotonPipelineResult result; -// private PhotonTrackedTarget target; -// private List targets; -// private int aprilID; -// public rasberryPiCamera() { - - -// // piCam = NetworkTableInstance.getDefault().getTable("photonvision"); -// // hasTarget = piCam.getEntry("hasTarget"); -// // targetArea = piCam.getEntry("targetArea"); -// // targetPose = piCam.getEntry("targetPose"); -// this.result = photonCamera.getLatestResult(); -// targets = result.getTargets(); - -// } - - -// public double aprilTagID(){ -// aprilID = target.getFiducialId(); -// return aprilID; -// } - - -// public boolean getHasTarget(){ -// return result.hasTargets(); -// } - -// public double getTargetArea(){ -// target = result.getBestTarget(); -// return target.getArea(); -// } - -// public double getTargetPose(){ -// target = result.getBestTarget(); -// return target.getPoseAmbiguity(); -// } - - - -// @Override -// public void periodic() { -// // This method will be called once per scheduler run -// result = photonCamera.getLatestResult(); -// target = result.getBestTarget(); -// } -// } From 71290551adbaeb566ceef4b7799ec1bfc1ea9162 Mon Sep 17 00:00:00 2001 From: Alex Nie <93626002+TheSparkyBoy@users.noreply.github.com> Date: Thu, 9 Mar 2023 15:02:51 -0700 Subject: [PATCH 03/41] Removed limit for drivePID --- .../java/frc/robot/subsystems/DriveTrain.java | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index 533d07b..4272b25 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -156,20 +156,22 @@ public void PIDdrive(double sensorInput, double limit) { driveOutput = Constants.kP*driveP + Constants.kI*driveI + Constants.kD*driveD; - if(driveOutput > limit){ - driveOutput = limit; - } + // if(driveOutput > limit){ + // driveOutput = limit; + // } - if(driveOutput < -limit){ - driveOutput = -limit; - } + // if(driveOutput < -limit){ + // driveOutput = -limit; + // } drivePrevError = driveError; prevDriveOutput = driveOutput; //SmartDashboard.putNumber("PID Drive output:", driveOutput); - } + public double getDriveOutput(){ + return driveOutput; + } public double getTurnOutput() { return turnOutput; From d04583887e3c1993eedd65a69d13ceea791ec85b Mon Sep 17 00:00:00 2001 From: Rewand727 Date: Thu, 9 Mar 2023 15:05:32 -0700 Subject: [PATCH 04/41] alex edits --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 4 +- .../frc/robot/commands/AutoVisionDrive.java | 52 +++++++++++++++++++ .../frc/robot/commands/autoDriveApriltag.java | 8 +-- .../java/frc/robot/commands/autoPlace.java | 6 +-- .../robot/subsystems/rasberryPiCamera.java | 6 +-- 6 files changed, 65 insertions(+), 13 deletions(-) create mode 100644 src/main/java/frc/robot/commands/AutoVisionDrive.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 23bdef7..2b98c2d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -48,7 +48,7 @@ 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()); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 76667a7..711ca7c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -16,7 +16,7 @@ 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; /** @@ -32,7 +32,7 @@ public class RobotContainer { public XboxController Xbox = new XboxController(Constants.xBoxControllerid); public XboxController Xbox2 = new XboxController(Constants.xBoxControllerid2); - public rasberryPiCamera rasberryPiCamera = new rasberryPiCamera(); + public RasberryPiCamera rasberryPiCamera = new RasberryPiCamera(); //Intake private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(); public final JoystickButton Xbutton = new JoystickButton(Xbox, Constants.Xbox_Button_X); 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..be19db5 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoVisionDrive.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.wpilibj2.command.CommandBase; +import frc.robot.subsystems.DriveTrain; +import frc.robot.subsystems.LimeLightSubsystem; +import frc.robot.subsystems.RasberryPiCamera; + +public class AutoVisionDrive extends CommandBase { + /** Creates a new AutoVisionDrive. */ + private final DriveTrain driveTrain; + private final RasberryPiCamera rasberryPiCamera; + private final LimeLightSubsystem limelights; + private final double target; + private int counter; + + public AutoVisionDrive(DriveTrain driveTrain, RasberryPiCamera rasberryPiCamera, LimeLightSubsystem limelight, double target) { + this.driveTrain = driveTrain; + this.limelights = limelight; + this.rasberryPiCamera = rasberryPiCamera; + this.target = target; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(driveTrain); + addRequirements(rasberryPiCamera); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + 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/commands/autoDriveApriltag.java b/src/main/java/frc/robot/commands/autoDriveApriltag.java index 8cfaea6..2ce46e8 100644 --- a/src/main/java/frc/robot/commands/autoDriveApriltag.java +++ b/src/main/java/frc/robot/commands/autoDriveApriltag.java @@ -8,7 +8,7 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.DriveTrain; import frc.robot.subsystems.LimeLightSubsystem; -import frc.robot.subsystems.rasberryPiCamera; +import frc.robot.subsystems.RasberryPiCamera; import frc.robot.subsystems.navXSubsystem; @@ -16,12 +16,12 @@ public class autoDriveApriltag extends CommandBase { /** Creates a new autoDriveApriltag. */ private DriveTrain driveTrain; private LimeLightSubsystem limeLightSubsystem; - private rasberryPiCamera rasberryPiCamera; + private RasberryPiCamera rasberryPiCamera; private double piAngle = 0; private double H_angle; private double distance; - public autoDriveApriltag(DriveTrain driveTrain, rasberryPiCamera rasberryPiCamera, LimeLightSubsystem limeLightSubsystem) { + public autoDriveApriltag(DriveTrain driveTrain, RasberryPiCamera rasberryPiCamera, LimeLightSubsystem limeLightSubsystem) { // Use addRequirements() here to declare subsystem dependencies. this.driveTrain = driveTrain; this.rasberryPiCamera = rasberryPiCamera; @@ -50,7 +50,7 @@ public void execute() { H_angle = H_angle / 2; distance = distance / 2; driveTrain.PIDdrive(distance, 0.5); - + driveTrain.PIDturn(H_angle); } diff --git a/src/main/java/frc/robot/commands/autoPlace.java b/src/main/java/frc/robot/commands/autoPlace.java index 5cd3ada..e6220c8 100644 --- a/src/main/java/frc/robot/commands/autoPlace.java +++ b/src/main/java/frc/robot/commands/autoPlace.java @@ -7,17 +7,17 @@ import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.LimeLightSubsystem; -import frc.robot.subsystems.rasberryPiCamera; +import frc.robot.subsystems.RasberryPiCamera; public class autoPlace extends CommandBase { /** Creates a new autoPlace. */ private final LimeLightSubsystem limeLightSubsystem; - private final rasberryPiCamera rasberryPiCamera; + private final RasberryPiCamera rasberryPiCamera; - public autoPlace(LimeLightSubsystem limeLightSubsystem, rasberryPiCamera rasberryPiCamera) { + public autoPlace(LimeLightSubsystem limeLightSubsystem, RasberryPiCamera rasberryPiCamera) { // Use addRequirements() here to declare subsystem dependencies. this.limeLightSubsystem = limeLightSubsystem; this.rasberryPiCamera = rasberryPiCamera; diff --git a/src/main/java/frc/robot/subsystems/rasberryPiCamera.java b/src/main/java/frc/robot/subsystems/rasberryPiCamera.java index 309d9f9..de8fe67 100644 --- a/src/main/java/frc/robot/subsystems/rasberryPiCamera.java +++ b/src/main/java/frc/robot/subsystems/rasberryPiCamera.java @@ -18,14 +18,14 @@ -public class rasberryPiCamera extends SubsystemBase { +public class RasberryPiCamera extends SubsystemBase { /** Creates a new rasber ryPiCamera. */ 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"); @@ -38,7 +38,7 @@ public rasberryPiCamera() { } - public double aprilTagID(){ + public double getaprilTagID(){ aprilID = target.getFiducialId(); return aprilID; } From 9331f99b02d9f9e88d03b01e3ba574b248702f55 Mon Sep 17 00:00:00 2001 From: Alex Nie <93626002+TheSparkyBoy@users.noreply.github.com> Date: Sat, 11 Mar 2023 12:09:33 -0700 Subject: [PATCH 05/41] Fixed Photonlib and RasberryPiClass Issues --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../{autoDriveApriltag.java => ApriltagAutoDrive.java} | 4 ++-- src/main/java/frc/robot/commands/autoArmPickup.java | 4 ++-- src/main/java/frc/robot/commands/autoPlace.java | 4 ++-- vendordeps/photonlib.json | 8 ++++---- 5 files changed, 11 insertions(+), 11 deletions(-) rename src/main/java/frc/robot/commands/{autoDriveApriltag.java => ApriltagAutoDrive.java} (95%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 711ca7c..18acaa8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -73,7 +73,7 @@ private void configureBindings() { Xbutton.toggleWhenPressed(new IntakeSwitch(intakeSubsystem, false)); Ybutton.toggleWhenPressed(new IntakeSwitch(intakeSubsystem, true)); - Abutton.toggleWhenPressed(new autoDriveApriltag(driveTrain, rasberryPiCamera, LimeLightSubsystem)); + Abutton.toggleWhenPressed(new ApriltagAutoDrive(driveTrain, rasberryPiCamera, LimeLightSubsystem)); diff --git a/src/main/java/frc/robot/commands/autoDriveApriltag.java b/src/main/java/frc/robot/commands/ApriltagAutoDrive.java similarity index 95% rename from src/main/java/frc/robot/commands/autoDriveApriltag.java rename to src/main/java/frc/robot/commands/ApriltagAutoDrive.java index 2ce46e8..58ec631 100644 --- a/src/main/java/frc/robot/commands/autoDriveApriltag.java +++ b/src/main/java/frc/robot/commands/ApriltagAutoDrive.java @@ -12,7 +12,7 @@ import frc.robot.subsystems.navXSubsystem; -public class autoDriveApriltag extends CommandBase { +public class ApriltagAutoDrive extends CommandBase { /** Creates a new autoDriveApriltag. */ private DriveTrain driveTrain; private LimeLightSubsystem limeLightSubsystem; @@ -21,7 +21,7 @@ public class autoDriveApriltag extends CommandBase { private double H_angle; private double distance; - public autoDriveApriltag(DriveTrain driveTrain, RasberryPiCamera rasberryPiCamera, LimeLightSubsystem limeLightSubsystem) { + public ApriltagAutoDrive(DriveTrain driveTrain, RasberryPiCamera rasberryPiCamera, LimeLightSubsystem limeLightSubsystem) { // Use addRequirements() here to declare subsystem dependencies. this.driveTrain = driveTrain; this.rasberryPiCamera = rasberryPiCamera; diff --git a/src/main/java/frc/robot/commands/autoArmPickup.java b/src/main/java/frc/robot/commands/autoArmPickup.java index 5b1de09..816407f 100644 --- a/src/main/java/frc/robot/commands/autoArmPickup.java +++ b/src/main/java/frc/robot/commands/autoArmPickup.java @@ -7,10 +7,10 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.ArmSubsystem; -public class autoArmPickup extends CommandBase { +public class AutoArmPickup extends CommandBase { /** Creates a new autoArmPickup. */ private ArmSubsystem armSubsystem; - public autoArmPickup() { + public AutoArmPickup() { // Use addRequirements() here to declare subsystem dependencies. } diff --git a/src/main/java/frc/robot/commands/autoPlace.java b/src/main/java/frc/robot/commands/autoPlace.java index e6220c8..4001b78 100644 --- a/src/main/java/frc/robot/commands/autoPlace.java +++ b/src/main/java/frc/robot/commands/autoPlace.java @@ -11,13 +11,13 @@ -public class autoPlace extends CommandBase { +public class AutoPlace extends CommandBase { /** Creates a new autoPlace. */ private final LimeLightSubsystem limeLightSubsystem; private final RasberryPiCamera rasberryPiCamera; - public autoPlace(LimeLightSubsystem limeLightSubsystem, RasberryPiCamera rasberryPiCamera) { + public AutoPlace(LimeLightSubsystem limeLightSubsystem, RasberryPiCamera rasberryPiCamera) { // Use addRequirements() here to declare subsystem dependencies. this.limeLightSubsystem = limeLightSubsystem; this.rasberryPiCamera = rasberryPiCamera; diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 8fdf301..dad3105 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2023.1.2", + "version": "v2023.4.2", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004 ", "mavenUrls": [ "https://maven.photonvision.org/repository/internal", @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "PhotonLib-cpp", - "version": "v2023.1.2", + "version": "v2023.4.2", "libName": "Photon", "headerClassifier": "headers", "sharedLibrary": true, @@ -30,12 +30,12 @@ { "groupId": "org.photonvision", "artifactId": "PhotonLib-java", - "version": "v2023.1.2" + "version": "v2023.4.2" }, { "groupId": "org.photonvision", "artifactId": "PhotonTargeting-java", - "version": "v2023.1.2" + "version": "v2023.4.2" } ] } \ No newline at end of file From f940a9feb6eb25cb7126dc27eb90681fa0f4779e Mon Sep 17 00:00:00 2001 From: Alex Nie <93626002+TheSparkyBoy@users.noreply.github.com> Date: Sat, 11 Mar 2023 15:16:05 -0700 Subject: [PATCH 06/41] Finished AutoBalancing Sequence for Auton Mode --- src/main/java/frc/robot/Constants.java | 2 + .../frc/robot/commands/AutoVisionDrive.java | 39 +++++++++++++----- .../frc/robot/commands/EncoderAutoDrive.java | 41 ++++++++++++++++--- .../frc/robot/commands/VisionAutoDrive.java | 28 +++++++++++-- .../java/frc/robot/subsystems/DriveTrain.java | 17 +++++--- 5 files changed, 103 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 64b3ece..830cf9d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -109,11 +109,13 @@ public static class OperatorConstants { public static final double turnkP = 0.1; public static final double turnkI = 0.00; 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 balancekI = 0; public static final double balancekD = 0; + public static final double levelConstant = 2; // auton drive PID constants //public static final int kTimeoutMs = 20; diff --git a/src/main/java/frc/robot/commands/AutoVisionDrive.java b/src/main/java/frc/robot/commands/AutoVisionDrive.java index be19db5..83dceb5 100644 --- a/src/main/java/frc/robot/commands/AutoVisionDrive.java +++ b/src/main/java/frc/robot/commands/AutoVisionDrive.java @@ -8,20 +8,26 @@ import frc.robot.subsystems.DriveTrain; import frc.robot.subsystems.LimeLightSubsystem; import frc.robot.subsystems.RasberryPiCamera; +import frc.robot.subsystems.navXSubsystem; +import frc.robot.Constants; public class AutoVisionDrive extends CommandBase { /** Creates a new AutoVisionDrive. */ private final DriveTrain driveTrain; private final RasberryPiCamera rasberryPiCamera; - private final LimeLightSubsystem limelights; - private final double target; + private final LimeLightSubsystem limelight; + private final navXSubsystem navX; + private final double initialSpeed; private int counter; + private boolean triggered; - public AutoVisionDrive(DriveTrain driveTrain, RasberryPiCamera rasberryPiCamera, LimeLightSubsystem limelight, double target) { + public AutoVisionDrive(DriveTrain driveTrain, RasberryPiCamera rasberryPiCamera, LimeLightSubsystem limelight, navXSubsystem navX, double initialSpeed) { this.driveTrain = driveTrain; - this.limelights = limelight; + this.limelight = limelight; this.rasberryPiCamera = rasberryPiCamera; - this.target = target; + this.navX = navX; + this.initialSpeed = initialSpeed; + this.triggered = false; // Use addRequirements() here to declare subsystem dependencies. addRequirements(driveTrain); addRequirements(rasberryPiCamera); @@ -30,23 +36,36 @@ public AutoVisionDrive(DriveTrain driveTrain, RasberryPiCamera rasberryPiCamera, // 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.PIDturn(rasberryPiCamera.getTargetYaw()); + driveTrain.setLeftSpeed(driveTrain.getTurnOutput()); + driveTrain.setRightSpeed(driveTrain.getTurnOutput()); + if(navX.getPitch() > Constants.levelConstant){ + triggered = true; + } } // 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(triggered == true && navX.getPitch() < Constants.levelConstant){ + return true; + }else{ + return false; + } } } diff --git a/src/main/java/frc/robot/commands/EncoderAutoDrive.java b/src/main/java/frc/robot/commands/EncoderAutoDrive.java index dd53730..86a9899 100644 --- a/src/main/java/frc/robot/commands/EncoderAutoDrive.java +++ b/src/main/java/frc/robot/commands/EncoderAutoDrive.java @@ -5,28 +5,59 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.DriveTrain; +import frc.robot.Constants; public class EncoderAutoDrive extends CommandBase { /** Creates a new EncoderAutoDrivve. */ - public EncoderAutoDrive() { + private final DriveTrain driveTrain; + private int counter; + private final double target; + public EncoderAutoDrive(DriveTrain driveTrain, double target) { + this.driveTrain = driveTrain; + counter = 0; + this.target = target; // Use addRequirements() here to declare subsystem dependencies. + addRequirements(driveTrain); } // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + driveTrain.resetDrivePID(); + driveTrain.setDriveTarget(target); + 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(target, 0); + driveTrain.setRightSpeed(driveTrain.getDriveOutput()); + driveTrain.setLeftSpeed(driveTrain.getDriveOutput()); + if(driveTrain.getDriveError() < Constants.maximumDriveError){ + counter++; + }else{ + counter = 0; + } + } // 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 > 10){ + return true; + }else{ + return false; + } } } diff --git a/src/main/java/frc/robot/commands/VisionAutoDrive.java b/src/main/java/frc/robot/commands/VisionAutoDrive.java index db5ac82..07327ad 100644 --- a/src/main/java/frc/robot/commands/VisionAutoDrive.java +++ b/src/main/java/frc/robot/commands/VisionAutoDrive.java @@ -5,20 +5,42 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.DriveTrain; +import frc.robot.subsystems.RasberryPiCamera; public class VisionAutoDrive extends CommandBase { /** Creates a new VisionAutoDrive. */ - public VisionAutoDrive() { + private final DriveTrain driveTrain; + private final RasberryPiCamera rasberryPiCamera; + private final double target; + private final int aprilID; + private int counter; + + public VisionAutoDrive(DriveTrain driveTrain, RasberryPiCamera rasberryPiCamera, double target, int aprilID) { + this.driveTrain = driveTrain; + this.rasberryPiCamera = rasberryPiCamera; + this.target = target; + this.aprilID = aprilID; + counter = 0; // Use addRequirements() here to declare subsystem dependencies. + addRequirements(driveTrain); + addRequirements(rasberryPiCamera); } // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + driveTrain.setDriveTarget(target); + driveTrain.resetDrivePID(); + driveTrain.setLeftSpeed(0); + driveTrain.setRightSpeed(0); + } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + + } // Called once the command ends or is interrupted. @Override diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index 7ca4fad..6ff3697 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -4,6 +4,8 @@ package frc.robot.subsystems; +import java.lang.annotation.Target; + import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.can.TalonFX; @@ -89,7 +91,7 @@ public void setLeftSpeed(double speed){ BL.set(ControlMode.PercentOutput, speed); } - //Turn and Drive PIDs + // PIDs reset public void resetDrivePID(){ driveTarget = 0; driveError = 0; @@ -161,15 +163,12 @@ public void PIDdrive(double sensorInput, double limit) { if(driveOutput > limit){ driveOutput = limit; } - if(driveOutput < -limit){ driveOutput = -limit; } drivePrevError = driveError; prevDriveOutput = driveOutput; - //SmartDashboard.putNumber("PID Drive output:", driveOutput); - } public double getTurnOutput() @@ -189,8 +188,7 @@ public void PIDBalance(double sensorInput) } - public double getBalanceOutput() - { + public double getBalanceOutput(){ return balanceOutput; } @@ -198,10 +196,17 @@ public double getDriveOutput(){ return driveOutput; } + public double getDriveError(){ + return driveError; + } + public void setDriveTarget(double encoderUnit){ driveTarget = encoderUnit; } + public void setTurnTarget(double degrees){ + turnTarget = degrees; + } @Override public void periodic() { // This method will be called once per scheduler run From bf65986fb6b93d525e1a4a0548216fe094dfc008 Mon Sep 17 00:00:00 2001 From: Alex Nie <93626002+TheSparkyBoy@users.noreply.github.com> Date: Sat, 11 Mar 2023 15:59:57 -0700 Subject: [PATCH 07/41] Fixed Merge from Andrew Branch with Auton Sequence --- src/main/java/frc/robot/Constants.java | 16 ++---- src/main/java/frc/robot/RobotContainer.java | 10 ++-- .../frc/robot/commands/EncoderAutoDrive.java | 41 +++++++++++-- .../java/frc/robot/subsystems/DriveTrain.java | 57 +++++++------------ 4 files changed, 64 insertions(+), 60 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 58591ad..830cf9d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -56,14 +56,6 @@ public final class Constants { public static final int XBOX_pinX2 = 0; public static final int Xbox_LTX2 = 2; public static final int Xbox_RTX2 = 3; - - //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 //Also some motor id constant for the intake public static final int deviceIdIntakeM1 = 9; public static final int deviceIdIntakeM2 = 10; @@ -114,14 +106,16 @@ public static class OperatorConstants { 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.1; + public static final double turnkI = 0.00; + 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 balancekI = 0; public static final double balancekD = 0; + public static final double levelConstant = 2; // auton drive PID constants //public static final int kTimeoutMs = 20; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b298916..18acaa8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -37,6 +37,7 @@ public class RobotContainer { 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); @@ -47,9 +48,7 @@ public class RobotContainer { private 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); + /** The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -74,7 +73,7 @@ private void configureBindings() { Xbutton.toggleWhenPressed(new IntakeSwitch(intakeSubsystem, false)); Ybutton.toggleWhenPressed(new IntakeSwitch(intakeSubsystem, true)); - Abutton.toggleWhenPressed(new autoDriveApriltag(driveTrain, rasberryPiCamera, LimeLightSubsystem)); + Abutton.toggleWhenPressed(new ApriltagAutoDrive(driveTrain, rasberryPiCamera, LimeLightSubsystem)); @@ -89,7 +88,6 @@ 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); } } diff --git a/src/main/java/frc/robot/commands/EncoderAutoDrive.java b/src/main/java/frc/robot/commands/EncoderAutoDrive.java index dd53730..86a9899 100644 --- a/src/main/java/frc/robot/commands/EncoderAutoDrive.java +++ b/src/main/java/frc/robot/commands/EncoderAutoDrive.java @@ -5,28 +5,59 @@ package frc.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.DriveTrain; +import frc.robot.Constants; public class EncoderAutoDrive extends CommandBase { /** Creates a new EncoderAutoDrivve. */ - public EncoderAutoDrive() { + private final DriveTrain driveTrain; + private int counter; + private final double target; + public EncoderAutoDrive(DriveTrain driveTrain, double target) { + this.driveTrain = driveTrain; + counter = 0; + this.target = target; // Use addRequirements() here to declare subsystem dependencies. + addRequirements(driveTrain); } // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + driveTrain.resetDrivePID(); + driveTrain.setDriveTarget(target); + 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(target, 0); + driveTrain.setRightSpeed(driveTrain.getDriveOutput()); + driveTrain.setLeftSpeed(driveTrain.getDriveOutput()); + if(driveTrain.getDriveError() < Constants.maximumDriveError){ + counter++; + }else{ + counter = 0; + } + } // 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 > 10){ + return true; + }else{ + return false; + } } } diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index c704fd0..6ff3697 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -4,6 +4,8 @@ package frc.robot.subsystems; +import java.lang.annotation.Target; + import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.can.TalonFX; @@ -89,7 +91,7 @@ public void setLeftSpeed(double speed){ BL.set(ControlMode.PercentOutput, speed); } - //Turn and Drive PIDs + // PIDs reset public void resetDrivePID(){ driveTarget = 0; driveError = 0; @@ -112,17 +114,6 @@ public void resetTurnPID(){ turnOutput = 0; } - //setters - public void setTurnTarget(double turnTarget){ - this.turnTarget = turnTarget; - } - - - - 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; @@ -169,40 +160,22 @@ public void PIDdrive(double sensorInput, double limit) { driveOutput = Constants.kP*driveP + Constants.kI*driveI + Constants.kD*driveD; - // if(driveOutput > limit){ - // driveOutput = limit; - // } - - // if(driveOutput < -limit){ - // driveOutput = -limit; - // } + if(driveOutput > limit){ + driveOutput = limit; + } + if(driveOutput < -limit){ + driveOutput = -limit; + } drivePrevError = driveError; prevDriveOutput = driveOutput; - //SmartDashboard.putNumber("PID Drive output:", driveOutput); } - public double getDriveOutput(){ - return 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) { @@ -215,17 +188,25 @@ public void PIDBalance(double sensorInput) } - public double getBalanceOutput() - { + public double getBalanceOutput(){ return balanceOutput; } + public double getDriveOutput(){ + return driveOutput; + } + public double getDriveError(){ + return driveError; + } public void setDriveTarget(double encoderUnit){ driveTarget = encoderUnit; } + public void setTurnTarget(double degrees){ + turnTarget = degrees; + } @Override public void periodic() { // This method will be called once per scheduler run From 888fa5aa772e372353b497d8c5837b1406c64637 Mon Sep 17 00:00:00 2001 From: Alex Nie <93626002+TheSparkyBoy@users.noreply.github.com> Date: Sat, 11 Mar 2023 16:01:30 -0700 Subject: [PATCH 08/41] Fixed DriveTrain getTurnError --- src/main/java/frc/robot/subsystems/DriveTrain.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index 6ff3697..fedd39a 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -200,6 +200,10 @@ public double getDriveError(){ return driveError; } + public double getTurnError(){ + return turnError; + } + public void setDriveTarget(double encoderUnit){ driveTarget = encoderUnit; } From 7ff026bde625e5af968b1bcabb16d767128418e9 Mon Sep 17 00:00:00 2001 From: Alex Nie <93626002+TheSparkyBoy@users.noreply.github.com> Date: Sat, 11 Mar 2023 16:38:31 -0700 Subject: [PATCH 09/41] Still trying to print out the encoder --- .../frc/robot/commands/AutoVisionDrive.java | 43 ++++++++++----- .../frc/robot/commands/VisionAutoDrive.java | 54 ------------------- .../frc/robot/commands/autoDriveApriltag.java | 4 +- .../java/frc/robot/subsystems/DriveTrain.java | 6 ++- 4 files changed, 38 insertions(+), 69 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/VisionAutoDrive.java diff --git a/src/main/java/frc/robot/commands/AutoVisionDrive.java b/src/main/java/frc/robot/commands/AutoVisionDrive.java index be19db5..1412447 100644 --- a/src/main/java/frc/robot/commands/AutoVisionDrive.java +++ b/src/main/java/frc/robot/commands/AutoVisionDrive.java @@ -5,48 +5,67 @@ 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.RasberryPiCamera; +import frc.robot.subsystems.navXSubsystem; +import frc.robot.Constants; public class AutoVisionDrive extends CommandBase { /** Creates a new AutoVisionDrive. */ private final DriveTrain driveTrain; private final RasberryPiCamera rasberryPiCamera; - private final LimeLightSubsystem limelights; - private final double target; + private final LimeLightSubsystem limelight; + private final navXSubsystem navX; + private final double initialSpeed; private int counter; + private boolean triggered; - public AutoVisionDrive(DriveTrain driveTrain, RasberryPiCamera rasberryPiCamera, LimeLightSubsystem limelight, double target) { + public AutoVisionDrive(DriveTrain driveTrain, RasberryPiCamera rasberryPiCamera, LimeLightSubsystem limelight, navXSubsystem navX, double initialSpeed) { this.driveTrain = driveTrain; - this.limelights = limelight; + this.limelight = limelight; this.rasberryPiCamera = rasberryPiCamera; - this.target = target; + 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.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.PIDturn(rasberryPiCamera.getTargetYaw()); + driveTrain.setLeftSpeed(driveTrain.getTurnOutput()); + driveTrain.setRightSpeed(driveTrain.getTurnOutput()); + if(navX.getPitch() > Constants.levelConstant){ + triggered = true; + } } // 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(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/VisionAutoDrive.java b/src/main/java/frc/robot/commands/VisionAutoDrive.java deleted file mode 100644 index 07327ad..0000000 --- a/src/main/java/frc/robot/commands/VisionAutoDrive.java +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.DriveTrain; -import frc.robot.subsystems.RasberryPiCamera; - -public class VisionAutoDrive extends CommandBase { - /** Creates a new VisionAutoDrive. */ - private final DriveTrain driveTrain; - private final RasberryPiCamera rasberryPiCamera; - private final double target; - private final int aprilID; - private int counter; - - public VisionAutoDrive(DriveTrain driveTrain, RasberryPiCamera rasberryPiCamera, double target, int aprilID) { - this.driveTrain = driveTrain; - this.rasberryPiCamera = rasberryPiCamera; - this.target = target; - this.aprilID = aprilID; - counter = 0; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(driveTrain); - addRequirements(rasberryPiCamera); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - driveTrain.setDriveTarget(target); - driveTrain.resetDrivePID(); - 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/commands/autoDriveApriltag.java b/src/main/java/frc/robot/commands/autoDriveApriltag.java index 2ce46e8..996c88f 100644 --- a/src/main/java/frc/robot/commands/autoDriveApriltag.java +++ b/src/main/java/frc/robot/commands/autoDriveApriltag.java @@ -12,7 +12,7 @@ import frc.robot.subsystems.navXSubsystem; -public class autoDriveApriltag extends CommandBase { +public class AutoDriveApriltag extends CommandBase { /** Creates a new autoDriveApriltag. */ private DriveTrain driveTrain; private LimeLightSubsystem limeLightSubsystem; @@ -21,7 +21,7 @@ public class autoDriveApriltag extends CommandBase { private double H_angle; private double distance; - public autoDriveApriltag(DriveTrain driveTrain, RasberryPiCamera rasberryPiCamera, LimeLightSubsystem limeLightSubsystem) { + public AutoDriveApriltag(DriveTrain driveTrain, RasberryPiCamera rasberryPiCamera, LimeLightSubsystem limeLightSubsystem) { // Use addRequirements() here to declare subsystem dependencies. this.driveTrain = driveTrain; this.rasberryPiCamera = rasberryPiCamera; diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index fedd39a..ffb02ad 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -8,6 +8,7 @@ import com.ctre.phoenix.motorcontrol.ControlMode; 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; @@ -187,7 +188,7 @@ public void PIDBalance(double sensorInput) balancePrevError = balanceError; } - +//getters public double getBalanceOutput(){ return balanceOutput; } @@ -204,6 +205,9 @@ public double getTurnError(){ return turnError; } + // public double getFRid(){ + // // return FR.value; + // } public void setDriveTarget(double encoderUnit){ driveTarget = encoderUnit; } From 1db9c57bc8f342cbbef7e6e682ef5ff02bef8d4d Mon Sep 17 00:00:00 2001 From: Rewand727 Date: Sat, 11 Mar 2023 16:39:31 -0700 Subject: [PATCH 10/41] alex plz --- .../frc/robot/commands/ApriltagAutoDrive.java | 78 ------------------- .../java/frc/robot/commands/AutoDrive.java | 50 ------------ 2 files changed, 128 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/ApriltagAutoDrive.java delete mode 100644 src/main/java/frc/robot/commands/AutoDrive.java diff --git a/src/main/java/frc/robot/commands/ApriltagAutoDrive.java b/src/main/java/frc/robot/commands/ApriltagAutoDrive.java deleted file mode 100644 index 58ec631..0000000 --- a/src/main/java/frc/robot/commands/ApriltagAutoDrive.java +++ /dev/null @@ -1,78 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.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.RasberryPiCamera; -import frc.robot.subsystems.navXSubsystem; - - -public class ApriltagAutoDrive extends CommandBase { - /** Creates a new autoDriveApriltag. */ - private DriveTrain driveTrain; - private LimeLightSubsystem limeLightSubsystem; - private RasberryPiCamera rasberryPiCamera; - private double piAngle = 0; - private double H_angle; - private double distance; - - public ApriltagAutoDrive(DriveTrain driveTrain, RasberryPiCamera rasberryPiCamera, LimeLightSubsystem limeLightSubsystem) { - // Use addRequirements() here to declare subsystem dependencies. - this.driveTrain = driveTrain; - this.rasberryPiCamera = rasberryPiCamera; - this.limeLightSubsystem = limeLightSubsystem; - addRequirements(driveTrain); - addRequirements(rasberryPiCamera); - addRequirements(limeLightSubsystem); - H_angle = limeLightSubsystem.getH_angle(); - distance = limeLightSubsystem.calculateXdistance();// distance to apriltag - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - driveTrain.resetDrivePID(); - driveTrain.setLeftSpeed(0); - driveTrain.setRightSpeed(0); - - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - - for (int i = 0; i < 3; i++){ - H_angle = H_angle / 2; - distance = distance / 2; - driveTrain.PIDdrive(distance, 0.5); - - driveTrain.PIDturn(H_angle); - } - - driveTrain.PIDturn(H_angle); - - // piAngle = rasberryPiCamera.getTargetPitch(); // or yaw or whatever is the right angle for turning - - // driveTrain.PIDturn(piAngle); - - // driveTrain.PIDdrive(rasberryPiCamera.targetDistance(), rasberryPiCamera.targetDistance() + 10); - - } - - // 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/commands/AutoDrive.java b/src/main/java/frc/robot/commands/AutoDrive.java deleted file mode 100644 index 78ff524..0000000 --- a/src/main/java/frc/robot/commands/AutoDrive.java +++ /dev/null @@ -1,50 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.ArmSubsystem; -import frc.robot.subsystems.DriveTrain; -import frc.robot.subsystems.navXSubsystem; - - -public class AutoDrive extends CommandBase { - /** Creates a new AutoDrive. */ - private final DriveTrain driveTrain; - 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; - this.navX = navX; - this.counter = 0; - this.millis = millis; - addRequirements(driveTrain); - addRequirements(navX); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - driveTrain.setLeftSpeed(0); - driveTrain.setRightSpeed(0); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - // driveTrain.PIDdrive(driveTrain.get, counter); - } - // 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; - } -} From 88436b65de9fc664c278e7a352fae4a708b7ae5d Mon Sep 17 00:00:00 2001 From: Alex Nie <93626002+TheSparkyBoy@users.noreply.github.com> Date: Sat, 11 Mar 2023 16:39:31 -0700 Subject: [PATCH 11/41] Deleted AutoDrive Repeated --- .../java/frc/robot/commands/AutoDrive.java | 50 ------------------- 1 file changed, 50 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/AutoDrive.java diff --git a/src/main/java/frc/robot/commands/AutoDrive.java b/src/main/java/frc/robot/commands/AutoDrive.java deleted file mode 100644 index 78ff524..0000000 --- a/src/main/java/frc/robot/commands/AutoDrive.java +++ /dev/null @@ -1,50 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.ArmSubsystem; -import frc.robot.subsystems.DriveTrain; -import frc.robot.subsystems.navXSubsystem; - - -public class AutoDrive extends CommandBase { - /** Creates a new AutoDrive. */ - private final DriveTrain driveTrain; - 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; - this.navX = navX; - this.counter = 0; - this.millis = millis; - addRequirements(driveTrain); - addRequirements(navX); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - driveTrain.setLeftSpeed(0); - driveTrain.setRightSpeed(0); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - // driveTrain.PIDdrive(driveTrain.get, counter); - } - // 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; - } -} From 754c5f222cc1bbcf476a11bff45a265e4e2b482e Mon Sep 17 00:00:00 2001 From: Rewand727 Date: Sat, 11 Mar 2023 16:45:08 -0700 Subject: [PATCH 12/41] alex plz --- src/main/java/frc/robot/RobotContainer.java | 1 - .../frc/robot/commands/VisionAutoDrive.java | 58 +++++++++++++++++++ 2 files changed, 58 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/commands/VisionAutoDrive.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 18acaa8..b9cbe20 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -73,7 +73,6 @@ private void configureBindings() { Xbutton.toggleWhenPressed(new IntakeSwitch(intakeSubsystem, false)); Ybutton.toggleWhenPressed(new IntakeSwitch(intakeSubsystem, true)); - Abutton.toggleWhenPressed(new ApriltagAutoDrive(driveTrain, rasberryPiCamera, LimeLightSubsystem)); diff --git a/src/main/java/frc/robot/commands/VisionAutoDrive.java b/src/main/java/frc/robot/commands/VisionAutoDrive.java new file mode 100644 index 0000000..4ff552c --- /dev/null +++ b/src/main/java/frc/robot/commands/VisionAutoDrive.java @@ -0,0 +1,58 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.DriveTrain; +import frc.robot.subsystems.LimeLightSubsystem; +import frc.robot.subsystems.RasberryPiCamera; + +public class VisionAutoDrive extends CommandBase { + /** Creates a new VisionAutoDrive. */ + private final DriveTrain driveTrain; + private final RasberryPiCamera rasberryPiCamera; + private final LimeLightSubsystem limeLightSubsystem; + private final double target; + private final int aprilID; + private int counter; + + public VisionAutoDrive(DriveTrain driveTrain, RasberryPiCamera rasberryPiCamera, LimeLightSubsystem limeLightSubsystem, double target, int aprilID) { + this.driveTrain = driveTrain; + this.limeLightSubsystem = limeLightSubsystem; + this.rasberryPiCamera = rasberryPiCamera; + this.target = target; + this.aprilID = aprilID; + counter = 0; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(driveTrain); + addRequirements(rasberryPiCamera); + addRequirements(limeLightSubsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + driveTrain.setDriveTarget(target); + driveTrain.resetDrivePID(); + 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; + } +} From f11b7d012519a56d771b02ec38de5af2d92eaece Mon Sep 17 00:00:00 2001 From: Alex Nie <93626002+TheSparkyBoy@users.noreply.github.com> Date: Sat, 11 Mar 2023 17:15:52 -0700 Subject: [PATCH 13/41] Still Testing Autonomous Commands --- src/main/java/frc/robot/Robot.java | 8 ++++++++ src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/subsystems/DriveTrain.java | 16 +++++++++++++--- .../frc/robot/subsystems/rasberryPiCamera.java | 8 ++++---- 4 files changed, 26 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2b98c2d..08189a4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -95,6 +95,10 @@ 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()); } @@ -120,6 +124,10 @@ 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()); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 18acaa8..a25a12b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -45,7 +45,7 @@ public class RobotContainer { // Replace with CommandPS4Controller or CommandJoystick if needed - private final DriveTrain driveTrain = new DriveTrain(); + public final DriveTrain driveTrain = new DriveTrain(); public navXSubsystem navX = new navXSubsystem(); diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index ffb02ad..5a496d6 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -7,6 +7,7 @@ import java.lang.annotation.Target; import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.can.TalonFX; import com.fasterxml.jackson.annotation.JacksonInject.Value; @@ -205,9 +206,18 @@ public double getTurnError(){ return turnError; } - // public double getFRid(){ - // // return FR.value; - // } + public double getFRid(){ + return FR.getSelectedSensorPosition(); + } + public double getFLid(){ + return FL.getSelectedSensorPosition(); + } + public double getBRid(){ + return BR.getSelectedSensorPosition(); + } + public double getBLid(){ + return BL.getSelectedSensorPosition(); + } public void setDriveTarget(double encoderUnit){ driveTarget = encoderUnit; } diff --git a/src/main/java/frc/robot/subsystems/rasberryPiCamera.java b/src/main/java/frc/robot/subsystems/rasberryPiCamera.java index de8fe67..2e79661 100644 --- a/src/main/java/frc/robot/subsystems/rasberryPiCamera.java +++ b/src/main/java/frc/robot/subsystems/rasberryPiCamera.java @@ -32,8 +32,8 @@ public RasberryPiCamera() { // hasTarget = piCam.getEntry("hasTarget"); // targetArea = piCam.getEntry("targetArea"); // targetPose = piCam.getEntry("targetPose"); - this.result = photonCamera.getLatestResult(); - targets = result.getTargets(); + // this.result = photonCamera.getLatestResult(); + // targets = result.getTargets(); } @@ -81,7 +81,7 @@ public double targetDistance(){ @Override public void periodic() { // This method will be called once per scheduler run - result = photonCamera.getLatestResult(); - target = result.getBestTarget(); + // result = photonCamera.getLatestResult(); + // target = result.getBestTarget(); } } From 19a93c15bdfaa91fe89cb55198ea1d6a08183dbf Mon Sep 17 00:00:00 2001 From: Rewand727 Date: Sat, 11 Mar 2023 17:30:08 -0700 Subject: [PATCH 14/41] pi fixed --- src/main/java/frc/robot/Robot.java | 14 ++--- .../frc/robot/commands/DriveToBalance.java | 46 +++++++++++++++ .../frc/robot/commands/VisionAutoDrive.java | 58 ------------------- .../robot/subsystems/rasberryPiCamera.java | 56 ++++++++++++++---- 4 files changed, 97 insertions(+), 77 deletions(-) create mode 100644 src/main/java/frc/robot/commands/DriveToBalance.java delete mode 100644 src/main/java/frc/robot/commands/VisionAutoDrive.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2b98c2d..a768c9d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -47,15 +47,15 @@ public void robotPeriodic() { // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); - SmartDashboard.putBoolean("Has Target", m_robotContainer.rasberryPiCamera.getHasTarget()); + // SmartDashboard.putBoolean("Has Target", m_robotContainer.rasberryPiCamera.getHasTarget()); 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("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()); diff --git a/src/main/java/frc/robot/commands/DriveToBalance.java b/src/main/java/frc/robot/commands/DriveToBalance.java new file mode 100644 index 0000000..5ababf8 --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveToBalance.java @@ -0,0 +1,46 @@ +// 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.navXSubsystem; +import frc.robot.subsystems.DriveTrain; + +public class DriveToBalance extends CommandBase { + /** Creates a new DriveToBalance. */ + private final navXSubsystem navX; + private final double initialSpeed; + private final DriveTrain driveTrain; + public DriveToBalance(navXSubsystem navX, DriveTrain driveTrain, double initialSpeed) { + this.initialSpeed = initialSpeed; + this.navX = navX; + 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.setTurnTarget(0); + driveTrain.setLeftSpeed(initialSpeed); + driveTrain.setRightSpeed(initialSpeed); + } + + // 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/commands/VisionAutoDrive.java b/src/main/java/frc/robot/commands/VisionAutoDrive.java deleted file mode 100644 index 4ff552c..0000000 --- a/src/main/java/frc/robot/commands/VisionAutoDrive.java +++ /dev/null @@ -1,58 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.DriveTrain; -import frc.robot.subsystems.LimeLightSubsystem; -import frc.robot.subsystems.RasberryPiCamera; - -public class VisionAutoDrive extends CommandBase { - /** Creates a new VisionAutoDrive. */ - private final DriveTrain driveTrain; - private final RasberryPiCamera rasberryPiCamera; - private final LimeLightSubsystem limeLightSubsystem; - private final double target; - private final int aprilID; - private int counter; - - public VisionAutoDrive(DriveTrain driveTrain, RasberryPiCamera rasberryPiCamera, LimeLightSubsystem limeLightSubsystem, double target, int aprilID) { - this.driveTrain = driveTrain; - this.limeLightSubsystem = limeLightSubsystem; - this.rasberryPiCamera = rasberryPiCamera; - this.target = target; - this.aprilID = aprilID; - counter = 0; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(driveTrain); - addRequirements(rasberryPiCamera); - addRequirements(limeLightSubsystem); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - driveTrain.setDriveTarget(target); - driveTrain.resetDrivePID(); - 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/rasberryPiCamera.java b/src/main/java/frc/robot/subsystems/rasberryPiCamera.java index de8fe67..e32f157 100644 --- a/src/main/java/frc/robot/subsystems/rasberryPiCamera.java +++ b/src/main/java/frc/robot/subsystems/rasberryPiCamera.java @@ -32,15 +32,22 @@ public RasberryPiCamera() { // 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 getaprilTagID(){ - aprilID = target.getFiducialId(); - return aprilID; + if (getHasTarget() == true){ + aprilID = target.getFiducialId(); + return aprilID; + } else { + return 404; + } + + } @@ -49,8 +56,13 @@ public boolean getHasTarget(){ } public double getTargetArea(){ + if (getHasTarget() == true) { target = result.getBestTarget(); return target.getArea(); + } else { + return 404; + } + } public double getTargetPose(){ @@ -59,29 +71,49 @@ public double getTargetPose(){ } public double getTargetSkew(){ - target = result.getBestTarget(); - return target.getSkew(); + if (getHasTarget() == true){ + target = result.getBestTarget(); + return target.getSkew(); + } else { + return 404; + } + } public double getTargetYaw(){ - target = result.getBestTarget(); - return target.getYaw(); + if (getHasTarget() == true){ + target = result.getBestTarget(); + return target.getYaw(); + } else { + return 404; + } + } public double getTargetPitch(){ - target = result.getBestTarget(); - return target.getPitch(); + if (getHasTarget() == true){ + target = result.getBestTarget(); + return target.getPitch(); + } else { + return 404; + } + } public double targetDistance(){ - target = result.getBestTarget(); - return PhotonUtils.calculateDistanceToTargetMeters(Constants.CAMERA_HEIGHT_METERS, Constants.TARGET_HEIGHT_METERS, Constants.CAMERA_PITCH_RADIANS, Units.radiansToDegrees(target.getPitch())); + 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(); } } From 03a579312f504c62f64f8b65ff25fd252ed512de Mon Sep 17 00:00:00 2001 From: Rewand727 Date: Sat, 11 Mar 2023 17:41:27 -0700 Subject: [PATCH 15/41] pi is gone o7 fix autoAprilTagDrive --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 4 +- .../frc/robot/commands/AutoVisionDrive.java | 6 +- .../robot/subsystems/rasberryPiCamera.java | 196 +++++++++--------- 4 files changed, 103 insertions(+), 105 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a768c9d..187fcd4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -48,7 +48,7 @@ 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.getaprilTagID()); + // SmartDashboard.putNumber("Fiducial ID", m_robotContainer.rasberryPiCamera.getaprilTagID()); // SmartDashboard.putNumber("Target Area", m_robotContainer.rasberryPiCamera.getTargetArea()); // SmartDashboard.putNumber("Target Pose", m_robotContainer.rasberryPiCamera.getTargetPose()); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b9cbe20..fc76d0d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -16,7 +16,7 @@ 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; /** @@ -32,7 +32,7 @@ public class RobotContainer { public XboxController Xbox = new XboxController(Constants.xBoxControllerid); public XboxController Xbox2 = new XboxController(Constants.xBoxControllerid2); - public RasberryPiCamera rasberryPiCamera = new RasberryPiCamera(); + // public RasberryPiCamera rasberryPiCamera = new RasberryPiCamera(); //Intake private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem(); public final JoystickButton Xbutton = new JoystickButton(Xbox, Constants.Xbox_Button_X); diff --git a/src/main/java/frc/robot/commands/AutoVisionDrive.java b/src/main/java/frc/robot/commands/AutoVisionDrive.java index 1412447..21e026c 100644 --- a/src/main/java/frc/robot/commands/AutoVisionDrive.java +++ b/src/main/java/frc/robot/commands/AutoVisionDrive.java @@ -15,23 +15,21 @@ public class AutoVisionDrive extends CommandBase { /** Creates a new AutoVisionDrive. */ private final DriveTrain driveTrain; - private final RasberryPiCamera rasberryPiCamera; private final LimeLightSubsystem limelight; private final navXSubsystem navX; private final double initialSpeed; private int counter; private boolean triggered; - public AutoVisionDrive(DriveTrain driveTrain, RasberryPiCamera rasberryPiCamera, LimeLightSubsystem limelight, navXSubsystem navX, double initialSpeed) { + public AutoVisionDrive(DriveTrain driveTrain, LimeLightSubsystem limelight, navXSubsystem navX, double initialSpeed) { this.driveTrain = driveTrain; this.limelight = limelight; - this.rasberryPiCamera = rasberryPiCamera; this.navX = navX; this.initialSpeed = initialSpeed; this.triggered = false; // Use addRequirements() here to declare subsystem dependencies. addRequirements(driveTrain); - addRequirements(rasberryPiCamera); + // addRequirements(rasberryPiCamera); } // Called when the command is initially scheduled. @Override diff --git a/src/main/java/frc/robot/subsystems/rasberryPiCamera.java b/src/main/java/frc/robot/subsystems/rasberryPiCamera.java index e32f157..7dd674a 100644 --- a/src/main/java/frc/robot/subsystems/rasberryPiCamera.java +++ b/src/main/java/frc/robot/subsystems/rasberryPiCamera.java @@ -1,119 +1,119 @@ -// 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. +// // 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; +// package frc.robot.subsystems; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; +// import edu.wpi.first.math.util.Units; +// import edu.wpi.first.wpilibj2.command.SubsystemBase; +// import frc.robot.Constants; -import java.util.List; +// 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; +// 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 { - /** Creates a new rasber ryPiCamera. */ - 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 class RasberryPiCamera extends SubsystemBase { +// /** Creates a new rasber ryPiCamera. */ +// 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() { - // piCam = NetworkTableInstance.getDefault().getTable("photonvision"); - // hasTarget = piCam.getEntry("hasTarget"); - // targetArea = piCam.getEntry("targetArea"); - // targetPose = piCam.getEntry("targetPose"); - this.result = photonCamera.getLatestResult(); +// // piCam = NetworkTableInstance.getDefault().getTable("photonvision"); +// // hasTarget = piCam.getEntry("hasTarget"); +// // targetArea = piCam.getEntry("targetArea"); +// // targetPose = piCam.getEntry("targetPose"); +// this.result = photonCamera.getLatestResult(); - targets = result.getTargets(); +// targets = result.getTargets(); - } +// } - public double getaprilTagID(){ - if (getHasTarget() == true){ - aprilID = target.getFiducialId(); - return aprilID; - } else { - return 404; - } +// public double getaprilTagID(){ +// if (getHasTarget() == true){ +// aprilID = target.getFiducialId(); +// return aprilID; +// } else { +// return 404; +// } - } +// } - public boolean getHasTarget(){ - return result.hasTargets(); - } +// public boolean getHasTarget(){ +// return result.hasTargets(); +// } - public double getTargetArea(){ - if (getHasTarget() == true) { - target = result.getBestTarget(); - return target.getArea(); - } else { - return 404; - } - - } - - public double getTargetPose(){ - target = result.getBestTarget(); - return target.getPoseAmbiguity(); - } +// public double getTargetArea(){ +// if (getHasTarget() == true) { +// target = result.getBestTarget(); +// return target.getArea(); +// } else { +// return 404; +// } + +// } + +// public double getTargetPose(){ +// target = result.getBestTarget(); +// 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; - } +// 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(); - } -} +// @Override +// public void periodic() { +// // This method will be called once per scheduler run +// result = photonCamera.getLatestResult(); +// // target = result.getBestTarget(); +// } +// } From c7bbf15143da245691b3d86bdd595397c175c090 Mon Sep 17 00:00:00 2001 From: Alex Nie <93626002+TheSparkyBoy@users.noreply.github.com> Date: Sat, 11 Mar 2023 17:44:01 -0700 Subject: [PATCH 16/41] Testing with Rasberry --- src/main/java/frc/robot/Robot.java | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 08189a4..f3a4683 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -47,15 +47,15 @@ public void robotPeriodic() { // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); - SmartDashboard.putBoolean("Has Target", m_robotContainer.rasberryPiCamera.getHasTarget()); - 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.putBoolean("Has Target", m_robotContainer.rasberryPiCamera.getHasTarget()); + // 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()); From 3fc709c6d5301c012589b9379b6da1593c23d62a Mon Sep 17 00:00:00 2001 From: Alex Nie <93626002+TheSparkyBoy@users.noreply.github.com> Date: Sat, 11 Mar 2023 17:47:20 -0700 Subject: [PATCH 17/41] Fixed Rasberry --- .../robot/subsystems/rasberryPiCamera.java | 21 +------------------ 1 file changed, 1 insertion(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/rasberryPiCamera.java b/src/main/java/frc/robot/subsystems/rasberryPiCamera.java index 87224c7..438dae0 100644 --- a/src/main/java/frc/robot/subsystems/rasberryPiCamera.java +++ b/src/main/java/frc/robot/subsystems/rasberryPiCamera.java @@ -28,20 +28,11 @@ // public RasberryPiCamera() { -<<<<<<< HEAD - // piCam = NetworkTableInstance.getDefault().getTable("photonvision"); - // hasTarget = piCam.getEntry("hasTarget"); - // targetArea = piCam.getEntry("targetArea"); - // targetPose = piCam.getEntry("targetPose"); - // this.result = photonCamera.getLatestResult(); - // targets = result.getTargets(); -======= // // piCam = NetworkTableInstance.getDefault().getTable("photonvision"); // // hasTarget = piCam.getEntry("hasTarget"); // // targetArea = piCam.getEntry("targetArea"); // // targetPose = piCam.getEntry("targetPose"); // this.result = photonCamera.getLatestResult(); ->>>>>>> 03a579312f504c62f64f8b65ff25fd252ed512de // targets = result.getTargets(); @@ -119,20 +110,10 @@ // } -<<<<<<< HEAD - @Override - public void periodic() { - // This method will be called once per scheduler run - // result = photonCamera.getLatestResult(); - // target = result.getBestTarget(); - } -} -======= // @Override // public void periodic() { // // This method will be called once per scheduler run // result = photonCamera.getLatestResult(); // // target = result.getBestTarget(); // } -// } ->>>>>>> 03a579312f504c62f64f8b65ff25fd252ed512de +// } \ No newline at end of file From db64b05cdaa7aa9f8d9613d3e042b11502cb6916 Mon Sep 17 00:00:00 2001 From: Alex Nie <93626002+TheSparkyBoy@users.noreply.github.com> Date: Sat, 11 Mar 2023 17:50:21 -0700 Subject: [PATCH 18/41] Fixed All Issues with Rasberry in Software --- .../frc/robot/commands/AutoVisionDrive.java | 3 +- .../frc/robot/commands/autoDriveApriltag.java | 128 +++++++++--------- .../java/frc/robot/commands/autoPlace.java | 6 +- 3 files changed, 66 insertions(+), 71 deletions(-) diff --git a/src/main/java/frc/robot/commands/AutoVisionDrive.java b/src/main/java/frc/robot/commands/AutoVisionDrive.java index 21e026c..4881a12 100644 --- a/src/main/java/frc/robot/commands/AutoVisionDrive.java +++ b/src/main/java/frc/robot/commands/AutoVisionDrive.java @@ -8,7 +8,6 @@ import frc.robot.subsystems.DriveTrain; import frc.robot.subsystems.LimeLightSubsystem; -import frc.robot.subsystems.RasberryPiCamera; import frc.robot.subsystems.navXSubsystem; import frc.robot.Constants; @@ -42,7 +41,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - driveTrain.PIDturn(rasberryPiCamera.getTargetYaw()); + driveTrain.PIDturn(limelight.getH_angle()); driveTrain.setLeftSpeed(driveTrain.getTurnOutput()); driveTrain.setRightSpeed(driveTrain.getTurnOutput()); if(navX.getPitch() > Constants.levelConstant){ diff --git a/src/main/java/frc/robot/commands/autoDriveApriltag.java b/src/main/java/frc/robot/commands/autoDriveApriltag.java index 996c88f..6f75234 100644 --- a/src/main/java/frc/robot/commands/autoDriveApriltag.java +++ b/src/main/java/frc/robot/commands/autoDriveApriltag.java @@ -1,78 +1,78 @@ -// 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.RasberryPiCamera; -import frc.robot.subsystems.navXSubsystem; - - -public class AutoDriveApriltag extends CommandBase { - /** Creates a new autoDriveApriltag. */ - private DriveTrain driveTrain; - private LimeLightSubsystem limeLightSubsystem; - private RasberryPiCamera rasberryPiCamera; - private double piAngle = 0; - private double H_angle; - private double distance; +// // 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.RasberryPiCamera; +// import frc.robot.subsystems.navXSubsystem; + + +// public class AutoDriveApriltag extends CommandBase { +// /** Creates a new autoDriveApriltag. */ +// private DriveTrain driveTrain; +// private LimeLightSubsystem limeLightSubsystem; +// private RasberryPiCamera rasberryPiCamera; +// private double piAngle = 0; +// private double H_angle; +// private double distance; - public AutoDriveApriltag(DriveTrain driveTrain, RasberryPiCamera rasberryPiCamera, LimeLightSubsystem limeLightSubsystem) { - // Use addRequirements() here to declare subsystem dependencies. - this.driveTrain = driveTrain; - this.rasberryPiCamera = rasberryPiCamera; - this.limeLightSubsystem = limeLightSubsystem; - addRequirements(driveTrain); - addRequirements(rasberryPiCamera); - addRequirements(limeLightSubsystem); - H_angle = limeLightSubsystem.getH_angle(); - distance = limeLightSubsystem.calculateXdistance();// distance to apriltag - } +// public AutoDriveApriltag(DriveTrain driveTrain, RasberryPiCamera rasberryPiCamera, LimeLightSubsystem limeLightSubsystem) { +// // Use addRequirements() here to declare subsystem dependencies. +// this.driveTrain = driveTrain; +// this.rasberryPiCamera = rasberryPiCamera; +// this.limeLightSubsystem = limeLightSubsystem; +// addRequirements(driveTrain); +// addRequirements(rasberryPiCamera); +// addRequirements(limeLightSubsystem); +// H_angle = limeLightSubsystem.getH_angle(); +// distance = limeLightSubsystem.calculateXdistance();// distance to apriltag +// } - // Called when the command is initially scheduled. - @Override - public void initialize() { - driveTrain.resetDrivePID(); - driveTrain.setLeftSpeed(0); - driveTrain.setRightSpeed(0); +// // Called when the command is initially scheduled. +// @Override +// public void initialize() { +// driveTrain.resetDrivePID(); +// driveTrain.setLeftSpeed(0); +// driveTrain.setRightSpeed(0); - } +// } - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { +// // Called every time the scheduler runs while the command is scheduled. +// @Override +// public void execute() { - for (int i = 0; i < 3; i++){ - H_angle = H_angle / 2; - distance = distance / 2; - driveTrain.PIDdrive(distance, 0.5); +// for (int i = 0; i < 3; i++){ +// H_angle = H_angle / 2; +// distance = distance / 2; +// driveTrain.PIDdrive(distance, 0.5); - driveTrain.PIDturn(H_angle); - } +// driveTrain.PIDturn(H_angle); +// } - driveTrain.PIDturn(H_angle); +// driveTrain.PIDturn(H_angle); - // piAngle = rasberryPiCamera.getTargetPitch(); // or yaw or whatever is the right angle for turning +// // piAngle = rasberryPiCamera.getTargetPitch(); // or yaw or whatever is the right angle for turning - // driveTrain.PIDturn(piAngle); +// // driveTrain.PIDturn(piAngle); - // driveTrain.PIDdrive(rasberryPiCamera.targetDistance(), rasberryPiCamera.targetDistance() + 10); +// // driveTrain.PIDdrive(rasberryPiCamera.targetDistance(), rasberryPiCamera.targetDistance() + 10); - } +// } - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { +// // 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; - } -} +// // Returns true when the command should end. +// @Override +// public boolean isFinished() { +// return false; +// } +// } diff --git a/src/main/java/frc/robot/commands/autoPlace.java b/src/main/java/frc/robot/commands/autoPlace.java index e6220c8..baeda5d 100644 --- a/src/main/java/frc/robot/commands/autoPlace.java +++ b/src/main/java/frc/robot/commands/autoPlace.java @@ -7,22 +7,18 @@ import edu.wpi.first.wpilibj.AddressableLED; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.LimeLightSubsystem; -import frc.robot.subsystems.RasberryPiCamera; public class autoPlace extends CommandBase { /** Creates a new autoPlace. */ private final LimeLightSubsystem limeLightSubsystem; - private final RasberryPiCamera rasberryPiCamera; - public autoPlace(LimeLightSubsystem limeLightSubsystem, RasberryPiCamera rasberryPiCamera) { + public autoPlace(LimeLightSubsystem limeLightSubsystem) { // Use addRequirements() here to declare subsystem dependencies. this.limeLightSubsystem = limeLightSubsystem; - this.rasberryPiCamera = rasberryPiCamera; addRequirements(limeLightSubsystem); - addRequirements(rasberryPiCamera); } // Called when the command is initially scheduled. From a2e9208d2e37863842886346bfb97d0aed844e18 Mon Sep 17 00:00:00 2001 From: Rewand727 Date: Sun, 12 Mar 2023 13:11:11 -0700 Subject: [PATCH 19/41] wrong branch --- .../frc/robot/commands/AutoVisionDrive.java | 69 +++++++++++++++++++ 1 file changed, 69 insertions(+) create mode 100644 src/main/java/frc/robot/commands/AutoVisionDrive.java 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..7fee7ae --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoVisionDrive.java @@ -0,0 +1,69 @@ +// 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.RasberryPiCamera; +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() { + driveTrain.PIDturn(limelight.yaw); + driveTrain.setLeftSpeed(driveTrain.getTurnOutput()); + driveTrain.setRightSpeed(driveTrain.getTurnOutput()); + if(navX.getPitch() > Constants.levelConstant){ + 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 From 774e0c3f747571ea5e12ad00dd0ca218df5483c7 Mon Sep 17 00:00:00 2001 From: Rewand727 Date: Sun, 12 Mar 2023 14:26:39 -0700 Subject: [PATCH 20/41] limelight pipeline command good --- .OutlineViewer/outlineviewer.json | 8 +++ src/main/java/frc/robot/Robot.java | 3 +- src/main/java/frc/robot/RobotContainer.java | 10 +++- .../java/frc/robot/commands/SetPipeline.java | 52 +++++++++++++++++++ src/main/java/frc/robot/commands/Wait.java | 47 +++++++++++++++++ .../robot/subsystems/LimeLightSubsystem.java | 32 +++++++++--- 6 files changed, 144 insertions(+), 8 deletions(-) create mode 100644 .OutlineViewer/outlineviewer.json create mode 100644 src/main/java/frc/robot/commands/SetPipeline.java create mode 100644 src/main/java/frc/robot/commands/Wait.java 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/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f3a4683..f36c0f9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -61,7 +61,8 @@ public void robotPeriodic() { 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()); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index feb508a..fc8ae2c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,6 +13,7 @@ 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; @@ -87,6 +88,13 @@ private void configureBindings() { */ public Command getAutonomousCommand() { // An example command will be run in autonomous - return 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) + ); } } 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/Wait.java b/src/main/java/frc/robot/commands/Wait.java new file mode 100644 index 0000000..4ffd488 --- /dev/null +++ b/src/main/java/frc/robot/commands/Wait.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.wpilibj2.command.CommandBase; + +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() { + counter = 0; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + counter++; + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + counter = 0; + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if(counter >= (millis/20)){ + return true; + }else{ + return false; + } + + } +} diff --git a/src/main/java/frc/robot/subsystems/LimeLightSubsystem.java b/src/main/java/frc/robot/subsystems/LimeLightSubsystem.java index e58c7f8..a239c7e 100644 --- a/src/main/java/frc/robot/subsystems/LimeLightSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LimeLightSubsystem.java @@ -16,31 +16,51 @@ public class LimeLightSubsystem extends SubsystemBase { /** Creates a new LimelightSubSystem. */ private NetworkTable limeLight; - private NetworkTableEntry V_angle; - private NetworkTableEntry H_angle; + private NetworkTableEntry V_angle, H_angle, hasTargets, botPose, aprilID; + // private double[] botPose; + private double zDistance; private double xDistance; - private NetworkTableEntry hasTargets; public LimeLightSubsystem() { limeLight = NetworkTableInstance.getDefault().getTable("limelight"); + V_angle = limeLight.getEntry("ty"); H_angle = limeLight.getEntry("tx"); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("").getDoubleArray(new double[6]); + 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 getV_angle(){ return V_angle.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 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)))); From 31299a90dddd6c8b9d4db7f5cb109b8427fc978c Mon Sep 17 00:00:00 2001 From: Alex Nie <93626002+TheSparkyBoy@users.noreply.github.com> Date: Tue, 14 Mar 2023 14:58:08 -0700 Subject: [PATCH 21/41] Found Problem with AutoTurn (Needs fixing) --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 3 +- .../java/frc/robot/commands/AutoTurn.java | 10 ++-- .../frc/robot/commands/AutoVisionDrive.java | 2 +- src/main/java/frc/robot/commands/Wait.java | 32 +++++++++++ .../java/frc/robot/subsystems/DriveTrain.java | 54 +++++++------------ 6 files changed, 59 insertions(+), 44 deletions(-) create mode 100644 src/main/java/frc/robot/commands/Wait.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 830cf9d..043ed3d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -106,7 +106,7 @@ public static class OperatorConstants { public static final int yLeftidX2 = 1; // turn PID constants - public static final double turnkP = 0.1; + public static final double turnkP = 0.003; public static final double turnkI = 0.00; public static final double turnkD = 0; public static final double maximumDriveError = 1000.0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index feb508a..8390ecb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -87,6 +87,7 @@ private void configureBindings() { */ public Command getAutonomousCommand() { // An example command will be run in autonomous - return new AutoBalancing(navX, driveTrain); + // return new AutoBalancing(navX, driveTrain); + return new AutoTurn(driveTrain, navX, 45, 30000); } } diff --git a/src/main/java/frc/robot/commands/AutoTurn.java b/src/main/java/frc/robot/commands/AutoTurn.java index d647852..067ac79 100644 --- a/src/main/java/frc/robot/commands/AutoTurn.java +++ b/src/main/java/frc/robot/commands/AutoTurn.java @@ -44,11 +44,11 @@ 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; - } + // if(Math.abs(driveTrain.getTurnError())<3){ + // errorCounter++; + // }else{ + // errorCounter = 0; + // } counter++; } diff --git a/src/main/java/frc/robot/commands/AutoVisionDrive.java b/src/main/java/frc/robot/commands/AutoVisionDrive.java index ab4be3d..1bb1d19 100644 --- a/src/main/java/frc/robot/commands/AutoVisionDrive.java +++ b/src/main/java/frc/robot/commands/AutoVisionDrive.java @@ -42,7 +42,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - driveTrain.PIDturn(limelight.yaw); + driveTrain.PIDturn(limelight.getH_angle()); driveTrain.setLeftSpeed(driveTrain.getTurnOutput()); driveTrain.setRightSpeed(driveTrain.getTurnOutput()); if(navX.getPitch() > Constants.levelConstant){ diff --git a/src/main/java/frc/robot/commands/Wait.java b/src/main/java/frc/robot/commands/Wait.java new file mode 100644 index 0000000..4e737ad --- /dev/null +++ b/src/main/java/frc/robot/commands/Wait.java @@ -0,0 +1,32 @@ +// 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; + +public class Wait extends CommandBase { + /** Creates a new Wait. */ + public Wait() { + // 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() {} + + // 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/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index 5a496d6..a24e45f 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -120,23 +120,12 @@ public void resetTurnPID(){ public void PIDturn(double sensorInput){ turnError = turnTarget - sensorInput; turnP = turnError; - if(turnError= 1) output = 1; - // if(output <= -1) output = -1; + turnPrevError = turnError; } public void resetEncoders(){ @@ -147,32 +136,25 @@ public void resetEncoders(){ } + public void PIDdrive(double sensorInput, double limit) { + driveError = driveTarget - sensorInput; + driveP = driveError; + driveI += driveError; + driveD = driveError - drivePrevError; + + + driveOutput = Constants.kP*driveP + Constants.kI*driveI + Constants.kD*driveD; + if(driveOutput > limit){ + driveOutput = limit; + } + if(driveOutput < -limit){ + driveOutput = -limit; + } - public void getEncoderPosition(){ - FL.getActiveTrajectoryVelocity(); + drivePrevError = driveError; + prevDriveOutput = driveOutput; } - - - public void PIDdrive(double sensorInput, double limit) { - driveError = driveTarget - sensorInput; - driveP = driveError; - driveI += driveError; - driveD = driveError - drivePrevError; - - - driveOutput = Constants.kP*driveP + Constants.kI*driveI + Constants.kD*driveD; - if(driveOutput > limit){ - driveOutput = limit; - } - if(driveOutput < -limit){ - driveOutput = -limit; - } - - drivePrevError = driveError; - prevDriveOutput = driveOutput; - } - public double getTurnOutput() { return turnOutput; From afe4363665d6f8e325b5df779b56f37b88f7521b Mon Sep 17 00:00:00 2001 From: Alex Nie <93626002+TheSparkyBoy@users.noreply.github.com> Date: Tue, 14 Mar 2023 15:38:51 -0700 Subject: [PATCH 22/41] Fixed turning issue, change getYaw to getAngle --- src/main/java/frc/robot/Constants.java | 6 +-- src/main/java/frc/robot/RobotContainer.java | 15 ++++-- .../java/frc/robot/subsystems/DriveTrain.java | 51 ++++++++----------- .../frc/robot/subsystems/navXSubsystem.java | 4 +- 4 files changed, 35 insertions(+), 41 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 830cf9d..d43cb75 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -106,9 +106,9 @@ public static class OperatorConstants { public static final int yLeftidX2 = 1; // turn PID constants - public static final double turnkP = 0.1; - public static final double turnkI = 0.00; - public static final double turnkD = 0; + public static final double turnkP = 0.003; + public static final double turnkI = 0.00005; + public static final double turnkD = 0.01; public static final double maximumDriveError = 1000.0; // balance PID constants diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fc8ae2c..3bf25ac 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -89,12 +89,17 @@ private void configureBindings() { public Command getAutonomousCommand() { // An example command will be run in autonomous // 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 SequentialCommandGroup( - new SetPipeline(LimeLightSubsystem, 0), - new Wait(10000), - new SetPipeline(LimeLightSubsystem, 1), - new Wait(10000), - new SetPipeline(LimeLightSubsystem, 2) + 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/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index 5a496d6..9c6a2bc 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -120,23 +120,14 @@ public void resetTurnPID(){ public void PIDturn(double sensorInput){ turnError = turnTarget - sensorInput; turnP = turnError; - if(turnError= 1) output = 1; - // if(output <= -1) output = -1; - + //clamp output between -50% and 50% + if(turnOutput >= 0.5) turnOutput = 0.5; + if(turnOutput <= -0.5) turnOutput = -0.5; } public void resetEncoders(){ @@ -152,26 +143,24 @@ public void getEncoderPosition(){ FL.getActiveTrajectoryVelocity(); } + public void PIDdrive(double sensorInput, double limit) { + driveError = driveTarget - sensorInput; + driveP = driveError; + driveI += driveError; + driveD = driveError - drivePrevError; + + + driveOutput = Constants.kP*driveP + Constants.kI*driveI + Constants.kD*driveD; + if(driveOutput > limit){ + driveOutput = limit; + } + if(driveOutput < -limit){ + driveOutput = -limit; + } - - public void PIDdrive(double sensorInput, double limit) { - driveError = driveTarget - sensorInput; - driveP = driveError; - driveI += driveError; - driveD = driveError - drivePrevError; - - - driveOutput = Constants.kP*driveP + Constants.kI*driveI + Constants.kD*driveD; - if(driveOutput > limit){ - driveOutput = limit; - } - if(driveOutput < -limit){ - driveOutput = -limit; - } - - drivePrevError = driveError; - prevDriveOutput = driveOutput; - } + drivePrevError = driveError; + prevDriveOutput = driveOutput; + } public double getTurnOutput() { 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() { From 8f0c619500f43ded16b1ecbbd97f1fcd70623366 Mon Sep 17 00:00:00 2001 From: Rewand727 Date: Wed, 15 Mar 2023 17:35:14 -0700 Subject: [PATCH 23/41] testing pid turn and drive --- simgui-ds.json | 92 +++++++++++++++++++ simgui.json | 16 ++++ src/main/java/frc/robot/Constants.java | 10 +- src/main/java/frc/robot/Robot.java | 6 +- src/main/java/frc/robot/RobotContainer.java | 29 ++++-- .../java/frc/robot/commands/AutoTurn.java | 3 +- .../frc/robot/commands/AutoVisionDrive.java | 10 +- .../frc/robot/commands/AutoVisionTurn.java | 32 +++++++ .../frc/robot/commands/EncoderAutoDrive.java | 31 ++++++- .../java/frc/robot/subsystems/DriveTrain.java | 30 ++++-- .../frc/robot/subsystems/navXSubsystem.java | 4 +- 11 files changed, 229 insertions(+), 34 deletions(-) create mode 100644 simgui-ds.json create mode 100644 simgui.json create mode 100644 src/main/java/frc/robot/commands/AutoVisionTurn.java 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 830cf9d..339dae3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -106,9 +106,9 @@ public static class OperatorConstants { public static final int yLeftidX2 = 1; // turn PID constants - public static final double turnkP = 0.1; - public static final double turnkI = 0.00; - public static final double turnkD = 0; + public static final double turnkP = 0.0035; //0.01 + public static final double turnkI = 0.0; //0.00 + public static final double turnkD = 0; // 0 public static final double maximumDriveError = 1000.0; // balance PID constants @@ -126,7 +126,7 @@ public static class OperatorConstants { public static final double kI = 0.0; public static final double kD = 0; - public static final double IactZone = 0.5; + public static final double IactZone = 0;//was 0.5 //pi constants public static final double CAMERA_HEIGHT_METERS = 0; @@ -137,6 +137,6 @@ public static class OperatorConstants { public static final double goalHeight = 107; //what are the units???? public static final double limeLightHeight = 32; public static final double limeLightInitAngle = 0; - + public static final double cameraOffset = 0; // degrees horizontal angle } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f36c0f9..0f597ce 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -63,7 +63,11 @@ public void robotPeriodic() { 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()); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fc8ae2c..6ae38d3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -89,12 +89,27 @@ private void configureBindings() { public Command getAutonomousCommand() { // An example command will be run in autonomous // 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 SequentialCommandGroup( + // new AutoVisionPitch(driveTrain, LimeLightSubsystem, navX, 0.5) + // ); + + + // return new SequentialCommandGroup( + + // new AutoTurn(driveTrain, navX, 180, 10000) + + // ); + return new SequentialCommandGroup( - new SetPipeline(LimeLightSubsystem, 0), - new Wait(10000), - new SetPipeline(LimeLightSubsystem, 1), - new Wait(10000), - new SetPipeline(LimeLightSubsystem, 2) + new EncoderAutoDrive(driveTrain, navX, 20000) ); - } -} + }} diff --git a/src/main/java/frc/robot/commands/AutoTurn.java b/src/main/java/frc/robot/commands/AutoTurn.java index d647852..74138ec 100644 --- a/src/main/java/frc/robot/commands/AutoTurn.java +++ b/src/main/java/frc/robot/commands/AutoTurn.java @@ -41,6 +41,7 @@ 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()); @@ -63,7 +64,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 index 4881a12..14b7563 100644 --- a/src/main/java/frc/robot/commands/AutoVisionDrive.java +++ b/src/main/java/frc/robot/commands/AutoVisionDrive.java @@ -41,10 +41,12 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - driveTrain.PIDturn(limelight.getH_angle()); - driveTrain.setLeftSpeed(driveTrain.getTurnOutput()); - driveTrain.setRightSpeed(driveTrain.getTurnOutput()); - if(navX.getPitch() > Constants.levelConstant){ + + + if(Math.abs(navX.getPitch()) > Constants.levelConstant){ + driveTrain.PIDturn(limelight.getH_angle()); + driveTrain.setLeftSpeed(driveTrain.getTurnOutput()); + driveTrain.setRightSpeed(driveTrain.getTurnOutput()); triggered = true; } } diff --git a/src/main/java/frc/robot/commands/AutoVisionTurn.java b/src/main/java/frc/robot/commands/AutoVisionTurn.java new file mode 100644 index 0000000..26cc51a --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoVisionTurn.java @@ -0,0 +1,32 @@ +// 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; + +public class AutoVisionTurn extends CommandBase { + /** Creates a new AutoVisionTurn. */ + public AutoVisionTurn() { + // 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() {} + + // 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/commands/EncoderAutoDrive.java b/src/main/java/frc/robot/commands/EncoderAutoDrive.java index 86a9899..3fdf482 100644 --- a/src/main/java/frc/robot/commands/EncoderAutoDrive.java +++ b/src/main/java/frc/robot/commands/EncoderAutoDrive.java @@ -4,28 +4,37 @@ 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.Constants; +import frc.robot.subsystems.navXSubsystem; public class EncoderAutoDrive extends CommandBase { /** Creates a new EncoderAutoDrivve. */ private final DriveTrain driveTrain; private int counter; private final double target; - public EncoderAutoDrive(DriveTrain driveTrain, double target) { + private final navXSubsystem navx; + public EncoderAutoDrive(DriveTrain driveTrain, navXSubsystem navx, double target) { //target in inches this.driveTrain = driveTrain; + this.navx = navx; counter = 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() { driveTrain.resetDrivePID(); - driveTrain.setDriveTarget(target); + driveTrain.resetTurnPID(); + driveTrain.resetEncoders(); + driveTrain.setDriveTarget(-target); + driveTrain.setTurnTarget(navx.getYaw()); driveTrain.setLeftSpeed(0); driveTrain.setRightSpeed(0); } @@ -33,10 +42,22 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - - driveTrain.PIDdrive(target, 0); + // SmartDashboard.putNumber("turn target", driveTrain.getTurnTarget()); + driveTrain.PIDdrive(driveTrain.getEncoderPosition(), 1); + driveTrain.PIDturn(navx.getYaw()); + + if (Math.abs(driveTrain.getRightEncoders()) > 1000-Math.abs(driveTrain.getLeftEncoders()) && Math.abs(driveTrain.getRightEncoders()) < 1000+Math.abs(driveTrain.getLeftEncoders())){ driveTrain.setRightSpeed(driveTrain.getDriveOutput()); - driveTrain.setLeftSpeed(driveTrain.getDriveOutput()); + driveTrain.setLeftSpeed(driveTrain.getDriveOutput()); + } else { + driveTrain.setRightSpeed(driveTrain.getTurnOutput()); + driveTrain.setLeftSpeed(driveTrain.getTurnOutput()); + } + // driveTrain.setRightSpeed(driveTrain.getDriveOutput()); + // driveTrain.setLeftSpeed(driveTrain.getDriveOutput()); + // driveTrain.setRightSpeed(driveTrain.getTurnOutput()); + // driveTrain.setLeftSpeed(-driveTrain.getTurnOutput()); + if(driveTrain.getDriveError() < Constants.maximumDriveError){ counter++; }else{ diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index 5a496d6..c8f326a 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -4,6 +4,7 @@ package frc.robot.subsystems; +import java.lang.annotation.Retention; import java.lang.annotation.Target; import com.ctre.phoenix.motorcontrol.ControlMode; @@ -21,6 +22,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; @@ -120,11 +123,7 @@ public void resetTurnPID(){ public void PIDturn(double sensorInput){ turnError = turnTarget - sensorInput; turnP = turnError; - if(turnError Date: Sat, 18 Mar 2023 12:19:22 -0700 Subject: [PATCH 24/41] Merged with Andrew Branch --- src/main/java/frc/robot/Constants.java | 49 ++++++++----------- src/main/java/frc/robot/RobotContainer.java | 5 +- .../java/frc/robot/commands/AutoTurn.java | 3 ++ .../java/frc/robot/subsystems/DriveTrain.java | 13 ++++- 4 files changed, 37 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d43cb75..7dde52c 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -26,8 +26,6 @@ public final class Constants { public static final int Xbox_LT = 2; public static final int Xbox_RT = 3; - - //We need some flight stick constants I guess //Controller Button IDs CONTROLLER 1 (port 0) public static final int Xbox_Button_A = 1; @@ -39,27 +37,28 @@ public final class Constants { public static final int Xbox_Button_LS = 9; public static final int Xbox_Button_RS = 10; - //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; + //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 int ampSpike= 8; public static final int logitechPort6 = 6; @@ -69,12 +68,6 @@ public final class 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; @@ -107,8 +100,8 @@ public static class OperatorConstants { // turn PID constants public static final double turnkP = 0.003; - public static final double turnkI = 0.00005; - public static final double turnkD = 0.01; + public static final double turnkI = 0.0128; + public static final double turnkD = 0; public static final double maximumDriveError = 1000.0; // balance PID constants diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3bf25ac..89cb692 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,7 +9,6 @@ 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; @@ -72,8 +71,8 @@ public RobotContainer() { private void configureBindings() { // Schedule `ExampleCommand` when `exampleCondition` changes to `true` - Xbutton.toggleWhenPressed(new IntakeSwitch(intakeSubsystem, false)); - Ybutton.toggleWhenPressed(new IntakeSwitch(intakeSubsystem, true)); + // Xbutton.toggleWhenPressed(new IntakeSwitch(intakeSubsystem, false)); + // Ybutton.toggleWhenPressed(new IntakeSwitch(intakeSubsystem, true)); diff --git a/src/main/java/frc/robot/commands/AutoTurn.java b/src/main/java/frc/robot/commands/AutoTurn.java index 067ac79..ac8ee65 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); @@ -42,6 +44,7 @@ public void initialize() { @Override public void execute() { driveTrain.PIDturn(navX.getYaw()); + driveTrain.setLeftSpeed(-driveTrain.getTurnOutput()); driveTrain.setRightSpeed(driveTrain.getTurnOutput()); // if(Math.abs(driveTrain.getTurnError())<3){ diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index 83782b8..96a67b4 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -8,6 +8,7 @@ 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; @@ -56,6 +57,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; @@ -126,8 +131,8 @@ public void PIDturn(double sensorInput){ turnOutput = Constants.turnkP*turnP + Constants.turnkI*turnI + Constants.turnkD*turnD; turnPrevError = turnError; //clamp output between -50% and 50% - if(turnOutput >= 0.5) turnOutput = 0.5; - if(turnOutput <= -0.5) turnOutput = -0.5; + if(turnOutput >= 0.6) turnOutput = 0.6; + if(turnOutput <= -0.6) turnOutput = -0.6; } public void resetEncoders(){ @@ -189,6 +194,10 @@ public double getTurnError(){ return turnError; } + public double getTurnPrevError(){ + return turnPrevError; + } + public double getFRid(){ return FR.getSelectedSensorPosition(); } From 97aab6bed922638fb576e9fbcf1a5007ab5c3364 Mon Sep 17 00:00:00 2001 From: Alex Nie <93626002+TheSparkyBoy@users.noreply.github.com> Date: Sat, 18 Mar 2023 13:01:06 -0700 Subject: [PATCH 25/41] Fixed Rogue AutoTurn --- src/main/java/frc/robot/Constants.java | 8 ++++---- src/main/java/frc/robot/subsystems/DriveTrain.java | 4 ---- 2 files changed, 4 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7dde52c..c3d8311 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -75,10 +75,10 @@ public final class Constants { public static final int armMotor = 0; //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; //DriveTrain Constants public static final double turnAdjustment = 0.5; diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index 96a67b4..c7417f4 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -125,14 +125,10 @@ public void resetTurnPID(){ public void PIDturn(double sensorInput){ turnError = turnTarget - sensorInput; turnP = turnError; - turnD = turnError - turnPrevError; turnOutput = Constants.turnkP*turnP + Constants.turnkI*turnI + Constants.turnkD*turnD; turnPrevError = turnError; - //clamp output between -50% and 50% - if(turnOutput >= 0.6) turnOutput = 0.6; - if(turnOutput <= -0.6) turnOutput = -0.6; } public void resetEncoders(){ From 905c88e80f6f973c3627d0419dc0cceeb1d8f8fc Mon Sep 17 00:00:00 2001 From: Rewand727 Date: Sat, 18 Mar 2023 15:23:32 -0700 Subject: [PATCH 26/41] before testing/changes --- src/main/java/frc/robot/Constants.java | 6 +- src/main/java/frc/robot/Robot.java | 1 + src/main/java/frc/robot/RobotContainer.java | 20 +-- .../frc/robot/commands/EncoderAutoDrive.java | 41 ++++-- .../java/frc/robot/commands/IntakeSwitch.java | 2 +- .../frc/robot/commands/ResetEncoders.java | 41 ++++++ src/main/java/frc/robot/commands/XboxArm.java | 6 +- .../frc/robot/commands/autoDriveApriltag.java | 127 ++++++++---------- .../frc/robot/subsystems/ArmSubsystem.java | 17 ++- .../java/frc/robot/subsystems/DriveTrain.java | 49 ++++++- 10 files changed, 203 insertions(+), 107 deletions(-) create mode 100644 src/main/java/frc/robot/commands/ResetEncoders.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7711eef..847915b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -13,10 +13,10 @@ * constants are needed, to reduce verbosity. */ public final class Constants { - public static final int FrontRightID = 0; + public static final int FrontRightID = 2; public static final int FrontLeftID = 3; public static final int BackRightID = 5; - public static final int BackLeftID = 2; + public static final int BackLeftID = 0; public static final int XBOX_R_XAXIS = 4; public static final int XBOX_R_YAXIS = 5; @@ -79,7 +79,7 @@ public static class OperatorConstants { public static final int talonLift1 = 2; public static final int talonLift2 = 3; public static final int extensionMotorID = 0; - public static final int armMotor = 0; + public static final int armMotorID = 0; //DriveTrain Motor ID's public static final int FRid = 0; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0f597ce..a5fe5c9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -68,6 +68,7 @@ public void robotPeriodic() { 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()); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 53adc04..e24a243 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -74,6 +74,7 @@ private void configureBindings() { Xbutton.toggleWhenPressed(new IntakeSwitch(intakeSubsystem, false)); Ybutton.toggleWhenPressed(new IntakeSwitch(intakeSubsystem, true)); + Abutton.whenPressed(new ResetEncoders(driveTrain)); @@ -103,11 +104,6 @@ public Command getAutonomousCommand() { // ); - // return new SequentialCommandGroup( - - // new AutoTurn(driveTrain, navX, 180, 10000) - - // ); // return new SequentialCommandGroup( @@ -117,9 +113,17 @@ public Command getAutonomousCommand() { // new Wait(10000), // new SetPipeline(LimeLightSubsystem, 2) // ); + + // return new SequentialCommandGroup( + // new AutoTurn(driveTrain, navX, 180, 2000), + // new AutoTurn(driveTrain, navX, 0, 2000), + // new AutoTurn(driveTrain, navX, -90, 2000) + // ); + return new SequentialCommandGroup( - new AutoTurn(driveTrain, navX, 180, 2000), - new AutoTurn(driveTrain, navX, 0, 2000), - new AutoTurn(driveTrain, navX, -90, 2000) + new EncoderAutoDrive(driveTrain, navX, 30000) ); + // return new SequentialCommandGroup( + // new testRightLeftMotor(driveTrain) + // ); }} diff --git a/src/main/java/frc/robot/commands/EncoderAutoDrive.java b/src/main/java/frc/robot/commands/EncoderAutoDrive.java index 3fdf482..1c096b6 100644 --- a/src/main/java/frc/robot/commands/EncoderAutoDrive.java +++ b/src/main/java/frc/robot/commands/EncoderAutoDrive.java @@ -17,6 +17,8 @@ public class EncoderAutoDrive extends CommandBase { private int counter; private final double target; private final navXSubsystem navx; + private double absRightEnc = 0; + private double absLeftEnc = 0; public EncoderAutoDrive(DriveTrain driveTrain, navXSubsystem navx, double target) { //target in inches this.driveTrain = driveTrain; this.navx = navx; @@ -43,22 +45,36 @@ public void initialize() { @Override public void execute() { // SmartDashboard.putNumber("turn target", driveTrain.getTurnTarget()); - driveTrain.PIDdrive(driveTrain.getEncoderPosition(), 1); + System.out.println("turn target is" + driveTrain.getTurnTarget()); + driveTrain.PIDdrive(driveTrain.getEncoderPosition(), .8); driveTrain.PIDturn(navx.getYaw()); - - if (Math.abs(driveTrain.getRightEncoders()) > 1000-Math.abs(driveTrain.getLeftEncoders()) && Math.abs(driveTrain.getRightEncoders()) < 1000+Math.abs(driveTrain.getLeftEncoders())){ - driveTrain.setRightSpeed(driveTrain.getDriveOutput()); - driveTrain.setLeftSpeed(driveTrain.getDriveOutput()); - } else { - driveTrain.setRightSpeed(driveTrain.getTurnOutput()); - driveTrain.setLeftSpeed(driveTrain.getTurnOutput()); - } - // driveTrain.setRightSpeed(driveTrain.getDriveOutput()); - // driveTrain.setLeftSpeed(driveTrain.getDriveOutput()); + absRightEnc = Math.abs(driveTrain.getRightEncoders()); + absLeftEnc = Math.abs(driveTrain.getLeftEncoders()); + + System.out.println("DriveOutput is " + driveTrain.getDriveOutput()); + System.out.println("TurnOutput is " + driveTrain.getTurnOutput()); + + System.out.println(driveTrain.getDriveError()); + + // if (absRightEnc <= absLeftEnc + 1000 && absRightEnc >= absLeftEnc - 1000){ + // System.out.println("condition met"); + driveTrain.setRightSpeed(driveTrain.getDriveOutput() + driveTrain.getTurnOutput()); + driveTrain.setLeftSpeed(driveTrain.getDriveOutput() - driveTrain.getTurnOutput()); + // } else { // driveTrain.setRightSpeed(driveTrain.getTurnOutput()); // driveTrain.setLeftSpeed(-driveTrain.getTurnOutput()); + // System.out.println("condition not met"); + // } + + - if(driveTrain.getDriveError() < Constants.maximumDriveError){ + + + + // driveTrain.setRightSpeed(driveTrain.getDriveOutput() + driveTrain.getTurnOutput()); + // driveTrain.setLeftSpeed(driveTrain.getDriveOutput() - driveTrain.getTurnOutput()); + + if(Math.abs(driveTrain.getDriveError()) < Constants.maximumDriveError){ counter++; }else{ counter = 0; @@ -80,5 +96,6 @@ public boolean isFinished() { }else{ return false; } + } } diff --git a/src/main/java/frc/robot/commands/IntakeSwitch.java b/src/main/java/frc/robot/commands/IntakeSwitch.java index b1507fc..c25e2cd 100644 --- a/src/main/java/frc/robot/commands/IntakeSwitch.java +++ b/src/main/java/frc/robot/commands/IntakeSwitch.java @@ -13,12 +13,12 @@ public class IntakeSwitch extends CommandBase { /** Creates a new IntakeSwitch. */ private final IntakeSubsystem intakeSubsystem; private final boolean reverse; - public IntakeSwitch(IntakeSubsystem intakeSubsystem, boolean reverse) { // Use addRequirements() here to declare subsystem dependencies. this.intakeSubsystem = intakeSubsystem; this.reverse = reverse; addRequirements(intakeSubsystem); + } // Called when the command is initially scheduled. 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..1a6ed00 --- /dev/null +++ b/src/main/java/frc/robot/commands/ResetEncoders.java @@ -0,0 +1,41 @@ +// 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.DriveTrain; + +public class ResetEncoders extends CommandBase { + /** Creates a new ResetEncoders. */ + private final DriveTrain driveTrain; + + public ResetEncoders(DriveTrain driveTrain) { + this.driveTrain = driveTrain; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(driveTrain); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + driveTrain.resetEncoders(); + } + + // 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/XboxArm.java b/src/main/java/frc/robot/commands/XboxArm.java index 0c2efd7..e232eaa 100644 --- a/src/main/java/frc/robot/commands/XboxArm.java +++ b/src/main/java/frc/robot/commands/XboxArm.java @@ -27,7 +27,7 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (x_Supplier.get() <= 0.5){ + if (Math.abs(x_Supplier.get()) <= 0.5){ armSubsystem.elevatorMove(x_Supplier.get()); } else { armSubsystem.elevatorMove(0.5); @@ -37,7 +37,9 @@ public void execute() { // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + + } // Returns true when the command should end. @Override diff --git a/src/main/java/frc/robot/commands/autoDriveApriltag.java b/src/main/java/frc/robot/commands/autoDriveApriltag.java index 6f75234..872858c 100644 --- a/src/main/java/frc/robot/commands/autoDriveApriltag.java +++ b/src/main/java/frc/robot/commands/autoDriveApriltag.java @@ -1,78 +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. +// 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; +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.RasberryPiCamera; -// import frc.robot.subsystems.navXSubsystem; +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 RasberryPiCamera rasberryPiCamera; -// private double piAngle = 0; -// private double H_angle; -// private double distance; +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, RasberryPiCamera rasberryPiCamera, LimeLightSubsystem limeLightSubsystem) { -// // Use addRequirements() here to declare subsystem dependencies. -// this.driveTrain = driveTrain; -// this.rasberryPiCamera = rasberryPiCamera; -// this.limeLightSubsystem = limeLightSubsystem; -// addRequirements(driveTrain); -// addRequirements(rasberryPiCamera); -// addRequirements(limeLightSubsystem); -// H_angle = limeLightSubsystem.getH_angle(); -// distance = limeLightSubsystem.calculateXdistance();// distance to apriltag -// } - -// // Called when the command is initially scheduled. -// @Override -// public void initialize() { -// driveTrain.resetDrivePID(); -// driveTrain.setLeftSpeed(0); -// driveTrain.setRightSpeed(0); - -// } - -// // Called every time the scheduler runs while the command is scheduled. -// @Override -// public void execute() { - -// for (int i = 0; i < 3; i++){ -// H_angle = H_angle / 2; -// distance = distance / 2; -// driveTrain.PIDdrive(distance, 0.5); - -// driveTrain.PIDturn(H_angle); -// } - -// driveTrain.PIDturn(H_angle); - -// // piAngle = rasberryPiCamera.getTargetPitch(); // or yaw or whatever is the right angle for turning - -// // driveTrain.PIDturn(piAngle); - -// // driveTrain.PIDdrive(rasberryPiCamera.targetDistance(), rasberryPiCamera.targetDistance() + 10); - -// } - -// // 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; -// } -// } + 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 0c519ff..c74bc4c 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -4,18 +4,21 @@ 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.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 armMotor = new TalonFX(Constants.armMotor); + private final CANSparkMax armMotor = new CANSparkMax(Constants.armMotorID, MotorType.kBrushless); private double armTarget; private double armError; private double armPrevError; @@ -28,7 +31,6 @@ public class ArmSubsystem extends SubsystemBase { public ArmSubsystem() { - armMotor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor); armTarget = 0; armError = 0; armPrevError = 0; @@ -41,7 +43,16 @@ public ArmSubsystem() //setters public void elevatorMove(double speed){ - armMotor.set(ControlMode.PercentOutput, speed); + armMotor.set(speed); + } + + public double getArmPosition(){ + return armMotor.getEncoder().getPosition(); + } + + public double getArmSpeed(){ + return armMotor.get(); + } public void resetArmPID(){ diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index 079b8b3..5f2cbc0 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -6,9 +6,11 @@ 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; @@ -56,9 +58,15 @@ public class DriveTrain extends SubsystemBase { public DriveTrain() { FR.setInverted(false); - BR.setInverted(false); - FL.setInverted(true); - BL.setInverted(true); + BR.setInverted(true); + FL.setInverted(false); + BL.setInverted(false); + + FR.setNeutralMode(NeutralMode.Coast); + BR.setNeutralMode(NeutralMode.Coast); + BL.setNeutralMode(NeutralMode.Coast); + FL.setNeutralMode(NeutralMode.Coast); + //turn variables turnTarget = 0; turnError = 0; @@ -129,8 +137,6 @@ public void PIDturn(double sensorInput){ turnOutput = Constants.turnkP*turnP + Constants.turnkI*turnI + Constants.turnkD*turnD; turnPrevError = turnError; //clamp output between -50% and 50% - if(turnOutput >= 0.5) turnOutput = 0.5; - if(turnOutput <= -0.5) turnOutput = -0.5; } public void resetEncoders(){ @@ -173,6 +179,25 @@ public void PIDdrive(double sensorInput, double limit) { prevDriveOutput = 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; @@ -232,6 +257,20 @@ public double getTurnTarget(){ 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 From cc0fdc758754806ec4ba82646d4df8d5bc730c54 Mon Sep 17 00:00:00 2001 From: Alex Nie <93626002+TheSparkyBoy@users.noreply.github.com> Date: Sat, 18 Mar 2023 16:58:45 -0700 Subject: [PATCH 27/41] AutoTurn and AutoDrive Working --- src/main/java/frc/robot/Constants.java | 12 ++++++--- src/main/java/frc/robot/RobotContainer.java | 9 ++++--- .../java/frc/robot/commands/AutoTurn.java | 10 ++------ .../frc/robot/commands/EncoderAutoDrive.java | 18 ++++++++----- .../java/frc/robot/subsystems/DriveTrain.java | 25 +++++++++++++------ 5 files changed, 45 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c3d8311..a968169 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -75,10 +75,14 @@ public final class Constants { public static final int armMotor = 0; //DriveTrain Motor ID's - 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 = 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; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 89cb692..c7e1ede 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -96,9 +96,12 @@ public Command getAutonomousCommand() { // new SetPipeline(LimeLightSubsystem, 2) // ); return new SequentialCommandGroup( - new AutoTurn(driveTrain, navX, 180, 2000), - new AutoTurn(driveTrain, navX, 0, 2000), - new AutoTurn(driveTrain, navX, -90, 2000) + // new AutoTurn(driveTrain, navX, 180, 3000), + // new AutoTurn(driveTrain, navX, 0, 3000), + // new AutoTurn(driveTrain, navX, -90, 2000), + // new AutoTurn(driveTrain, navX, 720, 6000) + new EncoderAutoDrive(driveTrain, -50000, navX) + // new EncoderAutoDrive(driveTrain, 100000, navX) ); } } diff --git a/src/main/java/frc/robot/commands/AutoTurn.java b/src/main/java/frc/robot/commands/AutoTurn.java index ac8ee65..373d6d8 100644 --- a/src/main/java/frc/robot/commands/AutoTurn.java +++ b/src/main/java/frc/robot/commands/AutoTurn.java @@ -44,14 +44,8 @@ public void initialize() { @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++; } diff --git a/src/main/java/frc/robot/commands/EncoderAutoDrive.java b/src/main/java/frc/robot/commands/EncoderAutoDrive.java index 86a9899..60dda9c 100644 --- a/src/main/java/frc/robot/commands/EncoderAutoDrive.java +++ b/src/main/java/frc/robot/commands/EncoderAutoDrive.java @@ -6,15 +6,18 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.DriveTrain; +import frc.robot.subsystems.navXSubsystem; import frc.robot.Constants; public class EncoderAutoDrive extends CommandBase { /** Creates a new EncoderAutoDrivve. */ private final DriveTrain driveTrain; + private final navXSubsystem navX; private int counter; private final double target; - public EncoderAutoDrive(DriveTrain driveTrain, double target) { + public EncoderAutoDrive(DriveTrain driveTrain, double target, navXSubsystem navX) { this.driveTrain = driveTrain; + this.navX = navX; counter = 0; this.target = target; // Use addRequirements() here to declare subsystem dependencies. @@ -24,8 +27,11 @@ public EncoderAutoDrive(DriveTrain driveTrain, double target) { // Called when the command is initially scheduled. @Override public void initialize() { + driveTrain.resetEncoders(); driveTrain.resetDrivePID(); + driveTrain.resetTurnPID(); driveTrain.setDriveTarget(target); + driveTrain.setTurnTarget(navX.getYaw()); driveTrain.setLeftSpeed(0); driveTrain.setRightSpeed(0); } @@ -33,11 +39,11 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - - driveTrain.PIDdrive(target, 0); - driveTrain.setRightSpeed(driveTrain.getDriveOutput()); - driveTrain.setLeftSpeed(driveTrain.getDriveOutput()); - if(driveTrain.getDriveError() < Constants.maximumDriveError){ + 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; diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index c7417f4..be4baae 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -30,6 +30,7 @@ public class DriveTrain extends SubsystemBase { private double turnI; private double turnD; private double turnOutput; + private double turnPrevOutput; //PIDdrive variables @@ -41,7 +42,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; @@ -73,12 +74,12 @@ 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; balanceError = 0; @@ -108,7 +109,7 @@ public void resetDrivePID(){ driveD = 0; driveOutput = 0; potDriveOutput = 0; - prevDriveOutput = 0; + drivePrevOutput = 0; } public void resetTurnPID(){ @@ -129,6 +130,7 @@ public void PIDturn(double sensorInput){ turnOutput = Constants.turnkP*turnP + Constants.turnkI*turnI + Constants.turnkD*turnD; turnPrevError = turnError; + turnPrevOutput = turnOutput; } public void resetEncoders(){ @@ -144,8 +146,7 @@ public void PIDdrive(double sensorInput, double limit) { driveI += driveError; driveD = driveError - drivePrevError; - - driveOutput = Constants.kP*driveP + Constants.kI*driveI + Constants.kD*driveD; + driveOutput = (Constants.kP*driveP + Constants.kI*driveI + Constants.kD*driveD); if(driveOutput > limit){ driveOutput = limit; } @@ -154,7 +155,7 @@ public void PIDdrive(double sensorInput, double limit) { } drivePrevError = driveError; - prevDriveOutput = driveOutput; + drivePrevOutput = driveOutput; } public double getTurnOutput() @@ -194,7 +195,10 @@ public double getTurnPrevError(){ return turnPrevError; } - public double getFRid(){ + 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(){ @@ -206,10 +210,15 @@ public double getBRid(){ 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 void setTurnTarget(double degrees){ turnTarget = degrees; } From 66d0ab90635bb88ad4e764c23832420be9b8bf03 Mon Sep 17 00:00:00 2001 From: Rewand727 Date: Sat, 18 Mar 2023 17:01:57 -0700 Subject: [PATCH 28/41] temp --- src/main/java/frc/robot/Constants.java | 12 ++++++++---- .../java/frc/robot/commands/EncoderAutoDrive.java | 1 - 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 847915b..3fad90a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -13,10 +13,14 @@ * constants are needed, to reduce verbosity. */ public final class Constants { - public static final int FrontRightID = 2; - public static final int FrontLeftID = 3; - public static final int BackRightID = 5; - public static final int BackLeftID = 0; + public static final int FrontRightID = 2; //was 0 + public static final int FrontLeftID = 3; //was 1 + public static final int BackRightID = 5; //was 3 + public static final int BackLeftID = 0; //was 6 + + + + public static final int XBOX_R_XAXIS = 4; public static final int XBOX_R_YAXIS = 5; diff --git a/src/main/java/frc/robot/commands/EncoderAutoDrive.java b/src/main/java/frc/robot/commands/EncoderAutoDrive.java index 1c096b6..58d8e8d 100644 --- a/src/main/java/frc/robot/commands/EncoderAutoDrive.java +++ b/src/main/java/frc/robot/commands/EncoderAutoDrive.java @@ -54,7 +54,6 @@ public void execute() { System.out.println("DriveOutput is " + driveTrain.getDriveOutput()); System.out.println("TurnOutput is " + driveTrain.getTurnOutput()); - System.out.println(driveTrain.getDriveError()); // if (absRightEnc <= absLeftEnc + 1000 && absRightEnc >= absLeftEnc - 1000){ // System.out.println("condition met"); From 1a5c6c302baa0b5573534e91d1c97a70e8e22158 Mon Sep 17 00:00:00 2001 From: Rewand727 Date: Sun, 19 Mar 2023 16:15:50 -0700 Subject: [PATCH 29/41] pre fnished bot code (lots of guesses) --- src/main/java/frc/robot/Constants.java | 17 +++++++--- src/main/java/frc/robot/Robot.java | 1 + src/main/java/frc/robot/RobotContainer.java | 28 ++++++++-------- .../frc/robot/commands/NavXAutoDrive.java | 32 ------------------- .../frc/robot/commands/ResetEncoders.java | 6 +++- src/main/java/frc/robot/commands/XboxArm.java | 13 +++++--- .../frc/robot/commands/autoArmPickup.java | 30 +++++++++++++---- .../frc/robot/subsystems/ArmSubsystem.java | 31 +++++++++++++----- .../java/frc/robot/subsystems/DriveTrain.java | 2 +- 9 files changed, 88 insertions(+), 72 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/NavXAutoDrive.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3fad90a..d1dd6fb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -63,6 +63,7 @@ public final class Constants { //Also some motor id constant for the intake public static final int deviceIdIntakeM1 = 9; public static final int deviceIdIntakeM2 = 10; + public static final int intakeGearRatio = 5; public static final double intakeMotorSpeed = .4; public static final int ampSpike= 8; @@ -78,12 +79,18 @@ public static class OperatorConstants { public static final int kDriverControllerPort = 0; } - + //arm Constants public static final int talonArmPivot = 1; public static final int talonLift1 = 2; public static final int talonLift2 = 3; public static final int extensionMotorID = 0; public static final int armMotorID = 0; + public static final double armFrontEncoderLimit = 0; + public static final double armBackEncoderLimit = 0; + public static final double kArmP = 0; + public static final double kArmI = 0; + public static final double kArmD = 0; + public static final double armGearRatio = 75;// 75:1 //DriveTrain Motor ID's public static final int FRid = 0; @@ -125,10 +132,10 @@ public static class OperatorConstants { //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 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 diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a5fe5c9..4c8891a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -69,6 +69,7 @@ public void robotPeriodic() { 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 Encoders", m_robotContainer.armSubsystem.getArmPosition()); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e24a243..d650332 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -42,7 +42,7 @@ public class RobotContainer { public final JoystickButton Abutton = new JoystickButton(Xbox, Constants.Xbox_Button_A); public final JoystickButton Bbutton = new JoystickButton(Xbox, Constants.Xbox_Button_B); - public final JoystickButton AButtonX2 = new JoystickButton(Xbox, Constants.Xbox_Button_AX2); + public final JoystickButton AbuttonX2 = new JoystickButton(Xbox2, Constants.Xbox_Button_AX2); // Replace with CommandPS4Controller or CommandJoystick if needed @@ -74,7 +74,7 @@ private void configureBindings() { Xbutton.toggleWhenPressed(new IntakeSwitch(intakeSubsystem, false)); Ybutton.toggleWhenPressed(new IntakeSwitch(intakeSubsystem, true)); - Abutton.whenPressed(new ResetEncoders(driveTrain)); + AbuttonX2.whenPressed(new ResetEncoders(driveTrain, armSubsystem)); @@ -90,14 +90,14 @@ private void configureBindings() { public Command getAutonomousCommand() { // An example command will be run in autonomous // 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 SequentialCommandGroup( + new SetPipeline(LimeLightSubsystem, 0), + new Wait(10000), + new SetPipeline(LimeLightSubsystem, 1), + new Wait(10000), + new SetPipeline(LimeLightSubsystem, 2) + ); + } // return new SequentialCommandGroup( // new AutoVisionPitch(driveTrain, LimeLightSubsystem, navX, 0.5) @@ -120,10 +120,10 @@ public Command getAutonomousCommand() { // new AutoTurn(driveTrain, navX, -90, 2000) // ); - return new SequentialCommandGroup( - new EncoderAutoDrive(driveTrain, navX, 30000) - ); + // return new SequentialCommandGroup( + // new EncoderAutoDrive(driveTrain, navX, 30000) + // ); // return new SequentialCommandGroup( // new testRightLeftMotor(driveTrain) // ); - }} + } diff --git a/src/main/java/frc/robot/commands/NavXAutoDrive.java b/src/main/java/frc/robot/commands/NavXAutoDrive.java deleted file mode 100644 index e239da9..0000000 --- a/src/main/java/frc/robot/commands/NavXAutoDrive.java +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -public class NavXAutoDrive extends CommandBase { - /** Creates a new NavXAutoDrive. */ - public NavXAutoDrive() { - // 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() {} - - // 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/commands/ResetEncoders.java b/src/main/java/frc/robot/commands/ResetEncoders.java index 1a6ed00..8f0c1c3 100644 --- a/src/main/java/frc/robot/commands/ResetEncoders.java +++ b/src/main/java/frc/robot/commands/ResetEncoders.java @@ -7,14 +7,17 @@ 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) { + public ResetEncoders(DriveTrain driveTrain, ArmSubsystem armSubsystem) { this.driveTrain = driveTrain; + this.armSubsystem = armSubsystem; // Use addRequirements() here to declare subsystem dependencies. addRequirements(driveTrain); } @@ -23,6 +26,7 @@ public ResetEncoders(DriveTrain driveTrain) { @Override public void initialize() { driveTrain.resetEncoders(); + armSubsystem.resetArmEncoders(); } // Called every time the scheduler runs while the command is scheduled. diff --git a/src/main/java/frc/robot/commands/XboxArm.java b/src/main/java/frc/robot/commands/XboxArm.java index e232eaa..5c88db4 100644 --- a/src/main/java/frc/robot/commands/XboxArm.java +++ b/src/main/java/frc/robot/commands/XboxArm.java @@ -7,6 +7,7 @@ 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 { @@ -27,10 +28,12 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (Math.abs(x_Supplier.get()) <= 0.5){ - armSubsystem.elevatorMove(x_Supplier.get()); - } else { - armSubsystem.elevatorMove(0.5); + if (armSubsystem.getArmPosition() >= Constants.armBackEncoderLimit && armSubsystem.getArmPosition() <= Constants.armFrontEncoderLimit){ + if (Math.abs(x_Supplier.get()) <= 0.5){ + armSubsystem.armMove(x_Supplier.get()); + } else { + armSubsystem.armMove(0); + } } } @@ -38,7 +41,7 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - + armSubsystem.armMove(0); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/autoArmPickup.java b/src/main/java/frc/robot/commands/autoArmPickup.java index 658a347..ab92f04 100644 --- a/src/main/java/frc/robot/commands/autoArmPickup.java +++ b/src/main/java/frc/robot/commands/autoArmPickup.java @@ -8,25 +8,39 @@ import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.DriveTrain; -public class autoArmPickup extends CommandBase { +public class AutoArmPickup extends CommandBase { /** Creates a new autoArmPickup. */ - private ArmSubsystem armSubsystem; + private final ArmSubsystem armSubsystem; + private double target, timeOut; + private int errorCounter, cycleCounter; - - public autoArmPickup(ArmSubsystem armSubsystem) { + public AutoArmPickup(ArmSubsystem armSubsystem, double target, double timeOut) { this.armSubsystem = armSubsystem; + this.target = target; + this.timeOut = timeOut; + errorCounter = 0; + cycleCounter = 0; addRequirements(armSubsystem); } // Called when the command is initially scheduled. @Override public void initialize() { + armSubsystem.resetArmPID(); + armSubsystem.armMove(0); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - // driveTrain.PIDdrive(driveTrain.get, counter); + armSubsystem.moveArmPID(armSubsystem.getArmPosition(), 0.5); + armSubsystem.armMove(armSubsystem.getArmMoveOutput()); + if (armSubsystem.getArmMoveError() <= 5000){ + errorCounter++; + } else{ + errorCounter = 0; + } + } // Called once the command ends or is interrupted. @Override @@ -35,6 +49,10 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return false; + if (errorCounter > 10 || cycleCounter > (timeOut/20)){ + return true; + } else { + return false; + } } } diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index c74bc4c..cc3d577 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -18,7 +18,8 @@ public class ArmSubsystem extends SubsystemBase { /** Creates a new ArmSubsystem. */ - private final CANSparkMax armMotor = new CANSparkMax(Constants.armMotorID, 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; @@ -42,18 +43,24 @@ public ArmSubsystem() } //setters - public void elevatorMove(double speed){ - armMotor.set(speed); + public void armMove(double speed){ + // armMotor.set(speed); + armMotor.set(ControlMode.PercentOutput, speed); + } + + public void resetArmEncoders(){ + armMotor.setSelectedSensorPosition(0); } public double getArmPosition(){ - return armMotor.getEncoder().getPosition(); + // return armMotor.getEncoder().getPosition(); + return armMotor.getSelectedSensorPosition(); } - public double getArmSpeed(){ - return armMotor.get(); + // public double getArmSpeed(){ + // return armMotor.get(); - } + // } public void resetArmPID(){ armTarget = 0; @@ -73,7 +80,7 @@ public void moveArmPID(double sensorInput, double limit){ armD = armError - armPrevError; - armOutput = Constants.kP*armP + Constants.kI*armI + Constants.kD*armD; + armOutput = Constants.kArmP*armP + Constants.kArmI*armI + Constants.kArmD*armD; if(armOutput > limit){ armOutput = limit; } @@ -85,6 +92,14 @@ public void moveArmPID(double sensorInput, double limit){ armPrevError = armError; prevArmOutput = armOutput; } + + public double getArmMoveOutput(){ + return armOutput; + } + + public double getArmMoveError(){ + return armError; + } //getters // public double getElevatorPosition(){//one revolution is 2048 encoder units. // double position1 = elevatorLiftMotor1.getSelectedSensorPosition(); diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index 5f2cbc0..9fd5400 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -167,7 +167,7 @@ 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; } From 92d876aa5cac7880479264e1568175d9e57da983 Mon Sep 17 00:00:00 2001 From: Rewand727 Date: Mon, 20 Mar 2023 15:24:48 -0700 Subject: [PATCH 30/41] fixed ids --- src/main/java/frc/robot/Constants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d1dd6fb..fc0c784 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -13,10 +13,10 @@ * constants are needed, to reduce verbosity. */ public final class Constants { - public static final int FrontRightID = 2; //was 0 + public static final int FrontRightID = 0; //was 0 public static final int FrontLeftID = 3; //was 1 public static final int BackRightID = 5; //was 3 - public static final int BackLeftID = 0; //was 6 + public static final int BackLeftID = 2; //was 6 From 367beaa263b5bdfd432e97b63b712db741799012 Mon Sep 17 00:00:00 2001 From: Alex Nie <93626002+TheSparkyBoy@users.noreply.github.com> Date: Mon, 20 Mar 2023 15:30:27 -0700 Subject: [PATCH 31/41] Still Testing AutoBalancing --- src/main/java/frc/robot/Constants.java | 23 ++++++++++--------- src/main/java/frc/robot/RobotContainer.java | 8 +++---- .../frc/robot/commands/AutoBalancing.java | 2 +- .../frc/robot/commands/EncoderAutoDrive.java | 4 ++-- .../java/frc/robot/subsystems/DriveTrain.java | 2 +- 5 files changed, 20 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a968169..7c4f0f4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -75,14 +75,14 @@ public final class Constants { public static final int armMotor = 0; //DriveTrain Motor ID's - // 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; + 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; @@ -103,13 +103,13 @@ public final class Constants { public static final int yLeftidX2 = 1; // turn PID constants - public static final double turnkP = 0.003; - public static final double turnkI = 0.0128; + 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; @@ -134,6 +134,7 @@ public final class 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; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c7e1ede..0cf24bb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -96,12 +96,12 @@ public Command getAutonomousCommand() { // new SetPipeline(LimeLightSubsystem, 2) // ); return new SequentialCommandGroup( - // new AutoTurn(driveTrain, navX, 180, 3000), + // new AutoTurn(driveTrain, navX, 90, 3000), // new AutoTurn(driveTrain, navX, 0, 3000), - // new AutoTurn(driveTrain, navX, -90, 2000), - // new AutoTurn(driveTrain, navX, 720, 6000) - new EncoderAutoDrive(driveTrain, -50000, navX) + // new AutoTurn(driveTrain, navX, -90, 2000) + // new EncoderAutoDrive(driveTrain, 10000, navX) // new EncoderAutoDrive(driveTrain, 100000, navX) + new AutoBalancing(navX, driveTrain) ); } } 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/EncoderAutoDrive.java b/src/main/java/frc/robot/commands/EncoderAutoDrive.java index 60dda9c..5a7c1c5 100644 --- a/src/main/java/frc/robot/commands/EncoderAutoDrive.java +++ b/src/main/java/frc/robot/commands/EncoderAutoDrive.java @@ -19,7 +19,7 @@ public EncoderAutoDrive(DriveTrain driveTrain, double target, navXSubsystem navX this.driveTrain = driveTrain; this.navX = navX; counter = 0; - this.target = target; + this.target = -target; // Use addRequirements() here to declare subsystem dependencies. addRequirements(driveTrain); } @@ -60,7 +60,7 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - if(counter > 10){ + if(counter > 100000){ return true; }else{ return false; diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index be4baae..788e5cf 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -81,7 +81,7 @@ public DriveTrain() { driveOutput = 0; potDriveOutput = 0; //BalancePID variables - balanceTarget = 0; + balanceTarget = Constants.balanceTarget; balanceError = 0; balancePrevError = 0; balanceP = 0; From 9de94f1f8d611e2ce5e0b8b298443064b786e4d9 Mon Sep 17 00:00:00 2001 From: Rewand727 Date: Tue, 21 Mar 2023 16:24:37 -0700 Subject: [PATCH 32/41] motor id --- src/main/java/frc/robot/Constants.java | 24 ++++++++++++++++-------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fc0c784..2cc3594 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -13,14 +13,22 @@ * constants are needed, to reduce verbosity. */ public final class Constants { - public static final int FrontRightID = 0; //was 0 - public static final int FrontLeftID = 3; //was 1 - public static final int BackRightID = 5; //was 3 - public static final int BackLeftID = 2; //was 6 + // public static final int FrontRightID = 0; //was 0 + // public static final int FrontLeftID = 3; //was 1 + // public static final int BackRightID = 5; //was 3 + // public static final int BackLeftID = 2; //was 6 - - +//new ids +// public static final int FrontRightID = 0; +// public static final int FrontLeftID = 3; +// public static final int BackRightID = 5; +// public static final int BackLeftID = 2; +//old ids +public static final int FrontRightID = 0; +public static final int FrontLeftID = 3; +public static final int BackRightID = 6; +public static final int BackLeftID = 1; public static final int XBOX_R_XAXIS = 4; public static final int XBOX_R_YAXIS = 5; @@ -85,8 +93,8 @@ public static class OperatorConstants { public static final int talonLift2 = 3; public static final int extensionMotorID = 0; public static final int armMotorID = 0; - public static final double armFrontEncoderLimit = 0; - public static final double armBackEncoderLimit = 0; + public static final double armFrontEncoderLimit = 1000; + public static final double armBackEncoderLimit = -100; public static final double kArmP = 0; public static final double kArmI = 0; public static final double kArmD = 0; From 853d675af7bdf0cb73bc9035d8865b7949d2b5ec Mon Sep 17 00:00:00 2001 From: Rewand727 Date: Tue, 21 Mar 2023 16:24:49 -0700 Subject: [PATCH 33/41] motor id --- src/main/java/frc/robot/Constants.java | 29 ++++++++++++-------------- 1 file changed, 13 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2cc3594..89af4ab 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -13,22 +13,8 @@ * constants are needed, to reduce verbosity. */ public final class Constants { - // public static final int FrontRightID = 0; //was 0 - // public static final int FrontLeftID = 3; //was 1 - // public static final int BackRightID = 5; //was 3 - // public static final int BackLeftID = 2; //was 6 - -//new ids -// public static final int FrontRightID = 0; -// public static final int FrontLeftID = 3; -// public static final int BackRightID = 5; -// public static final int BackLeftID = 2; - -//old ids -public static final int FrontRightID = 0; -public static final int FrontLeftID = 3; -public static final int BackRightID = 6; -public static final int BackLeftID = 1; + + public static final int XBOX_R_XAXIS = 4; public static final int XBOX_R_YAXIS = 5; @@ -106,6 +92,17 @@ public static class OperatorConstants { public static final int FLid = 3; public static final int BRid = 6; + //new ids +// public static final int FrontRightID = 0; +// public static final int FrontLeftID = 3; +// public static final int BackRightID = 5; +// public static final int BackLeftID = 2; + +//old ids +// public static final int FrontRightID = 0; +// public static final int FrontLeftID = 3; +// public static final int BackRightID = 6; +// public static final int BackLeftID = 1; //DriveTrain Constants public static final double turnAdjustment = 0.5; public static final double powerAdjustment = 0.5; From f36e0319ee9c9b7761384adb4bdce179a087cede Mon Sep 17 00:00:00 2001 From: Rewand727 Date: Tue, 21 Mar 2023 23:23:58 -0700 Subject: [PATCH 34/41] some stuff works ig --- src/main/java/frc/robot/Constants.java | 23 +++++++++---------- src/main/java/frc/robot/RobotContainer.java | 7 +++--- src/main/java/frc/robot/commands/XboxArm.java | 12 ++++++---- .../{IntakeSwitch.java => XboxIntake.java} | 14 ++++------- .../frc/robot/commands/autoArmPickup.java | 6 +++-- .../frc/robot/subsystems/IntakeSubsystem.java | 18 ++++++--------- 6 files changed, 38 insertions(+), 42 deletions(-) rename src/main/java/frc/robot/commands/{IntakeSwitch.java => XboxIntake.java} (78%) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 89af4ab..efe2ffe 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -56,7 +56,7 @@ public final class Constants { 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 int deviceIdIntakeM2 = 10; public static final int intakeGearRatio = 5; public static final double intakeMotorSpeed = .4; @@ -78,7 +78,7 @@ public static class OperatorConstants { public static final int talonLift1 = 2; public static final int talonLift2 = 3; public static final int extensionMotorID = 0; - public static final int armMotorID = 0; + public static final int armMotorID = 7; public static final double armFrontEncoderLimit = 1000; public static final double armBackEncoderLimit = -100; public static final double kArmP = 0; @@ -86,17 +86,17 @@ public static class OperatorConstants { public static final double kArmD = 0; public static final double armGearRatio = 75;// 75:1 - //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; + // //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; //new ids -// public static final int FrontRightID = 0; -// public static final int FrontLeftID = 3; -// public static final int BackRightID = 5; -// public static final int BackLeftID = 2; +public static final int FRid = 0; +public static final int FLid = 3; +public static final int BRid = 5; +public static final int BLid = 2; //old ids // public static final int FrontRightID = 0; @@ -154,5 +154,4 @@ public static class OperatorConstants { public static final double limeLightHeight = 32; public static final double limeLightInitAngle = 0; public static final double cameraOffset = 0; // degrees horizontal angle - } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d650332..86ea181 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -56,7 +56,8 @@ public class RobotContainer { public RobotContainer() { // Configure the trigger bindings driveTrain.setDefaultCommand(new XboxDrive(driveTrain, () -> Xbox.getRightX(), () -> Xbox.getLeftY())); - armSubsystem.setDefaultCommand(new XboxArm(() -> Xbox2.getRightX(), armSubsystem)); + armSubsystem.setDefaultCommand(new XboxArm(() -> Xbox2.getLeftY(), armSubsystem)); + intakeSubsystem.setDefaultCommand(new XboxIntake(intakeSubsystem, () -> Xbox2.getRightY())); configureBindings(); } @@ -72,8 +73,8 @@ public RobotContainer() { private void configureBindings() { // Schedule `ExampleCommand` when `exampleCondition` changes to `true` - Xbutton.toggleWhenPressed(new IntakeSwitch(intakeSubsystem, false)); - Ybutton.toggleWhenPressed(new IntakeSwitch(intakeSubsystem, true)); + // Xbutton.toggleWhenPressed(new XboxIntake(intakeSubsystem, false)); + // Ybutton.toggleWhenPressed(new XboxIntake(intakeSubsystem, true)); AbuttonX2.whenPressed(new ResetEncoders(driveTrain, armSubsystem)); diff --git a/src/main/java/frc/robot/commands/XboxArm.java b/src/main/java/frc/robot/commands/XboxArm.java index 5c88db4..0815f2f 100644 --- a/src/main/java/frc/robot/commands/XboxArm.java +++ b/src/main/java/frc/robot/commands/XboxArm.java @@ -13,11 +13,11 @@ public class XboxArm extends CommandBase { /** Creates a new XboxArm. */ private final ArmSubsystem armSubsystem; - private final Supplier x_Supplier; - public XboxArm(Supplier x_supplier,ArmSubsystem armSubsystem) { + private final Supplier y_Supplier; + public XboxArm(Supplier y_supplier, ArmSubsystem armSubsystem) { // Use addRequirements() here to declare subsystem dependencies. this.armSubsystem = armSubsystem; - this.x_Supplier = x_supplier; + this.y_Supplier = y_supplier; addRequirements(armSubsystem); } @@ -28,9 +28,11 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + + if (armSubsystem.getArmPosition() >= Constants.armBackEncoderLimit && armSubsystem.getArmPosition() <= Constants.armFrontEncoderLimit){ - if (Math.abs(x_Supplier.get()) <= 0.5){ - armSubsystem.armMove(x_Supplier.get()); + if (Math.abs(y_Supplier.get()) <= 0.5){ + armSubsystem.armMove(y_Supplier.get()); } else { armSubsystem.armMove(0); } diff --git a/src/main/java/frc/robot/commands/IntakeSwitch.java b/src/main/java/frc/robot/commands/XboxIntake.java similarity index 78% rename from src/main/java/frc/robot/commands/IntakeSwitch.java rename to src/main/java/frc/robot/commands/XboxIntake.java index c25e2cd..96993db 100644 --- a/src/main/java/frc/robot/commands/IntakeSwitch.java +++ b/src/main/java/frc/robot/commands/XboxIntake.java @@ -9,14 +9,14 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.IntakeSubsystem; -public class IntakeSwitch extends CommandBase { +public class XboxIntake extends CommandBase { /** Creates a new IntakeSwitch. */ private final IntakeSubsystem intakeSubsystem; - private final boolean reverse; - public IntakeSwitch(IntakeSubsystem intakeSubsystem, boolean reverse) { + private final Supplier rightY_Supplier; + public XboxIntake(IntakeSubsystem intakeSubsystem, Supplier rightY_Supplier) { // Use addRequirements() here to declare subsystem dependencies. this.intakeSubsystem = intakeSubsystem; - this.reverse = reverse; + this.rightY_Supplier = rightY_Supplier; addRequirements(intakeSubsystem); } @@ -30,11 +30,7 @@ public void initialize() { // 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(rightY_Supplier.get()); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/autoArmPickup.java b/src/main/java/frc/robot/commands/autoArmPickup.java index ab92f04..b15a89a 100644 --- a/src/main/java/frc/robot/commands/autoArmPickup.java +++ b/src/main/java/frc/robot/commands/autoArmPickup.java @@ -35,7 +35,7 @@ public void initialize() { public void execute() { armSubsystem.moveArmPID(armSubsystem.getArmPosition(), 0.5); armSubsystem.armMove(armSubsystem.getArmMoveOutput()); - if (armSubsystem.getArmMoveError() <= 5000){ + if (Math.abs(armSubsystem.getArmMoveError()) <= 5000){ errorCounter++; } else{ errorCounter = 0; @@ -44,7 +44,9 @@ public void execute() { } // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + armSubsystem.armMove(0); + } // Returns true when the command should end. @Override 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() From 9a456a95130869fe0c3b0e5eba7654ddffa81711 Mon Sep 17 00:00:00 2001 From: Alex Nie <93626002+TheSparkyBoy@users.noreply.github.com> Date: Tue, 21 Mar 2023 23:28:39 -0700 Subject: [PATCH 35/41] Fixed Some Renaming Issues --- src/main/java/frc/robot/commands/autoArmPickup.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/autoArmPickup.java b/src/main/java/frc/robot/commands/autoArmPickup.java index b15a89a..2fc165f 100644 --- a/src/main/java/frc/robot/commands/autoArmPickup.java +++ b/src/main/java/frc/robot/commands/autoArmPickup.java @@ -40,14 +40,12 @@ public void execute() { } else{ errorCounter = 0; } - } // 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() { From 07c64bce9d949488112bf878e68b7a686ddf1556 Mon Sep 17 00:00:00 2001 From: Alex Nie <93626002+TheSparkyBoy@users.noreply.github.com> Date: Wed, 22 Mar 2023 11:09:22 -0700 Subject: [PATCH 36/41] PID Arm Coded - needs testing --- src/main/java/frc/robot/Constants.java | 17 ++--- src/main/java/frc/robot/Robot.java | 6 +- src/main/java/frc/robot/RobotContainer.java | 29 ++++----- .../frc/robot/commands/EncoderAutoDrive.java | 2 +- .../java/frc/robot/commands/IntakeCone.java | 47 ++++++++++++++ .../{XboxIntake.java => IntakeCube.java} | 19 +++--- .../commands/PIDAssistedArmMovement.java | 62 +++++++++++++++++++ src/main/java/frc/robot/commands/XboxArm.java | 18 ++---- .../frc/robot/subsystems/ArmSubsystem.java | 20 +++--- 9 files changed, 164 insertions(+), 56 deletions(-) create mode 100644 src/main/java/frc/robot/commands/IntakeCone.java rename src/main/java/frc/robot/commands/{XboxIntake.java => IntakeCube.java} (66%) create mode 100644 src/main/java/frc/robot/commands/PIDAssistedArmMovement.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 13c77d8..8e90eb0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -15,12 +15,11 @@ public final class Constants { - + //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; @@ -57,7 +56,7 @@ public final class Constants { 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; @@ -67,13 +66,15 @@ public final class Constants { 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 = 1000; - public static final double armBackEncoderLimit = -100; - public static final double kArmP = 0; - public static final double kArmI = 0; + public static final double armFrontEncoderLimit = 40000; + public static final double armBackEncoderLimit = -40000; + public static final double kArmP = 0.0001; + public static final double kArmI = 0.00001; public static final double kArmD = 0; public static final double armGearRatio = 75;// 75:1 + public static final double kArmPID = 400; //DriveTrain Motor ID's public static final int FRid = 2; @@ -136,4 +137,4 @@ public final class Constants { 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 4c8891a..74c835a 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -31,6 +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.armSubsystem.resetArmEncoders(); } /** @@ -69,8 +70,7 @@ public void robotPeriodic() { 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 Encoders", m_robotContainer.armSubsystem.getArmPosition()); - + SmartDashboard.putNumber("Arm Position", m_robotContainer.armSubsystem.getArmPosition()); } /** This function is called once each time the robot enters Disabled mode. */ @@ -106,6 +106,7 @@ public void autonomousPeriodic() { 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()); } @@ -135,6 +136,7 @@ public void teleopPeriodic() { 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 b1204b4..6bb9d7d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -34,13 +34,18 @@ public class RobotContainer { // 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); - - public final JoystickButton AbuttonX2 = new JoystickButton(Xbox2, Constants.Xbox_Button_AX2); + 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); // Replace with CommandPS4Controller or CommandJoystick if needed @@ -54,8 +59,7 @@ public class RobotContainer { public RobotContainer() { // Configure the trigger bindings driveTrain.setDefaultCommand(new XboxDrive(driveTrain, () -> Xbox.getRightX(), () -> Xbox.getLeftY())); - armSubsystem.setDefaultCommand(new XboxArm(() -> Xbox2.getLeftY(), armSubsystem)); - intakeSubsystem.setDefaultCommand(new XboxIntake(intakeSubsystem, () -> Xbox2.getRightY())); + armSubsystem.setDefaultCommand(new PIDAssistedArmMovement(armSubsystem, () -> Xbox2.getRawAxis(Constants.XBOX_L_YAXIS))); configureBindings(); } @@ -70,12 +74,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)); - // Xbutton.toggleWhenPressed(new XboxIntake(intakeSubsystem, false)); - // Ybutton.toggleWhenPressed(new XboxIntake(intakeSubsystem, true)); AbuttonX2.whenPressed(new ResetEncoders(driveTrain, armSubsystem)); + LBbuttonX2.whenHeld(new IntakeCube(intakeSubsystem, Xbox2)); + RBbuttonX2.whenHeld(new IntakeCone(intakeSubsystem, Xbox2)); diff --git a/src/main/java/frc/robot/commands/EncoderAutoDrive.java b/src/main/java/frc/robot/commands/EncoderAutoDrive.java index 55b2a5e..d03174b 100644 --- a/src/main/java/frc/robot/commands/EncoderAutoDrive.java +++ b/src/main/java/frc/robot/commands/EncoderAutoDrive.java @@ -25,7 +25,7 @@ public EncoderAutoDrive(DriveTrain driveTrain, double target, navXSubsystem navX this.target = -target; // Use addRequirements() here to declare subsystem dependencies. addRequirements(driveTrain); - addRequirements(navx); + addRequirements(navX); } // Called when the command is initially scheduled. 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..d93fad8 --- /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/XboxIntake.java b/src/main/java/frc/robot/commands/IntakeCube.java similarity index 66% rename from src/main/java/frc/robot/commands/XboxIntake.java rename to src/main/java/frc/robot/commands/IntakeCube.java index 96993db..1200d02 100644 --- a/src/main/java/frc/robot/commands/XboxIntake.java +++ b/src/main/java/frc/robot/commands/IntakeCube.java @@ -6,37 +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 XboxIntake extends CommandBase { +public class IntakeCube extends CommandBase { /** Creates a new IntakeSwitch. */ private final IntakeSubsystem intakeSubsystem; - private final Supplier rightY_Supplier; - public XboxIntake(IntakeSubsystem intakeSubsystem, Supplier rightY_Supplier) { + private final XboxController xbox2; + public IntakeCube(IntakeSubsystem intakeSubsystem, XboxController xbox2) { // Use addRequirements() here to declare subsystem dependencies. this.intakeSubsystem = intakeSubsystem; - this.rightY_Supplier = rightY_Supplier; + 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() { - intakeSubsystem.intakeSetSpeed(rightY_Supplier.get()); + 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..ef90608 --- /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(), 1); + 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/XboxArm.java b/src/main/java/frc/robot/commands/XboxArm.java index 0815f2f..59ee1f6 100644 --- a/src/main/java/frc/robot/commands/XboxArm.java +++ b/src/main/java/frc/robot/commands/XboxArm.java @@ -13,7 +13,7 @@ public class XboxArm extends CommandBase { /** Creates a new XboxArm. */ private final ArmSubsystem armSubsystem; - private final Supplier y_Supplier; + private Supplier y_Supplier; public XboxArm(Supplier y_supplier, ArmSubsystem armSubsystem) { // Use addRequirements() here to declare subsystem dependencies. this.armSubsystem = armSubsystem; @@ -23,21 +23,15 @@ public XboxArm(Supplier y_supplier, ArmSubsystem armSubsystem) { // Called when the command is initially scheduled. @Override - public void initialize() {} + 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() >= Constants.armBackEncoderLimit && armSubsystem.getArmPosition() <= Constants.armFrontEncoderLimit){ - if (Math.abs(y_Supplier.get()) <= 0.5){ - armSubsystem.armMove(y_Supplier.get()); - } else { - armSubsystem.armMove(0); - } - } - + armSubsystem.armMove(y_Supplier.get()); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index cc3d577..6521b39 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -9,6 +9,7 @@ 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; @@ -40,6 +41,7 @@ public ArmSubsystem() armD = 0; armOutput = 0; prevArmOutput = 0; + armMotor.setNeutralMode(NeutralMode.Coast); } //setters @@ -53,7 +55,6 @@ public void resetArmEncoders(){ } public double getArmPosition(){ - // return armMotor.getEncoder().getPosition(); return armMotor.getSelectedSensorPosition(); } @@ -79,7 +80,6 @@ public void moveArmPID(double sensorInput, double limit){ armI += armError; armD = armError - armPrevError; - armOutput = Constants.kArmP*armP + Constants.kArmI*armI + Constants.kArmD*armD; if(armOutput > limit){ armOutput = limit; @@ -93,6 +93,7 @@ public void moveArmPID(double sensorInput, double limit){ prevArmOutput = armOutput; } + //getters public double getArmMoveOutput(){ return armOutput; } @@ -100,16 +101,11 @@ public double getArmMoveOutput(){ public double getArmMoveError(){ return armError; } - //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. - // } - // } + + //setters + public void setPIDtarget(double target){ + armTarget = target; + } @Override public void periodic() { // This method will be called once per scheduler run From 4e53d70e555668f7d28b0e9d774d0bf956c169f9 Mon Sep 17 00:00:00 2001 From: Rewand727 Date: Wed, 22 Mar 2023 13:26:57 -0700 Subject: [PATCH 37/41] replacement for pid in auton --- .../{autoArmPickup.java => AutoArmMove.java} | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) rename src/main/java/frc/robot/commands/{autoArmPickup.java => AutoArmMove.java} (70%) diff --git a/src/main/java/frc/robot/commands/autoArmPickup.java b/src/main/java/frc/robot/commands/AutoArmMove.java similarity index 70% rename from src/main/java/frc/robot/commands/autoArmPickup.java rename to src/main/java/frc/robot/commands/AutoArmMove.java index b15a89a..c9641df 100644 --- a/src/main/java/frc/robot/commands/autoArmPickup.java +++ b/src/main/java/frc/robot/commands/AutoArmMove.java @@ -8,39 +8,39 @@ import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.DriveTrain; -public class AutoArmPickup extends CommandBase { +public class AutoArmMove extends CommandBase { /** Creates a new autoArmPickup. */ private final ArmSubsystem armSubsystem; - private double target, timeOut; + private double target, timeOut, distanceMove; private int errorCounter, cycleCounter; - public AutoArmPickup(ArmSubsystem armSubsystem, double target, double timeOut) { + public AutoArmMove(ArmSubsystem armSubsystem, double target, double timeOut) { this.armSubsystem = armSubsystem; this.target = target; this.timeOut = timeOut; errorCounter = 0; cycleCounter = 0; + distanceMove = 0; addRequirements(armSubsystem); } // Called when the command is initially scheduled. @Override public void initialize() { - armSubsystem.resetArmPID(); armSubsystem.armMove(0); } // Called every time the scheduler runs while the command is scheduled. @Override + public void execute() { - armSubsystem.moveArmPID(armSubsystem.getArmPosition(), 0.5); - armSubsystem.armMove(armSubsystem.getArmMoveOutput()); - if (Math.abs(armSubsystem.getArmMoveError()) <= 5000){ - errorCounter++; - } else{ - errorCounter = 0; + distanceMove = armSubsystem.getArmPosition() - target; + if(distanceMove > 0){ + armSubsystem.armMove(-.1); + } else if(distanceMove < 0){ + armSubsystem.armMove(.1); } - + cycleCounter++; } // Called once the command ends or is interrupted. @Override @@ -51,7 +51,7 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - if (errorCounter > 10 || cycleCounter > (timeOut/20)){ + if (armSubsystem.getArmPosition() == target || cycleCounter > timeOut/20){ return true; } else { return false; From bfaa45bc679d21b7b8f557b87d411c5f01d813fe Mon Sep 17 00:00:00 2001 From: Alex Nie <93626002+TheSparkyBoy@users.noreply.github.com> Date: Wed, 22 Mar 2023 22:11:43 -0700 Subject: [PATCH 38/41] Before Comp Practice Day --- src/main/java/frc/robot/Constants.java | 10 ++-- src/main/java/frc/robot/RobotContainer.java | 48 ++++++++----------- .../robot/commands/AutonomousSequences.java | 8 ++++ .../java/frc/robot/commands/IntakeCone.java | 2 +- .../commands/PIDAssistedArmMovement.java | 4 +- .../frc/robot/subsystems/ArmSubsystem.java | 1 - 6 files changed, 35 insertions(+), 38 deletions(-) create mode 100644 src/main/java/frc/robot/commands/AutonomousSequences.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8e90eb0..dc7449f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -68,13 +68,13 @@ public final class Constants { public static final int extensionMotorID = 0; //arm constant public static final int armMotorID = 7; - public static final double armFrontEncoderLimit = 40000; - public static final double armBackEncoderLimit = -40000; - public static final double kArmP = 0.0001; - public static final double kArmI = 0.00001; + 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 = 400; + public static final double kArmPID = 30000; //DriveTrain Motor ID's public static final int FRid = 2; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6bb9d7d..7fbdd14 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -30,10 +30,11 @@ public class RobotContainer { public ArmSubsystem armSubsystem = new ArmSubsystem(); public XboxController Xbox = new XboxController(Constants.xBoxControllerid); public XboxController Xbox2 = new XboxController(Constants.xBoxControllerid2); - - // public RasberryPiCamera rasberryPiCamera = new RasberryPiCamera(); - //Intake + public final DriveTrain driveTrain = new DriveTrain(); + public navXSubsystem navX = new navXSubsystem(); 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); @@ -47,14 +48,6 @@ public class RobotContainer { private final JoystickButton RBbuttonX2 = new JoystickButton(Xbox2, Constants.Xbox_Button_RB); private final JoystickButton LBbuttonX2 = new JoystickButton(Xbox2, Constants.Xbox_Button_LB); - // Replace with CommandPS4Controller or CommandJoystick if needed - - public final DriveTrain driveTrain = new DriveTrain(); - public navXSubsystem navX = new navXSubsystem(); - - - - /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { // Configure the trigger bindings @@ -74,14 +67,9 @@ public RobotContainer() { */ private void configureBindings() { // Schedule `ExampleCommand` when `exampleCondition` changes to `true` - AbuttonX2.whenPressed(new ResetEncoders(driveTrain, armSubsystem)); - LBbuttonX2.whenHeld(new IntakeCube(intakeSubsystem, Xbox2)); - RBbuttonX2.whenHeld(new IntakeCone(intakeSubsystem, Xbox2)); - - - - // 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)); } /** @@ -91,16 +79,18 @@ private void configureBindings() { */ public Command getAutonomousCommand() { // An example command will be run in autonomous - // return new AutoBalancing(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 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), 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/IntakeCone.java b/src/main/java/frc/robot/commands/IntakeCone.java index d93fad8..a59e666 100644 --- a/src/main/java/frc/robot/commands/IntakeCone.java +++ b/src/main/java/frc/robot/commands/IntakeCone.java @@ -24,7 +24,7 @@ public IntakeCone(IntakeSubsystem intake, XboxController xbox2) { // Called when the command is initially scheduled. @Override public void initialize() { - intake.intakeSetSpeed(Constants.intakeMotorSpeed); + intake.intakeSetSpeed(-Constants.intakeMotorSpeed); xbox2.setRumble(RumbleType.kRightRumble, 1); } diff --git a/src/main/java/frc/robot/commands/PIDAssistedArmMovement.java b/src/main/java/frc/robot/commands/PIDAssistedArmMovement.java index ef90608..00a33fc 100644 --- a/src/main/java/frc/robot/commands/PIDAssistedArmMovement.java +++ b/src/main/java/frc/robot/commands/PIDAssistedArmMovement.java @@ -36,11 +36,11 @@ public void initialize() { public void execute() { value = xbox.get(); if(Math.abs(value) > 0.04){ - target += value*Constants.kArmPID; + target = value*Constants.kArmPID; } SmartDashboard.putNumber("Arm PID Target:", target); arm.setPIDtarget(target); - arm.moveArmPID(arm.getArmPosition(), 1); + arm.moveArmPID(arm.getArmPosition(), 0.5); if(arm.getArmPosition()Constants.armBackEncoderLimit){ arm.armMove(arm.getArmMoveOutput()); }else{ diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 6521b39..a18e554 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -88,7 +88,6 @@ public void moveArmPID(double sensorInput, double limit){ if(armOutput < -limit){ armOutput = -limit; } - armPrevError = armError; prevArmOutput = armOutput; } From 44d92d7377096042dd9b3a7e5be7757506b77362 Mon Sep 17 00:00:00 2001 From: Rewand727 Date: Thu, 23 Mar 2023 09:06:20 -0700 Subject: [PATCH 39/41] added autointake --- .../java/frc/robot/commands/AutoIntake.java | 54 +++++++++++++++++++ 1 file changed, 54 insertions(+) create mode 100644 src/main/java/frc/robot/commands/AutoIntake.java 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; + } + } +} From 4f280412ba12784f985bf51750d238958a940b84 Mon Sep 17 00:00:00 2001 From: Rewand727 Date: Thu, 23 Mar 2023 09:06:48 -0700 Subject: [PATCH 40/41] autointake --- src/main/java/frc/robot/RobotContainer.java | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 86ea181..83c30e1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -91,13 +91,20 @@ private void configureBindings() { public Command getAutonomousCommand() { // An example command will be run in autonomous // 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 SequentialCommandGroup( - new SetPipeline(LimeLightSubsystem, 0), - new Wait(10000), - new SetPipeline(LimeLightSubsystem, 1), - new Wait(10000), - new SetPipeline(LimeLightSubsystem, 2) - ); + return + + + + ) } // return new SequentialCommandGroup( @@ -127,4 +134,6 @@ public Command getAutonomousCommand() { // return new SequentialCommandGroup( // new testRightLeftMotor(driveTrain) // ); + + } From d410a4163e4bf21d82fe870870129efa00d9e96a Mon Sep 17 00:00:00 2001 From: Rewand727 Date: Thu, 23 Mar 2023 09:37:53 -0700 Subject: [PATCH 41/41] fixed --- src/main/java/frc/robot/commands/XboxArm.java | 8 ++++---- src/main/java/frc/robot/subsystems/ArmSubsystem.java | 8 +++----- 2 files changed, 7 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/commands/XboxArm.java b/src/main/java/frc/robot/commands/XboxArm.java index 90ef6e9..6077477 100644 --- a/src/main/java/frc/robot/commands/XboxArm.java +++ b/src/main/java/frc/robot/commands/XboxArm.java @@ -24,7 +24,7 @@ public XboxArm(Supplier y_supplier, ArmSubsystem armSubsystem) { // Called when the command is initially scheduled. @Override public void initialize() { - armSubsystem.armMove(0, 0); + armSubsystem.armMove(0); armSubsystem.resetArmEncoders(); } @@ -32,9 +32,9 @@ public void initialize() { @Override public void execute() { if (armSubsystem.getArmPosition() > 70000){ - armSubsystem.armMove(0, 0); + armSubsystem.armMove(0); } else { - armSubsystem.armMove(y_Supplier.get(), 0.3); + armSubsystem.armMove(y_Supplier.get()); } @@ -43,7 +43,7 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - armSubsystem.armMove(0, 0); + armSubsystem.armMove(0); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 2413055..35e20e0 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -45,13 +45,11 @@ public ArmSubsystem() } //setters - public void armMove(double speed, double limit){ + public void armMove(double speed){ // armMotor.set(speed); - if (speed > limit){ - armMotor.set(ControlMode.PercentOutput, limit); - } else { + armMotor.set(ControlMode.PercentOutput, speed); - } + } public void resetArmEncoders(){