diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6e097ef..420279a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -57,12 +57,31 @@ public final class Constants { public static final int BACK_RIGHT_MODULE_STEER_MOTOR = 23; public static final int BACK_RIGHT_MODULE_STEER_ENCODER = 3; + public static final int TELESCOPING_MOTOR_ID = 7; + public static final int ARM_ROTATION_MOTOR_ID = 8; + public static final int ELEVATOR_CAN_ID = 9; + public static final int LEFT_LINK_CAN_ID = 10; + public static final int RIGHT_LINK_CAN_ID = 11; + public static final double DRIVE_MOTOR_MIN_VOLTAGE = 0.19; public static final double DRIVE_MOTOR_MAX_VOLTAGE = 0.8; public static final double STEER_MOTOR_MIN_VOLTAGE = 0.02; public static final double STEER_MOTOR_MAX_VOLTAGE = 0.5; - public static final double GEAR_RATIO = 6.75; - public static final double WHEEL_DIAMETER= 4; //inches + // public static final double GEAR_RATIO = 6.75; + public static final double ELEVATOR_SPROCKET_DIAMETER= 1.05; //inches public static final double TICKS_PER_REV = 2048; - public static final double TICKS_PER_INCH = TICKS_PER_REV*GEAR_RATIO/WHEEL_DIAMETER/Math.PI; //talonfx drive encoder + public static final double ELEVATOR_GEAR_RATIO = 25.0; // 25 motor spins for 1 shaft spin + + public static final double TICKS_PER_INCH = (TICKS_PER_REV*ELEVATOR_GEAR_RATIO)/(ELEVATOR_SPROCKET_DIAMETER*Math.PI); //talonfx drive encoder + + public static final int kTimeoutMs = 1000; + public static final int kPIDLoopIdx = 0; //not sure what this value does or if this value should be diff for rotation, telescoping, elevator + + /* Choose so that Talon does not report sensor out of phase */ + public static boolean kSensorPhase = true; //Sensor phase describes the relationship between the motor output direction (positive vs negative) and sensor velocity (positive vs negative). For soft-limits and closed-loop features to function correctly, the sensor measurement and motor output must be “in-phase”. + + //public static double deadband = 0.5; //arbitrary + + public static int topLimitSwitchPort = 2; + public static int bottomLimitSwitchPort = 1; } diff --git a/src/main/java/frc/robot/Gains.java b/src/main/java/frc/robot/Gains.java new file mode 100644 index 0000000..9ecbe2c --- /dev/null +++ b/src/main/java/frc/robot/Gains.java @@ -0,0 +1,22 @@ +/** + * Class that organizes gains used when assigning values to slots + */ +package frc.robot; + +public class Gains { //Gain is a proportional value that shows the relationship between the magnitude of an input signal to the magnitude of an output signal at steady-state. + public final double kP; + public final double kI; + public final double kD; + public final int kIzone; //can be used to auto clear the integral accumulator if the sensor pos is too far from the target. This prevent unstable oscillation if the kI is too large. Value is in sensor units. + public final double kPeakOutput; //Absolute max motor output during closed-loop control modes only. A value of ‘1’ represents full output in both directions. + + + public Gains(double _kP, double _kI, double _kD, int _kIzone, double _kPeakOutput){ + kP = _kP; + kI = _kI; + kD = _kD; + + kIzone = _kIzone; + kPeakOutput = _kPeakOutput; + } +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 405fc6d..d8405be 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,27 +6,15 @@ import edu.wpi.first.wpilibj.TimedRobot; import frc.robot.subsystems.*; -import frc.robot.OI; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.autonomous.*; -import frc.robot.Constants; +import frc.robot.subsystems.ElevatorSubsystem.ElevatorStates; -import com.fasterxml.jackson.core.sym.Name; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.networktables.GenericEntry; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTable.*; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; + /** * The VM is configured to automatically run this class, and to call the functions corresponding to * each mode, as described in the TimedRobot documentation. If you change the name of this class or @@ -34,31 +22,23 @@ * project. */ +/*public class Robot extends TimedRobot { + private static final String kDefaultAuto = "Default"; + private static final String kCustomAuto = "My Auto"; + private String m_autoSelected; + private final SendableChooser m_chooser = new SendableChooser<>(); + //private final AutonomousBase autonomousBasePD = new AutonomousBasePD(new Pose2d(0*Constants.TICKS_PER_INCH, -20*Constants.TICKS_PER_INCH, new Rotation2d(0)), 0.0, new Pose2d(), 0.0); + public static final DrivetrainSubsystem m_drivetrainSubsystem = new DrivetrainSubsystem(); + +*/ //private AutonomousBasePD mScore = new AutonomousBasePD(new Translation2d(222.037, 0), new Translation2d(135.091, -41.307), new Translation2d(0, -44.163), new Translation2d(222.894, -50.377), new Translation2d(0, -65.388), new Translation2d(0, -65.388)); public class Robot extends TimedRobot { - private AutonomousBase m_autoSelected; private final SendableChooser m_chooser = new SendableChooser(); - private AutonomousBasePD noGo = new AutonomousBasePD(new Pose2d(0, 0, new Rotation2d(0)), new Pose2d(0, 0, new Rotation2d(0)), new Pose2d(0, 0, new Rotation2d(0)), new Pose2d(0,0, new Rotation2d(0)), new Pose2d(0, 0, new Rotation2d(0)), new Pose2d(0,0, new Rotation2d(0)), new Pose2d(0,0, new Rotation2d(0))); - private AutonomousBasePD placeNLeave = new AutonomousBasePD(new Pose2d(0, 0, new Rotation2d(0)), new Pose2d(160.0, 0, new Rotation2d(0)), new Pose2d(160.0, 0, new Rotation2d(0)), new Pose2d(160.0, 0, new Rotation2d(0)), new Pose2d(160.0, 0, new Rotation2d(0)), new Pose2d(160.0, 0, new Rotation2d(0)), new Pose2d(160.0, 0, new Rotation2d(0))); - private AutonomousBasePD antiCharge = new AutonomousBasePD(new Pose2d(86.840, -45.282, new Rotation2d(0)), new Pose2d(221.978, 19.463, new Rotation2d(0)), new Pose2d(135.091, -19.421, new Rotation2d(0)), new Pose2d(0, -22.277, new Rotation2d(0)), new Pose2d(222.491, -28.492, new Rotation2d(0)), new Pose2d(0, -43.502, new Rotation2d(0)), new Pose2d(0, -43.502, new Rotation2d(0))); - private AutonomousBasePD antiChargeOpposite = new AutonomousBasePD(new Pose2d(86.840, 45.282, new Rotation2d(0)), new Pose2d(221.978, -19.463, new Rotation2d(0)), new Pose2d(135.091, 19.421, new Rotation2d(0)), new Pose2d(0, 22.277, new Rotation2d(0)), new Pose2d(222.491, 28.492, new Rotation2d(0)), new Pose2d(0, 43.502, new Rotation2d(0)), new Pose2d(0, 43.502, new Rotation2d(0))); - private AutonomousBasePD engageCharge = new AutonomousBasePD(new Pose2d(97.759, 0, new Rotation2d(0)), new Pose2d(97.759, 0, new Rotation2d(0)), new Pose2d(97.759, 0, new Rotation2d(0)), new Pose2d(97.759, 0, new Rotation2d(0)), new Pose2d(97.759, 0, new Rotation2d(0)), new Pose2d(97.759, 0, new Rotation2d(0)), new Pose2d(97.759, 0, new Rotation2d(0))); - private AutonomousBasePD placeTwoEngage = new AutonomousBasePD(new Pose2d(223.014, 16.468, new Rotation2d(0)), new Pose2d(0, 22.683, new Rotation2d(0)), new Pose2d(135.615, 25.539, new Rotation2d(0)), new Pose2d(222.191, 64.230, new Rotation2d(0)), new Pose2d(97.711, 64.230, new Rotation2d(0)), new Pose2d(97.711, 64.230, new Rotation2d(0)), new Pose2d(97.711, 64.230, new Rotation2d(0))); - private AutonomousBaseTimed timedPath = new AutonomousBaseTimed(); - private AutonomousBasePD testPath = new AutonomousBasePD(new Pose2d(0, 20, new Rotation2d(0)), new Pose2d(0, 20, new Rotation2d(0)), new Pose2d(0, 20, new Rotation2d(0)), new Pose2d(0, 20, new Rotation2d(0)), new Pose2d(0, 20, new Rotation2d(0)), new Pose2d(0, 20, new Rotation2d(0)), new Pose2d(0, 20, new Rotation2d(0))); - private AutonomousBaseMP motionProfiling = new AutonomousBaseMP(Trajectories.uno, Trajectories.dos, Trajectories.tres); - - //above coordinates didn't work - private AutonomousBasePD leave = new AutonomousBasePD(new Pose2d(56.069, 17.332, new Rotation2d(0)), new Pose2d(219.915, 17.332, new Rotation2d(0)), new Pose2d(219.915, 17.332, new Rotation2d(0)), new Pose2d(219.915, 17.332, new Rotation2d(0)), new Pose2d(219.915, 17.332, new Rotation2d(0)), new Pose2d(219.915, 17.332, new Rotation2d(0)), new Pose2d(219.915, 17.332, new Rotation2d(0))); - private AutonomousBasePD placeTwoEngageReal = new AutonomousBasePD(new Pose2d(56.069, 17.332, new Rotation2d(0)), new Pose2d(278.999, 37.193, new Rotation2d(0)), new Pose2d(257.650, 64.004, new Rotation2d(0)), new Pose2d(156.859, 83.368, new Rotation2d(0)), new Pose2d(156.859, 83.368, new Rotation2d(0)), new Pose2d(156.859, 83.368, new Rotation2d(0)), new Pose2d(156.859, 83.368, new Rotation2d(0))); - - //these paths score 3 balls without touching the charge station, requires 7 Pose2ds! - private AutonomousBasePD threeUnderChargeStation = new AutonomousBasePD(new Pose2d(56.069, 17.332, new Rotation2d(0)), new Pose2d(278.999, 37.193, new Rotation2d(0)), new Pose2d(56.222, 43.068, new Rotation2d(0)), new Pose2d(197.484,45.934, new Rotation2d(0)), new Pose2d(279.077, 85.622, new Rotation2d(0)), new Pose2d(197.484,40.000, new Rotation2d(0)), new Pose2d(56.154,66.117, new Rotation2d(0))); - private AutonomousBasePD threeAboveChargeStation = new AutonomousBasePD(new Pose2d(56.069, 200.046, new Rotation2d(0)), new Pose2d(278.999, 180.683, new Rotation2d(0)), new Pose2d(56.069, 174.725, new Rotation2d(0)), new Pose2d(207.006, 174.725, new Rotation2d(0)), new Pose2d(278.006, 133.515, new Rotation2d(0)), new Pose2d(200.552, 185.151, new Rotation2d(0)), new Pose2d(57.062, 154.368, new Rotation2d(0))); - - + + public ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem(); + public static final DrivetrainSubsystem m_drivetrainSubsystem = new DrivetrainSubsystem(); //if anything breaks in the future it might be this private final Field2d m_field = new Field2d(); ChassisSpeeds m_ChassisSpeeds; @@ -67,6 +47,10 @@ public class Robot extends TimedRobot { public static GenericEntry kI; public static GenericEntry kD; + public static DrivetrainSubsystem getDrivetrainSubsystem(){ + return m_drivetrainSubsystem; + } + /** * This function is run when the robot is first started up and should be used for any * initialization code. @@ -74,25 +58,23 @@ public class Robot extends TimedRobot { @Override public void robotInit() { //creates options for different autopaths, names are placeholders - System.out.println("#I'm Awake"); - m_chooser.setDefaultOption("Default Auto", testPath); - m_chooser.addOption("My Auto 1", noGo); - m_chooser.addOption("My Auto 2",placeNLeave); - m_chooser.addOption("My Auto 3", antiCharge); - m_chooser.addOption("My Auto 4", antiChargeOpposite); + // System.out.println("#I'm Awake"); + // m_chooser.setDefaultOption("Default Auto", testPath); + // m_chooser.addOption("My Auto 1", noGo); + // m_chooser.addOption("My Auto 2",placeNLeave); + // m_chooser.addOption("My Auto 3", antiCharge); + // m_chooser.addOption("My Auto 4", antiChargeOpposite); //m_chooser.addOption(name: "My Auto 5", engageCharge); - // m_chooser.addOption(name: "My Auto 6",placeTwoEngage); - m_chooser.addOption("My Auto timed", timedPath); - m_chooser.addOption("Motion profiling path", motionProfiling); + //m_chooser.addOption(name: "My Auto 6",placeTwoEngage); + //m_chooser.addOption("My Auto timed", timedPath); + //m_chooser.addOption("Motion profiling path", motionProfiling); SmartDashboard.putData("Auto choices", m_chooser); - ShuffleboardTab tab = DrivetrainSubsystem.tab; - kP = tab.add("Auto kP", 0.1).getEntry(); - kI = tab.add("Auto kI", 0.0).getEntry(); - kD = tab.add("Auto kD", 0.0).getEntry(); - - + // ShuffleboardTab tab = DrivetrainSubsystem.tab; + // kP = tab.add("Auto kP", 0.1).getEntry(); + // kI = tab.add("Auto kI", 0.0).getEntry(); + // kD = tab.add("Auto kD", 0.0).getEntry(); } /** @@ -121,18 +103,25 @@ public void robotPeriodic() { */ @Override public void autonomousInit() { - m_autoSelected = m_chooser.getSelected(); - m_autoSelected.init(); + + //autonomousBasePD.init(); + + elevatorSubsystem.init(); + // m_autoSelected = m_chooser.getSelected(); + // m_autoSelected = SmartDashboard.getString("Auto Selector", kDefaultAuto); + // System.out.println("Auto selected: " + m_autoSelected); + } /** This function is called periodically during autonomous. */ @Override public void autonomousPeriodic() { - m_autoSelected.periodic(); - m_drivetrainSubsystem.drive(); - //System.out.println("Odometry: "+ DrivetrainSubsystem.m_odometry.getPoseMeters()); - + + //autonomousBasePD.periodic(); + elevatorSubsystem.setState(ElevatorStates.SHELF_ELEVATOR_HEIGHT); + elevatorSubsystem.periodic(); + //m_drivetrainSubsystem.drive(); } /** This function is called once when teleop is enabled. */ @@ -168,17 +157,23 @@ public void disabledPeriodic() {} /** This function is called once when test mode is enabled. */ @Override public void testInit() { - System.out.println("Trajectory: " + Trajectories.uno); + ElevatorSubsystem.elevatorMotor.setSelectedSensorPosition(0, Constants.kPIDLoopIdx, Constants.kTimeoutMs); + //m_drivetrainSubsystem.m_pose = new Pose2d(20, 30, new Rotation2d(Math.PI/4)); + //System.out.println("m_pose: " + m_drivetrainSubsystem.m_pose); + //autonomousBasePD.init(); } /** This function is called periodically during test mode. */ @Override public void testPeriodic() { + //elevatorSubsystem.setState(ElevatorStates.ZERO); + //elevatorSubsystem.periodic(); + /*intial test!*/ - m_drivetrainSubsystem.setSpeed(new ChassisSpeeds(-0.2, -0.2, 0)); + // m_drivetrainSubsystem.setSpeed(new ChassisSpeeds(-0.2, -0.2, 0)); - m_drivetrainSubsystem.drive(); + // m_drivetrainSubsystem.drive(); } /** This function is called once when the robot is first started up. */ @@ -188,4 +183,4 @@ public void simulationInit() {} /** This function is called periodically whilst in simulation. */ @Override public void simulationPeriodic() {} -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/autonomous/AutonomousBaseMP.java b/src/main/java/frc/robot/autonomous/AutonomousBaseMP.java deleted file mode 100644 index dd33ab2..0000000 --- a/src/main/java/frc/robot/autonomous/AutonomousBaseMP.java +++ /dev/null @@ -1,146 +0,0 @@ -package frc.robot.autonomous; - -import java.util.ArrayList; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.math.controller.HolonomicDriveController; -import edu.wpi.first.math.geometry.*; -import frc.robot.Robot; -import frc.robot.subsystems.DrivetrainSubsystem; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.trajectory.constraint.SwerveDriveKinematicsConstraint; -import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint; -import edu.wpi.first.math.trajectory.constraint.MaxVelocityConstraint; - -public class AutonomousBaseMP extends AutonomousBase{ - private double timeStart; - private double timeElapsed = 0; - private Trajectory trajectory1; - private Trajectory trajectory2; - private Trajectory trajectory3; - private Trajectory.State end; - private HolonomicDriveController controller = new HolonomicDriveController( - new PIDController(1, 0, 0), new PIDController(1, 0, 0), //TODO: CHANGE KP - new ProfiledPIDController(1, 0, 0, - new TrapezoidProfile.Constraints(6.28, Math.PI))); - // trapezoid profile takes in max rotation velocity and max rotation acceleration - // PIDController #1: the first arg rep how many m/s added in the x direction for every meter of error in the x direction - // PIDController #2 : the first arg rep how many m/s added in the y direction for every meter of error in the y direction - - private static DrivetrainSubsystem drivetrainSubsystem = Robot.m_drivetrainSubsystem; - - public AutonomousBaseMP(Trajectory trajectory1, Trajectory trajectory2, Trajectory trajectory3){ - this.trajectory1 = trajectory1; - this.trajectory2 = trajectory2; - this.trajectory3 = trajectory3; - } - - @Override - public void init(){ - timeStart = System.currentTimeMillis(); - timeElapsed = 0; - - double timeCheck = trajectory1.getTotalTimeSeconds(); - end = trajectory1.sample(timeCheck); - System.out.println("total time: " + timeCheck); - } - - @Override - public void periodic(){ - if (doing == Doing.FIRST){ - timeStart = System.currentTimeMillis(); - setDoing(Doing.TRAJECTORY1); - } else if (doing == Doing.TRAJECTORY1){ - followTrajectory(trajectory1); - if (trajectoryDone(trajectory1)){ - System.out.println("STOP"); - setDoing(Doing.STOP); - } - } else if (doing == Doing.TRAJECTORY2){ - followTrajectory(trajectory2); - if (trajectoryDone(trajectory2)){ - setDoing(Doing.TRAJECTORY3); - } - } else if (doing == Doing.TRAJECTORY3){ - followTrajectory(trajectory3); - if (trajectoryDone(trajectory3)){ - setDoing(Doing.STOP); - } - } else if (doing == Doing.PLACEHIGH){ - - } else if (doing == Doing.PICKUP){ - - } else if (doing == Doing.BALANCE){ - - } else { - drivetrainSubsystem.stopDrive(); - } - - timeElapsed = System.currentTimeMillis() - timeStart; - } - - public boolean trajectoryDone(Trajectory trajectory){ - - if(Math.abs(end.poseMeters.getX() - DrivetrainSubsystem.m_pose.getX()) < 0.5 /*/&& - Math.abs(end.poseMeters.getY() - DrivetrainSubsystem.m_pose.getY()) < 2 && - Math.abs(end.poseMeters.getRotation().getDegrees() - DrivetrainSubsystem.m_pose.getRotation().getDegrees()) < 2 */){ - return true; - } - return false; - } - - public static enum Doing{ - TRAJECTORY1, - TRAJECTORY2, - TRAJECTORY3, - PLACEHIGH, - PICKUP, - BALANCE, - STOP, - FIRST; - } - public Doing doing = Doing.FIRST; - - public void setDoing(Doing newDoing){ - doing = newDoing; - } - - public static Trajectory generateTrajectory(Pose2d starting, Pose2d ending, Translation2d interior1, Translation2d interior2, Translation2d interior3){ - ArrayList interiorWaypoints = new ArrayList(); - interiorWaypoints.add(interior1); - interiorWaypoints.add(interior2); - interiorWaypoints.add(interior3); - - SwerveDriveKinematicsConstraint swerveDriveKinematicsConstraint = new SwerveDriveKinematicsConstraint(drivetrainSubsystem.m_kinematics, DrivetrainSubsystem.MAX_VELOCITY_METERS_PER_SECOND*.75); - MaxVelocityConstraint maxVelocityConstraint = new MaxVelocityConstraint(DrivetrainSubsystem.MAX_VELOCITY_METERS_PER_SECOND*.75); - - TrajectoryConfig config = new TrajectoryConfig(DrivetrainSubsystem.MAX_VELOCITY_METERS_PER_SECOND*0.75, 1); //4.96, 2.8 //we should maybe look into this further - config.addConstraint(swerveDriveKinematicsConstraint); - config.addConstraint(maxVelocityConstraint); - - Trajectory trajectory = TrajectoryGenerator.generateTrajectory( - starting, - interiorWaypoints, - ending, - config - ); - return trajectory; - } - - public void followTrajectory(Trajectory trajectory){ - Trajectory.State goal = trajectory.sample(timeElapsed/1000); - - ChassisSpeeds adjustedSpeeds = controller.calculate( - DrivetrainSubsystem.m_pose, goal, Rotation2d.fromDegrees(0)); - - drivetrainSubsystem.setSpeed(adjustedSpeeds); - System.out.println("Actual time elapsed: " + timeElapsed/1000 + "\n" + "Speed: " + adjustedSpeeds.vxMetersPerSecond + ", " + adjustedSpeeds.vyMetersPerSecond + ", " + adjustedSpeeds.omegaRadiansPerSecond + " Goal endpoint: " + goal); - } - - - -} \ No newline at end of file diff --git a/src/main/java/frc/robot/autonomous/AutonomousBasePD.java b/src/main/java/frc/robot/autonomous/AutonomousBasePD.java index dc276b7..e43ecb6 100644 --- a/src/main/java/frc/robot/autonomous/AutonomousBasePD.java +++ b/src/main/java/frc/robot/autonomous/AutonomousBasePD.java @@ -54,8 +54,8 @@ public void init(){ directionController.setTolerance(TURN_DEADBAND); distanceController.setTolerance(DRIVE_DEADBAND*Constants.TICKS_PER_INCH); states = States.FIRST; - System.out.println("INIT!\nINIT!\nINIT!"); - + + System.out.println("auto init!"); } public static enum States{ diff --git a/src/main/java/frc/robot/autonomous/Trajectories.java b/src/main/java/frc/robot/autonomous/Trajectories.java index 83a8e74..062578b 100644 --- a/src/main/java/frc/robot/autonomous/Trajectories.java +++ b/src/main/java/frc/robot/autonomous/Trajectories.java @@ -10,29 +10,29 @@ //IN METERS! public class Trajectories{ - public static Trajectory uno = AutonomousBaseMP.generateTrajectory( - new Pose2d(), - new Pose2d(2.0, 0.0, new Rotation2d(0)), - new Translation2d(1.0, 0.0), - new Translation2d(1.2, 0.0), - new Translation2d(1.4, 0.0) - ); + // public static Trajectory uno = AutonomousBaseMP.generateTrajectory( + // new Pose2d(), + // new Pose2d(2.0, 0.0, new Rotation2d(0)), + // new Translation2d(1.0, 0.0), + // new Translation2d(1.2, 0.0), + // new Translation2d(1.4, 0.0) + // ); - public static Trajectory dos = AutonomousBaseMP.generateTrajectory( - new Pose2d(), - new Pose2d(1.5, 2.3, new Rotation2d(0)), - new Translation2d(.2, .5), - new Translation2d(.9, 1.5), - new Translation2d(1.5, 2.0) - ); + // public static Trajectory dos = AutonomousBaseMP.generateTrajectory( + // new Pose2d(), + // new Pose2d(1.5, 2.3, new Rotation2d(0)), + // new Translation2d(.2, .5), + // new Translation2d(.9, 1.5), + // new Translation2d(1.5, 2.0) + // ); - public static Trajectory tres = AutonomousBaseMP.generateTrajectory( - new Pose2d(), - new Pose2d(2.0, -1.0, new Rotation2d(0)), - new Translation2d(.2, -.4), - new Translation2d(.9, -.6), - new Translation2d(1.5, -.8) - ); + // public static Trajectory tres = AutonomousBaseMP.generateTrajectory( + // new Pose2d(), + // new Pose2d(2.0, -1.0, new Rotation2d(0)), + // new Translation2d(.2, -.4), + // new Translation2d(.9, -.6), + // new Translation2d(1.5, -.8) + // ); } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/AprilTagSubsystem.java b/src/main/java/frc/robot/subsystems/AprilTagSubsystem.java new file mode 100644 index 0000000..b55594b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/AprilTagSubsystem.java @@ -0,0 +1,163 @@ +package frc.robot.subsystems; +import org.opencv.core.Mat; +import org.opencv.imgproc.Imgproc; + +import edu.wpi.first.apriltag.AprilTagDetection; +import edu.wpi.first.apriltag.AprilTagDetector; +import edu.wpi.first.apriltag.AprilTagPoseEstimator; + +import edu.wpi.first.apriltag.AprilTagPoseEstimator.Config; +import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.cscore.CvSink; +import edu.wpi.first.cscore.UsbCamera; +import edu.wpi.first.cscore.VideoSink; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.autonomous.AutonomousBasePD; + +//import frc.robot.subsystems.AprilTagFieldEnum; + +public class AprilTagSubsystem { + + + //System.setProperty("java.library.path","C:\\Users\\Gatorbotics\\Downloads\\opencv\\build\\java\\x64") + //System.load("C:\\Users\\Gatorbotics\\Downloads\\opencv\\build\\java\\x64"); + + /*public enum AprilTagFields { + k2023ChargedUp("2023-chargedup.json"); + }*/ + + public static UsbCamera camera0; + //public static VideoSink sink; + private String family = "tag16h5"; + Mat source; + Mat grayMat; + CvSink cvSink; + AprilTagDetection[] detectedAprilTagsArray; + AprilTagDetection detectedAprilTag; + VideoSink server; + /*int aprilTagIds[]; + + float decisionMargin[]; + double centerX[]; + double centerY[];*/ + double drivetrainXPosition; + //Path path = Filesystem.getDefault().getPath("2023-chargedup.json"); + //public final String m_resourceFile; + + //CvSource outputStream; + + private static double tagSize = 4.0; //change to actual size(inches) + private static double fx;//need to add value + private static double fy;//need to add value + private static double cx;//need to add value + private static double cy;//need to add value + private static SwerveDrivePoseEstimator swerveDrivePoseEstimator = new SwerveDrivePoseEstimator(null, null, null, null); //what arguments go here? + + public static enum AprilTagSequence{ + DETECT, + CORRECTPOSITION; + } + + private static AprilTagSequence states = AprilTagSequence.DETECT; + + public void setState(AprilTagSequence newState){ + states = newState; + } + + private static AprilTagDetector aprilTagDetector = new AprilTagDetector(); + + //private static AutonomousBasePD autonomousBasePD = new AutonomousBasePD(); + + public final AprilTagPoseEstimator.Config aprilTagPoseEstimatorConfig = new Config(tagSize, fx, fy, cx, cy); + public AprilTagPoseEstimator aprilTagPoseEstimator = new AprilTagPoseEstimator(aprilTagPoseEstimatorConfig); + //AprilTagFieldLayout aprilTagFieldLayout = new AprilTagFieldLayout("src/main/deploy/2023-chargedup.json"); + //AprilTagFieldLayout secondAprilTagFieldLayout = new AprilTagFieldLayout(AprilTagFieldLayout.loadFromResource(AprilTagFields.k2023ChargedUp.m_resourceFile)); + //AprilTagFieldLayout thirdAprilTagFieldLayout = new AprilTagFieldLayout("/edu/wpi/first/apriltag/2023-chargedup.json"); + //Path trajectoryPath = Filesystem.getDeployDirectory().toPath().resolve(trajectory); + + public void init(){ + System.loadLibrary("opencv_java460"); + //camera0 = new UsbCamera("USB Camera 0", 0); + aprilTagDetector.addFamily(family, 0); //added 0 + camera0 = CameraServer.startAutomaticCapture(); //deleted 0 + server = CameraServer.getServer(); + //sink = CameraServer.getServer(); + source = new Mat(); + grayMat = new Mat(); + + + System.out.println("source from init: " + source); + //source.release(); + cvSink = CameraServer.getVideo(); + //outputStream = CameraServer.putVideo("camera stream", 320, 240); + } + + + //kaylin wrote this but probably did it wrong :) + // public void addVisionToOdometry(){ + // Transform3d aprilTagError = aprilTagPoseEstimator.estimate(detectedAprilTag);//april tag pose estimator in Transform 3d + // Pose2d aprilTagPose2D = AprilTagLocation.aprilTagPoses[detectedAprilTag.getId()-1].toPose2d();//pose 2d of the actual april tag + // Rotation2d robotSubtractedAngle = Rotation2d.fromDegrees(aprilTagPose2D.getRotation().getDegrees()-aprilTagError.getRotation().toRotation2d().getDegrees());//angle needed to create pose 2d of robot position, don't know if toRotatation2D converts Rotation3D properly + // Pose2d robotPost2DAprilTag = new Pose2d(aprilTagPose2D.getX()-aprilTagError.getX(), aprilTagPose2D.getY()-aprilTagError.getY(), robotSubtractedAngle); + // swerveDrivePoseEstimator.addVisionMeasurement(robotPost2DAprilTag, Timer.getFPGATimestamp()); + // } + + public void periodic(){ + if(states == AprilTagSequence.DETECT){ + detectTag(); + setState(AprilTagSequence.CORRECTPOSITION); + }else if(states == AprilTagSequence.CORRECTPOSITION){ + // correctPosition(); + } + } + + private void detectTag(){ + long time = cvSink.grabFrame(source); + if(time ==0){ + System.out.println("failed to grab a frame"); + return; + } + Imgproc.cvtColor(source, grayMat,Imgproc.COLOR_BGR2GRAY); + System.out.println("value of source in periodic: " + source); + System.out.println(grayMat.empty()); + System.out.println(grayMat.width()); + System.out.println(grayMat.height()); + //outputStream.putFrame(source); + + detectedAprilTagsArray = aprilTagDetector.detect(grayMat); + detectedAprilTag = detectedAprilTagsArray[0]; + + + /*for(int i = 0; i < detectedAprilTags.length; i++){ + aprilTagIds[i] = detectedAprilTags[i].getId(); + decisionMargin[i] = detectedAprilTags[i].getDecisionMargin(); + centerX[i] = detectedAprilTags[i].getCenterX(); + centerY[i] = detectedAprilTags[i].getCenterY(); + }*/ + + + + System.out.println("Detected Apriltag: " + detectedAprilTag); + + } + + + // private void correctPosition(){ + // Pose2d prePose = autonomousBasePD.preDDD(DrivetrainSubsystem.m_pose, AprilTagLocation.aprilTagPoses[detectedAprilTag.getId()].toPose2d()); + // autonomousBasePD.driveDesiredDistance(prePose); + // } + + + /*private void getError(){ + drivetrainXPosition = Robot.m_drivetrainSubsystem.m_pose.getX(); + for(int i = 0; i < aprilTagIds.length; i++){ + XError[i] = AprilTagLocation.aprilTagPoses[aprilTagIds[i]].getX() - drivetrainXPosition; + + //AprilTagLocation + } + }*/ +} diff --git a/src/main/java/frc/robot/subsystems/ArmTelescopingSubsystem.java b/src/main/java/frc/robot/subsystems/ArmTelescopingSubsystem.java new file mode 100644 index 0000000..6a9dc27 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ArmTelescopingSubsystem.java @@ -0,0 +1,141 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.can.TalonFX; + +import edu.wpi.first.math.controller.PIDController; +import frc.robot.Constants; +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.NeutralMode; + +import frc.robot.Gains; + +public class ArmTelescopingSubsystem { + + //PIDController armLengthController = new PIDController(armLengthKP, armLengthKI, armLengthKD); + public static TelescopingStates tState = TelescopingStates.RETRACTED;//should this be retracted or mid? what is the equivalent to off? + + public TalonFX telescopingMotor = new TalonFX(Constants.TELESCOPING_MOTOR_ID); + private double startTime; + private double desiredInches; + private double desiredTicks; + public double tareEncoder; + //telescopingMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, Constants.kPIDLoopIdx, Constants.kTimeoutMs);//determine what these values would be for us + + public int _kIzone = 0; + public double _kPeakOutput = 1.0; + + double telescopeKP = 0.05; + double telescopeKD = 0; + double telescopeKI = 0; + private int deadband = 25000; + //public PIDController telescopeController = new PIDController(telescopeKP, telescopeKI, telescopeKD) + //public Gains telescopeGains = new Gains(telescopeKP, telescopeKI, telescopeKD, _kIzone, _kPeakOutput); + + public static enum TelescopingStates{ + RETRACTED, //zero + LOW_ARM_LENGTH, + SHELF_ARM_LENGTH, + MID_ARM_LENGTH, + HIGH_ARM_LENGTH; + } + + public void init(){ + System.out.println("telescoping init!! :)"); + telescopingMotor.setInverted(false); //forward = clockwise, changed on 2/9 + telescopingMotor.setNeutralMode(NeutralMode.Brake); + + //configuring deadband + telescopingMotor.configAllowableClosedloopError(0, Constants.kPIDLoopIdx, Constants.kTimeoutMs); + telescopingMotor.config_kP(0, telescopeKP); + telescopingMotor.config_kI(0, telescopeKI); + telescopingMotor.config_kP(0, telescopeKD); + } + + public void periodic(){//sam requests that we can operate arm length by stick on xbox + //telescopingMotor.set(ControlMode.PercentOutput, 0.2); + System.out.println("current telescoping arm motor position:" + telescopingMotor.getSelectedSensorPosition()); + if (tState == TelescopingStates.RETRACTED){ + telescopingMotor.set(ControlMode.Position, 0); + desiredInches = 0; + desiredTicks = 0; + telescopeDeadband(); + } else if (tState == TelescopingStates.LOW_ARM_LENGTH){ + desiredInches = 3; //official 2/13 + // determineRightTicks(); + System.out.println("desired ticks: " + desiredTicks); + telescopingMotor.set(ControlMode.Position, desiredTicks); + System.out.println("error: " + (desiredTicks - telescopingMotor.getSelectedSensorPosition())); + telescopeDeadband(); + System.out.println(telescopingMotor.getClosedLoopError()); + System.out.println(telescopingMotor.getMotorOutputPercent()); + System.out.println(telescopingMotor.getClosedLoopTarget()); + + } else if (tState == TelescopingStates.SHELF_ARM_LENGTH){ + desiredInches = 6; //official 2/13 + // determineRightTicks(); + telescopingMotor.set(ControlMode.Position, desiredTicks-tareEncoder); // goes with 90 degrees rotation + System.out.println("error: " + (desiredTicks - telescopingMotor.getSelectedSensorPosition())); + telescopeDeadband(); + }else if (tState == TelescopingStates.MID_ARM_LENGTH){ + desiredInches = 23; //official 2/13 + // determineRightTicks(); + telescopingMotor.set(ControlMode.Position, desiredTicks-tareEncoder); // goes with 90 degrees rotation + System.out.println("error: " + (desiredTicks - telescopingMotor.getSelectedSensorPosition())); + telescopeDeadband(); + }else{ //high arm length + desiredInches = 32; //official 2/13 + // determineRightTicks(); + telescopingMotor.set(ControlMode.Position, desiredTicks-tareEncoder); + telescopeDeadband(); + } + } + + // public void determineRightTicks(){ + // if (telescopingMotor.getSelectedSensorPosition() < 2 * Constants.UNDER_TWO_TICKS_PER_INCH){ + // desiredTicks = 2 * Constants.UNDER_TWO_TICKS_PER_INCH; + // } else{ + // desiredTicks = (desiredInches-2) * Constants.OVER_TWO_TICKS_PER_INCH; + // } + // } + public void telescopeDeadband(){ + if (Math.abs(desiredTicks - telescopingMotor.getSelectedSensorPosition()) < deadband){ + telescopingMotor.set(ControlMode.PercentOutput, 0); + System.out.println("STOPPED"); + } + } + + public double getArmPosition(){ + return telescopingMotor.getSelectedSensorPosition(); + } + + public void timedMoveArm(double time, boolean forwards){ //time in seconds + System.out.println("moving arm >:)"); + System.out.println("start time: " + startTime); + double milliTime = time * 1000; + System.out.println("milli time: " + milliTime); + if(forwards == true){ + while(System.currentTimeMillis() - startTime <= milliTime){ + telescopingMotor.set(ControlMode.PercentOutput,0.2); + } + telescopingMotor.set(ControlMode.PercentOutput,0); + }else{ + while(System.currentTimeMillis() - startTime <= milliTime){ + telescopingMotor.set(ControlMode.PercentOutput,-0.2); + } + telescopingMotor.set(ControlMode.PercentOutput,0); + } + } + + public void setTState(TelescopingStates newState){ + tState = newState; + } + + public void tareEncoder() { + tareEncoder = telescopingMotor.getSelectedSensorPosition(); + + } + public double getTicks() { + return telescopingMotor.getSelectedSensorPosition() - tareEncoder; + } + +} diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index 9b8295a..b11f587 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -25,6 +25,8 @@ import static frc.robot.Constants.*; import frc.robot.OI; +import frc.robot.subsystems.ArmTelescopingSubsystem.TelescopingStates; +import frc.robot.subsystems.ElevatorSubsystem.ElevatorStates; public class DrivetrainSubsystem { @@ -91,6 +93,8 @@ public class DrivetrainSubsystem { //ChassisSpeeds takes in y velocity, x velocity, speed of rotation private ChassisSpeeds m_chassisSpeeds = new ChassisSpeeds(0.0, 0.0, 0.0); + private ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem(); + private ArmTelescopingSubsystem armTelescopingSubsystem = new ArmTelescopingSubsystem(); public DrivetrainSubsystem() { @@ -230,6 +234,8 @@ public void setSpeed(ChassisSpeeds chassisSpeeds) { m_chassisSpeeds = chassisSpeeds; } + + // public Pose2d getCurrentPose(){ // return m_pose; // } @@ -244,6 +250,16 @@ public void driveTeleop(){ DoubleSupplier m_rotationSupplier = () -> -modifyAxis(OI.m_controller.getRightX()) * DrivetrainSubsystem.MAX_ANGULAR_VELOCITY_RADIANS_PER_SECOND; //setting speed + if (elevatorSubsystem.elevatorState == ElevatorStates.HIGH_ELEVATOR_HEIGHT){ + setSpeed( + ChassisSpeeds.fromFieldRelativeSpeeds( + m_translationXSupplier.getAsDouble(), + m_translationYSupplier.getAsDouble(), + m_rotationSupplier.getAsDouble(), + getGyroscopeRotation() + ) + ); + } setSpeed( ChassisSpeeds.fromFieldRelativeSpeeds( m_translationXSupplier.getAsDouble(), @@ -307,6 +323,22 @@ public double getMedian(double[] num){ } } + public void scoreLow(){ + elevatorSubsystem.setState(ElevatorStates.LOW_ELEVATOR_HEIGHT); + armTelescopingSubsystem.setTState(TelescopingStates.LOW_ARM_LENGTH); + } + + public void scoreMid(){ + elevatorSubsystem.setState(ElevatorStates.MID_ELEVATOR_HEIGHT); + armTelescopingSubsystem.setTState(TelescopingStates.MID_ARM_LENGTH); + } + + public void scoreHigh(){ + elevatorSubsystem.setState(ElevatorStates.HIGH_ELEVATOR_HEIGHT); + armTelescopingSubsystem.setTState(TelescopingStates.HIGH_ARM_LENGTH); + } + + //AUTO AND FAILSAFE public void stopDrive() { setSpeed(new ChassisSpeeds(0.0, 0.0, 0.0)); diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java new file mode 100644 index 0000000..0fb4a4c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -0,0 +1,77 @@ +package frc.robot.subsystems; +import com.ctre.phoenix.motorcontrol.can.TalonFX; +import com.ctre.phoenix.motorcontrol.NeutralMode; +import frc.robot.Constants; +import frc.robot.Gains; +import edu.wpi.first.wpilibj.DigitalInput; + +import com.ctre.phoenix.motorcontrol.ControlMode; + +//NOTES ON MEASUREMENTS +//22.5inches on inside of metal frame that chain moves in +//10 in on moving mechanism thing + +public class ElevatorSubsystem { + + public double _kP = 0.05; + public double _kI = 0.0; + public double _kD = 0.0; + public int _kIzone = 0; + public double _kPeakOutput = 1.0; + + public static TalonFX elevatorMotor = new TalonFX(Constants.ELEVATOR_CAN_ID); + public static ElevatorStates elevatorState = ElevatorStates.LOW_ELEVATOR_HEIGHT; + + // DigitalInput top_limit_switch = new DigitalInput(Constants.topLimitSwitchPort); + // DigitalInput bottom_limit_switch = new DigitalInput(Constants.bottomLimitSwitchPort); + + public Gains elevatorGains = new Gains(_kP, _kI, _kD, _kIzone, _kPeakOutput); + + public static enum ElevatorStates{ + ZERO, + LOW_ELEVATOR_HEIGHT, + SHELF_ELEVATOR_HEIGHT, + MID_ELEVATOR_HEIGHT, + HIGH_ELEVATOR_HEIGHT; + } + + public void init(){ + System.out.println("elevator init!!!!"); + elevatorMotor.setInverted(false); // looking from the front of the robot, clockwise is false (: + elevatorMotor.setNeutralMode(NeutralMode.Brake); + + //configuring deadband + elevatorMotor.configAllowableClosedloopError(0, Constants.kPIDLoopIdx, Constants.kTimeoutMs); + /* Config Position Closed Loop gains in slot0, typically kF stays zero. */ + elevatorMotor.config_kP(Constants.kPIDLoopIdx, elevatorGains.kP, Constants.kTimeoutMs); + elevatorMotor.config_kI(Constants.kPIDLoopIdx, elevatorGains.kI, Constants.kTimeoutMs); + elevatorMotor.config_kD(Constants.kPIDLoopIdx, elevatorGains.kD, Constants.kTimeoutMs); + } + + public void periodic(){//still need to scale down the values or figure out why it is overshooting + System.out.println("elevator periodic!!!!!!!"); + if (elevatorState == ElevatorStates.ZERO){ //emergency stop + elevatorMotor.set(ControlMode.Position, 0); + } else if (elevatorState == ElevatorStates.LOW_ELEVATOR_HEIGHT){ + elevatorMotor.set(ControlMode.Position, 5 * Constants.TICKS_PER_INCH); //official 2/13 is 5 + } else if (elevatorState == ElevatorStates.SHELF_ELEVATOR_HEIGHT) { + elevatorMotor.set(ControlMode.Position, 30 * Constants.TICKS_PER_INCH); //oficial 2/13 is 39 + System.out.println("desired ticks: " + 30*Constants.TICKS_PER_INCH); + System.out.println("current ticks: " + elevatorMotor.getSelectedSensorPosition()); + System.out.println("error: " + (30*Constants.TICKS_PER_INCH - elevatorMotor.getSelectedSensorPosition())); + } else if (elevatorState == ElevatorStates.MID_ELEVATOR_HEIGHT){ + elevatorMotor.set(ControlMode.Position, 40 * Constants.TICKS_PER_INCH); //official 2/13 + } else { //high elevator height + elevatorMotor.set(ControlMode.Position, 48 * Constants.TICKS_PER_INCH); //change value once we know robot dimensions + } + + // if(top_limit_switch.get() || bottom_limit_switch.get()){ + // setState(ElevatorStates.STOP); + // System.out.println("Top/bottom limit switch triggered"); + // } + } + + public void setState(ElevatorStates newElevatorState){ + elevatorState = newElevatorState; + } +}