diff --git a/autonomous.chor b/autonomous.chor index 50430b1..beb797d 100644 --- a/autonomous.chor +++ b/autonomous.chor @@ -3,8 +3,8 @@ "robotConfiguration": { "mass": 37.04398850154597, "rotationalInertia": 3, - "motorMaxTorque": 0.18121546961325966, - "motorMaxVelocity": 4704, + "motorMaxTorque": 0.7248618784530386, + "motorMaxVelocity": 2822, "gearing": 8.14, "wheelbase": 0.6667496399551944, "trackWidth": 0.6667496399551944, @@ -22,11 +22,11 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 12 + "controlIntervalCount": 13 }, { "x": 0, - "y": 1, + "y": 2, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -39,118 +39,127 @@ "x": 0, "y": 0, "heading": 0, - "angularVelocity": -7.600376309198775e-18, - "velocityX": 9.47848126641677e-34, - "velocityY": 2.3685455382964595, + "angularVelocity": -3.947033998189832e-17, + "velocityX": -3.612854008750732e-31, + "velocityY": 1.2005140973885657, "timestamp": 0 }, { - "x": 3.444002287789685e-36, - "y": 0.16666663815137714, - "heading": -2.2326890701092606e-22, - "angularVelocity": -3.2118068414505653e-21, - "velocityX": 4.9543256977745e-35, - "velocityY": 2.3975617978327155, - "timestamp": 0.0695150541279211 + "x": 1.1944785629958918e-34, + "y": 0.17191154064183406, + "heading": 8.946622442095673e-18, + "angularVelocity": 9.597955586605163e-17, + "velocityX": 1.2814391555427755e-33, + "velocityY": 1.8442706648059815, + "timestamp": 0.0932138345647434 }, { - "x": 1.80940340937385e-36, - "y": 0.31818178117808316, - "heading": -8.171022190596369e-22, - "angularVelocity": -8.542513749172842e-21, - "velocityX": -2.3514315537398912e-35, - "velocityY": 2.1796018851958148, - "timestamp": 0.1390301082558422 + "x": 3.0218756793156384e-34, + "y": 0.34382308128367217, + "heading": -1.6139197453297265e-18, + "angularVelocity": -1.132937212242932e-16, + "velocityX": 1.960435468763232e-33, + "velocityY": 1.8442706648060243, + "timestamp": 0.1864276691294868 }, { - "x": 5.9379803896552905e-37, - "y": 0.4545454170640189, - "heading": -1.0153633602375655e-21, - "angularVelocity": -2.8520602469919215e-21, - "velocityX": -1.7486936861866492e-35, - "velocityY": 1.9616417997028441, - "timestamp": 0.2085451623837633 + "x": 5.424652677084512e-34, + "y": 0.5157346219255102, + "heading": -1.5884909113524752e-17, + "angularVelocity": -1.5309947750602212e-16, + "velocityX": 2.577704286588845e-33, + "velocityY": 1.8442706648060245, + "timestamp": 0.2796415036942302 }, { - "x": 9.346108964575416e-37, - "y": 0.575757541803813, - "heading": -9.823771839587742e-22, - "angularVelocity": 4.745184644957e-22, - "velocityX": 4.902720218198206e-36, - "velocityY": 1.7436816565911075, - "timestamp": 0.2780602165116844 + "x": 8.747257947587387e-34, + "y": 0.6876461625673483, + "heading": 1.7441665329815146e-18, + "angularVelocity": 1.8912509853096619e-16, + "velocityX": 3.564497995151033e-33, + "velocityY": 1.8442706648060245, + "timestamp": 0.3728553382589736 }, { - "x": 2.094062831073701e-36, - "y": 0.6818181533947788, - "heading": -8.40480447256135e-22, - "angularVelocity": 2.0412375100710908e-21, - "velocityX": 1.6679149082686126e-35, - "velocityY": 1.5257214846699805, - "timestamp": 0.34757527063960547 + "x": 9.332526707530359e-34, + "y": 0.8595577032091865, + "heading": 4.4302423015963514e-18, + "angularVelocity": 2.881627798231134e-17, + "velocityX": 6.278775686801844e-34, + "velocityY": 1.8442706648060245, + "timestamp": 0.466069172823717 }, { - "x": 4.0842527693840064e-36, - "y": 0.7727272506353046, - "heading": -6.555813623569812e-22, - "angularVelocity": 2.6598423207322688e-21, - "velocityX": 2.8629625672203317e-35, - "velocityY": 1.3077612954632174, - "timestamp": 0.41709032476752655 + "x": 7.404560247182679e-34, + "y": 1.0314692438510245, + "heading": -1.9174121317431025e-17, + "angularVelocity": -2.532281150029562e-16, + "velocityX": -2.0683265264103736e-33, + "velocityY": 1.8442706648060245, + "timestamp": 0.5592830073884604 }, { - "x": -2.861998005146919e-37, - "y": 0.8484848327243157, - "heading": -4.732144278399494e-22, - "angularVelocity": 2.6234163690356904e-21, - "velocityX": -6.287059025950303e-35, - "velocityY": 1.0898010947326966, - "timestamp": 0.48660537889544764 + "x": 6.388428375393719e-34, + "y": 1.2033807844928626, + "heading": -1.926556919165743e-17, + "angularVelocity": -9.810547399257286e-19, + "velocityX": -1.0901084402040666e-33, + "velocityY": 1.8442706648060245, + "timestamp": 0.6524968419532038 }, { - "x": -2.03857738269056e-36, - "y": 0.9090908990896158, - "heading": -3.0214961018999903e-22, - "angularVelocity": 2.4608311899881687e-21, - "velocityX": -2.520860526747833e-35, - "velocityY": 0.87184088577092, - "timestamp": 0.5561204330233688 + "x": 3.0162184012138363e-34, + "y": 1.3752923251347007, + "heading": 4.527535991323288e-18, + "angularVelocity": 2.5525293851584663e-16, + "velocityX": -3.6177140401587575e-33, + "velocityY": 1.8442706648060245, + "timestamp": 0.7457106765179472 }, { - "x": 5.106160606722849e-38, - "y": 0.9545454493020581, - "heading": -1.6145441512322172e-22, - "angularVelocity": 2.0239528885973894e-21, - "velocityX": 3.006023625587289e-35, - "velocityY": 0.6538806706357015, - "timestamp": 0.6256354871512899 + "x": 3.354628939612959e-34, + "y": 1.5472038657765388, + "heading": 3.130040657204647e-17, + "angularVelocity": 2.872199250866309e-16, + "velocityX": 3.63047576810033e-34, + "velocityY": 1.8442706648060245, + "timestamp": 0.8389245110826906 }, { - "x": 1.7914848768309607e-36, - "y": 0.984848483027861, - "heading": -5.689927869393762e-23, - "angularVelocity": 1.5040646843361851e-21, - "velocityX": 2.5036637789133933e-35, - "velocityY": 0.43592045069891716, - "timestamp": 0.695150541279211 + "x": -2.3908944051347553e-34, + "y": 1.719115406418377, + "heading": 6.073122238228363e-17, + "angularVelocity": 3.157344180470925e-16, + "velocityX": -6.163809634413478e-33, + "velocityY": 1.8442706648060243, + "timestamp": 0.932138345647434 }, { - "x": 2.176452252156848e-48, - "y": 1, - "heading": 5.357751950103021e-41, - "angularVelocity": 8.185174103588329e-22, - "velocityX": -2.577117806968029e-35, - "velocityY": 0.21796022692088016, - "timestamp": 0.7646655954071322 + "x": -6.918462356301768e-36, + "y": 1.8910269470602104, + "heading": 4.515056239569573e-17, + "angularVelocity": -1.6714965175867783e-16, + "velocityX": 2.490735185578158e-33, + "velocityY": 1.8442706648059772, + "timestamp": 1.0253521802121774 }, { "x": 0, - "y": 1, - "heading": 1.7637632388066702e-41, - "angularVelocity": -5.169863684148192e-40, - "velocityX": -3.141686539318957e-47, - "velocityY": 5.499408435381289e-40, - "timestamp": 0.8341806495350533 + "y": 2, + "heading": 0, + "angularVelocity": -4.843761937970241e-16, + "velocityX": 7.422140407242795e-35, + "velocityY": 1.1690652299482462, + "timestamp": 1.1185660147769207 + }, + { + "x": 0, + "y": 2, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 0, + "timestamp": 1.211779849341664 } ], "constraints": [ @@ -159,7 +168,7 @@ 1 ], "type": "StopPoint", - "uuid": "4e292adc-83c9-4a24-8725-0a82efddefef" + "uuid": "40f8b823-8306-4dc8-8a96-0fc4aeda883f" } ], "usesControlIntervalGuessing": true, diff --git a/src/main/deploy/choreo/TestingPath.traj b/src/main/deploy/choreo/TestingPath.traj index 219a707..12599ab 100644 --- a/src/main/deploy/choreo/TestingPath.traj +++ b/src/main/deploy/choreo/TestingPath.traj @@ -4,118 +4,127 @@ "x": 0, "y": 0, "heading": 0, - "angularVelocity": -7.600376309198775e-18, - "velocityX": 9.47848126641677e-34, - "velocityY": 2.3685455382964595, + "angularVelocity": -3.947033998189832e-17, + "velocityX": -3.612854008750732e-31, + "velocityY": 1.2005140973885657, "timestamp": 0 }, { - "x": 3.444002287789685e-36, - "y": 0.16666663815137714, - "heading": -2.2326890701092606e-22, - "angularVelocity": -3.2118068414505653e-21, - "velocityX": 4.9543256977745e-35, - "velocityY": 2.3975617978327155, - "timestamp": 0.0695150541279211 + "x": 1.1944785629958918e-34, + "y": 0.17191154064183406, + "heading": 8.946622442095673e-18, + "angularVelocity": 9.597955586605163e-17, + "velocityX": 1.2814391555427755e-33, + "velocityY": 1.8442706648059815, + "timestamp": 0.0932138345647434 }, { - "x": 1.80940340937385e-36, - "y": 0.31818178117808316, - "heading": -8.171022190596369e-22, - "angularVelocity": -8.542513749172842e-21, - "velocityX": -2.3514315537398912e-35, - "velocityY": 2.1796018851958148, - "timestamp": 0.1390301082558422 + "x": 3.0218756793156384e-34, + "y": 0.34382308128367217, + "heading": -1.6139197453297265e-18, + "angularVelocity": -1.132937212242932e-16, + "velocityX": 1.960435468763232e-33, + "velocityY": 1.8442706648060243, + "timestamp": 0.1864276691294868 }, { - "x": 5.9379803896552905e-37, - "y": 0.4545454170640189, - "heading": -1.0153633602375655e-21, - "angularVelocity": -2.8520602469919215e-21, - "velocityX": -1.7486936861866492e-35, - "velocityY": 1.9616417997028441, - "timestamp": 0.2085451623837633 + "x": 5.424652677084512e-34, + "y": 0.5157346219255102, + "heading": -1.5884909113524752e-17, + "angularVelocity": -1.5309947750602212e-16, + "velocityX": 2.577704286588845e-33, + "velocityY": 1.8442706648060245, + "timestamp": 0.2796415036942302 }, { - "x": 9.346108964575416e-37, - "y": 0.575757541803813, - "heading": -9.823771839587742e-22, - "angularVelocity": 4.745184644957e-22, - "velocityX": 4.902720218198206e-36, - "velocityY": 1.7436816565911075, - "timestamp": 0.2780602165116844 + "x": 8.747257947587387e-34, + "y": 0.6876461625673483, + "heading": 1.7441665329815146e-18, + "angularVelocity": 1.8912509853096619e-16, + "velocityX": 3.564497995151033e-33, + "velocityY": 1.8442706648060245, + "timestamp": 0.3728553382589736 }, { - "x": 2.094062831073701e-36, - "y": 0.6818181533947788, - "heading": -8.40480447256135e-22, - "angularVelocity": 2.0412375100710908e-21, - "velocityX": 1.6679149082686126e-35, - "velocityY": 1.5257214846699805, - "timestamp": 0.34757527063960547 + "x": 9.332526707530359e-34, + "y": 0.8595577032091865, + "heading": 4.4302423015963514e-18, + "angularVelocity": 2.881627798231134e-17, + "velocityX": 6.278775686801844e-34, + "velocityY": 1.8442706648060245, + "timestamp": 0.466069172823717 }, { - "x": 4.0842527693840064e-36, - "y": 0.7727272506353046, - "heading": -6.555813623569812e-22, - "angularVelocity": 2.6598423207322688e-21, - "velocityX": 2.8629625672203317e-35, - "velocityY": 1.3077612954632174, - "timestamp": 0.41709032476752655 + "x": 7.404560247182679e-34, + "y": 1.0314692438510245, + "heading": -1.9174121317431025e-17, + "angularVelocity": -2.532281150029562e-16, + "velocityX": -2.0683265264103736e-33, + "velocityY": 1.8442706648060245, + "timestamp": 0.5592830073884604 }, { - "x": -2.861998005146919e-37, - "y": 0.8484848327243157, - "heading": -4.732144278399494e-22, - "angularVelocity": 2.6234163690356904e-21, - "velocityX": -6.287059025950303e-35, - "velocityY": 1.0898010947326966, - "timestamp": 0.48660537889544764 + "x": 6.388428375393719e-34, + "y": 1.2033807844928626, + "heading": -1.926556919165743e-17, + "angularVelocity": -9.810547399257286e-19, + "velocityX": -1.0901084402040666e-33, + "velocityY": 1.8442706648060245, + "timestamp": 0.6524968419532038 }, { - "x": -2.03857738269056e-36, - "y": 0.9090908990896158, - "heading": -3.0214961018999903e-22, - "angularVelocity": 2.4608311899881687e-21, - "velocityX": -2.520860526747833e-35, - "velocityY": 0.87184088577092, - "timestamp": 0.5561204330233688 + "x": 3.0162184012138363e-34, + "y": 1.3752923251347007, + "heading": 4.527535991323288e-18, + "angularVelocity": 2.5525293851584663e-16, + "velocityX": -3.6177140401587575e-33, + "velocityY": 1.8442706648060245, + "timestamp": 0.7457106765179472 }, { - "x": 5.106160606722849e-38, - "y": 0.9545454493020581, - "heading": -1.6145441512322172e-22, - "angularVelocity": 2.0239528885973894e-21, - "velocityX": 3.006023625587289e-35, - "velocityY": 0.6538806706357015, - "timestamp": 0.6256354871512899 + "x": 3.354628939612959e-34, + "y": 1.5472038657765388, + "heading": 3.130040657204647e-17, + "angularVelocity": 2.872199250866309e-16, + "velocityX": 3.63047576810033e-34, + "velocityY": 1.8442706648060245, + "timestamp": 0.8389245110826906 }, { - "x": 1.7914848768309607e-36, - "y": 0.984848483027861, - "heading": -5.689927869393762e-23, - "angularVelocity": 1.5040646843361851e-21, - "velocityX": 2.5036637789133933e-35, - "velocityY": 0.43592045069891716, - "timestamp": 0.695150541279211 + "x": -2.3908944051347553e-34, + "y": 1.719115406418377, + "heading": 6.073122238228363e-17, + "angularVelocity": 3.157344180470925e-16, + "velocityX": -6.163809634413478e-33, + "velocityY": 1.8442706648060243, + "timestamp": 0.932138345647434 }, { - "x": 2.176452252156848e-48, - "y": 1, - "heading": 5.357751950103021e-41, - "angularVelocity": 8.185174103588329e-22, - "velocityX": -2.577117806968029e-35, - "velocityY": 0.21796022692088016, - "timestamp": 0.7646655954071322 + "x": -6.918462356301768e-36, + "y": 1.8910269470602104, + "heading": 4.515056239569573e-17, + "angularVelocity": -1.6714965175867783e-16, + "velocityX": 2.490735185578158e-33, + "velocityY": 1.8442706648059772, + "timestamp": 1.0253521802121774 }, { "x": 0, - "y": 1, - "heading": 1.7637632388066702e-41, - "angularVelocity": -5.169863684148192e-40, - "velocityX": -3.141686539318957e-47, - "velocityY": 5.499408435381289e-40, - "timestamp": 0.8341806495350533 + "y": 2, + "heading": 0, + "angularVelocity": -4.843761937970241e-16, + "velocityX": 7.422140407242795e-35, + "velocityY": 1.1690652299482462, + "timestamp": 1.1185660147769207 + }, + { + "x": 0, + "y": 2, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 0, + "timestamp": 1.211779849341664 } ] } \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fa922ab..bc60061 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -28,13 +28,29 @@ public static final class ModuleConstants { public static final double kPModuleDriveController = 1; } + + public static final class Vision{ + public static final double DistanceFromTarget = 0; + } + public static final class DriveEncoders { + public static final double FRONTLEFT_DRIVE_ENCODER = 0; + public static final double FRONTRIGHT_DRIVE_ENCODER = 0; + public static final double BACKLEFT_DRIVE_ENCODER = 0; + public static final double BACKRIGHT_DRIVE_ENCODER = 0; + } + public static final class Hook { + public static final int MOTOR_CAN_ID = 3; // Defines the CAN id of the Spark Max motor controller + public static final double EXTENDED_HOOK_HEIGHT = 5; //Defines Extended Height for Hook + public static final double WITHDRAWN_HOOK_HEIGHT = 0; //Defines Withdrawn height for Hook + public static final double LIFT_HOOK_HEIGHT = 5; //Defines Lifting height for Hook + } public static final class Shooter { public static final double shooterKS = 0.667; public static final double shooterKV = 2.44; public static final double shooterKA = 0.27; - public static final double shooterKP = 1.0; + public static final double shooterKP = 2.0; public static final double shooterKI = 0.0; public static final double shooterKD = 0.0; public static final double shooterKFF = 0.0; @@ -44,24 +60,26 @@ public static final class Shooter { /* Motor Inverts */ public static final boolean driveInvert = true; - public static final int shooterID_1 = 22; - public static final int shooterID_2 = 25; + public static final int shooterID_1 = 25; + public static final int shooterID_2 = 22; + public static final int bumpID = 26; /* Swerve Current Limiting */ - public static final int angleContinuousCurrentLimit = 10; - public static final int driveContinuousCurrentLimit = 10; - public static final double voltageComp = 12.0; + public static final int angleContinuousCurrentLimit = 40; + public static final int driveContinuousCurrentLimit = 40; + public static final double voltageComp = 16.0; - public static final int speakerStrength = 15760; - public static final int ampStrength = 2560; + public static final int speakerStrength = 10000; + public static final double ampStrength = .095; + public static final int receive = 100; } public static final class Swerve { - public static final double fastDriveSpeedMultiplier = 1.0; - public static final double normalDriveSpeedMultiplier = 1.0; - public static final double slowDriveSpeedMultiplier = 1.0; + public static final double fastDriveSpeedMultiplier = .8; + public static final double normalDriveSpeedMultiplier = .6; + public static final double slowDriveSpeedMultiplier = .4; public static final int PIGEON2_ID = 10; //SET ME Each Run... public static final double robotOffset = 0.0; @@ -160,8 +178,8 @@ public static final class Mod1 { /* Back Left Module - Module 2 */ public static final class Mod2 { - public static final int driveMotorID = 8; - public static final int angleMotorID = 7; + public static final int driveMotorID = 7; + public static final int angleMotorID = 8; //public static final int canCoderID = 3; public static final Rotation2d angleOffset = Rotation2d.fromDegrees(470.5); //public static final Rotation2d angleOffset = Rotation2d.fromDegrees(204.7); @@ -187,9 +205,9 @@ public static final class AutoConstants { public static final double kMaxAngularSpeedRadiansPerSecond = 2 * Math.PI; public static final double kMaxAngularSpeedRadiansPerSecondSquared = 2 * Math.PI; - public static final double kPXController = 0.3; - public static final double kPYController = 0.3; - public static final double kPThetaController = 0.3; + public static final double kPXController = 1; + public static final double kPYController = 1; + public static final double kPThetaController = 1; // Constraint for the motion profilied robot angle controller public static final TrapezoidProfile.Constraints kThetaControllerConstraints = diff --git a/src/main/java/frc/robot/Constants204.java b/src/main/java/frc/robot/Constants204.java index d687160..9cef8f3 100644 --- a/src/main/java/frc/robot/Constants204.java +++ b/src/main/java/frc/robot/Constants204.java @@ -6,7 +6,8 @@ public static final class Autonomous { public static final String CHOREO_PATH_FILE = "TestingPath"; // omit file extension } public static final class Controller { - public static final int PORT = 1; + public static final int DRIVER_PORT = 1; + public static final int OPERATOR_PORT = 2; } // BELOW IS UNUSED diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1584e37..13de508 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -2,6 +2,8 @@ import edu.wpi.first.net.PortForwarder; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -17,6 +19,11 @@ public class Robot extends TimedRobot { private Command teleopCommand; private RobotContainer robotContainer; + public enum ControlMode { + SINGLE, COMPETITION + } + public static final SendableChooser ControlModeChooser = new SendableChooser<>(); + /** * This function is run when the robot is first started up and should be used for any * initialization code. @@ -29,6 +36,10 @@ public void robotInit() { robotContainer = new RobotContainer(); //CameraServer.startAutomaticCapture(); // use for USB camera PortForwarder.add(8888, "10.2.4.69", 80); + + ControlModeChooser.setDefaultOption("Single Controller (Driver:1 Operator:1)", ControlMode.SINGLE); + ControlModeChooser.addOption("Competition (Driver:1 Operator:2)", ControlMode.COMPETITION); + SmartDashboard.putData("Control Mode", ControlModeChooser); } /** * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6d6de79..590fe47 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -2,10 +2,14 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc.robot.commands.TeleopSwerve; import frc.robot.subsystems.*; import frc.robot.util.*; +import static frc.robot.Robot.ControlModeChooser; +import static frc.robot.Robot.ControlMode; + /* * This class is where the bulk of the robot should be declared. Since Command-based is a * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} @@ -15,25 +19,38 @@ public class RobotContainer { public final SwerveSubsystem SwerveSubsystem = new SwerveSubsystem(); public final ShooterSubsystem ShooterSubsystem = new ShooterSubsystem(); + public final LinearActuatorSubsystem LinearActuatorSubsystem = new LinearActuatorSubsystem(); private final AutonomousManager AutonomousManager = new AutonomousManager(SwerveSubsystem); - Gamepad CONTROLLER = new Gamepad(Constants204.Controller.PORT); + Gamepad DRIVER = new Gamepad(Constants204.Controller.DRIVER_PORT); + Gamepad OPERATOR = new Gamepad(Constants204.Controller.DRIVER_PORT); /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + ControlModeChooser.onChange((ControlMode mode) -> { + if (mode == ControlMode.SINGLE) { + OPERATOR = new Gamepad(Constants204.Controller.DRIVER_PORT); + } else { + OPERATOR = new Gamepad(Constants204.Controller.OPERATOR_PORT); + } + configureButtonBindings(); + System.out.println("Switched control mode to " + mode); + }); + + configureButtonBindings(); SwerveSubsystem.setDefaultCommand( new TeleopSwerve( SwerveSubsystem, - () -> CONTROLLER.getLeftX(), - () -> -1 * CONTROLLER.getLeftY(), - () -> -1 * CONTROLLER.getRightX(), + () -> DRIVER.getLeftX(), + () -> -1 * DRIVER.getLeftY(), + () -> -1 * DRIVER.getRightX(), () -> false, - () -> CONTROLLER.getYButton(), - () -> CONTROLLER.getAButton())); + () -> DRIVER.getYButton(), + () -> DRIVER.getAButton())); - if (CONTROLLER.getBButton()) { + if (DRIVER.getBButton()) { SwerveSubsystem.gyro.reset(); // s_Swerve.m_gyro_P2.calib; System.out.println("you have calibed the gyro"); @@ -41,8 +58,29 @@ public RobotContainer() { ShooterSubsystem.setDefaultCommand( new RunCommand( - () -> ShooterSubsystem.speakerShot(CONTROLLER.getXButton()), + () -> ShooterSubsystem.speakerShot(OPERATOR.getXButton()), ShooterSubsystem)); + + LinearActuatorSubsystem.setDefaultCommand( + new RunCommand( + () -> LinearActuatorSubsystem.shift(OPERATOR.getPOV()==0, OPERATOR.getPOV()==180), + LinearActuatorSubsystem + ) + ); + } + private void configureButtonBindings() { + new JoystickButton(OPERATOR, 5) + .whileTrue( + new RunCommand(() -> ShooterSubsystem.receive(true), ShooterSubsystem)); + + new JoystickButton(OPERATOR, 6) + .whileTrue( + new RunCommand(() -> ShooterSubsystem.bump(true), ShooterSubsystem)); + + new JoystickButton(OPERATOR, 10) + .whileTrue( + new RunCommand(() -> ShooterSubsystem.ampShot(true), ShooterSubsystem)); + } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/HookSubsystem.java b/src/main/java/frc/robot/subsystems/HookSubsystem.java new file mode 100644 index 0000000..d9c6456 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/HookSubsystem.java @@ -0,0 +1,129 @@ +//package frc.robot.subsystems; +//import java.util.*; +//import com.revrobotics.*; +//import com.revrobotics.CANSparkLowLevel.MotorType; +// +//import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +//import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +//import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +//import edu.wpi.first.wpilibj2.command.SubsystemBase; +//import frc.robot.Constants; +// +//public class HookSubsystem extends SubsystemBase { +// private CANSparkMax hookMotor; // Create the Spark Max object in the program and include the CAN id and the type of motor used with the controller +// private RelativeEncoder hookEncoder; // Grab the attached encoder +// private final SparkPIDController hookController; +// +// public HookSubsystem() { +// hookMotor = new CANSparkMax(Constants.Hook.MOTOR_CAN_ID, MotorType.kBrushless); // Create the Spark Max object in the program and include the CAN id and the type of motor used with the controller +// hookEncoder = hookMotor.getEncoder(); // Grab the attached encoder +// hookController = hookMotor.getPIDController(); +// hookMotor.restoreFactoryDefaults(); +// hookMotor.setSmartCurrentLimit(Constants.Shooter.driveContinuousCurrentLimit); +// hookMotor.setInverted(Constants.Shooter.driveInvert); +// hookMotor.setIdleMode(Constants.Shooter.driveNeutralMode); +// hookController.setP(1.0); +// hookController.setI(0); +// hookController.setD(0); +// hookController.setFF(0); +// hookMotor.burnFlash(); +// hookEncoder.setPosition(0.0); +// } +// +// +// +// +// +// double i = hookEncoder.getPosition(); +// +// //raises the hook motor +// public void liftHook() { +// //private CANSparkMax hookRaising = new CANSparkMax(Constants.MAXIMUMHOOK_HEIGHT, MotorType.kBrushless); +// System.out.println("Raising Hook"); +// hookController.setReference(Constants.Hook.LIFT_HOOK_HEIGHT, ControlType.kPosition); +// } +// +// //lowers the hook motor +// public void downHook() { +// //private CANSparkMax hookDepth = new CANSparkMax(Constants.MAXIMUMHOOK_DEPTH, MotorType.kBrushless); +// System.out.println("Down Hook"); +// hookController.setReference(Constants.Hook.WITHDRAWN_HOOK_HEIGHT, ControlType.kPosition); +// } +// +// //lowers the hook motor +// public void extendHook() { +// //private CANSparkMax hookDepth = new CANSparkMax(Constants.MAXIMUMHOOK_DEPTH, MotorType.kBrushless); +// System.out.println("Down Hook"); +// hookController.setReference(Constants.Hook.EXTENDED_HOOK_HEIGHT, ControlType.kPosition); +// } +// +// //Shuffleboard +// protected void execute() { +// SmartDashboard.putNumber("Hook Lifting", hookEncoder.getPosition()); +// SmartDashboard.putNumber("Hook Extension", hookEncoder.getPosition()); +// SmartDashboard.putNumber("Hook Withdrawn", hookEncoder.getPosition()); +// //SmartDashboard.putNumber("Shooter Angle", .getPosition()); +// //SmartDashboard.putNumber("Speed", ); +// //SmartDashboard.putNumber("Front Left Drive Encoder", 0); +// //SmartDashboard.putNumber("Front Right Drive Encoder", 0); +// //SmartDashboard.putNumber("Back Left Drive Encoder", 0); +// //SmartDashboard.putNumber("Back Right Drive Encoder", 0); +// //SmartDashboard.putNumber("Swerve Angle", 0); +// } +// Shuffleboard.getTab("Hook Lifting") +// .addPersistent("Lifting Height", Constants.Hook.LIFT_HOOK_HEIGHT) +// .withWidget(BuiltInWidgets.kNumberSlider) +// .withProperties(Map.of("min", 0, "max", 5)) +// .withSize(330,270) +// .withPosition(500,0) +// .getEntry(); +// Shuffleboard.getTab("Hook Extension") +// .addPersistent("Extended Height", Constants.Hook.EXTENDED_HOOK_HEIGHT) +// .withWidget(BuiltInWidgets.kNumberSlider) +// .withProperties(Map.of("min", 0, "max", 5)) +// .withSize(330,270) +// .withPosition(500,275) +// .getEntry(); +// Shuffleboard.getTab("Hook Withdrawn") +// .addPersistent("Max Depth", Constants.Hook.WITHDRAWN_HOOK_HEIGHT) +// .withWidget(BuiltInWidgets.kNumberSlider) +// .withProperties(Map.of("min", 0, "max", -5)) +// .withSize(330,270) +// .withPosition(500,550) +// .getEntry(); +// Shuffleboard.getTab("Shooter Angle") +// .addPersistent("Shooter Angle", SHOOTER_ANGLE) +// .withSize(800,530) +// .withPosition(850,0) +// .getEntry(); +// Shuffleboard.getTab("Speed") +// .addPersistent("Speed", SPEED) +// .withSize(330,270) +// .withPosition(500,825) +// .getEntry(); +// Shuffleboard.getTab("Front Left Drive Encoder") +// .addPersistent("Front Left Drive Encoder", FRONTLEFT_DRIVE_ENCODER) +// .withSize(495,269) +// .withPosition(0,0) +// .getEntry(); +// Shuffleboard.getTab("Front Right Drive Encoder") +// .addPersistent("Front Right Drive Encoder", FRONTRIGHT_DRIVE_ENCODER) +// .withSize(495,269) +// .withPosition(0,270) +// .getEntry(); +// Shuffleboard.getTab("Back Left Drive Encoder") +// .addPersistent("Back Left Drive Encoder", BACKLEFT_DRIVE_ENCODER) +// .withSize(495,269) +// .withPosition(0,540) +// .getEntry(); +// Shuffleboard.getTab("Right Drive Encoder") +// .addPersistent("Back Right Drive Encoder", BACKRIGHT_DRIVE_ENCODER) +// .withSize(495,269) +// .withPosition(0,810) +// .getEntry(); +// Shuffleboard.getTab("Swerve Angle") +// .addPersistent("Swerve Angle", SWERVE_ANGLE) +// .withSize(800,530) +// .withPosition(850,545) +// .getEntry(); +//} diff --git a/src/main/java/frc/robot/subsystems/LinearActuatorSubsystem.java b/src/main/java/frc/robot/subsystems/LinearActuatorSubsystem.java new file mode 100644 index 0000000..2ce3975 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LinearActuatorSubsystem.java @@ -0,0 +1,34 @@ +package frc.robot.subsystems; + +import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkMax; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.lib.util.CANSparkMaxUtil; +import frc.lib.util.CANSparkMaxUtil.Usage; +import frc.robot.Constants; + +public class LinearActuatorSubsystem extends SubsystemBase { + private final CANSparkMax actuatorMotor; + + public LinearActuatorSubsystem() { + actuatorMotor = new CANSparkMax(27, MotorType.kBrushed); + + actuatorMotor.restoreFactoryDefaults(); + CANSparkMaxUtil.setCANSparkMaxBusUsage(actuatorMotor, Usage.kAll); + actuatorMotor.setSmartCurrentLimit(Constants.Shooter.driveContinuousCurrentLimit); + actuatorMotor.setIdleMode(CANSparkBase.IdleMode.kBrake); + actuatorMotor.enableVoltageCompensation(Constants.Shooter.voltageComp); + actuatorMotor.burnFlash(); + } + + public void shift(boolean up, boolean down) { + if (up && !down) { + actuatorMotor.set(-1.0); + } else if (!up && down) { + actuatorMotor.set(1.0); + } else { + actuatorMotor.set(0); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 6a524f3..c852e0f 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -5,6 +5,8 @@ import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkPIDController; + +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.util.CANSparkMaxUtil; import frc.lib.util.CANSparkMaxUtil.Usage; @@ -13,6 +15,7 @@ public class ShooterSubsystem extends SubsystemBase { private final CANSparkMax shooterMotor_1; private final CANSparkMax shooterMotor_2; + private final CANSparkMax bumpMotor; private final RelativeEncoder shooterRelativeEncoder_1; private final RelativeEncoder shooterRelativeEncoder_2; @@ -32,6 +35,8 @@ public ShooterSubsystem() { shooterMotor_2 = new CANSparkMax(Constants.Shooter.shooterID_2, MotorType.kBrushless); shooterRelativeEncoder_2 = shooterMotor_2.getEncoder(); shooterPidController_2 = shooterMotor_2.getPIDController(); + + bumpMotor = new CANSparkMax(Constants.Shooter.bumpID, MotorType.kBrushless); configShooterMotors(); } @@ -59,23 +64,42 @@ private void configShooterMotors() { shooterMotor_2.setIdleMode(Constants.Shooter.driveNeutralMode); // shooterRelativeEncoder_1.setVelocityConversionFactor(Constants.Shooter.driveConversionVelocityFactor); // shooterRelativeEncoder_1.setPositionConversionFactor(Constants.Shooter.driveConversionPositionFactor); - shooterPidController_2.setP(Constants.Shooter.shooterKP); - shooterPidController_2.setI(Constants.Shooter.shooterKI); - shooterPidController_2.setD(Constants.Shooter.shooterKD); - shooterPidController_2.setFF(Constants.Shooter.shooterKFF); - shooterMotor_2.enableVoltageCompensation(Constants.Shooter.voltageComp); + // shooterPidController_2.setP(Constants.Shooter.shooterKP); + //shooterPidController_2.setI(Constants.Shooter.shooterKI); + //shooterPidController_2.setD(Constants.Shooter.shooterKD); + //shooterPidController_2.setFF(Constants.Shooter.shooterKFF); + //shooterMotor_2.enableVoltageCompensation(Constants.Shooter.voltageComp); shooterMotor_2.burnFlash(); shooterRelativeEncoder_2.setPosition(0.0); + + bumpMotor.restoreFactoryDefaults(); + CANSparkMaxUtil.setCANSparkMaxBusUsage(bumpMotor, Usage.kAll); + bumpMotor.setSmartCurrentLimit(Constants.Shooter.driveContinuousCurrentLimit); + bumpMotor.setInverted(!Constants.Shooter.driveInvert); + bumpMotor.setIdleMode(Constants.Shooter.driveNeutralMode); + bumpMotor.burnFlash(); } public void speakerShot(boolean shoot) { if (shoot) { - shooterPidController_1.setReference( + + /* shooterPidController_1.setReference( Constants.Shooter.speakerStrength, ControlType.kVelocity); shooterPidController_2.setReference( Constants.Shooter.speakerStrength, + ControlType.kVelocity);*/ + shooterMotor_1.set(1.0); + shooterMotor_2.set(1.0); + /* shooterPidController_1.setReference( + Constants.Shooter.speakerStrength, ControlType.kVelocity); + shooterPidController_2.setReference( + Constants.Shooter.speakerStrength, + ControlType.kVelocity);*/ + //Rotation2d r1 = Rotation2d.fromDegrees(720); + //shooterPidController_1.setReference(720.0, ControlType.kPosition); + //shooterMotor_2.set(1.0); } else { shooterPidController_1.setReference( 0, @@ -85,25 +109,80 @@ public void speakerShot(boolean shoot) { 0, ControlType.kVelocity); shooterMotor_2.set(0); + bumpMotor.set(0); } } - - public void ampShot(boolean shoot) { + public void speakerShot(boolean shoot, int i) { if (shoot) { + + + //Rotation2d r1 = Rotation2d.fromDegrees(720); + shooterPidController_1.setReference(-1020.0, ControlType.kPosition); + //shooterMotor_2.set(1.0); + } else { shooterPidController_1.setReference( - Constants.Shooter.ampStrength, + 0, ControlType.kVelocity); + shooterMotor_1.set(0); shooterPidController_2.setReference( - Constants.Shooter.ampStrength, + 0, ControlType.kVelocity); + shooterMotor_2.set(0); + bumpMotor.set(0); + } + + } + + + + public void ampShot(boolean shoot) { + if (shoot) { + shooterMotor_1.set(Constants.Shooter.ampStrength); + shooterMotor_2.set(Constants.Shooter.ampStrength-.025); } else { shooterPidController_1.setReference( - Constants.Shooter.ampStrength, + 0, ControlType.kVelocity); + shooterMotor_1.set(0); shooterPidController_2.setReference( - Constants.Shooter.ampStrength, + 0, ControlType.kVelocity); + shooterMotor_2.set(0); + bumpMotor.set(0); + } + } + public void receive(boolean shoot) { + if (shoot) { + shooterMotor_1.set(-.05); + shooterMotor_2.set(-.05); + bumpMotor.set(-.05); + } + else { + // shooterPidController_1.setReference( + // 0, + // ControlType.kVelocity); + //shooterMotor_1.set(0); + //shooterPidController_2.setReference( + // 0, + // ControlType.kVelocity); + //shooterMotor_2.set(0); + } + } + public void bump(boolean shoot) { + if (shoot) { + bumpMotor.set(1); + //shooterMotor_2.set(-.15); + } + else { + // shooterPidController_1.setReference( + // 0, + // ControlType.kVelocity); + //shooterMotor_1.set(0); + //shooterPidController_2.setReference( + // 0, + // ControlType.kVelocity); + //shooterMotor_2.set(0); } } } diff --git a/src/main/java/frc/robot/subsystems/ShuffleboardSubsystem b/src/main/java/frc/robot/subsystems/ShuffleboardSubsystem new file mode 100644 index 0000000..03a969f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ShuffleboardSubsystem @@ -0,0 +1,143 @@ +package frc.robot.subsystems; + +import java.util.Map; + +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.util.StateHandler; +import frc.robot.util.StateVariables.GamePieceMode; +import frc.robot.util.StateVariables.IntakePositions; +import frc.robot.util.StateVariables.VerticalLocations; + +import frc.robot.Constants; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; + +//importing SUBSYSTEMS +import frc.robot.subsystems.ArmSubsystem; +import frc.robot.subsystems.HookSubsystem; +import frc.robot.subsystems.LinearActuatorSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.SingleStrafeSubsystem; +import frc.robot.subsystems.StrafeSubsystem; +import frc.robot.subsystems.SwerveSubsystem; +import frc.robot.subsystems.StrafeModule; +import frc.robot.subsystems.SwerveModule; + +public class ShuffleboardSubsystem extends SubsystemBase { + private StateHandler stateHandler = StateHandler.getInstance(); + + public ShuffleboardTab driverDashboard = Shuffleboard.getTab("Driver Dashboard") + .withSize(20,2) + .withPosition(960,0); + + //Hook + private GenericEntry hookLift = driverDashboard.add("LiftingHook", false) + .addPersistent("Lifting Height", Constants.Hook.LIFT_HOOK_HEIGHT) + .withWidget(BuiltInWidgets.kNumberSlider) + .withProperties(Map.of("min", 0, "max", 5)) + .withSize(330,270) + .withPosition(500,0) + .getEntry(); + private GenericEntry hookExtend = driverDashboard.add("ExtendingHook", false) + .addPersistent("Extended Height", Constants.Hook.EXTENDED_HOOK_HEIGHT) + .withWidget(BuiltInWidgets.kNumberSlider) + .withProperties(Map.of("min", 0, "max", 5)) + .withSize(330,270) + .withPosition(500,275) + .getEntry(); + private GenericEntry hookWithdraw = driverDashboard.add("WithdrawingHook", false) + .addPersistent("Max Depth", Constants.Hook.WITHDRAWN_HOOK_HEIGHT) + .withWidget(BuiltInWidgets.kNumberSlider) + .withProperties(Map.of("min", 0, "max", -5)) + .withSize(330,270) + .withPosition(500,550) + .getEntry(); + + //Drive Encoders + private GenericEntry driveEncoderFL = driverDashboard.add("Front Left Drive Encoder") + .addPersistent("Front Left Drive Encoder", FRONTLEFT_DRIVE_ENCODER) + .withSize(495,269) + .withPosition(0,0) + double frontleftDriveEncoder = smartDashboard.getEntry("Back Left Drive Encoder").getDouble(); + private GenericEntry driveEncoderFR = driverDashboard.add("Front Right Drive Encoder") + .addPersistent("Front Right Drive Encoder", FRONTRIGHT_DRIVE_ENCODER) + .withSize(495,269) + .withPosition(0,270) + double FrontRightDriveEncoder = smartDashboard.getEntry("Front Right Drive Encoder").getDouble(); + private GenericEntry driveEncoderBL = driverDashboard.add("Back Left Drive Encoder") + .addPersistent("Back Left Drive Encoder", BACKLEFT_DRIVE_ENCODER) + .withSize(495,269) + .withPosition(0,540) + double backleftDriveEncoder = smartDashboard.getEntry("Back Left Drive Encoder").getDouble(); + private GenericEntry driveEncoderBR = driverDashboard.add("Back Right Drive Encoder") + .addPersistent("Back Right Drive Encoder", BACKRIGHT_DRIVE_ENCODER) + .withSize(495,269) + .withPosition(0,810) + double backrightDriveEncoder = smartDashboard.getEntry("Back Left Drive Encoder").getDouble(); + + private GenericEntry speed = driverDashboard.add("Speed") + .addPersistent("Speed", SPEED) + .withSize(330,270) + .withPosition(500,825) + .getEntry(); + + private GenericEntry shootAngle = driverDashBoard.add("Shooter Angle") + .addPersistent("Shooter Angle", SHOOTER_ANGLE) + .withSize(800,530) + .withPosition(850,0) + .getEntry(); + + private GenericEntry shootAngle = driverDashBoard.add("Swerve Angle") + .addPersistent("Swerve Angle", SWERVE_ANGLE) + .withSize(800,530) + .withPosition(850,545) + .getEntry(); + + class VisionCalculator { + private ShuffleboardTab tab = Shuffleboard.getTab("Vision"); + private NetworkTableEntry distanceEntry = + tab.add("Distance to target", DistanceFromTarget) + .withSize(4,2) + .withPosition(1916, 1078) + .getEntry(); + + public void calculate() { + double distance = ...; + distanceEntry.setDouble(distance); + } + } + @Override + public void periodic() { + VerticalLocations currentVerticalLocation = stateHandler.getCurrentVerticalLocation(); + + if (currentVerticalLocation == VerticalLocations.LiftingHook) { + hookLift.setBoolean(true); + } else { + hookLift.setBoolean(false); + } + + if (currentVerticalLocation == VerticalLocations.ExtendingHook) { + hookExtend.setBoolean(true); + } else { + hookExtend.setBoolean(false); + } + + if (currentVerticalLocation == VerticalLocations.WithdrawingHook) { + hookWithdraw.setBoolean(true); + } else { + hookWithdraw.setBoolean(false); + } + + double leftDriveEncoder = smartDashboard.getEntry("Left Drive Encoder").getDouble(); + double rightDriveEncoder = smartDashboard.getEntry("Right Drive Encoder").getDouble(); + + System.out.println("Left Drive Encoder: " + leftDriveEncoder); + System.out.println("Right Drive Encoder: " + rightDriveEncoder); + + desiredIntakePosition.setString(stateHandler.getDesiredIntakePosition().toString()); + + } +} \ No newline at end of file