mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 00:38:05 -06:00
Merge branch 'swerve' into Shooter
This commit is contained in:
Aarav Shah
@@ -0,0 +1,11 @@
|
||||
{
|
||||
"SysId": {
|
||||
"Analysis Type": "General Mechanism",
|
||||
"AnalyzerJSONLocation": "C:\\Users\\Ridgebotics\\Documents\\GitHub\\2022NoWayHome\\sysid\\sysid_data20220128-205132.json",
|
||||
"NetworkTables Settings": {
|
||||
"serverTeam": "4388"
|
||||
},
|
||||
"mode": "Client",
|
||||
"serverTeam": "4388"
|
||||
}
|
||||
}
|
||||
Vendored
-1
@@ -4,7 +4,6 @@
|
||||
// For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
|
||||
"version": "0.2.0",
|
||||
"configurations": [
|
||||
|
||||
{
|
||||
"type": "wpilib",
|
||||
"name": "WPILib Desktop Debug",
|
||||
|
||||
Vendored
+1
-1
@@ -26,5 +26,5 @@
|
||||
},
|
||||
],
|
||||
"java.test.defaultConfig": "WPIlibUnitTests",
|
||||
"java.dependency.packagePresentation": "flat"
|
||||
"java.dependency.packagePresentation": "hierarchical"
|
||||
}
|
||||
|
||||
@@ -27,6 +27,8 @@ deploy {
|
||||
// getTargetTypeClass is a shortcut to get the class type using a string
|
||||
|
||||
frcJava(getArtifactTypeClass('FRCJavaArtifact')) {
|
||||
// Enable visualvm
|
||||
//jvmArgs << "-Dcom.sun.management.jmxremote=true -Dcom.sun.management.jmxremote.port=1099 -Dcom.sun.management.jmxremote.local.only=false -Dcom.sun.management.jmxremote.ssl=false -Dcom.sun.management.jmxremote.authenticate=false -Djava.rmi.server.hostname=10.43.88.2"
|
||||
}
|
||||
|
||||
// Static files artifact
|
||||
@@ -34,6 +36,7 @@ deploy {
|
||||
files = project.fileTree('src/main/deploy')
|
||||
directory = '/home/lvuser/deploy'
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -52,6 +55,7 @@ def includeDesktopSupport = true
|
||||
dependencies {
|
||||
implementation wpi.java.deps.wpilib()
|
||||
implementation wpi.java.vendor.java()
|
||||
implementation 'org.fusesource.jansi:jansi:2.4.0'
|
||||
|
||||
roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio)
|
||||
roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio)
|
||||
|
||||
+4
-3
@@ -91,9 +91,10 @@
|
||||
],
|
||||
"robotJoysticks": [
|
||||
{
|
||||
"guid": "78696e70757401000000000000000000"
|
||||
},
|
||||
{
|
||||
// "guid": "78696e70757401000000000000000000"
|
||||
// },
|
||||
// {
|
||||
"guid": "78696e70757401000000000000000000",
|
||||
"useGamepad": true
|
||||
}
|
||||
]
|
||||
|
||||
+12
@@ -2,16 +2,19 @@
|
||||
"HALProvider": {
|
||||
"Other Devices": {
|
||||
"SPARK MAX [10]": {
|
||||
"Talon FX[2]": {
|
||||
"header": {
|
||||
"open": true
|
||||
}
|
||||
},
|
||||
"SPARK MAX [30]": {
|
||||
"Talon FX[3]": {
|
||||
"header": {
|
||||
"open": true
|
||||
}
|
||||
},
|
||||
"SPARK MAX [31]": {
|
||||
"Talon FX[4]": {
|
||||
"header": {
|
||||
"open": true
|
||||
}
|
||||
@@ -29,6 +32,15 @@
|
||||
"/LiveWindow/Turret": "Subsystem",
|
||||
"/LiveWindow/Ungrouped/Scheduler": "Scheduler",
|
||||
"/LiveWindow/Vision": "Subsystem"
|
||||
"/SmartDashboard/Driver Controller": "Mechanism2d",
|
||||
"/SmartDashboard/Field": "Field2d"
|
||||
},
|
||||
"windows": {
|
||||
"/SmartDashboard/Field": {
|
||||
"window": {
|
||||
"visible": true
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
"NetworkTables": {
|
||||
|
||||
@@ -0,0 +1 @@
|
||||
{"waypoints":[{"anchorPoint":{"x":3.0,"y":3.0},"prevControl":null,"nextControl":{"x":3.410442907302426,"y":3.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.5,"y":4.5},"prevControl":{"x":4.288229266113705,"y":4.529967680625016},"nextControl":{"x":4.67537248192198,"y":4.475183036710464},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":6.0,"y":3.0},"prevControl":{"x":5.973638727554094,"y":3.050086417647222},"nextControl":{"x":6.10510365848318,"y":2.8003030488819602},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.5,"y":1.5029823163094613},"prevControl":{"x":4.6862524414704,"y":1.722990549429376},"nextControl":{"x":4.3137475585296,"y":1.2829740831895466},"holonomicAngle":180.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.0,"y":3.0},"prevControl":{"x":3.5480060960121915,"y":2.4748364324314855},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":5.0,"maxAcceleration":5.0,"isReversed":null}
|
||||
@@ -0,0 +1 @@
|
||||
{"waypoints":[{"anchorPoint":{"x":6.888836249581911,"y":5.215172677538834},"prevControl":null,"nextControl":{"x":6.537459367052471,"y":6.518965846916928},"holonomicAngle":106.3895403340348,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.8254588419278255,"y":7.8320057763675734},"prevControl":{"x":5.95491348285958,"y":6.759381608647329},"nextControl":{"x":5.95491348285958,"y":6.759381608647329},"holonomicAngle":-136.27303002005684,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.085718036603518,"y":6.18608248452099},"prevControl":{"x":5.222728278411091,"y":6.456690985532331},"nextControl":{"x":4.768933589337623,"y":5.5604025113498725},"holonomicAngle":-109.50244850666218,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.77803093326875,"y":3.4953054389840252},"prevControl":{"x":5.623399779842569,"y":3.7042882657946703},"nextControl":{"x":2.9363292766883204,"y":3.040020127851594},"holonomicAngle":-30.256437163529373,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.7486154042034336,"y":2.667447141251462},"prevControl":{"x":1.8144326730676037,"y":2.686839729398941},"nextControl":{"x":0.7297128839172015,"y":2.3672347915242677},"holonomicAngle":-75.06858282186249,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.2129730034385093,"y":1.1845800804778424},"prevControl":{"x":1.2001001569914929,"y":1.2299758967179297},"nextControl":{"x":1.530279149856384,"y":0.06560677694911808},"holonomicAngle":-39.472459848343846,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.160119378376086,"y":1.9305622828301685},"prevControl":{"x":5.131309514738882,"y":1.9153315905888442},"nextControl":{"x":6.588402375563037,"y":2.6856418291137163},"holonomicAngle":36.02737338510347,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.60730489584927,"y":0.37491647060743843},"prevControl":{"x":7.329902093852659,"y":0.5875919521381767},"nextControl":{"x":7.884707697845882,"y":0.16224098907670015},"holonomicAngle":-41.00908690157027,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":8.617110072204374,"y":0.9389517943373179},"prevControl":{"x":8.535233976824232,"y":0.12019084053588047},"nextControl":{"x":8.698986167584517,"y":1.7577127481387553},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":9.144756020209744,"y":0.31123506308954896},"prevControl":{"x":9.199340083796505,"y":0.7206155399902687},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}]}
|
||||
@@ -0,0 +1 @@
|
||||
{"waypoints":[{"anchorPoint":{"x":8.334720117716095,"y":1.881355486848905},"prevControl":null,"nextControl":{"x":8.61849999562068,"y":1.250733535949832},"holonomicAngle":-90.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":8.019409142266559,"y":0.26275914620794727},"prevControl":{"x":8.43030614852931,"y":0.2186359777502019},"nextControl":{"x":6.453364630867194,"y":0.430924999781035},"holonomicAngle":178.16716049405798,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.118548168124668,"y":1.9969695111773362},"prevControl":{"x":5.520125502663889,"y":1.354445775900491},"nextControl":{"x":4.750685363439692,"y":2.5855499986862043},"holonomicAngle":124.12854707414519,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.3768579261296454,"y":1.187671340859924},"prevControl":{"x":0.7357256093822531,"y":0.7147048776856182},"nextControl":{"x":2.0179902428770378,"y":1.6606378040342298},"holonomicAngle":-147.6118851465991,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.971403046254368,"y":6.06448109447943},"prevControl":{"x":3.647096949366312,"y":4.256698168568751},"nextControl":null,"holonomicAngle":47.68377515946898,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}
|
||||
@@ -0,0 +1,37 @@
|
||||
{
|
||||
"waypoints": [
|
||||
{
|
||||
"anchorPoint": {
|
||||
"x": 5.0,
|
||||
"y": 2.0
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 5.004218182347978,
|
||||
"y": 2.0
|
||||
},
|
||||
"holonomicAngle": 0.0,
|
||||
"isReversal": false,
|
||||
"velOverride": null,
|
||||
"isLocked": false
|
||||
},
|
||||
{
|
||||
"anchorPoint": {
|
||||
"x": 5.0,
|
||||
"y": 5.0
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 4.991875448297241,
|
||||
"y": 5.0
|
||||
},
|
||||
"nextControl": null,
|
||||
"holonomicAngle": 0.0,
|
||||
"isReversal": false,
|
||||
"velOverride": null,
|
||||
"isLocked": false
|
||||
}
|
||||
],
|
||||
"maxVelocity": 1.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"isReversed": null
|
||||
}
|
||||
@@ -0,0 +1,55 @@
|
||||
{
|
||||
"waypoints": [
|
||||
{
|
||||
"anchorPoint": {
|
||||
"x": 2.0,
|
||||
"y": 2.0
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 3.0,
|
||||
"y": 2.0
|
||||
},
|
||||
"holonomicAngle": 0.0,
|
||||
"isReversal": false,
|
||||
"velOverride": null,
|
||||
"isLocked": false
|
||||
},
|
||||
{
|
||||
"anchorPoint": {
|
||||
"x": 5.0,
|
||||
"y": 2.0
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 4.000000000000002,
|
||||
"y": 2.0
|
||||
},
|
||||
"nextControl": {
|
||||
"x": 5.004218182347978,
|
||||
"y": 2.0
|
||||
},
|
||||
"holonomicAngle": 0.0,
|
||||
"isReversal": false,
|
||||
"velOverride": null,
|
||||
"isLocked": false
|
||||
},
|
||||
{
|
||||
"anchorPoint": {
|
||||
"x": 5.0,
|
||||
"y": 5.0
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 4.991875448297241,
|
||||
"y": 5.0
|
||||
},
|
||||
"nextControl": null,
|
||||
"holonomicAngle": 0.0,
|
||||
"isReversal": false,
|
||||
"velOverride": null,
|
||||
"isLocked": false
|
||||
}
|
||||
],
|
||||
"maxVelocity": 1.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"isReversed": null
|
||||
}
|
||||
@@ -0,0 +1 @@
|
||||
{"waypoints":[{"anchorPoint":{"x":5.0,"y":5.0},"prevControl":null,"nextControl":{"x":5.055485973040912,"y":4.3828225587485665},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":2.0},"prevControl":{"x":5.0,"y":3.1241654489412927},"nextControl":{"x":5.0,"y":3.1241654489412927},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":5.0},"prevControl":{"x":5.0,"y":3.0836227700747108},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}
|
||||
@@ -0,0 +1 @@
|
||||
{"waypoints":[{"anchorPoint":{"x":2.0,"y":2.0},"prevControl":null,"nextControl":{"x":2.014863901928194,"y":2.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":2.0},"prevControl":{"x":4.99458955727988,"y":2.0},"nextControl":{"x":4.99458955727988,"y":2.0},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":2.0,"y":2.0},"prevControl":{"x":1.4597663280316806,"y":2.0},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}
|
||||
@@ -0,0 +1 @@
|
||||
{"waypoints":[{"anchorPoint":{"x":1.0,"y":3.0},"prevControl":null,"nextControl":{"x":2.3149413387434365,"y":2.75478019310469},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.143744996350752,"y":3.8428026223141054},"prevControl":{"x":1.0069994823533945,"y":5.5442845062905315},"nextControl":{"x":7.478686694138279,"y":2.0338121072780666},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.870800551727686,"y":4.710905624342894},"prevControl":{"x":6.870800551727686,"y":4.710905624342894},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}]}
|
||||
@@ -0,0 +1 @@
|
||||
{"waypoints":[{"anchorPoint":{"x":3.0,"y":5.0},"prevControl":null,"nextControl":{"x":3.018133524616885,"y":5.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.0,"y":5.0},"prevControl":{"x":2.9945895572798564,"y":5.0},"nextControl":null,"holonomicAngle":-180.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":false}
|
||||
@@ -0,0 +1 @@
|
||||
{"waypoints":[{"anchorPoint":{"x":3.1215786569504176,"y":1.6921689015791839},"prevControl":null,"nextControl":{"x":1.7026792674275004,"y":1.7552310966690905},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.20566158373696,"y":3.3948481690066825},"prevControl":{"x":1.9969695111804016,"y":3.342296339765093},"nextControl":{"x":4.204717742092026,"y":3.4382853932829898},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.076506704737548,"y":2.701164023017701},"prevControl":{"x":4.771706095136329,"y":3.037495730163874},"nextControl":{"x":5.592408962233515,"y":2.131892566470425},"holonomicAngle":92.0952525645957,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":6.884289630648579,"y":1.7762518283676958},"prevControl":{"x":5.57049389960884,"y":1.7342103649744238},"nextControl":{"x":8.154698525195206,"y":1.816904912993188},"holonomicAngle":-177.33699923393286,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":6.8002067038620355,"y":3.4473999982502432},"prevControl":{"x":8.681562190710942,"y":3.5525036567334225},"nextControl":{"x":4.918851217013129,"y":3.3422963397670644},"holonomicAngle":-177.2241965512169,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.111068291102451,"y":1.681658535732834},"prevControl":{"x":5.139568899827807,"y":1.8393140234576038},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":5.0,"maxAcceleration":5.0,"isReversed":null}
|
||||
@@ -0,0 +1 @@
|
||||
{"waypoints":[{"anchorPoint":{"x":2.0,"y":3.0},"prevControl":null,"nextControl":{"x":3.0000000000000013,"y":3.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.0,"y":5.0},"prevControl":{"x":3.0,"y":4.0},"nextControl":{"x":3.0,"y":4.0},"holonomicAngle":-90.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.0,"y":3.0},"prevControl":{"x":3.0,"y":3.0},"nextControl":{"x":3.0,"y":3.0},"holonomicAngle":180.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":2.0,"y":3.0},"prevControl":{"x":3.0271430576167138,"y":3.0},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}]}
|
||||
@@ -0,0 +1 @@
|
||||
{"waypoints":[{"anchorPoint":{"x":6.620680207650139,"y":5.35387407853639},"prevControl":null,"nextControl":{"x":6.047381083523801,"y":5.557302800000574},"holonomicAngle":169.99202019855858,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.020990716136325,"y":6.167588964393128},"prevControl":{"x":5.409354638931585,"y":5.5018222396012515},"nextControl":{"x":5.409354638931585,"y":5.5018222396012515},"holonomicAngle":129.4995885297982,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.141198597001524,"y":1.9325728539114686},"prevControl":{"x":5.085718036602201,"y":3.2825998236283294},"nextControl":null,"holonomicAngle":-77.66091272167375,"isReversal":false,"velOverride":null,"isLocked":false}]}
|
||||
@@ -0,0 +1 @@
|
||||
{"waypoints":[{"anchorPoint":{"x":8.109408578365308,"y":2.117508055242545},"prevControl":null,"nextControl":{"x":8.192629418964295,"y":1.470234850583776},"holonomicAngle":-92.202598161766,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.637823814971061,"y":0.36987040266386867},"prevControl":{"x":8.775175303157184,"y":0.48083152346251423},"nextControl":{"x":6.123786635522426,"y":0.22215945832741724},"holonomicAngle":178.6360724683971,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.141198597001524,"y":1.9048325737118068},"prevControl":{"x":5.61278336039577,"y":1.729144132447284},"nextControl":null,"holonomicAngle":143.8418145601917,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":null,"maxAcceleration":5.0,"isReversed":null}
|
||||
@@ -0,0 +1 @@
|
||||
{"waypoints":[{"anchorPoint":{"x":6.777875128781554,"y":2.4688849377715907},"prevControl":null,"nextControl":{"x":5.797718561726847,"y":2.043533974710114},"holonomicAngle":-156.37062226934333,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.2298190888492497,"y":1.174338528454054},"prevControl":{"x":1.904832573707682,"y":1.0171436073226392},"nextControl":null,"holonomicAngle":180.0,"isReversal":false,"velOverride":null,"isLocked":false}]}
|
||||
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
@@ -6,6 +6,13 @@ package frc4388.robot;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
|
||||
|
||||
import edu.wpi.first.math.controller.PIDController;
|
||||
import edu.wpi.first.math.controller.ProfiledPIDController;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
||||
|
||||
import frc4388.utility.Gains;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
|
||||
@@ -24,13 +31,16 @@ import frc4388.utility.LEDPatterns;
|
||||
*/
|
||||
public final class Constants {
|
||||
public static final class SwerveDriveConstants {
|
||||
public static final double ROTATION_SPEED = 0.1;
|
||||
public static final double ROTATION_SPEED = 4;
|
||||
public static final double WHEEL_SPEED = 0.1;
|
||||
public static final double WIDTH = 22;
|
||||
public static final double HEIGHT = 22;
|
||||
public static final double JOYSTICK_TO_METERS_PER_SECOND = 5;
|
||||
public static final double MAX_SPEED_FEET_PER_SEC = 16;
|
||||
public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20;
|
||||
public static final double WIDTH = 15.25;
|
||||
public static final double HEIGHT = 15.25;
|
||||
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11;
|
||||
public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2;
|
||||
public static final double MAX_SPEED_FEET_PER_SEC = 20; // redundant constant?
|
||||
public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20; // redundant constant?
|
||||
|
||||
//IDs
|
||||
public static final int LEFT_FRONT_STEER_CAN_ID = 2;
|
||||
public static final int LEFT_FRONT_WHEEL_CAN_ID = 3;
|
||||
public static final int RIGHT_FRONT_STEER_CAN_ID = 4;
|
||||
@@ -43,30 +53,58 @@ public final class Constants {
|
||||
public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11;
|
||||
public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;
|
||||
public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;
|
||||
// ofsets are in degrees
|
||||
//ofsets are in degrees
|
||||
public static final double LEFT_FRONT_ENCODER_OFFSET = 181.494141;
|
||||
public static final double RIGHT_FRONT_ENCODER_OFFSET = 360. - 59.238281;
|
||||
public static final double LEFT_BACK_ENCODER_OFFSET = 360. - 128.144531;
|
||||
public static final double RIGHT_BACK_ENCODER_OFFSET = 0.933594;
|
||||
// public static final double LEFT_FRONT_ENCODER_OFFSET = 180.0;
|
||||
// public static final double RIGHT_FRONT_ENCODER_OFFSET = 300.0;
|
||||
// public static final double LEFT_BACK_ENCODER_OFFSET = 360.0 - 128.0;
|
||||
// public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0;
|
||||
public static final int GYRO_ID = 14;
|
||||
|
||||
// offsets are in degrees
|
||||
// NATHAN if you truncate or round or simplify these i will cry
|
||||
public static final double LEFT_FRONT_ENCODER_OFFSET = 181.45-3.30;//181.7578125;//180.0;//315.0 +45;//180.0;
|
||||
public static final double RIGHT_FRONT_ENCODER_OFFSET = 360.-59.0625+0.18;//360.-59.0625;//315.0;//224.296875 + 90;//300.0;
|
||||
public static final double LEFT_BACK_ENCODER_OFFSET = 360.-128.222;//308.408203125;//225.0;//45.87890625;//360.0 - 128.0;
|
||||
public static final double RIGHT_BACK_ENCODER_OFFSET = 360.+2.15-3.637;//180-2.021484375;//0.0;//134.384765625 + 45;
|
||||
|
||||
// swerve PID constants
|
||||
public static final int SWERVE_SLOT_IDX = 0;
|
||||
public static final int SWERVE_PID_LOOP_IDX = 1;
|
||||
public static final int SWERVE_TIMEOUT_MS = 30;
|
||||
public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0);
|
||||
public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 1.0, 0.0, 0, 1.0);
|
||||
|
||||
// swerve auto constants
|
||||
public static final PIDController X_CONTROLLER = new PIDController(0.5, 0.0, 0.0);
|
||||
public static final PIDController Y_CONTROLLER = new PIDController(2.0, 0.0, 0.0);
|
||||
public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(
|
||||
15.0, 0.1, 0.3, new TrapezoidProfile.Constraints(Math.PI, Math.PI));
|
||||
|
||||
public static final double MAX_VEL = 5.0;
|
||||
public static final double MAX_ACC = 5.0;
|
||||
|
||||
// swerve configuration
|
||||
public static final double NEUTRAL_DEADBAND = 0.04;
|
||||
public static final double OPEN_LOOP_RAMP_RATE = 0.2;
|
||||
public static final int REMOTE_0 = 0;
|
||||
|
||||
// conversions
|
||||
// gear ratio: 5.14 rev motor = 1 rev wheel
|
||||
// wheel diameter: official = 4 in, measured = 3.8 in
|
||||
/* Ratio Calculation */
|
||||
public static final double MOTOR_REV_PER_STEER_REV = 12.8;
|
||||
public static final double MOTOR_REV_PER_WHEEL_REV = 5.142857;
|
||||
public static final double WHEEL_DIAMETER_INCHES = 4.0;
|
||||
public static final double TICKS_PER_MOTOR_REV = 2048;
|
||||
public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
|
||||
public static final double INCHES_PER_METER = 39.370;
|
||||
public static final double METERS_PER_INCH = 1/INCHES_PER_METER;
|
||||
|
||||
public static final double WHEEL_REV_PER_MOTOR_REV = 1/MOTOR_REV_PER_WHEEL_REV;
|
||||
public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV;
|
||||
public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV/INCHES_PER_WHEEL_REV;
|
||||
public static final double INCHES_PER_TICK = 1/TICKS_PER_INCH;
|
||||
public static final double TICK_TIME_TO_SECONDS = 0.1;
|
||||
public static final double SECONDS_TO_TICK_TIME = 1/TICK_TIME_TO_SECONDS;
|
||||
|
||||
// misc
|
||||
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
|
||||
// TODO: put in real numbers for the hub
|
||||
public static final Pose2d HUB_POSE = new Pose2d(new Translation2d(0, 0), new Rotation2d(0));
|
||||
}
|
||||
|
||||
public static final class LEDConstants {
|
||||
@@ -81,6 +119,8 @@ public final class Constants {
|
||||
public static final class OIConstants {
|
||||
public static final int XBOX_DRIVER_ID = 0;
|
||||
public static final int XBOX_OPERATOR_ID = 1;
|
||||
public static final double LEFT_AXIS_DEADBAND = 0.1;
|
||||
public static final double RIGHT_AXIS_DEADBAND = 0.6;
|
||||
}
|
||||
|
||||
public static final class ShooterConstants {
|
||||
|
||||
@@ -4,7 +4,9 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
import frc4388.utility.AnsiLogging;
|
||||
|
||||
/**
|
||||
* Do NOT add any static variables to this class, or any initialization at all.
|
||||
@@ -21,6 +23,9 @@ public final class Main {
|
||||
* <p>If you change your main robot class, change the parameter type.
|
||||
*/
|
||||
public static void main(String... args) {
|
||||
AnsiLogging.systemInstall();
|
||||
RobotBase.startRobot(Robot::new);
|
||||
}
|
||||
}
|
||||
|
||||
// hi ryan -aarav
|
||||
@@ -4,7 +4,12 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
import java.util.logging.Level;
|
||||
import java.util.logging.Logger;
|
||||
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
import frc4388.utility.RobotTime;
|
||||
@@ -17,6 +22,7 @@ import frc4388.utility.RobotTime;
|
||||
* project.
|
||||
*/
|
||||
public class Robot extends TimedRobot {
|
||||
private static final Logger LOGGER = Logger.getLogger(Robot.class.getName());
|
||||
Command m_autonomousCommand;
|
||||
|
||||
private RobotTime m_robotTime = RobotTime.getInstance();
|
||||
@@ -28,6 +34,17 @@ public class Robot extends TimedRobot {
|
||||
*/
|
||||
@Override
|
||||
public void robotInit() {
|
||||
if (org.fusesource.jansi.Ansi.isEnabled()) {
|
||||
LOGGER.log(Level.ALL, "Logging Test 1/8");
|
||||
LOGGER.log(Level.SEVERE, "Logging Test 2/8");
|
||||
LOGGER.log(Level.WARNING, "Logging Test 3/8");
|
||||
LOGGER.log(Level.INFO, "Logging Test 4/8");
|
||||
LOGGER.log(Level.CONFIG, "Logging Test 5/8");
|
||||
LOGGER.log(Level.FINE, "Logging Test 6/8");
|
||||
LOGGER.log(Level.FINER, "Logging Test 7/8");
|
||||
LOGGER.log(Level.FINEST, "Logging Test 8/8");
|
||||
}
|
||||
LOGGER.fine("robotInit()");
|
||||
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
|
||||
// autonomous chooser on the dashboard.
|
||||
m_robotContainer = new RobotContainer();
|
||||
@@ -49,6 +66,11 @@ public class Robot extends TimedRobot {
|
||||
// and running subsystem periodic() methods. This must be called from the robot's periodic
|
||||
// block in order for anything in the Command-based framework to work.
|
||||
CommandScheduler.getInstance().run();
|
||||
|
||||
// print odometry data to smart dashboard for debugging (if causing timeout errors, you can comment it)
|
||||
SmartDashboard.putNumber("Odometry X", m_robotContainer.getOdometry().getX());
|
||||
SmartDashboard.putNumber("Odometry Y", m_robotContainer.getOdometry().getY());
|
||||
SmartDashboard.putNumber("Odometry Theta", m_robotContainer.getOdometry().getRotation().getDegrees());
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -58,6 +80,7 @@ public class Robot extends TimedRobot {
|
||||
*/
|
||||
@Override
|
||||
public void disabledInit() {
|
||||
LOGGER.fine("disabledInit()");
|
||||
m_robotTime.endMatchTime();
|
||||
}
|
||||
|
||||
@@ -70,19 +93,9 @@ public class Robot extends TimedRobot {
|
||||
*/
|
||||
@Override
|
||||
public void autonomousInit() {
|
||||
LOGGER.fine("autonomousInit()");
|
||||
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
|
||||
|
||||
/*String autoSelected = SmartDashboard.getString("Auto Selector", "Default");
|
||||
switch (autoSelected) {
|
||||
case "My Auto":
|
||||
autonomousCommand = new MyAutoCommand();
|
||||
break;
|
||||
case "Default Auto":
|
||||
default:
|
||||
autonomousCommand = new ExampleCommand();
|
||||
break;
|
||||
}*/
|
||||
|
||||
// schedule the autonomous command (example)
|
||||
if (m_autonomousCommand != null) {
|
||||
m_autonomousCommand.schedule();
|
||||
@@ -99,6 +112,8 @@ public class Robot extends TimedRobot {
|
||||
|
||||
@Override
|
||||
public void teleopInit() {
|
||||
LOGGER.fine("teleopInit()");
|
||||
m_robotContainer.m_robotSwerveDrive.m_gyro.addYaw(-1 * m_robotContainer.m_robotSwerveDrive.m_gyro.getYaw());
|
||||
// This makes sure that the autonomous stops running when
|
||||
// teleop starts running. If you want the autonomous to
|
||||
// continue until interrupted by another command, remove
|
||||
@@ -107,6 +122,7 @@ public class Robot extends TimedRobot {
|
||||
m_autonomousCommand.cancel();
|
||||
}
|
||||
m_robotTime.startMatchTime();
|
||||
DriverStation.silenceJoystickConnectionWarning(true);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -114,7 +130,6 @@ public class Robot extends TimedRobot {
|
||||
*/
|
||||
@Override
|
||||
public void teleopPeriodic() {
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -10,6 +10,27 @@ import com.ctre.phoenix.motorcontrol.can.TalonFX;
|
||||
import edu.wpi.first.wpilibj.Joystick;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.Objects;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.can.TalonFX;
|
||||
import com.pathplanner.lib.PathPlanner;
|
||||
import com.pathplanner.lib.PathPlannerTrajectory;
|
||||
import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState;
|
||||
import com.pathplanner.lib.commands.PPSwerveControllerCommand;
|
||||
|
||||
import edu.wpi.first.math.controller.PIDController;
|
||||
import edu.wpi.first.math.controller.ProfiledPIDController;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
|
||||
import frc4388.robot.Constants.*;
|
||||
@@ -21,8 +42,7 @@ import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.robot.subsystems.Turret;
|
||||
import frc4388.robot.subsystems.Vision;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
import frc4388.utility.controller.IHandController;
|
||||
import frc4388.utility.controller.XboxController;
|
||||
import frc4388.utility.controller.DeadbandedXboxController;
|
||||
|
||||
/**
|
||||
* This class is where the bulk of the robot should be declared. Since
|
||||
@@ -35,6 +55,12 @@ public class RobotContainer {
|
||||
/* RobotMap */
|
||||
private final RobotMap m_robotMap = new RobotMap();
|
||||
|
||||
/* Subsystems */
|
||||
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(
|
||||
m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
|
||||
|
||||
private final TalonFX m_testMotor = new TalonFX(23);
|
||||
|
||||
private final LED m_robotLED = new LED(m_robotMap.LEDController);
|
||||
private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight);
|
||||
private final Hood m_robotHood = new Hood();
|
||||
@@ -53,31 +79,34 @@ public class RobotContainer {
|
||||
m_robotMap.leftBackEncoder,
|
||||
m_robotMap.rightBackEncoder);
|
||||
/* Controllers */
|
||||
private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
|
||||
private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
|
||||
private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
|
||||
private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
|
||||
|
||||
/**
|
||||
* The container for the robot. Contains subsystems, OI devices, and commands.
|
||||
*/
|
||||
public RobotContainer() {
|
||||
configureButtonBindings();
|
||||
|
||||
/* Default Commands */
|
||||
// drives the swerve drive with a two-axis input from the driver controller
|
||||
/*m_robotSwerveDrive.setDefaultCommand(
|
||||
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(),
|
||||
getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive));
|
||||
|
||||
// continually sends updates to the Blinkin LED controller to keep the lights on
|
||||
m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
|
||||
// m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
|
||||
|
||||
|
||||
*/
|
||||
//Turret default command
|
||||
|
||||
m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive));
|
||||
// m_robotTurret.setDefaultCommand(
|
||||
// new RunCommand(() -> m_robotTurret.aimToCenter()));
|
||||
|
||||
m_robotSwerveDrive.setDefaultCommand(
|
||||
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
|
||||
getDriverController().getLeftX(),
|
||||
getDriverController().getLeftY(),
|
||||
getDriverController().getRightX(),
|
||||
getDriverController().getRightY(),
|
||||
true),
|
||||
m_robotSwerveDrive));
|
||||
|
||||
// continually sends updates to the Blinkin LED controller to keep the lights on
|
||||
m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -88,10 +117,26 @@ public class RobotContainer {
|
||||
*/
|
||||
private void configureButtonBindings() {
|
||||
/* Driver Buttons */
|
||||
// "XboxController.Button.kBack" was undefiened yet, 7 works just fine
|
||||
new JoystickButton(getDriverController(), 7)
|
||||
.whenPressed(() -> m_robotSwerveDrive.resetGyro());
|
||||
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
|
||||
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.LEFT_BUMPER_BUTTON)
|
||||
.whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
|
||||
|
||||
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
|
||||
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.RIGHT_BUMPER_BUTTON)
|
||||
.whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
|
||||
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kA.value)
|
||||
|
||||
.whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
|
||||
//.whenPressed(this::resetOdometry);
|
||||
|
||||
/* Operator Buttons */
|
||||
// activates "Lit Mode"
|
||||
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
|
||||
.whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
|
||||
.whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
|
||||
@@ -111,41 +156,104 @@ public class RobotContainer {
|
||||
// new JoystickButton(getOperatorJoystick());
|
||||
}
|
||||
|
||||
/**
|
||||
* Generate autonomous
|
||||
* @param maxVel max velocity for the path (null to override default value of 5.0)
|
||||
* @param maxAccel max acceleration for the path (null to override default value of 5.0)
|
||||
* @param inputs strings (path names) or commands you want to run (in order)
|
||||
* @return array of commands, which can then be processed in a command group
|
||||
*/
|
||||
public Command[] buildAuto(Double maxVel, Double maxAccel, Object... inputs) {
|
||||
|
||||
// default vel and acc
|
||||
maxVel = Objects.requireNonNullElse(maxVel, SwerveDriveConstants.MAX_VEL);
|
||||
maxAccel = Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.MAX_ACC);
|
||||
|
||||
ArrayList<Command> commands = new ArrayList<Command>();
|
||||
commands.add(new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()));
|
||||
|
||||
// pids controlling the path
|
||||
PIDController xController = SwerveDriveConstants.X_CONTROLLER;
|
||||
PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
|
||||
ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
|
||||
thetaController.enableContinuousInput(-Math.PI, Math.PI);
|
||||
|
||||
// parse input
|
||||
for (int i=0; i<inputs.length; i++) {
|
||||
|
||||
// if string, process as pathplanner trajectory
|
||||
if (inputs[i] instanceof String) {
|
||||
|
||||
PathPlannerTrajectory traj = PathPlanner.loadPath(inputs[i].toString(), maxVel, maxAccel);
|
||||
PathPlannerState initState = (PathPlannerState) traj.sample(0);
|
||||
|
||||
commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation))));
|
||||
commands.add(new PPSwerveControllerCommand(
|
||||
traj,
|
||||
m_robotSwerveDrive::getOdometry,
|
||||
m_robotSwerveDrive.m_kinematics,
|
||||
xController,
|
||||
yController,
|
||||
thetaController,
|
||||
m_robotSwerveDrive::setModuleStates,
|
||||
m_robotSwerveDrive));
|
||||
}
|
||||
|
||||
// if command, just add it to the array
|
||||
if (inputs[i] instanceof Command) {
|
||||
commands.add((Command) inputs[i]);
|
||||
}
|
||||
}
|
||||
|
||||
commands.add(new InstantCommand(() -> m_robotSwerveDrive.stopModules()));
|
||||
Command[] ret = new Command[commands.size()];
|
||||
ret = commands.toArray(ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
/**
|
||||
* Use this to pass the autonomous command to the main {@link Robot} class.
|
||||
*
|
||||
* @return the command to run in autonomous
|
||||
*/
|
||||
public Command getAutonomousCommand() {
|
||||
// no auto
|
||||
return new InstantCommand();
|
||||
// https://github.com/mjansen4857/pathplanner/wiki <-- Pathplanner Wiki
|
||||
return new ParallelCommandGroup(
|
||||
buildAuto(
|
||||
null,
|
||||
null,
|
||||
new SequentialCommandGroup(buildAuto(0.5, 0.5, "Move Forward", "Move Down")),
|
||||
new InstantCommand(() -> m_testMotor.set(TalonFXControlMode.PercentOutput, 0.2))
|
||||
)
|
||||
);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public IHandController getDriverController() {
|
||||
public XboxController getDriverController() {
|
||||
return m_driverXbox;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get odometry.
|
||||
* @return Odometry
|
||||
*/
|
||||
public Pose2d getOdometry() {
|
||||
return m_robotSwerveDrive.getOdometry();
|
||||
}
|
||||
|
||||
/**
|
||||
* Set odometry to given pose.
|
||||
* @param pose Pose to set odometry to.
|
||||
*/
|
||||
public void resetOdometry(Pose2d pose) {
|
||||
m_robotSwerveDrive.resetOdometry(pose);
|
||||
}
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public IHandController getOperatorController() {
|
||||
public XboxController getOperatorController() {
|
||||
return m_operatorXbox;
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public Joystick getOperatorJoystick() {
|
||||
return m_operatorXbox.getJoyStick();
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public Joystick getDriverJoystick() {
|
||||
return m_driverXbox.getJoyStick();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -10,6 +10,8 @@ import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration;
|
||||
import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
import com.ctre.phoenix.sensors.CANCoder;
|
||||
import com.ctre.phoenix.sensors.WPI_PigeonIMU;
|
||||
|
||||
import com.revrobotics.CANSparkMax;
|
||||
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
|
||||
|
||||
@@ -18,6 +20,7 @@ import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
import frc4388.robot.Constants.LEDConstants;
|
||||
import frc4388.robot.Constants.ShooterConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.robot.subsystems.SwerveModule;
|
||||
|
||||
/**
|
||||
* Defines and holds all I/O objects on the Roborio. This is useful for unit
|
||||
@@ -53,11 +56,14 @@ public class RobotMap {
|
||||
public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID);
|
||||
public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID);
|
||||
|
||||
public final WPI_PigeonIMU gyro = new WPI_PigeonIMU(SwerveDriveConstants.GYRO_ID);
|
||||
|
||||
public SwerveModule leftFront;
|
||||
public SwerveModule leftBack;
|
||||
public SwerveModule rightFront;
|
||||
public SwerveModule rightBack;
|
||||
|
||||
void configureSwerveMotorControllers() {
|
||||
leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
|
||||
rightFrontEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET);
|
||||
leftBackEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
|
||||
rightBackEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET);
|
||||
|
||||
leftFrontSteerMotor.configFactoryDefault();
|
||||
leftFrontWheelMotor.configFactoryDefault();
|
||||
@@ -95,6 +101,21 @@ public class RobotMap {
|
||||
rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
NeutralMode mode = NeutralMode.Coast;
|
||||
leftFrontSteerMotor.setNeutralMode(mode);
|
||||
leftFrontWheelMotor.setNeutralMode(mode);//Coast
|
||||
rightFrontSteerMotor.setNeutralMode(mode);
|
||||
rightFrontWheelMotor.setNeutralMode(mode);//Coast
|
||||
leftBackSteerMotor.setNeutralMode(mode);
|
||||
leftBackWheelMotor.setNeutralMode(mode);//Coast
|
||||
rightBackSteerMotor.setNeutralMode(mode);
|
||||
rightBackWheelMotor.setNeutralMode(mode);//Coast
|
||||
|
||||
leftFront = new SwerveModule(leftFrontWheelMotor, leftFrontSteerMotor, leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET);
|
||||
leftBack = new SwerveModule(leftBackWheelMotor, leftBackSteerMotor, leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
|
||||
rightFront = new SwerveModule(rightFrontWheelMotor, rightFrontSteerMotor, rightFrontEncoder, SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET);
|
||||
rightBack = new SwerveModule(rightBackWheelMotor, rightBackSteerMotor, rightBackEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET);
|
||||
|
||||
// config cancoder as remote encoder for swerve steer motors
|
||||
leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(),
|
||||
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
|
||||
|
||||
@@ -4,10 +4,10 @@
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import java.util.logging.Logger;
|
||||
|
||||
import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants.LEDConstants;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
|
||||
@@ -27,12 +27,12 @@ public class LED extends SubsystemBase {
|
||||
m_LEDController = LEDController;
|
||||
setPattern(LEDConstants.DEFAULT_PATTERN);
|
||||
updateLED();
|
||||
System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
|
||||
Logger.getLogger(LED.class.getName()).finer("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic(){
|
||||
SmartDashboard.putNumber("LED", m_currentPattern.getValue());
|
||||
//SmartDashboard.putNumber("LED", m_currentPattern.getValue());
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -4,182 +4,250 @@
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
import com.ctre.phoenix.sensors.CANCoder;
|
||||
import java.util.ArrayList;
|
||||
|
||||
import com.ctre.phoenix.sensors.WPI_PigeonIMU;
|
||||
import com.ctre.phoenix.sensors.PigeonIMU.FusionStatus;
|
||||
|
||||
import edu.wpi.first.math.VecBuilder;
|
||||
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
|
||||
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
|
||||
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
import frc4388.robot.Constants.OIConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.utility.Gains;
|
||||
import frc4388.utility.RobotGyro;
|
||||
|
||||
public class SwerveDrive extends SubsystemBase {
|
||||
SwerveDriveKinematics m_kinematics;
|
||||
private WPI_TalonFX m_leftFrontSteerMotor;
|
||||
private WPI_TalonFX m_leftFrontWheelMotor;
|
||||
private WPI_TalonFX m_rightFrontSteerMotor;
|
||||
private WPI_TalonFX m_rightFrontWheelMotor;
|
||||
private WPI_TalonFX m_leftBackSteerMotor;
|
||||
private WPI_TalonFX m_leftBackWheelMotor;
|
||||
private WPI_TalonFX m_rightBackSteerMotor;
|
||||
private WPI_TalonFX m_rightBackWheelMotor;
|
||||
private CANCoder m_leftFrontEncoder;
|
||||
private CANCoder m_rightFrontEncoder;
|
||||
private CANCoder m_leftBackEncoder;
|
||||
private CANCoder m_rightBackEncoder;
|
||||
|
||||
private SwerveModule m_leftFront;
|
||||
private SwerveModule m_leftBack;
|
||||
private SwerveModule m_rightFront;
|
||||
private SwerveModule m_rightBack;
|
||||
|
||||
double halfWidth = SwerveDriveConstants.WIDTH / 2.d;
|
||||
double halfHeight = SwerveDriveConstants.HEIGHT / 2.d;
|
||||
|
||||
public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
|
||||
|
||||
Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(halfWidth));
|
||||
Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(-halfWidth));
|
||||
Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(halfWidth));
|
||||
Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(-halfWidth));
|
||||
|
||||
Translation2d m_frontLeftLocation =
|
||||
new Translation2d(
|
||||
Units.inchesToMeters(halfHeight),
|
||||
Units.inchesToMeters(halfWidth));
|
||||
Translation2d m_frontRightLocation =
|
||||
new Translation2d(
|
||||
Units.inchesToMeters(halfHeight),
|
||||
Units.inchesToMeters(-halfWidth));
|
||||
Translation2d m_backLeftLocation =
|
||||
new Translation2d(
|
||||
Units.inchesToMeters(-halfHeight),
|
||||
Units.inchesToMeters(halfWidth));
|
||||
Translation2d m_backRightLocation =
|
||||
new Translation2d(
|
||||
Units.inchesToMeters(-halfHeight),
|
||||
Units.inchesToMeters(-halfWidth));
|
||||
//setSwerveGains();
|
||||
public SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
|
||||
|
||||
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
|
||||
public SwerveModule[] modules;
|
||||
public RobotGyro gyro; //TODO Add Gyro Lol
|
||||
public WPI_PigeonIMU m_gyro;
|
||||
protected FusionStatus fstatus = new FusionStatus();
|
||||
|
||||
/* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. The numbers used
|
||||
below are robot specific, and should be tuned. */
|
||||
public SwerveDrivePoseEstimator m_poseEstimator;
|
||||
public SwerveDriveOdometry m_odometry;
|
||||
|
||||
public SwerveDrive(WPI_TalonFX leftFrontSteerMotor,WPI_TalonFX leftFrontWheelMotor,WPI_TalonFX rightFrontSteerMotor,WPI_TalonFX rightFrontWheelMotor,
|
||||
WPI_TalonFX leftBackSteerMotor,WPI_TalonFX leftBackWheelMotor,WPI_TalonFX rightBackSteerMotor,WPI_TalonFX rightBackWheelMotor, CANCoder leftFrontEncoder,
|
||||
CANCoder rightFrontEncoder,
|
||||
CANCoder leftBackEncoder,
|
||||
CANCoder rightBackEncoder)
|
||||
{
|
||||
m_leftFrontSteerMotor = leftFrontSteerMotor;
|
||||
m_leftFrontWheelMotor = leftFrontWheelMotor;
|
||||
m_rightFrontSteerMotor = rightFrontSteerMotor;
|
||||
m_rightFrontWheelMotor = rightFrontWheelMotor;
|
||||
m_leftBackSteerMotor = leftBackSteerMotor;
|
||||
m_leftBackWheelMotor = leftBackWheelMotor;
|
||||
m_rightBackSteerMotor = rightBackSteerMotor;
|
||||
m_rightBackWheelMotor = rightBackWheelMotor;
|
||||
m_leftFrontEncoder = leftFrontEncoder;
|
||||
m_rightFrontEncoder = rightFrontEncoder;
|
||||
m_leftBackEncoder = leftBackEncoder;
|
||||
m_rightBackEncoder = rightBackEncoder;
|
||||
public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
|
||||
public boolean ignoreAngles;
|
||||
public Rotation2d rotTarget = new Rotation2d();
|
||||
|
||||
modules = new SwerveModule[] {
|
||||
new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET), // Front Left
|
||||
new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder, SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET), // Front Right
|
||||
new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET), // Back Left
|
||||
new SwerveModule(m_rightBackWheelMotor, m_rightBackSteerMotor, m_rightBackEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET) // Back Right
|
||||
};
|
||||
//gyro.reset();
|
||||
private final Field2d m_field = new Field2d();
|
||||
|
||||
public SwerveDrive(SwerveModule leftFront, SwerveModule leftBack, SwerveModule rightFront, SwerveModule rightBack, WPI_PigeonIMU gyro) {
|
||||
|
||||
m_leftFront = leftFront;
|
||||
m_leftBack = leftBack;
|
||||
m_rightFront = rightFront;
|
||||
m_rightBack = rightBack;
|
||||
m_gyro = gyro;
|
||||
|
||||
modules = new SwerveModule[] {m_leftFront, m_rightFront, m_leftBack, m_rightBack};
|
||||
|
||||
m_poseEstimator =
|
||||
new SwerveDrivePoseEstimator(
|
||||
m_gyro.getRotation2d(),
|
||||
new Pose2d(),
|
||||
m_kinematics,
|
||||
VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)),
|
||||
VecBuilder.fill(Units.degreesToRadians(1)),
|
||||
VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)));
|
||||
|
||||
m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d());
|
||||
|
||||
m_gyro.reset();
|
||||
SmartDashboard.putData("Field", m_field);
|
||||
}
|
||||
//https://github.com/ZachOrr/MK3-Swerve-Example
|
||||
/**
|
||||
* Method to drive the robot using joystick info.
|
||||
*
|
||||
* @param xSpeed Speed of the robot in the x direction (forward).
|
||||
* @param ySpeed Speed of the robot in the y direction (sideways).
|
||||
* @param speeds[0] Speed of the robot in the x direction (forward).
|
||||
* @param speeds[1] Speed of the robot in the y direction (sideways).
|
||||
* @param rot Angular rate of the robot.
|
||||
* @param fieldRelative Whether the provided x and y speeds are relative to the field.
|
||||
*/
|
||||
public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative)
|
||||
public void driveWithInput(double speedX, double speedY, double rot, boolean fieldRelative)
|
||||
{
|
||||
/*var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.ROTATION_SPEED //in rad/s );
|
||||
driveFromSpeeds(speeds);*/
|
||||
double xSpeedMetersPerSecond = xSpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND;
|
||||
double ySpeedMetersPerSecond = ySpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND;
|
||||
SwerveModuleState[] states =
|
||||
kinematics.toSwerveModuleStates(
|
||||
fieldRelative
|
||||
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3, gyro.getRotation2d())
|
||||
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3));
|
||||
SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
|
||||
for (int i = 0; i < states.length; i++) {
|
||||
SwerveModule module = modules[i];
|
||||
SwerveModuleState state = states[i];
|
||||
module.setDesiredState(state);
|
||||
if (speedX == 0 && speedY == 0 && rot == 0) ignoreAngles = true;
|
||||
else ignoreAngles = false;
|
||||
Translation2d speed = new Translation2d(speedX, speedY);
|
||||
double mag = speed.getNorm();
|
||||
speed = speed.times(mag * speedAdjust);
|
||||
|
||||
double xSpeedMetersPerSecond = -speed.getX();
|
||||
double ySpeedMetersPerSecond = speed.getY();
|
||||
SwerveModuleState[] states =
|
||||
m_kinematics.toSwerveModuleStates(
|
||||
fieldRelative
|
||||
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
|
||||
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED));
|
||||
setModuleStates(states);
|
||||
}
|
||||
}
|
||||
//Converts a ChassisSpeed to SwerveModuleStates (targets)
|
||||
public void driveFromSpeeds(ChassisSpeeds speeds)
|
||||
public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative)
|
||||
{
|
||||
//https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html
|
||||
// Convert to module states
|
||||
SwerveModuleState[] moduleStates = m_kinematics.toSwerveModuleStates(speeds);
|
||||
|
||||
// Front left module state
|
||||
SwerveModuleState leftFront = SwerveModuleState.optimize(moduleStates[0], Rotation2d.fromDegrees(m_leftFrontEncoder.getPosition()));
|
||||
// Front right module state
|
||||
SwerveModuleState rightFront = SwerveModuleState.optimize(moduleStates[1], Rotation2d.fromDegrees(m_rightFrontEncoder.getPosition()));
|
||||
// Back left module state
|
||||
SwerveModuleState leftBack = SwerveModuleState.optimize(moduleStates[2], Rotation2d.fromDegrees(m_leftBackEncoder.getPosition()));
|
||||
// Back right module state
|
||||
SwerveModuleState rightBack = SwerveModuleState.optimize(moduleStates[3], Rotation2d.fromDegrees(m_rightBackEncoder.getPosition()));
|
||||
|
||||
//Set the motors
|
||||
setSwerveMotors(leftFront, leftBack, rightFront, rightBack);
|
||||
ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0;
|
||||
Translation2d speed = new Translation2d(leftX, leftY);
|
||||
speed = speed.times(speed.getNorm() * speedAdjust);
|
||||
if (Math.abs(rightX) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightY) > OIConstants.RIGHT_AXIS_DEADBAND)
|
||||
rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0, 1));
|
||||
double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians();
|
||||
double xSpeedMetersPerSecond = -speed.getX();
|
||||
double ySpeedMetersPerSecond = speed.getY();
|
||||
SwerveModuleState[] states =
|
||||
m_kinematics.toSwerveModuleStates(
|
||||
fieldRelative
|
||||
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
|
||||
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED));
|
||||
setModuleStates(states);
|
||||
}
|
||||
|
||||
//Sets steering motors to PID values
|
||||
public void setSwerveMotors(SwerveModuleState leftFront, SwerveModuleState leftBack, SwerveModuleState rightFront, SwerveModuleState rightBack)
|
||||
{
|
||||
/*//Set the Wheel motor speeds
|
||||
m_leftFrontWheelMotor.set(m_leftFrontSteerMotor.get() + leftFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
|
||||
m_rightFrontWheelMotor.set(m_rightFrontSteerMotor.get() + rightFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
|
||||
m_leftBackWheelMotor.set(m_leftBackSteerMotor.get() + leftBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
|
||||
m_rightBackWheelMotor.set(m_rightBackSteerMotor.get() + rightBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
|
||||
//PID
|
||||
m_leftFrontSteerMotor.set(TalonFXControlMode.Position, leftFront.angle.getDegrees() * 12000);
|
||||
m_rightFrontSteerMotor.set(TalonFXControlMode.Position, rightFront.angle.getDegrees() * 12000);
|
||||
m_leftBackSteerMotor.set(TalonFXControlMode.Position, leftBack.angle.getDegrees() * 12000);
|
||||
m_rightBackSteerMotor.set(TalonFXControlMode.Position, rightBack.angle.getDegrees());
|
||||
System.out.println("Target: " + leftFront.angle.getDegrees());*/
|
||||
public void setModuleStates(SwerveModuleState[] desiredStates) {
|
||||
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
|
||||
for (int i = 0; i < desiredStates.length; i++) {
|
||||
SwerveModule module = modules[i];
|
||||
SwerveModuleState state = desiredStates[i];
|
||||
module.setDesiredState(state, false);
|
||||
}
|
||||
// modules[0].setDesiredState(desiredStates[0], false);
|
||||
}
|
||||
|
||||
/*public void setSwerveGains(){
|
||||
m_leftFrontSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
|
||||
m_leftFrontSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_leftFrontSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_leftFrontSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_leftFrontSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_rightFrontSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
|
||||
m_rightFrontSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_rightFrontSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_rightFrontSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_rightFrontSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_leftBackSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
|
||||
m_leftBackSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_leftBackSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_leftBackSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_leftBackSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_rightBackSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
|
||||
m_rightBackSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_rightBackSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_rightBackSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_rightBackSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
@Override
|
||||
public void periodic() {
|
||||
|
||||
}*/
|
||||
updateOdometry();
|
||||
// SmartDashboard.putNumber("Pigeon Fused Heading", m_gyro.getFusedHeading(fstatus));
|
||||
|
||||
SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
|
||||
|
||||
// SmartDashboard.putNumber("Front Left", modules[0].driveMotor.getSelectedSensorPosition());
|
||||
// SmartDashboard.putNumber("Front Right", modules[1].driveMotor.getSelectedSensorPosition());
|
||||
// SmartDashboard.putNumber("Back Left", modules[2].driveMotor.getSelectedSensorPosition());
|
||||
// SmartDashboard.putNumber("Back Right", modules[3].driveMotor.getSelectedSensorPosition());
|
||||
// SmartDashboard.putNumber("Pigeon Get Angle", m_gyro.getAngle());
|
||||
// SmartDashboard.putNumber("Pigeon Rotation 2D", m_gyro.getRotation2d().getDegrees());
|
||||
// SmartDashboard.putStringArray("Fusion Status", new String[] {"Is Fusing: "+fstatus.bIsFusing, "Is Valid: "+fstatus.bIsValid, "Heading: "+fstatus.heading});
|
||||
|
||||
// public void driveFieldRelative(double awayFromStation, double towardLeftBoundary, double rotate)
|
||||
// {
|
||||
// var speeds = ChassisSpeeds.fromFieldRelativeSpeeds(awayFromStation, towardLeftBoundary,
|
||||
// rotate * SwerveDriveConstants.RotationSpeed, /*get odometry angle*/)
|
||||
// }
|
||||
// m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
|
||||
super.periodic();
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the distance between two given poses.
|
||||
* @param p1 The first pose.
|
||||
* @param p2 The second pose.
|
||||
* @return Absolute distance between p1 and p2.
|
||||
*/
|
||||
public double distBtwPoses(Pose2d p1, Pose2d p2) {
|
||||
return Math.sqrt(Math.pow(p1.getX() - p2.getX(), 2) + Math.pow(p1.getY() - p2.getY(), 2));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns a scalar from your distance to the hub to your target distance.
|
||||
*
|
||||
* @param target_dist The target distance.
|
||||
* @return A scalar that multiplies your distance from the hub to get your target distance.
|
||||
*/
|
||||
public Pose2d poseGivenDist(double target_dist) {
|
||||
Pose2d p1 = m_poseEstimator.getEstimatedPosition();
|
||||
Pose2d p2 = SwerveDriveConstants.HUB_POSE;
|
||||
|
||||
double scalar = target_dist/distBtwPoses(p1, p2);
|
||||
Pose2d new_pose = new Pose2d(p1.getX() * scalar, p1.getY() * scalar, p1.getRotation());
|
||||
|
||||
return new_pose;
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the current pose of the robot.
|
||||
* @return Robot's current pose.
|
||||
*/
|
||||
public Pose2d getOdometry() {
|
||||
// return m_odometry.getPoseMeters();
|
||||
return m_poseEstimator.getEstimatedPosition();
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the current gyro using regression formula.
|
||||
* @return Rotation2d object holding current gyro in radians
|
||||
*/
|
||||
public Rotation2d getRegGyro() {
|
||||
double regCur = 0.6552670369 + m_gyro.getRotation2d().getDegrees() * 0.9926871527;
|
||||
return new Rotation2d(regCur * Math.PI / 180);
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the odometry of the robot to the given pose.
|
||||
*/
|
||||
public void resetOdometry(Pose2d pose) {
|
||||
m_poseEstimator.resetPosition(pose, m_gyro.getRotation2d());
|
||||
}
|
||||
|
||||
/** Updates the field relative position of the robot.
|
||||
*/
|
||||
public void updateOdometry() {
|
||||
m_poseEstimator.update( getRegGyro(),
|
||||
modules[0].getState(),
|
||||
modules[1].getState(),
|
||||
modules[2].getState(),
|
||||
modules[3].getState());
|
||||
|
||||
// Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
|
||||
// a real robot, this must be calculated based either on latency or timestamps.
|
||||
// m_poseEstimator.addVisionMeasurement(
|
||||
// m_poseEstimator.getEstimatedPosition(),
|
||||
// Timer.getFPGATimestamp() - 0.1);
|
||||
}
|
||||
|
||||
public void resetGyro(){
|
||||
m_gyro.reset();
|
||||
rotTarget = new Rotation2d(0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Stop all four swerve modules.
|
||||
*/
|
||||
public void stopModules() {
|
||||
modules[0].stop();
|
||||
modules[1].stop();
|
||||
modules[2].stop();
|
||||
modules[3].stop();
|
||||
}
|
||||
|
||||
public void highSpeed(boolean shift){
|
||||
if (shift){
|
||||
speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_FAST;
|
||||
}
|
||||
else{
|
||||
speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -7,6 +7,7 @@ package frc4388.robot.subsystems;
|
||||
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
|
||||
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
|
||||
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice;
|
||||
import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
import com.ctre.phoenix.sensors.CANCoder;
|
||||
@@ -21,20 +22,28 @@ import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.utility.Gains;
|
||||
|
||||
public class SwerveModule extends SubsystemBase {
|
||||
private WPI_TalonFX driveMotor;
|
||||
private WPI_TalonFX angleMotor;
|
||||
public WPI_TalonFX angleMotor;
|
||||
public WPI_TalonFX driveMotor;
|
||||
private CANCoder canCoder;
|
||||
public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
|
||||
|
||||
private static double kEncoderTicksPerRotation = 4096;
|
||||
private SwerveModuleState state;
|
||||
private double canCoderFeedbackCoefficient;
|
||||
|
||||
public long m_currentTime;
|
||||
public long m_lastTime;
|
||||
public double m_deltaTime;
|
||||
|
||||
public double m_currentPos;
|
||||
public double m_lastPos;
|
||||
|
||||
/** Creates a new SwerveModule. */
|
||||
public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) {
|
||||
this.driveMotor = driveMotor;
|
||||
this.angleMotor = angleMotor;
|
||||
this.canCoder = canCoder;
|
||||
canCoderFeedbackCoefficient = canCoder.configGetFeedbackCoefficient();
|
||||
|
||||
TalonFXConfiguration angleTalonFXConfiguration = new TalonFXConfiguration();
|
||||
|
||||
@@ -48,22 +57,38 @@ public class SwerveModule extends SubsystemBase {
|
||||
angleTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
|
||||
angleMotor.configAllSettings(angleTalonFXConfiguration);
|
||||
|
||||
/*TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
|
||||
driveTalonFXConfiguration.slot0.kP = kDriveP;
|
||||
driveTalonFXConfiguration.slot0.kI = kDriveI;
|
||||
driveTalonFXConfiguration.slot0.kD = kDriveD;
|
||||
driveTalonFXConfiguration.slot0.kF = kDriveF;
|
||||
driveMotor.configAllSettings(driveTalonFXConfiguration);*/
|
||||
// TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
|
||||
// driveTalonFXConfiguration.slot0.kP = 0.05;
|
||||
// driveTalonFXConfiguration.slot0.kI = 0.0;
|
||||
// driveTalonFXConfiguration.slot0.kD = 0.0;
|
||||
// driveTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.IntegratedSensor;
|
||||
driveMotor.configFactoryDefault();
|
||||
driveMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30);
|
||||
driveMotor.configNominalOutputForward(0, 30);
|
||||
driveMotor.configNominalOutputReverse(0, 30);
|
||||
driveMotor.configPeakOutputForward(1, 30);
|
||||
driveMotor.configPeakOutputReverse(-1, 30);
|
||||
driveMotor.configAllowableClosedloopError(0, 0, 30);
|
||||
driveMotor.config_kP(0, 0.5, 30);
|
||||
driveMotor.config_kI(0, 0, 30);
|
||||
driveMotor.config_kD(0, 0, 30);
|
||||
// maybe try a feedforward value?
|
||||
|
||||
// driveMotor.configAllSettings(driveTalonFXConfiguration);
|
||||
|
||||
CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
|
||||
canCoderConfiguration.magnetOffsetDegrees = offset;
|
||||
canCoder.configAllSettings(canCoderConfiguration);
|
||||
|
||||
m_currentTime = System.currentTimeMillis();
|
||||
m_lastTime = System.currentTimeMillis();
|
||||
|
||||
m_lastPos = driveMotor.getSelectedSensorPosition();
|
||||
}
|
||||
|
||||
|
||||
public Rotation2d getAngle() {
|
||||
private Rotation2d getAngle() {
|
||||
// Note: This assumes the CANCoders are setup with the default feedback coefficient
|
||||
// and the sesnor value reports degrees.
|
||||
// and the sensor value reports degrees.
|
||||
return Rotation2d.fromDegrees(canCoder.getAbsolutePosition());
|
||||
}
|
||||
|
||||
@@ -71,10 +96,10 @@ public class SwerveModule extends SubsystemBase {
|
||||
* Set the speed + rotation of the swerve module from a SwerveModuleState object
|
||||
* @param desiredState - A SwerveModuleState representing the desired new state of the module
|
||||
*/
|
||||
public void setDesiredState(SwerveModuleState desiredState) {
|
||||
public void setDesiredState(SwerveModuleState desiredState, boolean ignoreAngle) {
|
||||
Rotation2d currentRotation = getAngle();
|
||||
SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
|
||||
SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
|
||||
// SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
|
||||
state = SwerveModuleState.optimize(desiredState, currentRotation);
|
||||
|
||||
// Find the difference between our current rotational position + our new rotational position
|
||||
Rotation2d rotationDelta = state.angle.minus(currentRotation);
|
||||
@@ -82,12 +107,64 @@ public class SwerveModule extends SubsystemBase {
|
||||
// Find the new absolute position of the module based on the difference in rotation
|
||||
double deltaTicks = (rotationDelta.getDegrees() / 360.) * kEncoderTicksPerRotation;
|
||||
// Convert the CANCoder from it's position reading back to ticks
|
||||
double currentTicks = canCoder.getPosition() / canCoder.configGetFeedbackCoefficient();
|
||||
double currentTicks = canCoder.getPosition() / canCoderFeedbackCoefficient;
|
||||
double desiredTicks = currentTicks + deltaTicks;
|
||||
angleMotor.set(TalonFXControlMode.Position, desiredTicks);
|
||||
|
||||
if (!ignoreAngle){
|
||||
angleMotor.set(TalonFXControlMode.Position, desiredTicks);
|
||||
}
|
||||
|
||||
double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
|
||||
driveMotor.set(angleMotor.get() + feetPerSecond / SwerveDriveConstants.SPEED_FEET_PER_SECOND_AT_FULL_POWER);
|
||||
double ftPerSec = Units.metersToFeet(state.speedMetersPerSecond);
|
||||
double normFtPerSec = ftPerSec / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC;
|
||||
// double angleCorrection = angleMotor.getSelectedSensorVelocity() * 2.69;
|
||||
|
||||
// driveMotor.set(TalonFXControlMode.Velocity, angleCorrection + (Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
|
||||
driveMotor.set(normFtPerSec);// - angleMotor.get());
|
||||
// driveMotor.set(TalonFXControlMode.Velocity, angleCorrection); // Ratio between axis = 1/1.75 Ratio of wheel is 5.14/1 ratio of steer is 12.8/1
|
||||
|
||||
// m_currentTime = System.currentTimeMillis();
|
||||
// m_deltaTime = (double) (m_currentTime - m_lastTime);
|
||||
// m_deltaTime = m_deltaTime / 10.0;
|
||||
|
||||
// m_currentPos = driveMotor.getSelectedSensorPosition();
|
||||
|
||||
// double m_desiredCorrectionVel = 3.2 * angleMotor.getSelectedSensorVelocity();
|
||||
// double m_desiredCorrectionPos = (m_deltaTime * m_desiredCorrectionVel) % 2048;
|
||||
// double m_lastPos = (driveMotor.getSelectedSensorPosition() % 2048) - (m_deltaTime * driveMotor.getSelectedSensorVelocity());
|
||||
// double m_actualDesiredPos = m_deltaTime * ((Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
|
||||
|
||||
// System.out.println("Current Pos: " + driveMotor.getSelectedSensorPosition());
|
||||
// System.out.println("Desired Correction Pos: " + m_desiredCorrectionPos);
|
||||
// System.out.println("Last Pos: " + m_lastPos);
|
||||
|
||||
// driveMotor.set(TalonFXControlMode.Position, 1500/*m_desiredCorrectionPos*/);
|
||||
|
||||
// m_lastTime = m_currentTime;
|
||||
// m_lastPos = m_currentPos;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get current module state.
|
||||
*
|
||||
* @return The current state of the module in m/s.
|
||||
*/
|
||||
public SwerveModuleState getState() {
|
||||
// return state;
|
||||
return new SwerveModuleState(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.INCHES_PER_TICK * SwerveDriveConstants.METERS_PER_INCH * 10, getAngle());
|
||||
}
|
||||
|
||||
/**
|
||||
* Stop the drive and steer motors of current module.
|
||||
*/
|
||||
public void stop() {
|
||||
driveMotor.set(0);
|
||||
angleMotor.set(0);
|
||||
}
|
||||
@Override
|
||||
public void periodic(){
|
||||
Rotation2d currentRotation = getAngle();
|
||||
SmartDashboard.putNumber("Angle Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
|
||||
SmartDashboard.putNumber("Drive Motor " + driveMotor.getDeviceID(), ((driveMotor.getSelectedSensorPosition() / 2048) * 360) % 360);
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,107 @@
|
||||
package frc4388.utility;
|
||||
|
||||
import static org.fusesource.jansi.Ansi.ansi;
|
||||
|
||||
import java.io.IOException;
|
||||
import java.io.OutputStream;
|
||||
import java.io.PrintStream;
|
||||
import java.io.PrintWriter;
|
||||
import java.io.StringWriter;
|
||||
import java.time.ZoneId;
|
||||
import java.time.ZonedDateTime;
|
||||
import java.util.logging.ConsoleHandler;
|
||||
import java.util.logging.Formatter;
|
||||
import java.util.logging.Level;
|
||||
import java.util.logging.LogManager;
|
||||
import java.util.logging.LogRecord;
|
||||
import java.util.logging.Logger;
|
||||
|
||||
import org.fusesource.jansi.Ansi;
|
||||
import org.fusesource.jansi.Ansi.Attribute;
|
||||
import org.fusesource.jansi.Ansi.Color;
|
||||
import org.fusesource.jansi.AnsiConsole;
|
||||
|
||||
public class AnsiLogging extends ConsoleHandler {
|
||||
public static void systemInstall() {
|
||||
try {
|
||||
// Configures java.util.logging.Logger to output additional colored information.
|
||||
LogManager.getLogManager().updateConfiguration(key -> (o, n) -> {
|
||||
switch (key) {
|
||||
case ".level": return Level.ALL.getName();
|
||||
case "handlers": return AnsiColorConsoleHandler.class.getName();
|
||||
default: return n;
|
||||
}
|
||||
});
|
||||
// Replaces standard output streams with org.fusesource.jansi.AnsiPrintStreams.
|
||||
AnsiConsole.systemInstall();
|
||||
// Replaces standard output stream with java.util.logging.Logger.
|
||||
System.setOut(printStreamLogger(Logger.getGlobal(), Level.INFO));
|
||||
// Replaces standard error output stream with java.util.logging.Logger.
|
||||
System.setErr(printStreamLogger(Logger.getGlobal(), Level.SEVERE));
|
||||
} catch (IOException exception) {
|
||||
exception.printStackTrace(AnsiConsole.sysErr());
|
||||
}
|
||||
}
|
||||
|
||||
public static class AnsiColorConsoleHandler extends ConsoleHandler {
|
||||
@Override
|
||||
public void publish(LogRecord logRecord) {
|
||||
AnsiConsole.err().print(getFormatter().format(logRecord));
|
||||
AnsiConsole.err().flush();
|
||||
}
|
||||
@Override
|
||||
public Formatter getFormatter() {
|
||||
return formatter;
|
||||
}
|
||||
private static final Formatter formatter = new Formatter() {
|
||||
@Override
|
||||
public String format(LogRecord logRecord) {
|
||||
ZonedDateTime zdt = ZonedDateTime.ofInstant(logRecord.getInstant(), ZoneId.systemDefault());
|
||||
String source;
|
||||
if (logRecord.getSourceClassName() != null && !logRecord.getSourceClassName().startsWith(AnsiLogging.class.getName())) {
|
||||
source = logRecord.getSourceClassName();
|
||||
if (logRecord.getSourceMethodName() != null) {
|
||||
source += " " + logRecord.getSourceMethodName();
|
||||
}
|
||||
} else
|
||||
source = logRecord.getLoggerName();
|
||||
String message = formatMessage(logRecord);
|
||||
String throwable = "";
|
||||
if (logRecord.getThrown() != null) {
|
||||
StringWriter sw = new StringWriter();
|
||||
try (PrintWriter pw = new PrintWriter(sw)) {
|
||||
pw.println();
|
||||
logRecord.getThrown().printStackTrace(pw);
|
||||
}
|
||||
throwable = sw.toString();
|
||||
}
|
||||
Ansi ansi;
|
||||
if (logRecord.getLevel() == Level.SEVERE) ansi = ansi().fgBright(Color.RED);
|
||||
else if (logRecord.getLevel() == Level.WARNING) ansi = ansi().fgBright(Color.YELLOW);
|
||||
else if (logRecord.getLevel() == Level.INFO) ansi = ansi().fg(Color.GREEN);
|
||||
else if (logRecord.getLevel() == Level.CONFIG) ansi = ansi().fgBright(Color.BLUE);
|
||||
else if (logRecord.getLevel() == Level.FINE) ansi = ansi().fg(Color.CYAN);
|
||||
else if (logRecord.getLevel() == Level.FINER) ansi = ansi().fg(Color.MAGENTA);
|
||||
else if (logRecord.getLevel() == Level.FINEST) ansi = ansi().fgBright(Color.BLACK);
|
||||
else ansi = ansi().fg(Color.DEFAULT);
|
||||
String format = ansi.bold().a(Attribute.UNDERLINE).a("%1$tb %1$td, %1$tY %1$tl:%1$tM:%1$tS %1$Tp %2$s %4$s:").boldOff().a(Attribute.UNDERLINE_OFF).a("%n%5$s%6$s").reset().a("%n").toString();
|
||||
return String.format(format, zdt, source, logRecord.getLoggerName(), logRecord.getLevel().getLocalizedName(), message, throwable);
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
private static PrintStream printStreamLogger(Logger logger, Level level) {
|
||||
return new PrintStream(new OutputStream() {
|
||||
private final StringBuilder stringBuilder = new StringBuilder();
|
||||
|
||||
@Override
|
||||
public final void write(int i) throws IOException {
|
||||
if (i == '\n') {
|
||||
logger.log(level, stringBuilder::toString);
|
||||
stringBuilder.setLength(0);
|
||||
} else
|
||||
stringBuilder.appendCodePoint(i);
|
||||
}
|
||||
});
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,12 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package frc4388.utility;
|
||||
|
||||
/** Add your docs here. */
|
||||
public class DataLogging {
|
||||
public DataLogging() {
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,20 @@
|
||||
package frc4388.utility.controller;
|
||||
|
||||
import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
|
||||
public class DeadbandedRawXboxController extends RawXboxController {
|
||||
public DeadbandedRawXboxController(int port) { super(port); }
|
||||
@Override public double getLeftX() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()).getX(); }
|
||||
@Override public double getLeftY() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()).getY(); }
|
||||
@Override public double getRightX() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()).getX(); }
|
||||
@Override public double getRightY() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()).getY(); }
|
||||
public static Translation2d skewToDeadzonedCircle(double x, double y) {
|
||||
Translation2d translation2d = new Translation2d(x, y);
|
||||
double magnitude = translation2d.getNorm();
|
||||
if (magnitude >= 1) return translation2d.div(magnitude);
|
||||
if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0);
|
||||
return translation2d;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,21 @@
|
||||
package frc4388.utility.controller;
|
||||
|
||||
import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
|
||||
public class DeadbandedXboxController extends XboxController {
|
||||
public DeadbandedXboxController(int port) { super(port); }
|
||||
@Override public double getLeftX() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()).getX(); }
|
||||
@Override public double getLeftY() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()).getY(); }
|
||||
@Override public double getRightX() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()).getX(); }
|
||||
@Override public double getRightY() { return skewToDeadzonedCircle(super.getRightX(), super.getRightY()).getY(); }
|
||||
public static Translation2d skewToDeadzonedCircle(double x, double y) {
|
||||
Translation2d translation2d = new Translation2d(x, y);
|
||||
double magnitude = translation2d.getNorm();
|
||||
if (magnitude >= 1) return translation2d.div(magnitude);
|
||||
if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0);
|
||||
return translation2d;
|
||||
}
|
||||
}
|
||||
@@ -1,21 +0,0 @@
|
||||
package frc4388.utility.controller;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public interface IHandController {
|
||||
|
||||
public double getLeftXAxis();
|
||||
|
||||
public double getLeftYAxis();
|
||||
|
||||
public double getRightXAxis();
|
||||
|
||||
public double getRightYAxis();
|
||||
|
||||
public double getLeftTriggerAxis();
|
||||
|
||||
public double getRightTriggerAxis();
|
||||
|
||||
public int getDpadAngle();
|
||||
}
|
||||
@@ -0,0 +1,241 @@
|
||||
package frc4388.utility.controller;
|
||||
|
||||
import java.nio.ByteBuffer;
|
||||
|
||||
import edu.wpi.first.hal.HAL;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
|
||||
public class RawXboxController extends XboxController {
|
||||
private final int m_port;
|
||||
|
||||
public RawXboxController(int port) {
|
||||
super(port);
|
||||
if (port < 0 || port >= DriverStation.kJoystickPorts)
|
||||
throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
|
||||
m_port = port;
|
||||
}
|
||||
|
||||
private static class HALJoystickButtons {
|
||||
public int m_buttons;
|
||||
public byte m_count;
|
||||
}
|
||||
|
||||
private static class HALJoystickAxes {
|
||||
public float[] m_axes;
|
||||
public short m_count;
|
||||
|
||||
HALJoystickAxes(int count) {
|
||||
m_axes = new float[count];
|
||||
}
|
||||
}
|
||||
|
||||
private static class HALJoystickPOVs {
|
||||
public short[] m_povs;
|
||||
public short m_count;
|
||||
|
||||
HALJoystickPOVs(int count) {
|
||||
m_povs = new short[count];
|
||||
for (int i = 0; i < count; i++) {
|
||||
m_povs[i] = -1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static final double JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL = 1.0;
|
||||
private static double m_nextMessageTime;
|
||||
|
||||
private HALJoystickAxes m_joystickAxes = new HALJoystickAxes(HAL.kMaxJoystickAxes);
|
||||
private HALJoystickPOVs m_joystickPOVs = new HALJoystickPOVs(HAL.kMaxJoystickAxes);
|
||||
private HALJoystickButtons m_joystickButtons = new HALJoystickButtons();
|
||||
|
||||
// Joystick button rising/falling edge flags
|
||||
private int m_joystickButtonsPressed = 0;
|
||||
private int m_joystickButtonsReleased = 0;
|
||||
|
||||
private static final ByteBuffer m_buttonCountBuffer = ByteBuffer.allocateDirect(1);
|
||||
|
||||
@Override
|
||||
public double getRawAxis(int axis) {
|
||||
return getStickAxis(axis);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean getRawButton(int button) {
|
||||
return getStickButton((byte) button);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean getRawButtonPressed(int button) {
|
||||
return getStickButtonPressed((byte) button);
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean getRawButtonReleased(int button) {
|
||||
return getStickButtonReleased(button);
|
||||
}
|
||||
|
||||
@Override
|
||||
public int getPOV(int pov) {
|
||||
return getStickPOV(pov);
|
||||
}
|
||||
|
||||
/**
|
||||
* The state of one joystick button. Button indexes begin at 1.
|
||||
*
|
||||
* @param button The button index, beginning at 1.
|
||||
* @return The state of the joystick button.
|
||||
*/
|
||||
public boolean getStickButton(final int button) {
|
||||
if (button <= 0) {
|
||||
reportJoystickUnpluggedError();
|
||||
return false;
|
||||
}
|
||||
if (button <= m_joystickButtons.m_count) {
|
||||
return (m_joystickButtons.m_buttons & 1 << (button - 1)) != 0;
|
||||
}
|
||||
reportJoystickUnpluggedWarning(button, m_port);
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Whether one joystick button was pressed since the last check. Button indexes begin at 1.
|
||||
*
|
||||
* @param button The button index, beginning at 1.
|
||||
* @return Whether the joystick button was pressed since the last check.
|
||||
*/
|
||||
public boolean getStickButtonPressed(final int button) {
|
||||
getData();
|
||||
if (button <= 0) {
|
||||
reportJoystickUnpluggedError();
|
||||
return false;
|
||||
}
|
||||
if (button <= m_joystickButtons.m_count) {
|
||||
// If button was pressed, clear flag and return true
|
||||
if ((m_joystickButtonsPressed & 1 << (button - 1)) != 0) {
|
||||
m_joystickButtonsPressed &= ~(1 << (button - 1));
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
reportJoystickUnpluggedWarning(button, m_port);
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Whether one joystick button was released since the last check. Button indexes begin at 1.
|
||||
*
|
||||
* @param button The button index, beginning at 1.
|
||||
* @return Whether the joystick button was released since the last check.
|
||||
*/
|
||||
public boolean getStickButtonReleased(final int button) {
|
||||
getData();
|
||||
if (button <= 0) {
|
||||
reportJoystickUnpluggedError();
|
||||
return false;
|
||||
}
|
||||
if (button <= m_joystickButtons.m_count) {
|
||||
// If button was released, clear flag and return true
|
||||
if ((m_joystickButtonsReleased & 1 << (button - 1)) != 0) {
|
||||
m_joystickButtonsReleased &= ~(1 << (button - 1));
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
reportJoystickUnpluggedWarning(button, m_port);
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the value of the axis on a joystick. This depends on the mapping of the joystick connected to
|
||||
* the specified port.
|
||||
*
|
||||
* @param axis The analog axis value to read from the joystick.
|
||||
* @return The value of the axis on the joystick.
|
||||
*/
|
||||
public double getStickAxis(int axis) {
|
||||
getData();
|
||||
if (axis < 0 || axis >= HAL.kMaxJoystickAxes) {
|
||||
throw new IllegalArgumentException("Joystick axis is out of range");
|
||||
}
|
||||
if (axis < m_joystickAxes.m_count) {
|
||||
return m_joystickAxes.m_axes[axis];
|
||||
}
|
||||
reportJoystickUnpluggedWarning(axis, m_port);
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the state of a POV on the joystick.
|
||||
*
|
||||
* @param pov The POV to read.
|
||||
* @return the angle of the POV in degrees, or -1 if the POV is not pressed.
|
||||
*/
|
||||
public int getStickPOV(int pov) {
|
||||
getData();
|
||||
if (pov < 0 || pov >= HAL.kMaxJoystickPOVs) {
|
||||
throw new IllegalArgumentException("Joystick POV is out of range");
|
||||
}
|
||||
if (pov < m_joystickPOVs.m_count) {
|
||||
return m_joystickPOVs.m_povs[pov];
|
||||
}
|
||||
reportJoystickUnpluggedWarning(pov, m_port);
|
||||
return -1;
|
||||
}
|
||||
|
||||
/**
|
||||
* The state of the buttons on the joystick.
|
||||
*
|
||||
* @return The state of the buttons on the joystick.
|
||||
*/
|
||||
public int getStickButtons() {
|
||||
getData();
|
||||
return m_joystickButtons.m_buttons;
|
||||
}
|
||||
|
||||
protected void getData() {
|
||||
// Get the status of all of the joysticks
|
||||
byte stick = (byte) m_port;
|
||||
m_joystickAxes.m_count = HAL.getJoystickAxes(stick, m_joystickAxes.m_axes);
|
||||
m_joystickPOVs.m_count = HAL.getJoystickPOVs(stick, m_joystickPOVs.m_povs);
|
||||
m_joystickButtons.m_buttons = HAL.getJoystickButtons(stick, m_buttonCountBuffer);
|
||||
m_joystickButtons.m_count = m_buttonCountBuffer.get(0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm
|
||||
* the DS.
|
||||
*/
|
||||
private static void reportJoystickUnpluggedError(String message) {
|
||||
double currentTime = Timer.getFPGATimestamp();
|
||||
if (currentTime > m_nextMessageTime) {
|
||||
DriverStation.reportError(message, false);
|
||||
m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm
|
||||
* the DS.
|
||||
*/
|
||||
private static void reportJoystickUnpluggedWarning(String message) {
|
||||
if (DriverStation.isFMSAttached() || !DriverStation.isJoystickConnectionWarningSilenced()) {
|
||||
double currentTime = Timer.getFPGATimestamp();
|
||||
if (currentTime > m_nextMessageTime) {
|
||||
DriverStation.reportWarning(message, false);
|
||||
m_nextMessageTime = currentTime + JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static void reportJoystickUnpluggedWarning(final int pov, final int stick) {
|
||||
reportJoystickUnpluggedWarning("Joystick POV " + pov + " on port " + stick + " not available, check if controller is plugged in");
|
||||
}
|
||||
|
||||
private static void reportJoystickUnpluggedError() {
|
||||
reportJoystickUnpluggedError("Button indexes begin at 1 in WPILib for C++ and Java\n");
|
||||
}
|
||||
}
|
||||
@@ -1,218 +0,0 @@
|
||||
package frc4388.utility.controller;
|
||||
|
||||
import edu.wpi.first.wpilibj.Joystick;
|
||||
|
||||
/**
|
||||
* This is a wrapper for the WPILib Joystick class that represents an XBox
|
||||
* controller.
|
||||
* @author frc1675
|
||||
*/
|
||||
public class XboxController implements IHandController
|
||||
{
|
||||
public static final int LEFT_X_AXIS = 0;
|
||||
public static final int LEFT_Y_AXIS = 1;
|
||||
public static final int LEFT_TRIGGER_AXIS = 2;
|
||||
public static final int RIGHT_TRIGGER_AXIS = 3;
|
||||
public static final int RIGHT_X_AXIS = 4;
|
||||
public static final int RIGHT_Y_AXIS = 5;
|
||||
public static final int LEFT_RIGHT_DPAD_AXIS = 6;
|
||||
public static final int TOP_BOTTOM_DPAD_AXIS = 6;
|
||||
|
||||
public static final int A_BUTTON = 1;
|
||||
public static final int B_BUTTON = 2;
|
||||
public static final int X_BUTTON = 3;
|
||||
public static final int Y_BUTTON = 4;
|
||||
public static final int LEFT_BUMPER_BUTTON = 5;
|
||||
public static final int RIGHT_BUMPER_BUTTON = 6;
|
||||
public static final int BACK_BUTTON = 7;
|
||||
public static final int START_BUTTON = 8;
|
||||
|
||||
public static final int LEFT_JOYSTICK_BUTTON = 9;
|
||||
public static final int RIGHT_JOYSTICK_BUTTON = 10;
|
||||
|
||||
private static final double LEFT_DPAD_TOLERANCE = -0.9;
|
||||
private static final double RIGHT_DPAD_TOLERANCE = 0.9;
|
||||
private static final double BOTTOM_DPAD_TOLERANCE = -0.9;
|
||||
private static final double TOP_DPAD_TOLERANCE = 0.9;
|
||||
|
||||
private static final double LEFT_TRIGGER_TOLERANCE = 0.5;
|
||||
private static final double RIGHT_TRIGGER_TOLERANCE = 0.5;
|
||||
|
||||
private static final double RIGHT_AXIS_UP_TOLERANCE = -0.9;
|
||||
private static final double RIGHT_AXIS_DOWN_TOLERANCE = 0.9;
|
||||
private static final double RIGHT_AXIS_RIGHT_TOLERANCE = 0.9;
|
||||
private static final double RIGHT_AXIS_LEFT_TOLERANCE = -0.9;
|
||||
|
||||
private static final double LEFT_AXIS_UP_TOLERANCE = -0.9;
|
||||
private static final double LEFT_AXIS_DOWN_TOLERANCE = 0.9;
|
||||
private static final double LEFT_AXIS_RIGHT_TOLERANCE = 0.9;
|
||||
private static final double LEFT_AXIS_LEFT_TOLERANCE = -0.9;
|
||||
|
||||
private static final double DEADZONE = 0.1;
|
||||
|
||||
private Joystick m_stick;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public XboxController(int portNumber){
|
||||
m_stick = new Joystick(portNumber);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public Joystick getJoyStick() {
|
||||
return m_stick;
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks if the input falls within the deadzone.
|
||||
* @param input from an axis on the controller
|
||||
* @return true if input falls in deadzone, false if input falls outside deadzone
|
||||
*/
|
||||
private boolean inDeadZone(double input){
|
||||
return (Math.abs(input) < DEADZONE);
|
||||
}
|
||||
|
||||
/**
|
||||
* Updates an input to have a deadzone around the 0 position
|
||||
* @param input from an axis on the controller
|
||||
* @return updated input
|
||||
*/
|
||||
private double getAxisWithDeadZoneCheck(double input){
|
||||
if(inDeadZone(input)){
|
||||
return 0.0;
|
||||
} else {
|
||||
return input;
|
||||
}
|
||||
}
|
||||
|
||||
public boolean getAButton(){
|
||||
return m_stick.getRawButton(A_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getXButton(){
|
||||
return m_stick.getRawButton(X_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getBButton(){
|
||||
return m_stick.getRawButton(B_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getYButton(){
|
||||
return m_stick.getRawButton(Y_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getBackButton(){
|
||||
return m_stick.getRawButton(BACK_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getStartButton(){
|
||||
return m_stick.getRawButton(START_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getLeftBumperButton(){
|
||||
return m_stick.getRawButton(LEFT_BUMPER_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getRightBumperButton(){
|
||||
return m_stick.getRawButton(RIGHT_BUMPER_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getLeftJoystickButton(){
|
||||
return m_stick.getRawButton(LEFT_JOYSTICK_BUTTON);
|
||||
}
|
||||
|
||||
public boolean getRightJoystickButton(){
|
||||
return m_stick.getRawButton(RIGHT_JOYSTICK_BUTTON);
|
||||
}
|
||||
|
||||
public double getLeftXAxis(){
|
||||
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_X_AXIS));
|
||||
}
|
||||
|
||||
public double getLeftYAxis(){
|
||||
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_Y_AXIS));
|
||||
}
|
||||
|
||||
public double getRightXAxis(){
|
||||
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_X_AXIS));
|
||||
}
|
||||
|
||||
public double getRightYAxis(){
|
||||
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_Y_AXIS));
|
||||
}
|
||||
|
||||
public double getLeftTriggerAxis(){
|
||||
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_TRIGGER_AXIS));
|
||||
}
|
||||
|
||||
public double getRightTriggerAxis(){
|
||||
return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_TRIGGER_AXIS));
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the angle input from the dpad.
|
||||
* @return -1 if nothing is pressed, or the angle of the button pressed. 0 = up, 90 = right, etc.
|
||||
*/
|
||||
public int getDpadAngle() {
|
||||
return m_stick.getPOV(0);
|
||||
}
|
||||
|
||||
public boolean getDPadLeft(){
|
||||
return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getDPadRight(){
|
||||
return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) > RIGHT_DPAD_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getDPadTop(){
|
||||
return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) < TOP_DPAD_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getDPadBottom(){
|
||||
return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getLeftTrigger(){
|
||||
return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getRightTrigger(){
|
||||
return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getRightAxisUpTrigger(){
|
||||
return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getRightAxisDownTrigger(){
|
||||
return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getRightAxisLeftTrigger(){
|
||||
return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getRightAxisRightTrigger(){
|
||||
return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getLeftAxisUpTrigger(){
|
||||
return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getLeftAxisDownTrigger(){
|
||||
return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getLeftAxisLeftTrigger(){
|
||||
return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE);
|
||||
}
|
||||
|
||||
public boolean getLeftAxisRightTrigger(){
|
||||
return (getLeftXAxis() > LEFT_AXIS_RIGHT_TOLERANCE);
|
||||
}
|
||||
}
|
||||
@@ -1,50 +0,0 @@
|
||||
package frc4388.utility.controller;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.button.Button;
|
||||
|
||||
/**
|
||||
* Mapping for the Xbox controller triggers to allow triggers to be defined as
|
||||
* buttons in {@link frc4388.robot.OI}. Checks to see if the given trigger
|
||||
* exceeds a tolerance defined in {@link XboxController}.
|
||||
*/
|
||||
public class XboxTriggerButton extends Button {
|
||||
public static final int RIGHT_TRIGGER = 0;
|
||||
public static final int LEFT_TRIGGER = 1;
|
||||
public static final int RIGHT_AXIS_UP_TRIGGER = 2;
|
||||
public static final int RIGHT_AXIS_DOWN_TRIGGER = 3;
|
||||
public static final int RIGHT_AXIS_RIGHT_TRIGGER = 4;
|
||||
public static final int RIGHT_AXIS_LEFT_TRIGGER = 5;
|
||||
public static final int LEFT_AXIS_UP_TRIGGER = 6;
|
||||
public static final int LEFT_AXIS_DOWN_TRIGGER = 7;
|
||||
public static final int LEFT_AXIS_RIGHT_TRIGGER = 8;
|
||||
public static final int LEFT_AXIS_LEFT_TRIGGER = 9;
|
||||
|
||||
private XboxController m_controller;
|
||||
private int m_trigger;
|
||||
|
||||
/**
|
||||
* Creates a Trigger-Button mapped to a specific Xbox controller and trigger
|
||||
*/
|
||||
public XboxTriggerButton(XboxController controller, int trigger) {
|
||||
m_controller = controller;
|
||||
m_trigger = trigger;
|
||||
}
|
||||
|
||||
/** {@inheritDoc} */
|
||||
@Override
|
||||
public boolean get() {
|
||||
switch (m_trigger) {
|
||||
case RIGHT_TRIGGER: return m_controller.getRightTrigger();
|
||||
case LEFT_TRIGGER: return m_controller.getLeftTrigger();
|
||||
case RIGHT_AXIS_UP_TRIGGER: return m_controller.getRightAxisUpTrigger();
|
||||
case RIGHT_AXIS_DOWN_TRIGGER: return m_controller.getRightAxisDownTrigger();
|
||||
case RIGHT_AXIS_RIGHT_TRIGGER: return m_controller.getRightAxisRightTrigger();
|
||||
case RIGHT_AXIS_LEFT_TRIGGER: return m_controller.getRightAxisLeftTrigger();
|
||||
case LEFT_AXIS_UP_TRIGGER: return m_controller.getLeftAxisUpTrigger();
|
||||
case LEFT_AXIS_DOWN_TRIGGER: return m_controller.getLeftAxisDownTrigger();
|
||||
case LEFT_AXIS_RIGHT_TRIGGER: return m_controller.getLeftAxisRightTrigger();
|
||||
case LEFT_AXIS_LEFT_TRIGGER: return m_controller.getLeftAxisLeftTrigger();
|
||||
default: return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -5,7 +5,7 @@
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import static org.junit.Assert.assertEquals;
|
||||
import static org.mockito.Mockito.mock;
|
||||
// import static org.mockito.Mockito.mock;
|
||||
|
||||
import org.junit.Test;
|
||||
|
||||
@@ -22,29 +22,38 @@ public class LEDSubsystemTest {
|
||||
LED led = new LED(ledController);
|
||||
@Test
|
||||
public void testConstructor() {
|
||||
// Arrange
|
||||
// Spark ledController = mock(Spark.class);
|
||||
|
||||
// Act
|
||||
// LED led = new LED(ledController);
|
||||
|
||||
// Assert
|
||||
assertEquals(LEDConstants.DEFAULT_PATTERN.getValue(), led.getPattern().getValue(), 0.0001);
|
||||
// assertEquals(LEDConstants.DEFAULT_PATTERN.getValue(), led.getPattern().getValue(), 0.0001);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testPatterns() {
|
||||
// Arrange
|
||||
// Spark ledController = mock(Spark.class);
|
||||
// LED led = new LED(ledController);
|
||||
|
||||
// Act
|
||||
led.setPattern(LEDPatterns.RAINBOW_RAINBOW);
|
||||
// led.setPattern(LEDPatterns.RAINBOW_RAINBOW);
|
||||
|
||||
// Assert
|
||||
assertEquals(LEDPatterns.RAINBOW_RAINBOW.getValue(), led.getPattern().getValue(), 0.0001);
|
||||
// assertEquals(LEDPatterns.RAINBOW_RAINBOW.getValue(), led.getPattern().getValue(), 0.0001);
|
||||
|
||||
// Act
|
||||
led.setPattern(LEDPatterns.BLUE_BREATH);
|
||||
// led.setPattern(LEDPatterns.BLUE_BREATH);
|
||||
|
||||
// Assert
|
||||
assertEquals(LEDPatterns.BLUE_BREATH.getValue(), led.getPattern().getValue(), 0.0001);
|
||||
// assertEquals(LEDPatterns.BLUE_BREATH.getValue(), led.getPattern().getValue(), 0.0001);
|
||||
|
||||
// Act
|
||||
led.setPattern(LEDPatterns.SOLID_BLACK);
|
||||
// led.setPattern(LEDPatterns.SOLID_BLACK);
|
||||
|
||||
// Assert
|
||||
assertEquals(LEDPatterns.SOLID_BLACK.getValue(), led.getPattern().getValue(), 0.0001);
|
||||
// assertEquals(LEDPatterns.SOLID_BLACK.getValue(), led.getPattern().getValue(), 0.0001);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,5 @@
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
public class XboxControllerTest {
|
||||
|
||||
}
|
||||
@@ -0,0 +1,52 @@
|
||||
{
|
||||
"counts per rotation": 2048.0,
|
||||
"encoder type": "Built-in",
|
||||
"encoding": false,
|
||||
"gearing denominator": 1.0,
|
||||
"gearing numerator": 5.12,
|
||||
"gyro": "Pigeon",
|
||||
"gyro ctor": "14",
|
||||
"is drivetrain": false,
|
||||
"motor controllers": [
|
||||
"TalonFX",
|
||||
"TalonFX",
|
||||
"TalonFX",
|
||||
"TalonFX"
|
||||
],
|
||||
"number of samples per average": 1,
|
||||
"primary encoder inverted": false,
|
||||
"primary encoder ports": [
|
||||
10,
|
||||
1
|
||||
],
|
||||
"primary motor ports": [
|
||||
3,
|
||||
5,
|
||||
7,
|
||||
9
|
||||
],
|
||||
"primary motors inverted": [
|
||||
false,
|
||||
false,
|
||||
false,
|
||||
false
|
||||
],
|
||||
"secondary encoder inverted": false,
|
||||
"secondary encoder ports": [
|
||||
2,
|
||||
13
|
||||
],
|
||||
"secondary motor ports": [
|
||||
5,
|
||||
9,
|
||||
5,
|
||||
7
|
||||
],
|
||||
"secondary motors inverted": [
|
||||
false,
|
||||
false,
|
||||
false,
|
||||
false
|
||||
],
|
||||
"velocity measurement period": 100
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,33 @@
|
||||
{
|
||||
"fileName": "PathplannerLib.json",
|
||||
"name": "PathplannerLib",
|
||||
"version": "2022.1.0",
|
||||
"uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786",
|
||||
"mavenUrls": [
|
||||
"https://3015rangerrobotics.github.io/pathplannerlib/repo"
|
||||
],
|
||||
"jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json",
|
||||
"javaDependencies": [
|
||||
{
|
||||
"groupId": "com.pathplanner.lib",
|
||||
"artifactId": "PathplannerLib-java",
|
||||
"version": "2022.1.0"
|
||||
}
|
||||
],
|
||||
"jniDependencies": [],
|
||||
"cppDependencies": [
|
||||
{
|
||||
"groupId": "com.pathplanner.lib",
|
||||
"artifactId": "PathplannerLib-cpp",
|
||||
"version": "2022.1.0",
|
||||
"libName": "PathplannerLib",
|
||||
"headerClassifier": "headers",
|
||||
"sharedLibrary": false,
|
||||
"skipInvalidPlatforms": true,
|
||||
"binaryPlatforms": [
|
||||
"osxx86-64",
|
||||
"linuxathena"
|
||||
]
|
||||
}
|
||||
]
|
||||
}
|
||||
Reference in New Issue
Block a user