From de25f504e24f77d592a3d40a7654f49265160429 Mon Sep 17 00:00:00 2001 From: Spooky1000 Date: Sun, 6 Oct 2024 01:30:54 -0700 Subject: [PATCH] code clean up rewrote most subsytems to be state based and have better code structure. implemented triggers for every subsystems to please review before making any code changes. Some aditional functionalities were added but should be easily removeable if desired. --- annotation-processor/build.gradle | 39 --- .../javax.annotation.processing.Processor | 1 - .../compileJava/previous-compilation-data.bin | Bin 6363 -> 0 bytes .../build/tmp/jar/MANIFEST.MF | 2 - .../java/processor/AnnotationProcessor.java | 234 ------------------ .../src/main/java/processor/Main.java | 7 - build.gradle | 3 - gradlew | 0 src/main/java/frc/team3128/Constants.java | 197 +++++++++------ src/main/java/frc/team3128/Robot.java | 5 +- .../java/frc/team3128/RobotContainer.java | 68 ++--- .../frc/team3128/autonomous/AutoPrograms.java | 4 - .../frc/team3128/autonomous/Trajectories.java | 46 +--- .../frc/team3128/commands/CmdManager.java | 152 ++---------- .../frc/team3128/commands/CmdSwerveDrive.java | 4 +- .../java/frc/team3128/subsystems/Amper.java | 109 ++++---- .../java/frc/team3128/subsystems/Hopper.java | 95 ++++--- .../java/frc/team3128/subsystems/Intake.java | 110 ++++---- .../java/frc/team3128/subsystems/Shooter.java | 185 +++++++------- .../java/frc/team3128/subsystems/Swerve.java | 46 +--- vendordeps/3128-common.json | 4 +- vendordeps/AdvantageKit.json | 10 +- vendordeps/Phoenix6.json | 48 ++-- 23 files changed, 509 insertions(+), 860 deletions(-) delete mode 100644 annotation-processor/build.gradle delete mode 100644 annotation-processor/build/classes/java/main/META-INF/services/javax.annotation.processing.Processor delete mode 100644 annotation-processor/build/tmp/compileJava/previous-compilation-data.bin delete mode 100644 annotation-processor/build/tmp/jar/MANIFEST.MF delete mode 100644 annotation-processor/src/main/java/processor/AnnotationProcessor.java delete mode 100644 annotation-processor/src/main/java/processor/Main.java mode change 100644 => 100755 gradlew diff --git a/annotation-processor/build.gradle b/annotation-processor/build.gradle deleted file mode 100644 index 705091b..0000000 --- a/annotation-processor/build.gradle +++ /dev/null @@ -1,39 +0,0 @@ -plugins { - id 'java' -} - -group 'processor' -version '1.0-SNAPSHOT' - -repositories { - mavenCentral() - maven { url 'https://jitpack.io/' } -} - -sourceSets { - main { - java { - srcDirs 'annotation-processor/src/main/java/processor', 'build/generated/sources/annotationProcessor/java/main' - } - } -} - -dependencies { - annotationProcessor 'com.google.auto.service:auto-service:1.0-rc5' - implementation 'com.google.auto.service:auto-service:1.0-rc5' - - implementation 'com.squareup:javapoet:1.13.0' - - implementation 'org.reflections:reflections:0.9.12' - - - - def narKitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.parentFile.getAbsolutePath() + "/vendordeps/3128-common.json").text) - - implementation "com.github.Team3128:3128-common:$narKitJson.version" -} - -java { - sourceCompatibility = JavaVersion.VERSION_11 - targetCompatibility = JavaVersion.VERSION_11 -} \ No newline at end of file diff --git a/annotation-processor/build/classes/java/main/META-INF/services/javax.annotation.processing.Processor b/annotation-processor/build/classes/java/main/META-INF/services/javax.annotation.processing.Processor deleted file mode 100644 index 196bf79..0000000 --- a/annotation-processor/build/classes/java/main/META-INF/services/javax.annotation.processing.Processor +++ /dev/null @@ -1 +0,0 @@ -processor.AnnotationProcessor diff --git a/annotation-processor/build/tmp/compileJava/previous-compilation-data.bin b/annotation-processor/build/tmp/compileJava/previous-compilation-data.bin deleted file mode 100644 index 13ad04b5151825ff6cb01bf399538e768092f37b..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 6363 zcmb7Id0fp||38autY7%Th_N zF&14?$J*@Ibo?zs6fs{eWUbmBd&%n}u(g3&K zCMY5#Kr}Wi473_#ZOR6-A#5m$P-y%aiq$UC%Oz1C@FR-lUIl1G9UX=uK<;qRqEYx6 z7|pCk7@OOUv^O0EXa>qhqX?x+q+?LDUa9gHB@8Yh3-^DyjJ5npHja&F6WBx)4GR1< zYD*NO*C|y>U5FK0`43=P3V+rRDsQ1EI`ys*|ZRiV}4TG~P~zrD@hKmg8JegkGm{ zL(yQ5T6S)-_Jvl+zUD%#WVt} zHSBs?$&Ckop*J_IcsVGHy=_G1!`k_xD4j;5($WPePAT_QYqUD0f+_*KFGe()ve3rQ zb({*VT%h;yQHfp}x#aB+7U|>$dFn|T=hguzt}!3K}8rT)e6awQgYQ-sd_Eb zF+Cd^h~j+YTAe5b`>TRb%tDBjR-+b#$M&z1T+~}+Cw&)WcgC_I!8RdQi)EpfOPrUo zw)AHdCzk+b;L|QcDQJ6(z7K2$Vh76T_<1kvISRg9=M9GKC4$??|@fghe`eP+Vk&xdint)Mk zW37tbt1w!jkp%nxGd9)DJCW3Fx@kngh9*Xj_o*~}cfZ0aOHesgQ40;tBNu0MH3)W@%@0;VS z_BX9+x2~5W3$|dk6|-bcxD6~(YPN%1Z;c*GAV4lblz?ZjdC})TQfg=Y>rj)J7Gth< z*nyo>u~;uGPQg%<*+4J)LQ4!34|92< z5Px-}`9f1^w(B#8vgo68v+RVXLoB>E9ntDPmA75*=;|~x2U{J%#z(R1G3;!59I*Zx zY9~5INbh+gBCK-C*{!omhdT9Xm79y*PGHA8%uZr`qWL_snt(GH z#Y=LvR%K|-Lby&08*yJZ06R`K9dkp>Ka1JFd92QXDlrfmpO;<>uoZgz+Mw>4d+Mcr zSf<}L<$?3q*x>>s*Ki?#*jjG?9Mk_Nv+4N-vrG0CV0IC+LY_&N0KkQMwNh6E!u;hr zh_u9o<^Hi^^%?xaDzlfL`?YbaT#GTg%!76X;9Up_4g zZe1ysInEGX%(@omHKfDSS@+LgXt0^=dk3?-nBC(xmw{BJ8tOjdRF-4 zB<$%l@?~XNLC$WQ$Cy3gTAu=mmt4z9=wS25NLIITO1VDv{6NK=XV|z3yH;aM)8kI$xBFTZmz_Jy%BB+Ad@Pt4x&t-pBA zc>4`E2O4c}TtC0xUe7DDw)p>A+xc}Jc6^WBKVY{=Z1NEbNGpEg6{u*;oM;14&J!%eiizO_MQ^bSP1v&%Cs~;wZHeGq?>5bYB zYd5^P)^jCc2?V0DilFGmsDPZ!&Ot@fY93b)tM@MnH)Amag|;Iz-&L!vZkL}6C1d-= zm8>Q#k+4g^2ISSlp8e|E#%W;X+c%rUBztQ!p@V(B*^b5I+^e2F9RK!3Vf!_NttD(7 z_b3V6=GA7%Gq6kPAHYpt(xF*J&e^gDtCD){b{TK?aRV`yK^~;ds_(BHci>(@r=Arj zat|!tNTzKfHjy|SZ-%MxD|}kv{D1?YiQ#!sdo16KB7-kB$b>aTxov8_-*$9d*JZ}1 zv;jA!2?I(40+yP*y-P+(bKiV@C$aeqmI=LEXRo+- zbwhFfu705#ug|}*i?H2XQ7VDV7hxnY6yaz8q3iG4v~S3o`P_49?x_4UGG!03OXm)T zlf9roy$_&*2}v#~7K8}R?{vIU`L6-pt44^EJ=T5Z0C&kO>-5x>cn#=%CiVNz*B8K z-_(A}tLwWjwxxB3#ptXP-a`)(c8IIU2ESo0)vI{bPdh0EXNKaxOp^~ypY6Hp`eEXn zhjX}RngCJKg$Solyq8t1yKOdYT8Zt{H8}eSaXm^bj}dm9uw2Lkd;+4*2bery`ZB{% z*OQ>5adHvjCd}w~b)MD1$%A@T&vaY;u?h>_7WY;7q#7M+_49}FL4Q;=KSkJSZdpF0 zz8K6~+KM5|yW34^-=vLiS$N)J3-2?8o#mMS8w$u*k_RjRnzQ+`$=kQsJ8p8!2#o*f z-I;TQohQ&ap7PGnZ2Ot8nYhVf`+5DARxe3ibb+t}p5gI)Q+u<0(?nKKur#vQ+@!ji zITwj>A#uG#Y)y-J3X3Xl-tpE%D2VII-a~FbXt&!o;h0cNWS5EC6=G?66{sXh9%3>> zSToqXPtdxAhYtewyYD>P)#w_5V-}b*MqZ-x>u38n?e*YZPQ`DVO9;Ep9oWY8uX4|F zHM1aDZf`7-0)IT|cZ0B-yl`(3lmHX3GC<#Wt^#Sv@w3!qaFAE~yMe09P7@B^Cajdp zxdSFFkZU;(`(K>)A9XxnS^ksYSX-AA;VxnKI1Xiim0TO5RsfCZ>mI#-Sw5os!S0H6 zukY8%?-N$eX(|kebBcXs^IInA*92wvUFLUW{M*Uy!m1-d9~{Q-UOq_vWYn%M6H|X7 z>{r5m+4A~E$A^VQ z6MiSGlCwSnjg1ZPdd{+?*nlN7Qj```Hb0g3;ebhY~LEkPu ziiLK456@K^A8T;Rtm*Og$tkPnguUR_yaX84aF+ru*O8Y)F70oglaP>@X1YODS3|5` z@w|XDO4Y5^==)aMyXl83+o0}&m9>QZ!7Y6a+%PZjAk?i2-RG+sa5&<^#s@#;y8rx! zOmDz%!Pf<%S{T{dxAx-dt;#c9*Y_TKp+=cX;g9^H{SZJKkX+Gpw~GW9c=o`>ss z{N;WP;D{b37Gc?4clu=KlyPvyD_(22cJp%T97Zkka5zQrP~#HZV_{lYLHQ7<5|*wm zs;f#!4ELXHp}f)J@{$P3A}OF61^WsgU$AaO$5j2>oz^+Yt@gMD4U&SRDT|>{hOrby z>!8&2N+6p3eB_p`W-~`^96tE>@S?C=7Q&H$OJQ%89sFhK#hrKM^=>GRdc;$gmDDU`TuY(0XcncTpF3yhq^ZaOkpWT#5@gYMhH< z=-mjibRjBHFdyF8)vNW{GcDF2|98Q2q)pU#Ae8(5itWcdjOv}{B;1`8_bgn#nX)b1 z&#m0ZV4bMuFAF?3pPVfC!Q-=W`JG2GY5nZ?1SQkygSLUil9I!Ja`KEZ^Md+RjIl3@ zzMs+L;CAY=gF2>Ampr_a!q~5amlDx?7Z?NcifT7SD?}dw@7TBgp)R|P>AR^5oH}m$ zA(h&uQR_X_J)PQZf}>|Ig}YVbHOzb1DlEje3|P{ouqegxkN7g-(uncl@IdwQ?Q;v~hRYvWB1P}xB$ zOTmZ08jU&|3;q#5Ps+Ku$+Ji4-KhE9hd0fpvZc$hAUKY^zHF?}|F7 z3GxywF1no>{P1Rl>gk9@eYdTHGcZ(d)Gpvf`)coHJ+iHj57FdMyCc;4D4lwY+UMcp z6os4Rf;O*(^aMpK)o_yzfawh`Xu@~pBR_n$U#9QzeJih%=a1XuQQ1lA;dlzblS@ke zj4?y#*jn04WEQ5uLc{j*vDHzhDa)sDcPQhBW~W+>pZ4&DV$-Y7=dMdhsy{eKV5a;X%!x`HcP%XJlfyIRB5- z9pBE9`Ry!r@Tlg1axi?3VwC&s0?q>~v2#GB@NJVvkK$LjhSoDqWL ziBU7o1T+JYIqaFCPv36!a{A5a$59CE|JKNVR|qNu11T8hfM$YKGL!`L7_xB_e2M2k zlnmeRI8d@G45?L$9RQ&1-)=`LmkWVD@a?SGoHZET!06QfuUmY5Z~339&iQu5d7!wt zoeaVZPD5&5gx|#63>Ctc@;ru40VVXU{RP3ZCqkrzvg_382K|xHCO6@@> annotationMap = new HashMap>(); - - static List generatedClassList = new LinkedList(); - - private boolean hasRun = false; - - @Override - public boolean process(Set annotations, RoundEnvironment roundEnv) { - //makes sure that the processor is only run once - if(hasRun){ - return false; - } - - try{ - - annotations.forEach(annotation -> { - - annotationMap.put(annotation.toString(), new ArrayList()); - - roundEnv.getElementsAnnotatedWith(annotation).forEach( - element -> { - annotationMap.get(annotation.toString()).add(element); - } - ); - - }); - - processNARUpdateable(annotationMap.get("common.utility.annotations.NARUpdateable")); - - processAutoCommand(annotationMap.get("common.utility.annotations.AutoCommand")); - - processShuffleboardData(annotationMap.get("common.utility.annotations.ShuffleboardData")); - - generateClassManger(); - - hasRun = true; - - } catch(Exception e){ - e.printStackTrace(); - return false; - } - - return true; - } - - private boolean handleAnnotationUsage(List elements, String annotationName){ - boolean isUsed = elements != null && elements.size() > 0; - if(isUsed){ - print("Generating " + annotationName + " Processor..."); - return true; - }else{ - print("No " + annotationName + " Annotations..."); - return false; - } - } - - public void processShuffleboardData(List shuffleboardData){ - if(!handleAnnotationUsage(shuffleboardData, "ShuffleboardData")){ - return; - } - - CodeBlock.Builder codeBlock = CodeBlock.builder(); - - for(Element element : shuffleboardData){ - String methodName = element.getSimpleName().toString(); - String tab = element.getAnnotation(ShuffleboardData.class).tab(); - String name = element.getAnnotation(ShuffleboardData.class).name(); - int x = element.getAnnotation(ShuffleboardData.class).x(); - int y = element.getAnnotation(ShuffleboardData.class).y(); - String className = element.getEnclosingElement().getSimpleName().toString(); - String packageName = processingEnv.getElementUtils().getPackageOf(element.getEnclosingElement()).getQualifiedName().toString(); - boolean hasGetInstance = element.getEnclosingElement().getEnclosedElements().stream().anyMatch(e -> e.getSimpleName().toString().equals("getInstance")); - - if (hasGetInstance) { - codeBlock.addStatement("$T.addData($S, $S, ()-> $T.getInstance().$L(), $L, $L)", - ClassName.get("common.utility.shuffleboard", "NAR_Shuffleboard"), - tab, - name, - ClassName.get(packageName, className), - methodName, - x, - y); - }else{ - codeBlock.addStatement("$T.addData($S, $S, ()-> $T.$L(), $L, $L)", - ClassName.get("common.utility.shuffleboard", "NAR_Shuffleboard"), - tab, - name, - ClassName.get(packageName, className), - methodName, - x, - y); - } - - - } - - generateClass("ShuffleboardDataProcessor", codeBlock.build()); - } - - public void processAutoCommand(List autoCommands){ - if(!handleAnnotationUsage(autoCommands, "AutoCommand")){ - return; - } - - CodeBlock.Builder codeBlock = CodeBlock.builder(); - - for(Element element : autoCommands){ - String methodName = element.getSimpleName().toString(); - String identifier = element.getAnnotation(AutoCommand.class).identifier().toString(); - String className = element.getEnclosingElement().getSimpleName().toString(); - String packageName = processingEnv.getElementUtils().getPackageOf(element.getEnclosingElement()).getQualifiedName().toString(); - boolean hasGetInstance = element.getEnclosingElement().getEnclosedElements().stream().anyMatch(e -> e.getSimpleName().toString().equals("getInstance")); - - if(hasGetInstance){ - codeBlock.addStatement("$T.registerCommand($S, $T.getInstance().$L())", - ClassName.get("com.pathplanner.lib.auto", "NamedCommands"), - identifier, - ClassName.get(packageName, className), - methodName); - }else{ - codeBlock.addStatement("$T.registerCommand($L, $L.$L())", - ClassName.get("com.pathplanner.lib.auto", "NamedCommands"), - identifier, - className, - methodName); - } - } - - generateClass("AutoCommandProcessor", codeBlock.build()); - } - - public void processNARUpdateable(List updateMethods){ - if(!handleAnnotationUsage(updateMethods, "NARUpdateable")){ - return; - } - CodeBlock.Builder codeBlock = CodeBlock.builder() - .addStatement("$T dashboard = $T.getInstance()", ClassName.get("common.utility.narwhaldashboard", "NarwhalDashboard"), ClassName.get("", "NarwhalDashboard")); - - for(Element element : updateMethods){ - String methodName = element.getSimpleName().toString(); - String dataName = element.getAnnotation(NARUpdateable.class).name(); - String className = element.getEnclosingElement().getSimpleName().toString(); - String packageName = processingEnv.getElementUtils().getPackageOf(element.getEnclosingElement()).getQualifiedName().toString(); - boolean hasGetInstance = element.getEnclosingElement().getEnclosedElements().stream().anyMatch(e -> e.getSimpleName().toString().equals("getInstance")); - - if (!hasGetInstance) { - continue; - } - - - codeBlock.addStatement("dashboard.addUpdate($S, () -> $T.getInstance().$L())", dataName, ClassName.get(packageName, className), methodName); - } - - generateClass("NARUpdateableProcessor", codeBlock.build()); - } - - public boolean generateClass(String className, CodeBlock... codeBlocks){ - MethodSpec.Builder method = MethodSpec.methodBuilder("process") - .addModifiers(Modifier.PUBLIC, Modifier.STATIC) - .returns(void.class); - - for(CodeBlock codeBlock : codeBlocks){ - method.addCode(codeBlock); - } - - TypeSpec generatedClass = TypeSpec.classBuilder(className) - .addModifiers(Modifier.PUBLIC) - .addMethod(method.build()) - .build(); - - JavaFile javaFile = JavaFile.builder("processor", generatedClass).build(); - - try { - javaFile.writeTo(processingEnv.getFiler()); - if (!className.equals("ClassManager")) { - generatedClassList.add("processor." + className); - } - return true; - } catch (Exception e) { - // e.printStackTrace(); - return false; - } - } - - public boolean generateClassManger(){ - print("Creating Class Manager..."); - CodeBlock.Builder codeBlock = CodeBlock.builder(); - for(String className : generatedClassList){ - codeBlock.addStatement("$T.process()", ClassName.get("", className)); - } - return generateClass("ClassManager", codeBlock.build()); - } - - private void print(String message) { - processingEnv.getMessager().printMessage(Diagnostic.Kind.OTHER, "Annotation Processor Update: " + message); - } - - @Override - public Set getSupportedAnnotationTypes() { - return Set.of( - NARUpdateable.class.getCanonicalName(), - ShuffleboardData.class.getCanonicalName(), - AutoCommand.class.getCanonicalName()); - } - -} \ No newline at end of file diff --git a/annotation-processor/src/main/java/processor/Main.java b/annotation-processor/src/main/java/processor/Main.java deleted file mode 100644 index 442185a..0000000 --- a/annotation-processor/src/main/java/processor/Main.java +++ /dev/null @@ -1,7 +0,0 @@ -package processor; - -public class Main { - public static void main(String[] args) { - System.out.println("3128's annotation processor is running!"); - } -} \ No newline at end of file diff --git a/build.gradle b/build.gradle index ff14235..1f4f945 100644 --- a/build.gradle +++ b/build.gradle @@ -74,9 +74,6 @@ dependencies { def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version" - annotationProcessor project(":annotation-processor") - implementation project(":annotation-processor") - implementation 'com.squareup:javapoet:1.13.0' implementation 'org.reflections:reflections:0.9.12' diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/frc/team3128/Constants.java b/src/main/java/frc/team3128/Constants.java index 48a1909..42d9186 100644 --- a/src/main/java/frc/team3128/Constants.java +++ b/src/main/java/frc/team3128/Constants.java @@ -1,12 +1,11 @@ package frc.team3128; -import java.util.HashMap; - import com.ctre.phoenix6.hardware.CANcoder; import com.pathplanner.lib.path.PathConstraints; import common.core.controllers.Controller; import common.core.controllers.PIDFFConfig; +import common.core.controllers.TrapController; import common.core.controllers.Controller.Type; import common.core.swerve.SwerveConversions; import common.core.swerve.SwerveModuleConfig; @@ -15,6 +14,7 @@ import common.hardware.motorcontroller.NAR_CANSpark; import common.hardware.motorcontroller.NAR_TalonFX; import common.hardware.motorcontroller.NAR_CANSpark.ControllerType; +import common.hardware.motorcontroller.NAR_CANSpark.SparkMaxConfig; import common.hardware.motorcontroller.NAR_Motor.MotorConfig; import common.hardware.motorcontroller.NAR_Motor.Neutral; import edu.wpi.first.math.geometry.Translation2d; @@ -23,6 +23,7 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DriverStation.Alliance; import frc.team3128.subsystems.Swerve; import edu.wpi.first.math.MathUtil; @@ -178,7 +179,6 @@ public static class SwerveConstants { } } - public static class VisionConstants { public static final double POSE_THRESH = 100; @@ -234,41 +234,65 @@ public static Rotation2d flipRotation(Rotation2d rotation) { } public static class FocalAimConstants { + public static final Translation2d focalPoint = new Translation2d(0.1, Units.inchesToMeters(218.29)); + public static final double speakerLength = 1.043; - public static final double speakerMidpointY = Units.inchesToMeters(218.29);//5.4; + public static final double speakerMidpointY = Units.inchesToMeters(218.29); //5.4; - ; //6.151 - speakerLength / 2; - public static final double focalPointX = 0.1; //0.229; //1.4583577128; - public static final Translation2d speakerMidpointBlue = new Translation2d(0, speakerMidpointY); - public static final Translation2d speakerMidpointRed = new Translation2d(FieldConstants.FIELD_X_LENGTH, speakerMidpointY); - public static final Translation2d focalPointBlue = new Translation2d(focalPointX, speakerMidpointY); - public static final Translation2d focalPointRed = new Translation2d(FieldConstants.FIELD_X_LENGTH - focalPointX, speakerMidpointY); - public static final double angleOffset = 0; - //testing: kV: drivetrain spinning consistently (ie. v1 = vel at vel at 1 rad/sec v2=2 rad/sec). 1/(v2-v1) = kV - //kS: plug kV into 1= kS + kV(v1) - public static final double offset = 0.3; - public static final double lowerBound = speakerMidpointY - offset; - public static final double higherBound = speakerMidpointY + offset; + //6.151 - speakerLength / 2; + // public static final double focalPointX = 0.1; //0.229; //1.4583577128; + // public static final Translation2d speakerMidpointBlue = new Translation2d(0, speakerMidpointY); + // public static final Translation2d speakerMidpointRed = new Translation2d(FieldConstants.FIELD_X_LENGTH, speakerMidpointY); + // public static final Translation2d focalPointBlue = new Translation2d(focalPointX, speakerMidpointY); + // public static final Translation2d focalPointRed = new Translation2d(FieldConstants.FIELD_X_LENGTH - focalPointX, speakerMidpointY); + // public static final double angleOffset = 0; + // //testing: kV: drivetrain spinning consistently (ie. v1 = vel at vel at 1 rad/sec v2=2 rad/sec). 1/(v2-v1) = kV + // //kS: plug kV into 1= kS + kV(v1) + // public static final double offset = 0.3; + // public static final double lowerBound = speakerMidpointY - offset; + // public static final double higherBound = speakerMidpointY + offset; } public static class ShooterConstants { - public static final int SHOOTER_MOTOR_ID = 60; - public static final int KICK_MOTOR_ID = 61; - public static final int KICK_SENSOR_ID = 0; - public static final int ROLLERS_SENSOR_ID = 1; - public static final NAR_CANSpark SHOOTER_MOTOR = new NAR_CANSpark(SHOOTER_MOTOR_ID); - public static final NAR_CANSpark KICK_MOTOR = new NAR_CANSpark(KICK_MOTOR_ID); + // Shooter Motor Constants + public static final int SHTR_MOTOR_ID = 60; + public static final NAR_CANSpark SHTR_MOTOR = new NAR_CANSpark(SHTR_MOTOR_ID); + public static final boolean SHTR_MOTOR_INVERT = false; + public static final int SHTR_CURRENT_LIMIT = 40; + public static final double SHTR_GEAR_RATIO = 1.0; + public static final SparkMaxConfig SHTR_STATUS_FRAME = SparkMaxConfig.VELOCITY; + public static final Neutral SHTR_NEUTRAL_MODE = Neutral.COAST; + // Kicker Motor Constants + public static final int KICK_MOTOR_ID = 61; + public static final NAR_CANSpark KICK_MOTOR = new NAR_CANSpark(KICK_MOTOR_ID); + public static final boolean KICK_MOTOR_INVERT = false; + public static final int KICK_CURRENT_LIMIT = 40; + public static final double KICK_GEAR_RATIO = 1.0; + public static final SparkMaxConfig KICK_STATUS_FRAME = SparkMaxConfig.VELOCITY; + public static final Neutral KICK_NEUTRAL_MODE = Neutral.COAST; + + // Shooter Sensor Constants + public static final int SHTR_SENSOR_ID = 1; + public static final DigitalInput SHTR_SENSOR = new DigitalInput(SHTR_SENSOR_ID); + + // Shooter System Constants + public static final double SHTR_ANGLE = 0; + public static final double VELOCITY_MIN = 0; + public static final double VELOCITY_MAX = 5500; + + // Controller Constants public static final PIDFFConfig PIDConstants = new PIDFFConfig(0.0025, 0, 0, 0, 0.00179104, 0); // 0.00187623 - public static final double kF = 0; - public static final double GEAR_RATIO = 1; - public static final double MAX_RPM = 5500; - public static final double MIN_RPM = 0; + public static final Controller CONTROLLER = new Controller(PIDConstants, Type.VELOCITY); public static final double TOLERANCE = 150; - public static final double AMP_RPM = 2500; - public static final double SHOOTER_RPM = 4500; + // Functional Constants + public static final double SHOOTER_RPM = 4500; + public static final double SOURCE_RPM = 2000; + + public static final double AMP_RPM = 2500; + public static final double AMP_ANGLE = 90; public static final double EDGE_FEED_RPM = 5000; public static final double EDGE_FEED_ANGLE = 35; public static final double MIDDLE_FEED_RPM = 4500; @@ -276,43 +300,45 @@ public static class ShooterConstants { public static final double INTAKE_POWER = 0.4; public static final double KICK_POWER = 0.2; - public static final double CURRENT_TEST_POWER = 0; - public static final double CURRENT_TEST_PLATEAU = 0; - public static final double CURRENT_TEST_TIMEOUT = 0; - public static final double CURRENT_TEST_TOLERANCE = 0; - public static final double CURRENT_TEST_EXPECTED_CURRENT = 0; - - public static final double SHOOTER_TEST_PLATEAU = 1; - public static final double SHOOTER_TEST_TIMEOUT = 2.5; - - public static final double PROJECTILE_SPEED = 100; // m/s } public static class IntakeConstants { - public static final int PIVOT_MOTOR_ID = 31; - public static final NAR_CANSpark PIVOT_MOTOR = new NAR_CANSpark(PIVOT_MOTOR_ID); - - public static final PIDFFConfig PIDConstants = new PIDFFConfig(0.1, 0, 0, 0.2, 0, 0, 0.3625); - public static final double MAX_VELOCITY = 1000000; - public static final double MAX_ACCELERATION = 100000; - public static final Constraints TRAP_CONSTRAINTS = new Constraints(MAX_VELOCITY, MAX_ACCELERATION); - - public static final double ANGLE_TOLERANCE = 3; - public static final double MIN_SETPOINT = 0; - public static final double MAX_SETPOINT = 220; - public static final int CURRENT_LIMIT = 40; - - public static final double GEAR_RATIO = 1.0 / 40.0; - public static final double UNIT_CONV_FACTOR = GEAR_RATIO * 360; + // Pivot Motor Constants + public static final int PIVT_MOTOR_ID = 31; + public static final NAR_CANSpark PIVT_MOTOR = new NAR_CANSpark(PIVT_MOTOR_ID); + public static final boolean PIVT_MOTOR_INVERT = false; + public static final int PIVT_CURRENT_LIMIT = 40; + public static final double PIVT_GEAR_RATIO = 1.0 / 40.0; + public static final SparkMaxConfig PIVT_STATUS_FRAME = SparkMaxConfig.POSITION; + public static final Neutral PIVT_NEUTRAL_MODE = Neutral.COAST; + + // Pivot System Constants + public static final double POSITION_MIN = 0; + public static final double POSITION_MAX = 220; + public static final double UNIT_CONV_FACTOR = PIVT_GEAR_RATIO * 360; + + // Roller Motor Constants public static final int ROLLER_MOTOR_ID = 30; public static final NAR_CANSpark ROLLER_MOTOR = new NAR_CANSpark(ROLLER_MOTOR_ID, ControllerType.CAN_SPARK_FLEX); + public static final boolean ROLLER_MOTOR_INVERT = false; + public static final int ROLLER_CURRENT_LIMIT = 40; + public static final double ROLLER_GEAR_RATIO = 1.0; + public static final SparkMaxConfig ROLLER_STATUS_FRAME = SparkMaxConfig.VELOCITY; + public static final Neutral ROLLER_NEUTRAL_MODE = Neutral.COAST; + public static final double ROLLER_VOLT_COMP = 9; + + // Controller Constants + public static final PIDFFConfig PIDConstants = new PIDFFConfig(0.1, 0, 0, 0.2, 0, 0, 0.3625); + public static final double MAX_VELOCTIY = 10000000; + public static final double MAX_ACCELERATION = 100000; + public static final Constraints TRAP_CONSTRAINTS = new Constraints(MAX_VELOCTIY, MAX_ACCELERATION); + public static final TrapController CONTROLLER = new TrapController(PIDConstants, TRAP_CONSTRAINTS); + public static final double POSITION_TOLERANCE = 0.25; - public static final double STALL_CURRENT = 50; - public static final double STALL_POWER = .05; + // Functional Constants public static final double OUTTAKE_POWER = -0.3; - public static final double INTAKE_POWER = 0.7 /0.75; - public static final double VOLT_COMP = 9; + public static final double INTAKE_POWER = 0.7 / 0.75; } public static class LimelightConstants { @@ -382,6 +408,7 @@ public enum Colors { } } + public static class ClimberConstants { public static final int CLIMB_MOTOR_ID = 50; // public static final NAR_CANSpark CLIMB_MOTOR = new NAR_CANSpark(CLIMB_MOTOR_ID); @@ -403,44 +430,62 @@ public static class ClimberConstants { } public static class HopperConstants { - public static final int HOPPER_MOTOR_ID = 40; - public static final NAR_CANSpark HOPPER_MOTOR = new NAR_CANSpark(HOPPER_MOTOR_ID); - public static final int HOPPER_FRONT_SENSOR_ID = 0; - public static final int HOPPER_BACK_SENSOR_ID = 2; + // Elevator Motor Constants + public static final int HPPR_MOTOR_ID = 21; + public static final NAR_CANSpark HPPR_MOTOR = new NAR_CANSpark(HPPR_MOTOR_ID); + public static final boolean HPPR_MOTOR_INVERT = false; + public static final int HPPR_CURRENT_LIMIT = 40; + public static final double HPPR_GEAR_RATIO = 1.0; + public static final SparkMaxConfig HPPR_STATUS_FRAME = SparkMaxConfig.VELOCITY; + public static final Neutral HPPR_NEUTRAL_MODE = Neutral.COAST; + public static final double HPPR_VOLT_COMP = 9; + + public static final int HPPR_SENSOR_ID = 0; + public static final DigitalInput HPPR_SENSOR = new DigitalInput(HPPR_SENSOR_ID); public static final double STALL_CURRENT = 50; - public static final double HOPPER_INTAKE_POWER = 0.5; - public static final double HOPPER_OUTTAKE_POWER = -0.5; - public static final double STALL_POWER = 0.05; - - public static final double VOLT_COMP = 9; - public static final int CURRENT_LIMIT = 40; + public static final double HPPR_INTAKE_POWER = 0.5; + public static final double HPPR_OUTTAKE_POWER = -0.5; + public static final double HPPR_STALL_POWER = 0.05; } public static class AmperConstants { + + // Elevator Motor Constants public static final int ELEV_MOTOR_ID = 21; public static final NAR_CANSpark ELEV_MOTOR = new NAR_CANSpark(ELEV_MOTOR_ID); + public static final boolean ELEV_MOTOR_INVERT = false; + public static final int ELEV_CURRENT_LIMIT = 40; + public static final double ELEV_GEAR_RATIO = 1.0 / (6 + 2/3); + public static final SparkMaxConfig ELEV_STATUS_FRAME = SparkMaxConfig.POSITION; + public static final Neutral ELEV_NEUTRAL_MODE = Neutral.COAST; + // Elevator System Constants + public static final double WHEEL_CIRCUMFERENCE = Units.inchesToMeters(0.9023) * Math.PI; + public static final double UNIT_CONV_FACTOR = ELEV_GEAR_RATIO * WHEEL_CIRCUMFERENCE * 100; + public static final double AMPER_ANGLE = 31.96; + public static final double POSITION_MIN = 0; + public static final double POSITION_MAX = 21.25; + + // Roller Motor Constants public static final int ROLLER_MOTOR_ID = 20; public static final NAR_CANSpark ROLLER_MOTOR = new NAR_CANSpark(ROLLER_MOTOR_ID, ControllerType.CAN_SPARK_FLEX); + public static final boolean ROLLER_MOTOR_INVERT = false; + public static final int ROLLER_CURRENT_LIMIT = 40; + public static final double ROLLER_GEAR_RATIO = 1.0; + public static final SparkMaxConfig ROLLER_STATUS_FRAME = SparkMaxConfig.VELOCITY; + public static final Neutral ROLLER_NEUTRAL_MODE = Neutral.COAST; + // Controller Constants public static final PIDFFConfig PIDConstants = new PIDFFConfig(0.75, 0, 0, 0.21115, 0.00182, 0.00182, 0.0); public static final double MAX_VELOCTIY = 10000000; public static final double MAX_ACCELERATION = 100000; public static final Constraints TRAP_CONSTRAINTS = new Constraints(MAX_VELOCTIY, MAX_ACCELERATION); - + public static final TrapController CONTROLLER = new TrapController(PIDConstants, TRAP_CONSTRAINTS); public static final double POSITION_TOLERANCE = 0.25; - public static final double MIN_SETPOINT = 0; - public static final double MAX_SETPOINT = 21.25; - public static final int CURRENT_LIMIT = 40; - - public static final double GEAR_RATIO = 1.0 / (6 + 2/3); - public static final double WHEEL_CIRCUMFERENCE = Units.inchesToMeters(0.9023) * Math.PI; - public static final double UNIT_CONV_FACTOR = GEAR_RATIO * WHEEL_CIRCUMFERENCE * 100; public static final double ROLLER_POWER = 0.5; - public static final double AMPER_ANGLE = 31.96; } } diff --git a/src/main/java/frc/team3128/Robot.java b/src/main/java/frc/team3128/Robot.java index f43d993..ef5921e 100644 --- a/src/main/java/frc/team3128/Robot.java +++ b/src/main/java/frc/team3128/Robot.java @@ -179,8 +179,9 @@ public void disabledInit() { @Override public void disabledExit() { Leds.getInstance().setDefaultColor(); - Swerve.getInstance().setBrakeMode(true - ); + Swerve.getInstance().setBrakeMode(true); + + CommandScheduler.getInstance().cancelAll(); } @Override diff --git a/src/main/java/frc/team3128/RobotContainer.java b/src/main/java/frc/team3128/RobotContainer.java index e7d3690..dcf8d44 100644 --- a/src/main/java/frc/team3128/RobotContainer.java +++ b/src/main/java/frc/team3128/RobotContainer.java @@ -11,11 +11,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import static edu.wpi.first.wpilibj2.command.Commands.*; -import static frc.team3128.Constants.ShooterConstants.EDGE_FEED_ANGLE; -import static frc.team3128.Constants.ShooterConstants.EDGE_FEED_RPM; -import static frc.team3128.Constants.ShooterConstants.MAX_RPM; -import static frc.team3128.Constants.ShooterConstants.MIDDLE_FEED_ANGLE; -import static frc.team3128.Constants.ShooterConstants.MIDDLE_FEED_RPM; +import static frc.team3128.Constants.ShooterConstants.*; import static frc.team3128.commands.CmdManager.*; import org.photonvision.PhotonPoseEstimator.PoseStrategy; @@ -38,12 +34,19 @@ import common.utility.shuffleboard.NAR_Shuffleboard; import common.utility.sysid.CmdSysId; import common.utility.tester.Tester; +import frc.team3128.subsystems.Amper; import frc.team3128.subsystems.Hopper; import frc.team3128.subsystems.Intake; // import common.utility.tester.Tester.UnitTest; import frc.team3128.subsystems.Leds; import frc.team3128.subsystems.Shooter; import frc.team3128.subsystems.Swerve; + +import frc.team3128.subsystems.Amper.AmpState; +import frc.team3128.subsystems.Hopper.HopperState; +import frc.team3128.subsystems.Intake.IntakeState; +import frc.team3128.subsystems.Shooter.ShooterState; + import java.util.ArrayList; import edu.wpi.first.apriltag.AprilTagFields; @@ -57,8 +60,12 @@ public class RobotContainer { private Swerve swerve; private Leds leds; + public static Amper amper; + // public static Climber climber; + public static Intake intake; + public static Hopper hopper; + public Shooter shooter; - // private NAR_ButtonBoard judgePad; private NAR_ButtonBoard buttonPad; public static NAR_XboxController controller; @@ -75,14 +82,20 @@ public RobotContainer() { NAR_Shuffleboard.WINDOW_WIDTH = 10; - // judgePad = new NAR_ButtonBoard(1); + amper = Amper.getInstance(); + // climber = Climber.getInstance(); + intake = Intake.getInstance(); + hopper = Hopper.getInstance(); + shooter = Shooter.getInstance(); + swerve = Swerve.getInstance(); + leds = Leds.getInstance(); + controller = new NAR_XboxController(2); buttonPad = new NAR_ButtonBoard(3); - //uncomment line below to enable driving - CommandScheduler.getInstance().setDefaultCommand(Swerve.getInstance(), new CmdSwerveDrive(controller::getLeftX,controller::getLeftY, controller::getRightX, true)); + CommandScheduler.getInstance().setDefaultCommand(swerve, new CmdSwerveDrive(controller::getLeftX,controller::getLeftY, controller::getRightX, true)); - initRobotTest(); + // initRobotTest(); DriverStation.silenceJoystickConnectionWarning(true); initCameras(); @@ -110,31 +123,31 @@ private void configureButtonBindings() { CmdSwerveDrive.setTurnSetpoint(Robot.getAlliance() == Alliance.Red ? 270 : 90); })); - controller.getButton(XboxButton.kLeftBumper).onTrue(intake(Intake.Setpoint.GROUND)); - controller.getButton(XboxButton.kRightBumper).onTrue(retractIntake()); + controller.getButton(XboxButton.kLeftBumper).onTrue(intake.setState(IntakeState.INTAKE)); + controller.getButton(XboxButton.kRightBumper).onTrue(intake.setState(IntakeState.RETRACTED)); - controller.getButton(XboxButton.kA).onTrue(Shooter.getInstance().runShooter(0.8)); - controller.getButton(XboxButton.kY).onTrue(Shooter.getInstance().runShooter(0)); - controller.getButton(XboxButton.kB).onTrue(Shooter.getInstance().runKickMotor(0.8)).onFalse(Shooter.getInstance().runKickMotor(0)); + controller.getButton(XboxButton.kA).onTrue(shooter.runShooter(0.8)); + controller.getButton(XboxButton.kY).onTrue(shooter.runShooter(0)); + controller.getButton(XboxButton.kB).onTrue(shooter.runKickMotor(0.8)).onFalse(shooter.runKickMotor(0)); - controller.getButton(XboxButton.kY).whileTrue(ampUp()).onFalse(ampFinAndDown()); + controller.getButton(XboxButton.kY).whileTrue(amper.setState(AmpState.PRIMED)).onFalse(amper.setState(AmpState.AMP)); // new Trigger(()->true).onTrue(queueNote()); - new Trigger(()-> Intake.getInstance().getMeasurement() > 90) - .and(()->!Hopper.getInstance().hasObjectPresent()) - .onTrue(Hopper.getInstance().runManipulator(0.8)) - .onFalse(Hopper.getInstance().runManipulator(0)); + new Trigger(()-> intake.getMeasurement() > 90) + .and(()-> !hopper.hasObjectPresent()) + .onTrue(hopper.runManipulator(0.8)) + .onFalse(hopper.runManipulator(0)); - new Trigger(()-> Shooter.getInstance().noteInRollers()).negate() - .and(()->Hopper.getInstance().hasObjectPresent()) + new Trigger(()-> shooter.hasObjectPresent()).negate() + .and(()-> hopper.hasObjectPresent()) .onTrue(sequence( - Shooter.getInstance().runKickMotor(0.8), + shooter.runKickMotor(0.8), Hopper.getInstance().runManipulator(0.8) )) .onFalse(sequence( - Shooter.getInstance().runKickMotor(0), + shooter.runKickMotor(0), Hopper.getInstance().runManipulator(0) )); @@ -164,13 +177,6 @@ public void initCameras() { // limelight = new Limelight("limelight-mason", 0, 0, 0); } - public static void toggleSideCams(boolean enable) { - for (Camera camera : sideCams) { - if (enable) camera.enable(); - else camera.disable(); - } - } - public void initDashboard() { dashboard = NarwhalDashboard.getInstance(); dashboard.addUpdate("time", ()-> Timer.getMatchTime()); diff --git a/src/main/java/frc/team3128/autonomous/AutoPrograms.java b/src/main/java/frc/team3128/autonomous/AutoPrograms.java index a5f588f..c8fe9a1 100644 --- a/src/main/java/frc/team3128/autonomous/AutoPrograms.java +++ b/src/main/java/frc/team3128/autonomous/AutoPrograms.java @@ -1,10 +1,6 @@ package frc.team3128.autonomous; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; -import frc.team3128.Robot; -import frc.team3128.subsystems.Swerve; import common.utility.narwhaldashboard.NarwhalDashboard; import static edu.wpi.first.wpilibj2.command.Commands.*; diff --git a/src/main/java/frc/team3128/autonomous/Trajectories.java b/src/main/java/frc/team3128/autonomous/Trajectories.java index b9fe40e..0502e44 100644 --- a/src/main/java/frc/team3128/autonomous/Trajectories.java +++ b/src/main/java/frc/team3128/autonomous/Trajectories.java @@ -5,11 +5,6 @@ import com.pathplanner.lib.util.ReplanningConfig; import common.core.commands.NAR_PIDCommand; -import common.core.controllers.Controller; -import common.core.controllers.PIDFFConfig; -import common.core.controllers.Controller.Type; -import common.utility.shuffleboard.NAR_Shuffleboard; - import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; @@ -17,7 +12,6 @@ import com.pathplanner.lib.pathfinding.Pathfinding; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation.Alliance; @@ -25,16 +19,10 @@ import static edu.wpi.first.wpilibj2.command.Commands.*; import static frc.team3128.Constants.AutoConstants.*; -import static frc.team3128.Constants.FocalAimConstants.focalPointBlue; -import static frc.team3128.Constants.FocalAimConstants.focalPointRed; -import static frc.team3128.Constants.ShooterConstants.MAX_RPM; -import static frc.team3128.Constants.ShooterConstants.SHOOTER_RPM; import static frc.team3128.Constants.SwerveConstants.*; import frc.team3128.Constants.AutoConstants; -import frc.team3128.Constants.ShooterConstants; import frc.team3128.Robot; -import frc.team3128.RobotContainer; import frc.team3128.commands.CmdManager; import frc.team3128.commands.CmdSwerveDrive; @@ -58,8 +46,8 @@ public static void initTrajectories() { // TODO: add commands NamedCommands.registerCommand("ramShoot", CmdManager.ramShoot(true)); - NamedCommands.registerCommand("ramShootNoStop", CmdManager.ramShootNoStop(true)); - NamedCommands.registerCommand("intakeAndStop", CmdManager.intakeAndStop(Intake.Setpoint.GROUND)); + NamedCommands.registerCommand("ramShootNoStop", CmdManager.ramShoot(false)); + NamedCommands.registerCommand("intakeAndStop", Intake.getInstance().setState(Intake.IntakeState.INTAKE)); AutoBuilder.configureHolonomic( swerve::getPose, @@ -93,33 +81,17 @@ public static void drive(ChassisSpeeds velocity) { } } - public static Command turnDegrees(boolean counterClockwise, double angle) { - DoubleSupplier setpoint = ()-> swerve.getYaw() + angle * (counterClockwise ? 1 : -1) * (Robot.getAlliance() == Alliance.Red ? -1 : 1); - Controller controller = new Controller(new PIDFFConfig(5, 0, 0, turnkS, 0, 0), Type.POSITION); - controller.enableContinuousInput(-180, 180); - controller.setTolerance(1); - return new NAR_PIDCommand( - controller, - ()-> swerve.getYaw(), //measurement - setpoint, //setpoint - (double output) -> { - Swerve.getInstance().drive(new ChassisSpeeds(vx, vy, Units.degreesToRadians(output))); - } - ).beforeStarting(runOnce(()-> CmdSwerveDrive.disableTurn())); - } - - public static Command turnInPlace() { - DoubleSupplier setpoint = ()-> swerve.getTurnAngle(Robot.getAlliance() == Alliance.Red ? focalPointRed : focalPointBlue); - Controller controller = new Controller(new PIDFFConfig(5, 0, 0, turnkS, 0, 0), Type.POSITION); - controller.enableContinuousInput(-180, 180); - controller.setTolerance(1); + public Command turnInPlace(DoubleSupplier setpoint, double timeout) { + TURN_CONTROLLER.enableContinuousInput(-180, 180); return new NAR_PIDCommand( - controller, + TURN_CONTROLLER, ()-> swerve.getYaw(), //measurement setpoint, //setpoint (double output) -> { - Swerve.getInstance().drive(new ChassisSpeeds(vx, vy, Units.degreesToRadians(output))); - } + drive(new ChassisSpeeds(vx, vy, Units.degreesToRadians(output))); + }, + timeout, + Swerve.getInstance() ).beforeStarting(runOnce(()-> CmdSwerveDrive.disableTurn())); } diff --git a/src/main/java/frc/team3128/commands/CmdManager.java b/src/main/java/frc/team3128/commands/CmdManager.java index 030d559..21f6079 100644 --- a/src/main/java/frc/team3128/commands/CmdManager.java +++ b/src/main/java/frc/team3128/commands/CmdManager.java @@ -1,23 +1,19 @@ package frc.team3128.commands; import static edu.wpi.first.wpilibj2.command.Commands.*; -import static frc.team3128.Constants.FocalAimConstants.focalPointBlue; -import static frc.team3128.Constants.FocalAimConstants.focalPointRed; -import static frc.team3128.Constants.HopperConstants.*; -import static frc.team3128.Constants.ShooterConstants.*; - import common.hardware.input.NAR_XboxController; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ScheduleCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; -import frc.team3128.Robot; import frc.team3128.RobotContainer; import frc.team3128.subsystems.Amper; import frc.team3128.subsystems.Hopper; import frc.team3128.subsystems.Intake; -import frc.team3128.subsystems.Intake.Setpoint; import frc.team3128.subsystems.Shooter; +import frc.team3128.subsystems.Amper.AmpState; +import frc.team3128.subsystems.Hopper.HopperState; +import frc.team3128.subsystems.Intake.IntakeState; +import frc.team3128.subsystems.Shooter.ShooterState; import frc.team3128.subsystems.Swerve; // import frc.team3128.subsystems.Climber; @@ -36,140 +32,28 @@ public static Command vibrateController(){ return new ScheduleCommand(new StartEndCommand(()-> controller.startVibrate(), ()-> controller.stopVibrate()).withTimeout(1)); } - public static Command kick() { - return either( - sequence( - shooter.runKickMotor(KICK_POWER), - waitUntil(() -> !shooter.noteInRollers()).withTimeout(0.3), - shooter.runKickMotor(0) - ), - none(), - () -> shooter.noteInRollers() - ); - } - - public static Command kick(boolean once) { + public static Command ramShoot(boolean once) { return sequence( - kick(), + amper.setState(AmpState.RETRACTED), + intake.setState(IntakeState.RETRACTED), + shooter.setState(ShooterState.SHOOT), + waitUntil(()-> !shooter.hasObjectPresent()), either( - // queueNote(), none(), - sequence( - // queueNote(), - waitSeconds(0.3), - kick() - ), - () -> once + hopper.setState(HopperState.IDLE), + ()-> once ) ); } - public static Command ramShoot(boolean once) { - return sequence( - ramShootNoStop(once), - shooter.stopMotors() - ); - } - - public static Command ramShootNoStop(boolean once) { - return sequence( - shooter.rampUpShooter(), - waitUntil(()->shooter.atSetpoint()), - kick(once) - ); - } - - // public static Command autoShoot(boolean once) { - // return deadline( - // rampUpShoot(), - // repeatingSequence( - // runOnce(()-> CmdSwerveDrive.setTurnSetpoint(swerve.getTurnAngle(Robot.getAlliance() == Alliance.Red ? focalPointRed : focalPointBlue))), - // waitSeconds(0.1) - // ) - // ).andThen(ramShoot(once)).andThen(runOnce(() -> CmdSwerveDrive.disableTurn())); - // } - - public static Command intake(Intake.Setpoint setpoint) { - return sequence( - // intake.pivotTo(setpoint), - intake.runIntakeRollers(), - hopper.runManipulator(HOPPER_INTAKE_POWER), - waitUntil(()->hopper.hasObjectPresent()), - hopper.runManipulator(0) - ); - } - - public static Command retractIntake() { - return sequence( - intake.stopRollers() - // hopper.runManipulator(0), - // intake.retract() - ); - } - - public static Command intakeAndStop(Intake.Setpoint setpoint) { - return sequence( - intake(setpoint), - retractIntake() - ); - } - - // public static Command feed(double rpm, double angle, boolean once) { + // public static Command hopperOuttake() { // return sequence( - // parallel( - // swerve.turnInPlace(() -> Robot.getAlliance() == Alliance.Blue ? 180-angle : angle).asProxy().withTimeout(1), - // runOnce(() -> shooter.startPID(rpm)), - // waitUntil(() -> shooter.atSetpoint()) - // ), - // kick(once), - // shooter.stopMotors() + // intake.setState(IntakeState.RETRACTED), + // // TODO: will this slow things down + // waitSeconds(0.3), + // hopper.runManipulator(HPPR_OUTTAKE_POWER), + // waitSeconds(0.35), + // hopper.runManipulator(0) // ); // } - - // public static Command queueNote() { - // return either( - // none(), - // sequence( - // shooter.runKickMotor(0.5), - // hopper.runManipulator(HOPPER_INTAKE_POWER), - // waitUntil(()-> shooter.noteInRollers()), - // hopper.runManipulator(0), - // shooter.runKickMotor(0) - // ), - // () -> (!hopper.hasObjectPresent() || shooter.noteInRollers()), - // hopper.runManipulator(0).onlyIf(()-> hopper.hasObjectPresent()) - - // ); - // } - - public static Command hopperOuttake() { - return sequence( - intake.pivotTo(Setpoint.NEUTRAL), - // TODO: will this slow things down - waitSeconds(0.3), - hopper.runManipulator(HOPPER_OUTTAKE_POWER), - waitSeconds(0.35), - hopper.runManipulator(0) - ); - } - - public static Command ampUp(){ - return sequence( - amper.partExtend(), - amper.runRollers(), - shooter.runShooter(0.3) - ); - } - - public static Command ampFinAndDown(){ - return sequence( - amper.fullExtend(), - waitUntil(() -> amper.atSetpoint()), - shooter.runKickMotor(1), - waitSeconds(1), - amper.stopRollers(), - shooter.stopMotors(), - amper.retract() - ); - } } \ No newline at end of file diff --git a/src/main/java/frc/team3128/commands/CmdSwerveDrive.java b/src/main/java/frc/team3128/commands/CmdSwerveDrive.java index 66d0ffc..181a4ca 100644 --- a/src/main/java/frc/team3128/commands/CmdSwerveDrive.java +++ b/src/main/java/frc/team3128/commands/CmdSwerveDrive.java @@ -25,8 +25,6 @@ public class CmdSwerveDrive extends Command { private final DoubleSupplier yAxis; private final DoubleSupplier zAxis; - private final SlewRateLimiter accelLimiter; - public static PIDController rController; private static boolean enabled = false; private static double rSetpoint; @@ -43,7 +41,7 @@ public CmdSwerveDrive(DoubleSupplier xAxis, DoubleSupplier yAxis, DoubleSupplier rController.setTolerance(1); rController.enableContinuousInput(-180, 180); - accelLimiter = new SlewRateLimiter(maxAcceleration); + new SlewRateLimiter(maxAcceleration); // rController = new PIDController(turnkP, 0, 0); swerve.fieldRelative = fieldRelative; } diff --git a/src/main/java/frc/team3128/subsystems/Amper.java b/src/main/java/frc/team3128/subsystems/Amper.java index b396114..4bd7e68 100644 --- a/src/main/java/frc/team3128/subsystems/Amper.java +++ b/src/main/java/frc/team3128/subsystems/Amper.java @@ -2,32 +2,42 @@ import static frc.team3128.Constants.AmperConstants.*; -import common.core.controllers.TrapController; import common.core.subsystems.ElevatorTemplate; -import common.hardware.motorcontroller.NAR_CANSpark.SparkMaxConfig; import common.hardware.motorcontroller.NAR_Motor.Control; -import common.hardware.motorcontroller.NAR_Motor.Neutral; import common.utility.shuffleboard.NAR_Shuffleboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.team3128.subsystems.Shooter.ShooterState; import static edu.wpi.first.wpilibj2.command.Commands.sequence; public class Amper extends ElevatorTemplate { - public enum Setpoint { + public enum AmpState { //TODO: correct EXTEND setpoints - FULLEXTEND(21.25), - PARTEXTEND(21.25*0.8), - RETRACTED(0); + AMP(POSITION_MAX, ROLLER_POWER), + PRIMED(POSITION_MAX * 0.8, ROLLER_POWER), + RETRACTED(0, 0); - private double setpoint; - private Setpoint(double setpoint) { + private final double setpoint; + private final double rollerPower; + + private AmpState(double setpoint, double rollerPower) { this.setpoint = setpoint; + this.rollerPower = rollerPower; + } + + public double getSetpoint() { + return setpoint; + } + + public double getRollerPower() { + return rollerPower; } } private static Amper instance; + private static AmpState goalState; public static synchronized Amper getInstance() { if (instance == null) @@ -36,28 +46,45 @@ public static synchronized Amper getInstance() { } private Amper() { - super(new TrapController(PIDConstants, TRAP_CONSTRAINTS), ELEV_MOTOR); + super(CONTROLLER, ELEV_MOTOR); + configController(); + configTriggers(); + initShuffleboard(); - //TODO: remove once done testing - this.setSafetyThresh(100); - // setkG_Function(() -> getMeasurement()*Math.sin(AMPER_ANGLE)); - - setTolerance(POSITION_TOLERANCE); - setConstraints(MIN_SETPOINT, MAX_SETPOINT); - // initShuffleboard(); + setState(AmpState.RETRACTED).schedule(); } @Override protected void configMotors() { - ELEV_MOTOR.setUnitConversionFactor(UNIT_CONV_FACTOR); - ELEV_MOTOR.setCurrentLimit(CURRENT_LIMIT); - ELEV_MOTOR.setInverted(true); + ELEV_MOTOR.setInverted(ELEV_MOTOR_INVERT); + ELEV_MOTOR.setCurrentLimit(ELEV_CURRENT_LIMIT); + ELEV_MOTOR.setUnitConversionFactor(ELEV_GEAR_RATIO); + ELEV_MOTOR.setStatusFrames(ELEV_STATUS_FRAME); + ELEV_MOTOR.setNeutralMode(ELEV_NEUTRAL_MODE); + + ROLLER_MOTOR.setInverted(ROLLER_MOTOR_INVERT); + ROLLER_MOTOR.setNeutralMode(ROLLER_NEUTRAL_MODE); + ROLLER_MOTOR.setUnitConversionFactor(ROLLER_GEAR_RATIO); + ROLLER_MOTOR.setStatusFrames(ROLLER_STATUS_FRAME); + ROLLER_MOTOR.setNeutralMode(ROLLER_NEUTRAL_MODE); + } - ELEV_MOTOR.setNeutralMode(Neutral.COAST); - ROLLER_MOTOR.setNeutralMode(Neutral.COAST); + public void configController(){ + CONTROLLER.setConstraints(POSITION_MIN, POSITION_MAX); + CONTROLLER.setTolerance(POSITION_TOLERANCE); + this.setSafetyThresh(2); + // setkG_Function(() -> getMeasurement()*Math.sin(AMPER_ANGLE)); + } + + public void configTriggers(){ + // assuming that we always want to amp two notes when given the chance + new Trigger(()-> Shooter.getInstance().hasObjectPresent()) + .and(()-> !Hopper.getInstance().hasObjectPresent()) + .debounce(0.25) // account for hopper to shooter transition + .onTrue(setState(AmpState.RETRACTED)); - ELEV_MOTOR.setStatusFrames(SparkMaxConfig.POSITION); - ROLLER_MOTOR.setDefaultStatusFrames(); + new Trigger(()-> Shooter.getInstance().goalStateIs(ShooterState.SHOOT)) + .onTrue(setState(AmpState.RETRACTED)); } public void setVoltage(double volts) { @@ -77,34 +104,26 @@ public double getPosition() { public void initShuffleboard(){ super.initShuffleboard(); - NAR_Shuffleboard.addData(getName(), "Measurement2", ()-> getPosition(), 5, 1); - NAR_Shuffleboard.addData(getName(), "Current", ()-> ELEV_MOTOR.getStallCurrent(), 5, 2); - NAR_Shuffleboard.addData(getName(), "Voltage", ()-> ELEV_MOTOR.getMotor().getBusVoltage(), 5, 3); - NAR_Shuffleboard.addData(getName(), "Velocity", ()-> getVelocity(), 1,0); - } - - public Command runRollers() { - return runRollers(ROLLER_POWER); - } - - public Command stopRollers(){ - return runRollers(0); + NAR_Shuffleboard.addData(getName(), "Roller Current", ()-> ROLLER_MOTOR.getStallCurrent(), 5, 2); } public Command runRollers(double power) { return runOnce(() -> ROLLER_MOTOR.set(power)); - } - - public Command partExtend() { - return moveElevator(Setpoint.PARTEXTEND.setpoint); } - public Command fullExtend(){ - return moveElevator(Setpoint.FULLEXTEND.setpoint); + public Command setState(AmpState state) { + goalState = state; + return sequence( + moveElevator(state.getSetpoint()), + runRollers(state.getRollerPower()) + ); } - public Command retract() { - return moveElevator(Setpoint.RETRACTED.setpoint); + public AmpState getGoalState() { + return goalState; } + public boolean goalStateIs(AmpState state) { + return goalState == state; + } } \ No newline at end of file diff --git a/src/main/java/frc/team3128/subsystems/Hopper.java b/src/main/java/frc/team3128/subsystems/Hopper.java index b5cc792..89c4fc0 100644 --- a/src/main/java/frc/team3128/subsystems/Hopper.java +++ b/src/main/java/frc/team3128/subsystems/Hopper.java @@ -1,19 +1,34 @@ package frc.team3128.subsystems; import common.core.subsystems.ManipulatorTemplate; -import common.hardware.motorcontroller.NAR_CANSpark.SparkMaxConfig; -import common.hardware.motorcontroller.NAR_Motor.Neutral; -import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.team3128.subsystems.Intake.IntakeState; +import frc.team3128.subsystems.Shooter.ShooterState; import static frc.team3128.Constants.HopperConstants.*; public class Hopper extends ManipulatorTemplate { - // front is the shooter - private DigitalInput frontSensor, backSensor; + public enum HopperState { + INTAKE(HPPR_INTAKE_POWER), + OUTTAKE(HPPR_OUTTAKE_POWER), + IDLE(HPPR_STALL_POWER), + HAND_OFF(HPPR_STALL_POWER); + + private final double power; + + private HopperState(double power) { + this.power = power; + } + + public double getPower() { + return power; + } + } private static Hopper instance; + private static HopperState goalState; public static synchronized Hopper getInstance() { if (instance == null) @@ -22,52 +37,56 @@ public static synchronized Hopper getInstance() { } private Hopper() { - super(STALL_CURRENT, HOPPER_INTAKE_POWER, HOPPER_OUTTAKE_POWER, STALL_POWER, 0.3, HOPPER_MOTOR); - frontSensor = new DigitalInput(HOPPER_FRONT_SENSOR_ID); - // backSensor = new DigitalInput(HOPPER_BACK_SENSOR_ID); - configMotors(); + super(STALL_CURRENT, HPPR_INTAKE_POWER, HPPR_OUTTAKE_POWER, HPPR_STALL_POWER, 0.3, HPPR_MOTOR); + + configTriggers(); // initShuffleboard(); + + setDefaultCommand(setState(HopperState.IDLE)); } @Override protected void configMotors() { - HOPPER_MOTOR.enableVoltageCompensation(VOLT_COMP); - HOPPER_MOTOR.setCurrentLimit(CURRENT_LIMIT); - - HOPPER_MOTOR.setNeutralMode(Neutral.COAST); + HPPR_MOTOR.setUnitConversionFactor(HPPR_GEAR_RATIO); + HPPR_MOTOR.setInverted(HPPR_MOTOR_INVERT); + HPPR_MOTOR.setCurrentLimit(HPPR_CURRENT_LIMIT); + HPPR_MOTOR.setNeutralMode(HPPR_NEUTRAL_MODE); + HPPR_MOTOR.setStatusFrames(HPPR_STATUS_FRAME); + HPPR_MOTOR.enableVoltageCompensation(HPPR_VOLT_COMP); + } - HOPPER_MOTOR.setStatusFrames(SparkMaxConfig.VELOCITY); + public void configTriggers() { + new Trigger(()-> !hasObjectPresent()) + .and(()-> Intake.getInstance().goalStateIs(IntakeState.INTAKE)) + .onTrue(setState(HopperState.INTAKE)); + + new Trigger(()-> !Shooter.getInstance().hasObjectPresent()) + .and(()-> hasObjectPresent()) + .onTrue(setState(HopperState.HAND_OFF)); + + // necessary to allow for two note intaking + new Trigger(()-> Shooter.getInstance().hasObjectPresent()) + .and(()-> hasObjectPresent()) + .or(()->Intake.getInstance().goalStateIs(IntakeState.RETRACTED) && + Shooter.getInstance().goalStateIs(ShooterState.IDLE)) + .onTrue(setState(HopperState.IDLE)); } @Override public boolean hasObjectPresent() { - return noteInFront(); - // return noteInFront() || noteInBack(); + return !HPPR_SENSOR.get(); } - // shooter side - public boolean noteInFront() { - return !frontSensor.get(); + public Command setState(HopperState state) { + goalState = state; + return runManipulator(state.getPower()); } - // public Command runHopper(double power){ - // return runOnce(() -> HOPPER_MOTOR.set(power)); - // } - - // public Command intakeHopper(){ - // return runHopper(HOPPER_INTAKE_POWER); - // } - - // public Command outtakeHopper(){ - // return runHopper(HOPPER_OUTTAKE_POWER); - // } - - // public Command stopHopper(){ - // return runHopper(0); - // } + public HopperState getGoalState() { + return goalState; + } - // // intake side - // public boolean noteInBack() { - // return !backSensor.get(); - // } + public boolean goalStateIs(HopperState state) { + return goalState == state; + } } diff --git a/src/main/java/frc/team3128/subsystems/Intake.java b/src/main/java/frc/team3128/subsystems/Intake.java index 85745d3..32336b0 100644 --- a/src/main/java/frc/team3128/subsystems/Intake.java +++ b/src/main/java/frc/team3128/subsystems/Intake.java @@ -1,32 +1,43 @@ package frc.team3128.subsystems; +import static frc.team3128.Constants.AmperConstants.POSITION_MAX; import static frc.team3128.Constants.IntakeConstants.*; -import common.core.controllers.TrapController; import common.core.subsystems.PivotTemplate; -import common.hardware.motorcontroller.NAR_CANSpark.SparkMaxConfig; -import common.hardware.motorcontroller.NAR_Motor.Neutral; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.button.Trigger; import static edu.wpi.first.wpilibj2.command.Commands.*; public class Intake extends PivotTemplate{ - public enum Setpoint { - GROUND(200), - SOURCE(60), - NEUTRAL(0); + public enum IntakeState { + INTAKE(POSITION_MAX, INTAKE_POWER), + OUTTAKE(POSITION_MAX, OUTTAKE_POWER), // lmao dont need + SOURCE(60, INTAKE_POWER), + RETRACTED(0, -0.3); - public final double angle; - private Setpoint(double angle) { + private final double angle; + private final double power; + + private IntakeState(double angle, double power) { this.angle = angle; + this.power = power; + } + + public double getAngle() { + return angle; + } + + public double getPower() { + return power; } } private static Intake instance; + private static IntakeState goalState; public static synchronized Intake getInstance() { if (instance == null) @@ -35,68 +46,63 @@ public static synchronized Intake getInstance() { } private Intake() { - super(new TrapController(PIDConstants, TRAP_CONSTRAINTS), PIVOT_MOTOR); + super(CONTROLLER, PIVT_MOTOR); setkG_Function(()-> Math.cos(Units.degreesToRadians(getSetpoint()))); - setTolerance(ANGLE_TOLERANCE); - setConstraints(MIN_SETPOINT, MAX_SETPOINT); - setSafetyThresh(5); - // initShuffleboard(); - - //TODO: remove once done testing - this.setSafetyThresh(1000); + configController(); + configTriggers(); + initShuffleboard(); + setDefaultCommand(setState(IntakeState.RETRACTED)); } @Override protected void configMotors() { - PIVOT_MOTOR.setInverted(false); - PIVOT_MOTOR.setUnitConversionFactor(UNIT_CONV_FACTOR); - PIVOT_MOTOR.setNeutralMode(Neutral.COAST); - PIVOT_MOTOR.setStatusFrames(SparkMaxConfig.POSITION); - - // ROLLER_MOTOR.setInverted(false); - // ROLLER_MOTOR.enableVoltageCompensation(VOLT_COMP); - // ROLLER_MOTOR.setNeutralMode(Neutral.COAST); - // ROLLER_MOTOR.setCurrentLimit(CURRENT_LIMIT); - // ROLLER_MOTOR.setStatusFrames(SparkMaxConfig.VELOCITY); + PIVT_MOTOR.setInverted(PIVT_MOTOR_INVERT); + PIVT_MOTOR.setCurrentLimit(PIVT_CURRENT_LIMIT); + PIVT_MOTOR.setUnitConversionFactor(PIVT_GEAR_RATIO); + PIVT_MOTOR.setStatusFrames(PIVT_STATUS_FRAME); + PIVT_MOTOR.setNeutralMode(PIVT_NEUTRAL_MODE); + + ROLLER_MOTOR.setInverted(ROLLER_MOTOR_INVERT); + ROLLER_MOTOR.setNeutralMode(ROLLER_NEUTRAL_MODE); + ROLLER_MOTOR.setUnitConversionFactor(ROLLER_GEAR_RATIO); + ROLLER_MOTOR.setStatusFrames(ROLLER_STATUS_FRAME); + ROLLER_MOTOR.setNeutralMode(ROLLER_NEUTRAL_MODE); + ROLLER_MOTOR.enableVoltageCompensation(ROLLER_VOLT_COMP); } - public Command retract() { - return sequence( - pivotTo(Setpoint.NEUTRAL), - waitUntil(()-> atSetpoint()).withTimeout(1.5), - runPivot(-0.2), - waitSeconds(0.1), - runPivot(0), - reset(0) - ); + public void configController(){ + CONTROLLER.setConstraints(POSITION_MIN, POSITION_MAX); + CONTROLLER.setTolerance(POSITION_TOLERANCE); + this.setSafetyThresh(2); } - @Override - public double getMeasurement() { - return -1 * motors[0].getPosition(); + public void configTriggers(){ + new Trigger(()-> Shooter.getInstance().hasObjectPresent()) + .and(()-> Hopper.getInstance().hasObjectPresent()) + .onTrue(setState(IntakeState.RETRACTED)); } - public Command pivotTo (Setpoint setpoint) { - return pivotTo(setpoint.angle); - } - - public Command runIntakeRollers() { - return runRollers(INTAKE_POWER); + public Command runRollers(double power) { + return runOnce(()-> ROLLER_MOTOR.set(power)); } - public Command runOuttakeRollers() { - return runRollers(OUTTAKE_POWER); + public Command setState(IntakeState state) { + goalState = state; + return sequence( + pivotTo(state.getAngle()), + runRollers(state.getPower()) + ); } - public Command stopRollers() { - return runRollers(0); + public IntakeState getGoalState() { + return goalState; } - public Command runRollers(double power) { - return runOnce(()-> ROLLER_MOTOR.set(power)); + public boolean goalStateIs(IntakeState state) { + return goalState == state; } } \ No newline at end of file diff --git a/src/main/java/frc/team3128/subsystems/Shooter.java b/src/main/java/frc/team3128/subsystems/Shooter.java index 36c6ef0..7215bbc 100644 --- a/src/main/java/frc/team3128/subsystems/Shooter.java +++ b/src/main/java/frc/team3128/subsystems/Shooter.java @@ -2,123 +2,142 @@ import common.core.controllers.Controller; import common.core.controllers.Controller.Type; -import common.core.controllers.ControllerBase; import common.core.subsystems.ShooterTemplate; -import common.hardware.motorcontroller.NAR_CANSpark.SparkMaxConfig; -import common.hardware.motorcontroller.NAR_Motor.Neutral; -import common.utility.narwhaldashboard.NarwhalDashboard.State; -import common.utility.shuffleboard.NAR_Shuffleboard; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.simulation.DIOSim; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.team3128.subsystems.Amper.AmpState; +import static edu.wpi.first.wpilibj2.command.Commands.*; import static frc.team3128.Constants.ShooterConstants.*; import java.util.function.DoubleSupplier; public class Shooter extends ShooterTemplate { - private static Shooter instance; + public enum ShooterState{ + IDLE(0, 0, null), + SHOOT(SHOOTER_RPM, KICK_POWER, Swerve.getInstance()::getTurnAngle), //set to null if not working + EDGE_FEED(EDGE_FEED_RPM, KICK_POWER, ()-> EDGE_FEED_ANGLE), + MIDDLE_FEED(MIDDLE_FEED_RPM, KICK_POWER, ()-> MIDDLE_FEED_ANGLE), + PRIMED(AMP_RPM, 0, ()-> AMP_ANGLE), + AMP(AMP_RPM, KICK_POWER, ()-> AMP_ANGLE), + HAND_OFF(0, KICK_POWER, ()-> AMP_ANGLE); + + private final double rmp; // can change to power if you want + private final double kickPower; + private DoubleSupplier robotAngle; + + private ShooterState(double rmp, double kickPower, DoubleSupplier robotAngle){ + this.rmp = rmp; + this.kickPower = kickPower; + this.robotAngle = robotAngle; + } + + public double getRPM(){ + return rmp; + } + + public double getKickPower(){ + return kickPower; + } + + public DoubleSupplier getRobotAngle(){ + return robotAngle; + } + } - private DoubleSupplier kF_Func; + private static Shooter instance; + private static ShooterState goalState; - private DigitalInput kickSensor, rollersSensor; + public static synchronized Shooter getInstance(){ + if (instance == null) + instance = new Shooter(); + return instance; + } private Shooter() { - super(new Controller(PIDConstants, Type.VELOCITY), SHOOTER_MOTOR); - setConstraints(MIN_RPM, MAX_RPM); - setTolerance(TOLERANCE); - - // kickSensor = new DigitalInput(KICK_SENSOR_ID); - rollersSensor = new DigitalInput(ROLLERS_SENSOR_ID); - // NAR_Shuffleboard.addData + super(new Controller(PIDConstants, Type.VELOCITY), SHTR_MOTOR); configMotors(); - // initShuffleboard(); - - ControllerBase controller = getController(); - controller.setkS(()-> controller.getkS()); - controller.setkV(()-> controller.getkV()); - controller.setkG(()-> controller.getkG()); - controller.setTolerance(TOLERANCE); - } + configController(); + configTriggers(); + initShuffleboard(); - public static synchronized Shooter getInstance(){ - if (instance == null) - instance = new Shooter(); - return instance; + setDefaultCommand(setState(ShooterState.IDLE)); } @Override protected void configMotors() { - SHOOTER_MOTOR.setCurrentLimit(80); - KICK_MOTOR.setCurrentLimit(80); - - SHOOTER_MOTOR.setInverted(false); - KICK_MOTOR.setInverted(false); + SHTR_MOTOR.setInverted(SHTR_MOTOR_INVERT); + SHTR_MOTOR.setCurrentLimit(SHTR_CURRENT_LIMIT); + SHTR_MOTOR.setUnitConversionFactor(SHTR_GEAR_RATIO); + SHTR_MOTOR.setStatusFrames(SHTR_STATUS_FRAME); + SHTR_MOTOR.setNeutralMode(SHTR_NEUTRAL_MODE); + + KICK_MOTOR.setInverted(KICK_MOTOR_INVERT); + KICK_MOTOR.setCurrentLimit(KICK_CURRENT_LIMIT); + KICK_MOTOR.setNeutralMode(KICK_NEUTRAL_MODE); + KICK_MOTOR.setUnitConversionFactor(KICK_GEAR_RATIO); + KICK_MOTOR.setStatusFrames(KICK_STATUS_FRAME); + } - SHOOTER_MOTOR.setUnitConversionFactor(GEAR_RATIO); + public void configController(){ + getController().setConstraints(VELOCITY_MIN, VELOCITY_MAX); + getController().setTolerance(TOLERANCE); + setSafetyThresh(2); + } - SHOOTER_MOTOR.setNeutralMode(Neutral.COAST); - KICK_MOTOR.setNeutralMode(Neutral.COAST); + public void configTriggers(){ + new Trigger(()-> hasObjectPresent()) + .and(()-> Amper.getInstance().goalStateIs(AmpState.PRIMED)) + .onTrue(setState(ShooterState.PRIMED)); - SHOOTER_MOTOR.setStatusFrames(SparkMaxConfig.VELOCITY); - KICK_MOTOR.setStatusFrames(SparkMaxConfig.VELOCITY); - } + new Trigger(()-> hasObjectPresent()) + .and(()-> Amper.getInstance().goalStateIs(AmpState.AMP)) + .and(()-> Amper.getInstance().atSetpoint()) + .onTrue(setState(ShooterState.AMP)); + + new Trigger(()-> !hasObjectPresent()) + .and(()-> !Hopper.getInstance().hasObjectPresent()) + .onTrue(setState(ShooterState.IDLE)); + + new Trigger(()-> goalStateIs(ShooterState.IDLE)) + .and(()-> Hopper.getInstance().goalStateIs(Hopper.HopperState.IDLE)) + .onTrue(setState(ShooterState.HAND_OFF)); - //NOT USING PID FOR NOW - // @Override - // public void startPID(double setpoint) { - // enable(); - // getController().setSetpoint(setpoint); - // } - - // @Override - // protected void useOutput(double output, double setpoint) { - // SHOOTER_MOTOR.setVolts(setpoint != 0 ? output + kF_Func.getAsDouble() : 0); - // } - - // @Override - // public double getMeasurement() { - // return SHOOTER_MOTOR.getVelocity(); - // } - - // public Command shoot(double setpoint) { - // return runOnce(()-> startPID(setpoint)); - // } - - public Command rampUpShooter(){ - return runShooter(SHOOTER_RPM); + new Trigger(()-> hasObjectPresent()) + .and(()-> goalStateIs(ShooterState.HAND_OFF)) + .onTrue(setState(ShooterState.IDLE)); } - public Command runShooter(double power) { - return runOnce(()-> SHOOTER_MOTOR.set(power)); + public boolean hasObjectPresent(){ + return !SHTR_SENSOR.get(); } public Command runKickMotor(double power) { return runOnce(() -> KICK_MOTOR.set(power)); } - public Command stopMotors(){ - return runOnce(()-> { - SHOOTER_MOTOR.set(0); - KICK_MOTOR.set(0); - }); + public Command setState(ShooterState state){ + goalState = state; + return parallel( + sequence( + shoot(state.getRPM()), + waitUntil(()-> atSetpoint()) + ), + either( // comment this out if you dont want auto rotate when state change + Swerve.getInstance().turnInPlace(state.getRobotAngle(), 2), + none(), + ()-> state.getRobotAngle() != null + ) + ).andThen(runKickMotor(state.getKickPower())); } - // public State getRunningState() { - // if (SHOOTER_MOTOR.getState() != State.DISCONNECTED && KICK_MOTOR.getState() != State.DISCONNECTED) - // return State.RUNNING; - // if (SHOOTER_MOTOR.getState() != State.DISCONNECTED || KICK_MOTOR.getState() != State.DISCONNECTED) - // return State.PARTIALLY_RUNNING; - // return State.DISCONNECTED; - // } - - // public boolean noteInKick() { - // return !kickSensor.get(); - // } + public ShooterState getGoalState(){ + return goalState; + } - public boolean noteInRollers() { - return !rollersSensor.get(); + public boolean goalStateIs(ShooterState state){ + return goalState == state; } } diff --git a/src/main/java/frc/team3128/subsystems/Swerve.java b/src/main/java/frc/team3128/subsystems/Swerve.java index f4a6725..ee42efb 100644 --- a/src/main/java/frc/team3128/subsystems/Swerve.java +++ b/src/main/java/frc/team3128/subsystems/Swerve.java @@ -23,11 +23,11 @@ import frc.team3128.Robot; import frc.team3128.RobotContainer; import frc.team3128.Constants.FieldConstants; -import frc.team3128.Constants.ShooterConstants; import frc.team3128.commands.CmdSwerveDrive; import static frc.team3128.Constants.SwerveConstants.*; import static frc.team3128.Constants.FocalAimConstants.*; +import static frc.team3128.Constants.FieldConstants.*; public class Swerve extends SwerveBase { @@ -59,8 +59,8 @@ private Swerve() { gyro.optimizeBusUtilization(); initShuffleboard(); - NAR_Shuffleboard.addData("Testing", "Name", ()-> getDist(speakerMidpointBlue), 0, 0); - NAR_Shuffleboard.addData("Testing", "Dist", ()-> getDistHorizontal(), 0, 1); + // NAR_Shuffleboard.addData("Testing", "Name", ()-> getDist(speakerMidpointBlue), 0, 0); + // NAR_Shuffleboard.addData("Testing", "Dist", ()-> getDistHorizontal(), 0, 1); NAR_Shuffleboard.addData("Auto", "Setpoint", ()-> TURN_CONTROLLER.atSetpoint()); initStateCheck(); } @@ -103,33 +103,6 @@ public void zeroGyro(double reset) { gyro.setYaw(reset); } - public double getPredictedDistance() { - final ChassisSpeeds velocity = getFieldVelocity(); - final Translation2d predictedPos = getPredictedPosition(velocity, RAMP_TIME); - final double shotTime = getProjectileTime(getDist(predictedPos)); - final Translation2d target = calculateTarget(Robot.getAlliance() == Alliance.Red ? speakerMidpointRed : speakerMidpointBlue, velocity, shotTime); - final double distance = getDist(predictedPos, target); - return distance; - } - - public double getPredictedAngle() { - final ChassisSpeeds velocity = getFieldVelocity(); - final Translation2d predictedPos = getPredictedPosition(velocity, RAMP_TIME); - final double shotTime = getProjectileTime(getDist(predictedPos)); - final Translation2d target = calculateTarget(Robot.getAlliance() == Alliance.Red ? speakerMidpointRed : speakerMidpointBlue, velocity, shotTime); - final double angle = getTurnAngle(predictedPos, target); - return angle; - } - - public Translation2d getPredictedPosition(ChassisSpeeds velocity, double time) { - final Translation2d currentPosition = getPose().getTranslation(); - return currentPosition.plus(new Translation2d(velocity.vxMetersPerSecond * time, velocity.vyMetersPerSecond * time)); - } - - public double getProjectileTime(double distance) { - return distance / ShooterConstants.PROJECTILE_SPEED; - } - public Translation2d calculateTarget(Translation2d target, ChassisSpeeds velocity, double time) { return target.minus(new Translation2d(0, velocity.vyMetersPerSecond * time)); } @@ -141,7 +114,7 @@ public double getDistHorizontal() { } public double getDist() { - return getDist(Robot.getAlliance() == Alliance.Red ? speakerMidpointRed : speakerMidpointBlue); + return getDist(allianceFlip(focalPoint)); } public double getDist(Translation2d point) { @@ -153,7 +126,7 @@ public double getDist(Translation2d point1, Translation2d point2) { } public double getTurnAngle() { - return getTurnAngle(Robot.getAlliance() == Alliance.Red ? focalPointRed : focalPointBlue); + return getTurnAngle(allianceFlip(focalPoint)); } public double getTurnAngle(Translation2d target) { @@ -162,14 +135,10 @@ public double getTurnAngle(Translation2d target) { } public double getTurnAngle(Translation2d robotPos, Translation2d targetPos) { - return Math.toDegrees(Math.atan2(targetPos.getY() - robotPos.getY(), targetPos.getX() - robotPos.getX())) + angleOffset; - } - - public Command turnInPlace(boolean moving) { - return turnInPlace(()-> moving ? getPredictedAngle() : getTurnAngle()); + return Math.toDegrees(Math.atan2(targetPos.getY() - robotPos.getY(), targetPos.getX() - robotPos.getX())); } - public Command turnInPlace(DoubleSupplier setpoint) { + public Command turnInPlace(DoubleSupplier setpoint, double timeout) { return new NAR_PIDCommand( TURN_CONTROLLER, ()-> getYaw(), //measurement @@ -187,6 +156,7 @@ public Command turnInPlace(DoubleSupplier setpoint) { Swerve.getInstance().drive(translation, Units.degreesToRadians(output), true); }, + timeout, Swerve.getInstance() ).beforeStarting(runOnce(()-> CmdSwerveDrive.disableTurn())); } diff --git a/vendordeps/3128-common.json b/vendordeps/3128-common.json index ed3d581..6c66d6f 100644 --- a/vendordeps/3128-common.json +++ b/vendordeps/3128-common.json @@ -1,6 +1,6 @@ { "name": "3128-common", - "version": "1.8.5", + "version": "1.8.8", "uuid": "ae3fa5a2-78d9-47e8-921a-dba45b889445", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.github.Team3128", "artifactId": "3128-common", - "version": "1.8.5" + "version": "1.8.8" } ], "jniDependencies": [], diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index d4bf7dd..63dacbb 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "3.2.0", + "version": "3.2.1", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2024", "mavenUrls": [], @@ -10,24 +10,24 @@ { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "wpilib-shim", - "version": "3.2.0" + "version": "3.2.1" }, { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "junction-core", - "version": "3.2.0" + "version": "3.2.1" }, { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-api", - "version": "3.2.0" + "version": "3.2.1" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-wpilibio", - "version": "3.2.0", + "version": "3.2.1", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json index 2b7d172..0322385 100644 --- a/vendordeps/Phoenix6.json +++ b/vendordeps/Phoenix6.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix6.json", "name": "CTRE-Phoenix (v6)", - "version": "24.2.0", + "version": "24.3.0", "frcYear": 2024, "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "24.2.0" + "version": "24.3.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -39,7 +39,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -52,7 +52,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -65,7 +65,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -78,7 +78,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -91,7 +91,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -104,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -117,7 +117,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -130,7 +130,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -143,7 +143,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.2.0", + "version": "24.3.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -158,7 +158,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -173,7 +173,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -188,7 +188,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -203,7 +203,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -218,7 +218,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -233,7 +233,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -248,7 +248,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -263,7 +263,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -278,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -293,7 +293,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -308,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -323,7 +323,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "24.2.0", + "version": "24.3.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true,