From 6430d71e3bc4e972fb97531231c295b0f3e92520 Mon Sep 17 00:00:00 2001 From: kredcool Date: Wed, 10 Jan 2024 16:24:46 -0500 Subject: [PATCH 01/61] a begining of a template for swerve --- Pipfile | 13 +++ Pipfile.lock | 163 ++++++++++++++++++++++++++++++ rio/constants/robot_hardware.yaml | 84 ++++++++------- rio/subsystems/swerveModule.py | 52 ++++++++++ 4 files changed, 269 insertions(+), 43 deletions(-) create mode 100644 Pipfile create mode 100644 Pipfile.lock create mode 100644 rio/subsystems/swerveModule.py diff --git a/Pipfile b/Pipfile new file mode 100644 index 00000000..b4814c99 --- /dev/null +++ b/Pipfile @@ -0,0 +1,13 @@ +[[source]] +url = "https://pypi.org/simple" +verify_ssl = true +name = "pypi" + +[packages] +wpilib = "*" +robotpy-wpilib-utilities = "*" + +[dev-packages] + +[requires] +python_version = "3.10" diff --git a/Pipfile.lock b/Pipfile.lock new file mode 100644 index 00000000..b588f777 --- /dev/null +++ b/Pipfile.lock @@ -0,0 +1,163 @@ +{ + "_meta": { + "hash": { + "sha256": "e51b4a093227813590fb89ffff3b65d5692aa47877734672b9a6530b6df5203c" + }, + "pipfile-spec": 6, + "requires": { + "python_version": "3.10" + }, + "sources": [ + { + "name": "pypi", + "url": "https://pypi.org/simple", + "verify_ssl": true + } + ] + }, + "default": { + "pyntcore": { + "hashes": [ + "sha256:089c9721b1aeba6f31f211a9033af9600cabb9148996d1b019cbc03eb4bd3d0b", + "sha256:2fd033786a948b0d63bd6596e9cf5ad0386f472abefe7cc1271edec85bb4c4e8", + "sha256:3422f6b07d0c5025e8c87b37964f7a9bc512d3c8087c5a1f88eaffdbbd220781", + "sha256:3f6961b8576e8a8a80d8ef28fae1129c53c1e5a91b231a12a9328ab4c6d1f9b1", + "sha256:479b8ca8d3611f85ab2c5dcca39af299f40db3d7ba4efe9256842f067a4443df", + "sha256:57080b81dbca5e0b28715f19867b848aa5534e09088d5b66d76b64f7cee998ee", + "sha256:6463a5640e52ac0ddda10649babf10616c6278858d5122363d09ff6784dca99e", + "sha256:6f0f5a89ae8a70c3994853f2ba1c29a73f3f6acf8becce71c60c232158f9db3f", + "sha256:803a3ce467527928e22e57c99525a923b79ba716dde9c94d1807b11caba09603", + "sha256:9d4afd35d0d38c30f3ad1de5e9d1bdf53d64e560fe3791536d7ea1b36f58ebbb", + "sha256:a30e97a6971fadaceacb7e357670d22af55d810d081a8b7cf808db7c62425096", + "sha256:a368189aa878be535ba2dd8c6e21173a45e2ccf446b6ef5904da4d07b2ec3acc", + "sha256:c6bd974f2e0eaa3f9f9705fe641793bc759268d75cb9cc424e20a97d587971e1", + "sha256:e68a04a9723d8c2300c1057a69cab1544ac60133b2506c45ecb16838f563d6a0", + "sha256:e93aa0c9951722e0b69288c2792f2bc2a7a65d02b26f8e8230de1edc371bf250" + ], + "markers": "python_version >= '3.8'", + "version": "==2024.1.1.1" + }, + "robotpy-cli": { + "hashes": [ + "sha256:9284035fa67058b33593b1846e13501eda0861fb9d9adce4bd01b101c9f1f26e", + "sha256:e6519f06856eb0a7ef5f25a57bfc8dcb20635cef84706fb37c0b602d59e33f39" + ], + "markers": "python_version >= '3.8'", + "version": "==2024.0.0" + }, + "robotpy-hal": { + "hashes": [ + "sha256:0cd34df381940f89626e1e2b206d2def4879c1cf66b617745447ab5323c24141", + "sha256:164ce03dda139902ecc2a1e7c1d18600fbd0f5f33386e9eff1309ec32c217cd5", + "sha256:19e445a2bd24e178e5a6ed583a21f8622da1ec214c0953c2cb028ff67b973b28", + "sha256:2254a91d6b3c1a73a4c786338e677eff7d50620eb6b24f831945a53901e98501", + "sha256:281f6bea3793106691568139df7c7722dcc40acbee95076c54070e4fe471aeac", + "sha256:547452b6a2c89ed1ca74eb4523da183bb5c0c0b7281823ce197fe99a738dee29", + "sha256:79d0810b4aee3769ee573e9d6a74f448940efea8ffaf70d86840db40122ce652", + "sha256:7b884b7d756034a42702fdde8e77e8a8b53c379872a47673a8b1b6b898d14352", + "sha256:93626c3a9fbb2ffa9a9dde9680675c845c173811a8b73bbc4790b50cf0d307ad", + "sha256:b5d3a20a1fb67eaf2bb6efc1303f3ed1db8bdea08a9fde284ee63681fffc7aa8", + "sha256:b9939b7076268c6f54f633879005685089a68d60b2df0fa52830b20c58af02cf", + "sha256:b9945c69fb063f49db01ae4b335a92689d42cc1068efadff37403390a19169f9", + "sha256:d201bbd0529701f150806abdf1c77c5203ee3fc82656fb7c9863d43b9ac3a364", + "sha256:df21128092e932490923b49f6aa9741dbdb3a225805f60900b13d1ededeee38e", + "sha256:fb8e58b726ed94c0a5f636f45b65580e5eefdeeb4c2cd2c898ba9ca0de7b1eec" + ], + "markers": "python_version >= '3.8'", + "version": "==2024.1.1.1" + }, + "robotpy-wpilib-utilities": { + "hashes": [ + "sha256:da0d3495d28b8f758c0bc12f1075996273aae831c5dd9d85d6b0581f8f08bcaa", + "sha256:f2e7e512e3e9ad938893175b22c827f97d0866ade47f34c25d68622c3f8a4c3a" + ], + "index": "pypi", + "version": "==2024.0.0" + }, + "robotpy-wpimath": { + "hashes": [ + "sha256:35eb656efcd6cf1d50b8a0ebe5b802c5f0581b6d20df934f8dd1fcb77d120192", + "sha256:4340882ab0f5c46487c4d5d9862430b58abc1762ef6d96753d9635c895987c6c", + "sha256:59466913060a697fee4cd1afc63b5b72397281f9e0019f3a8cc6a1a6dbfa252e", + "sha256:649ce400c5f3b45deaf73dbd8223be71fcb44f61d41d665bbf21ad947a7d5df3", + "sha256:7167ea5181c815fe10f3e46df495ffc08ae2f90dbf399d455fe3c72643602394", + "sha256:7b27bf280c9ae55e874b2e46f9174a3b07896ee9e4d5b90c4e16dab5173ad72e", + "sha256:81e850df98fd62e8497a49b2bceec4014b9ed3cea155642fa4e034f77d870e18", + "sha256:951ed4f556f9d2bc2addf70be331789742420147aca23e008ec5830488ec97e4", + "sha256:9d570d4dbe27bbee560a14a212dc36d7149c3714c0e9ff77528615817c295a08", + "sha256:bba421856198139d5c2384a66e04a18d47562ba1c7aaa78e4374a586bbafe427", + "sha256:cdbe1585543d5f081c6a86f199d26b2355cb0713c3ff115e065f3697c064611d", + "sha256:e3aef1a9c03d76236ed6049efdb28d4d021921d7a2dac2df7d32fb0137834704", + "sha256:f987609fec0adbf253e47388812f61969c2ffcec1958f63a097bb5a3515c1e24", + "sha256:fd6657f4a41b048f09791c1a2e556f7df12dc6e94631ae84deb1496ed246ca99", + "sha256:fd7f3e247e248c2442ed8dab5b7b807a702576c3eab9d6725796af172d4102fc" + ], + "markers": "python_version >= '3.8'", + "version": "==2024.1.1.1" + }, + "robotpy-wpinet": { + "hashes": [ + "sha256:04084d2289de0b1982ec71f9690f20bb62ac0606ba13bafaf087397979f5b168", + "sha256:120e2a371f9a53a3571ebf3e3e933e95c451de80f61e7e14f8c3b69021d101cd", + "sha256:1faf0a5cb40b27538ff20ba1e5a098490f4106a3704c89403a4851a844c25a1e", + "sha256:2816bf1c4dd18412b63c9526e413852f784d680e7b1def0eb9426496ee543bc8", + "sha256:2caf345812138b0b76695f7c858c3ba364403f4de08a9bb21074d1724ad9f326", + "sha256:3a07601b53193179310e8d433d1100ff554f62e87ffd62daf8c9470847b0cce8", + "sha256:52d0fbb187120416846261d3a1150282a33542a672495fdd5189d0f0d7f211a4", + "sha256:63ee412582a42a40cba5c851a3723d84a69f9a8787e66446eea6c2426ea2859e", + "sha256:6a58178cab524f6943027dd93a8e2ace92b2a23cdefac596297aabf15830f28e", + "sha256:79cad99f417c0c3d08d40755fe1d303a7841726befe878bc7f5221b2c8b0271b", + "sha256:8729c00e2b6e8993e3bb7ea9f3d0f70b7028dd91c172b3d8e8e62a5cd04627c5", + "sha256:8ed88d6aa152be5899ff5ac441aebf53dffb529cda552e82031687d1bb13e4a7", + "sha256:94bf23400e7891f9ee4665570c1f5fc638bf8c89049d508af2b99a31c81d2c49", + "sha256:a9dc643a8a00055cbc7d6998ae02fbfa92da81498da4a105ee2edb76d7ef8319", + "sha256:f3ce06ddcce3f458cabb126bffbb78242d541c0500c1b703f0afd212ae64e7cf" + ], + "markers": "python_version >= '3.8'", + "version": "==2024.1.1.1" + }, + "robotpy-wpiutil": { + "hashes": [ + "sha256:0084575cd93088ab71ee3140506ef497ae3292cab4c786dbf3f8f7b19333b6ee", + "sha256:0b552be4e0d5f411237e70a4c9fed959906917f9e5681e5db8b06b17ac857a56", + "sha256:0d8579546617a5e2967b1e3325a6778ca6e56d87585b09f766258077722fcc47", + "sha256:1aa7bc8abd9635517119ccd6a322e28a5da5ab92bf1c66c0b90ffbb865549710", + "sha256:3008cb3a9e94ae9a0b2c15aacd4e18886a621723a196a20a459d7419ba504ed3", + "sha256:3c72957302d7caa65f2a71274889e481a8b3490bd7bd425cef0340defe09b179", + "sha256:5096c16e90eef7bec892e3ad7ea648110c4d525e0c79d1aa07d9d380a0c0c66c", + "sha256:5f62c29d06f9adf5159c82b2b7bdab98d2d247f6cc84df0aebee6570c916f735", + "sha256:90d600535afbd61892a056e8c292b4de9fa4cd593b0d0a7e1b3585d5801f43d1", + "sha256:ab5f7e193f1f044feb74dea87867811132ce71b19d1cb55b06b03db7fdec2489", + "sha256:ae84c668de63383d633a56d8c089b76ea3e2835f36ab49374ed498c118fd5c47", + "sha256:b1aead0364f3069f82c2eb5bbb860906a8a2be89b520afa1da9d7ff58f04b0cf", + "sha256:f3ee870245ee3cec80f1b74a6f674e93b574d156924c83c239970a07a3ab6dec", + "sha256:fd98ab7f66447387cb329ed8451736c616dbf4c4d31e9ad746853ea9046ca506", + "sha256:ff4f4be6e5d9d6ac9a0bcc1e72fa7756810e986939bfa1ad2522b597dab7f85c" + ], + "markers": "python_version >= '3.8'", + "version": "==2024.1.1.1" + }, + "wpilib": { + "hashes": [ + "sha256:023f1b6f404a5ab3a5018bf59b681215c8d1455ab114bbe1c70ad6648fd36750", + "sha256:244745f00b9fc6db7b66b2992a1bfc9c7da5c6951bb85aeda0f277de6589eca6", + "sha256:4e277c109c14550dff64f6e3e19202a0517de6776e59484447ca50539c35830b", + "sha256:6b296068ba2901bd4ec8bc0bcee499ef54fd2e18d68a927b174445397d1a9e65", + "sha256:6f04c81531e67a3f06e7a50198a21657095f0fc5bdd823cd2ada4931317b2e41", + "sha256:82a2cee6ba2ff21b545388c3752ccbf5eafb60c73188d30a2161e2fce4ec2f7f", + "sha256:9edad7cd2483e514864c0fa4128abf9aeb929d2888c95fbc37a00579daca98c9", + "sha256:a5ef39347c780cc50d7ef0f079377541298070174654446272b3e392824739ab", + "sha256:b9bbd811fc38689e20714bd584bf9af13f1a1bc885ba0de282ff0ef100ebd528", + "sha256:c2816c5fb4febaeb4c7055e5528780103b0ea705084e7d99c5e219d9873a96fb", + "sha256:c8172b7ccd416a0335aaf44cc54d2ee6d92a404f880026d9934445977c74e9d0", + "sha256:c825d146270ef9c2ceaa9abfde2ea5f27270149cf97c1ccf07f3dab9ab659077", + "sha256:d4120bc2482b3af2eaaaa1562a18e86a96e7830194926dbdd66de679727d7871", + "sha256:e3b4063c4068fadd48e7cfb220476bfd529e59614e0b3f8a825d06115d2a9b44", + "sha256:fcfa81321de93933406e313b7f3f89d6300ab5bc9d757c89b4214858fb4393d3" + ], + "index": "pypi", + "version": "==2024.1.1.1" + } + }, + "develop": {} +} diff --git a/rio/constants/robot_hardware.yaml b/rio/constants/robot_hardware.yaml index 5af00073..68e2dbab 100644 --- a/rio/constants/robot_hardware.yaml +++ b/rio/constants/robot_hardware.yaml @@ -4,48 +4,46 @@ # of arms, and similar should go here. drivetrain: - max_velocity: 1.5 # Updated Never by Nobody - encoderConversionFactor: 21.43 # Updated 1/27 by Joe - - # All pose coordinates are in meters. - leftMotor: - Motor1Port: 2 # Updated 1/12 by Keegan - Motor2Port: 1 # Updated 1/12 by Keegan - EncoderPorts: [2, 1] # Updated 2/12 by Keegan + wheelRadius: 1.5 # Updated Never by Nobody + # cycles: 2048, counts: 8192 per revolution + encoderResolution: 2048 # Updated Never by Nobody + + FPMotors: + Motor1Port: 1 # Updated Never by Nobody + Motor2Port: 2 # Updated Never by Nobody + EncoderPorts: [1, 2] # Updated Never by Nobody EncoderReversed: False # Updated Never by Nobody Inverted: False # Updated Never by Nobody - - rightMotor: - Motor1Port: 3 # Updated 1/8 by Keegan - Motor2Port: 4 # Updated 1/8 by Keegan - EncoderPorts: [3, 4] # Updated Never by Nobody - EncoderReversed: True # Updated Never by Nobody - Inverted: True # Updated Never by Nobody - - navX: # NavX ids - can_id: 0 # Updated 1/13 by Keegan - yaw: 0 # Updated 1/13 by Keegan - pitch: 0 # Updated 1/13 by Keegan - roll: 0 # Updated 1/13 by Keegan - -arm: - elevatorMotor: # Drives the spool to move the lift in and out - MotorPort: 5 # Updated 1/22 by Keegan - Inverted: False # Updated 1/22 by Keegan - LimitSwitch: 0 # Updated 2/15 by Keegan - ConversionFactor: 1 # Updated 1/18 by Joe - Start: 0 # Updated 3/18 by Keegan - - ladderMotor: # Drives the lead screw to run the lift up and down - MotorPort: 6 # Updated 1/22 by Keegan - Inverted: False # Updated 1/22 by Keegan - ConversionFactor: 1 # Updated 1/18 by Joe - Start: 55 # Updated 3/18 by Keegan - - - -claw: - MotorPort: 7 # Updated 1/22 by Keegan - Inverted: False # Updated 1/22 by Keegan - EncoderPorts: 7 # Updated 2/11 by Keegan - EncoderReversed: False # Updated 1/22 by Keegan \ No newline at end of file + + APMotors: + Motor1Port: 3 # Updated Never by Nobody + Motor2Port: 4 # Updated Never by Nobody + EncoderPorts: [3, 4] # Updated Never by Nobody + EncoderReversed: False # Updated Never by Nobody + Inverted: False # Updated Never by Nobody + + FSMotors: + Motor1Port: 5 # Updated Never by Nobody + Motor2Port: 6 # Updated Never by Nobody + EncoderPorts: [5, 6] # Updated Never by Nobody + EncoderReversed: False # Updated Never by Nobody + Inverted: False # Updated Never by Nobody + + ASMotors: + Motor1Port: 7 # Updated Never by Nobody + Motor2Port: 8 # Updated Never by Nobody + EncoderPorts: [7, 8] # Updated Never by Nobody + EncoderReversed: False # Updated Never by Nobody + Inverted: False # Updated Never by Nobody + + DrivePIDValues: + P: 0 # Updated Never by Nobody + I: 0 # Updated Never by Nobody + D: 0 # Updated Never by Nobody + FF: 0 # Updated Never by Nobody + + TurnPIDValues: + P: 0 # Updated Never by Nobody + I: 0 # Updated Never by Nobody + D: 0 # Updated Never by Nobody + FF: 0 # Updated Never by Nobody \ No newline at end of file diff --git a/rio/subsystems/swerveModule.py b/rio/subsystems/swerveModule.py new file mode 100644 index 00000000..d6302402 --- /dev/null +++ b/rio/subsystems/swerveModule.py @@ -0,0 +1,52 @@ +from rev import CANSparkMax, CANSparkMaxLowLevel +from constants.constants import getConstants + + +class SwerveModule: + def __init__( + self, + driveChannel, + turnChannel, + driveEncoder, + turnEncoder, + ) -> None: + """ + LMAO look at this joke code + it 'claims' to be a swerve module + + driveChannel: drive motor id + turnChannel: turn motor id + driveEncoder: encoder channel + turnEncoder: encoder channel + """ + + # constants + constants = getConstants("robot_hardware") + self.drivePID = constants["DrivePIDValues"] + self.turnPID = constants["TurnPIDValues"] + + # motors + self.driveMotor = CANSparkMax( + driveChannel, CANSparkMaxLowLevel.MotorType.kBrushless + ) + self.turnMotor = CANSparkMax( + turnChannel, CANSparkMaxLowLevel.MotorType.kBrushless + ) + + # encoders + self.driveEncoder = self.driveMotor.getEncoder() + self.turnEncoder = self.turnMotor.getEncoder() + + # PID + self.drivePIDController = self.driveMotor.getPIDController() + self.turnPIDController = self.turnMotor.getPIDController() + + self.drivePIDController.setP(self.drivePID["P"]) + self.drivePIDController.setI(self.drivePID["I"]) + self.drivePIDController.setD(self.drivePID["D"]) + self.drivePIDController.setFF(self.drivePID["FF"]) + + self.turnPIDController.setP(self.turnPID["P"]) + self.turnPIDController.setI(self.turnPID["I"]) + self.turnPIDController.setD(self.turnPID["D"]) + self.turnPIDController.setFF(self.turnPID["FF"]) From b5202b2bbe7efcec3871535db91370208c67a97d Mon Sep 17 00:00:00 2001 From: kredcool Date: Wed, 10 Jan 2024 16:46:59 -0500 Subject: [PATCH 02/61] may zero all theory --- rio/subsystems/swerveModule.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/rio/subsystems/swerveModule.py b/rio/subsystems/swerveModule.py index d6302402..55754963 100644 --- a/rio/subsystems/swerveModule.py +++ b/rio/subsystems/swerveModule.py @@ -1,5 +1,6 @@ from rev import CANSparkMax, CANSparkMaxLowLevel from constants.constants import getConstants +from wpilib import DutyCycleEncoder class SwerveModule: @@ -50,3 +51,12 @@ def __init__( self.turnPIDController.setI(self.turnPID["I"]) self.turnPIDController.setD(self.turnPID["D"]) self.turnPIDController.setFF(self.turnPID["FF"]) + + # Copied from rev, be skeptical + def find_zero(self): + """ + remember how much this sucked + last year, zero's serve maybe + """ + # this may be the wrong motor + self.turnMotor.setPosition(0) From 0d0dd38d360c67641506da38ff3aa7bdf46db3bd Mon Sep 17 00:00:00 2001 From: kredcool Date: Wed, 10 Jan 2024 17:17:35 -0500 Subject: [PATCH 03/61] init created --- rio/constants/robot_hardware.yaml | 16 +++++----- rio/subsystems/swerveModule.py | 26 +++++++++++++--- rio/subsystems/swervesubsystem.py | 49 +++++++++++++++++++++++++++++++ 3 files changed, 79 insertions(+), 12 deletions(-) create mode 100644 rio/subsystems/swervesubsystem.py diff --git a/rio/constants/robot_hardware.yaml b/rio/constants/robot_hardware.yaml index 68e2dbab..612a281d 100644 --- a/rio/constants/robot_hardware.yaml +++ b/rio/constants/robot_hardware.yaml @@ -9,29 +9,29 @@ drivetrain: encoderResolution: 2048 # Updated Never by Nobody FPMotors: - Motor1Port: 1 # Updated Never by Nobody - Motor2Port: 2 # Updated Never by Nobody + drivePort: 1 # Updated Never by Nobody + turnPort: 2 # Updated Never by Nobody EncoderPorts: [1, 2] # Updated Never by Nobody EncoderReversed: False # Updated Never by Nobody Inverted: False # Updated Never by Nobody APMotors: - Motor1Port: 3 # Updated Never by Nobody - Motor2Port: 4 # Updated Never by Nobody + drivePort: 3 # Updated Never by Nobody + turnPort: 4 # Updated Never by Nobody EncoderPorts: [3, 4] # Updated Never by Nobody EncoderReversed: False # Updated Never by Nobody Inverted: False # Updated Never by Nobody FSMotors: - Motor1Port: 5 # Updated Never by Nobody - Motor2Port: 6 # Updated Never by Nobody + drivePort: 5 # Updated Never by Nobody + turnPort: 6 # Updated Never by Nobody EncoderPorts: [5, 6] # Updated Never by Nobody EncoderReversed: False # Updated Never by Nobody Inverted: False # Updated Never by Nobody ASMotors: - Motor1Port: 7 # Updated Never by Nobody - Motor2Port: 8 # Updated Never by Nobody + drivePort: 7 # Updated Never by Nobody + turnPort: 8 # Updated Never by Nobody EncoderPorts: [7, 8] # Updated Never by Nobody EncoderReversed: False # Updated Never by Nobody Inverted: False # Updated Never by Nobody diff --git a/rio/subsystems/swerveModule.py b/rio/subsystems/swerveModule.py index 55754963..f70ae63f 100644 --- a/rio/subsystems/swerveModule.py +++ b/rio/subsystems/swerveModule.py @@ -52,11 +52,29 @@ def __init__( self.turnPIDController.setD(self.turnPID["D"]) self.turnPIDController.setFF(self.turnPID["FF"]) - # Copied from rev, be skeptical - def find_zero(self): + # based on rev, be skeptical + def zeroEncoders(self): + """ + sets the ecoder pos to zero + maybe + """ + # this may be the wrong encoder + self.turnEncoder.setPosition(0) + + # + + def gotoZero(self): """ remember how much this sucked last year, zero's serve maybe """ - # this may be the wrong motor - self.turnMotor.setPosition(0) + if not self.zero: + if self.turnEncoder.getPosition() != 0: + self.steer_motor.set(0.165) # CHANGEME + + else: + self.turnEncoder.setPosition(0) + self.zero = True + + else: + self.turnEncoder.setPosition(0) diff --git a/rio/subsystems/swervesubsystem.py b/rio/subsystems/swervesubsystem.py new file mode 100644 index 00000000..4c9d5c56 --- /dev/null +++ b/rio/subsystems/swervesubsystem.py @@ -0,0 +1,49 @@ +# rename to drivesubsystem.py when completed + +from commands2 import Subsystem +from constants.constants import getConstants +from .swerveModule import SwerveModule + + +class drivesubsystem(Subsystem): + def __init__(self) -> None: + # constants + constants = getConstants("robot_hardware") + self.driveConst = constants["drivetrain"] + + # modules + self.fPMotors = self.driveConst["FPMotors"] + self.fSMotors = self.driveConst["FSMotors"] + self.aPMotors = self.driveConst["APMotors"] + self.aSMotors = self.driveConst["ASMotors"] + + # create swerve modules + self.fpModule = SwerveModule( + self.fPMotors["drivePort"], + self.fPMotors["turnPort"], + self.fPMotors["EncoderPort"][0], + self.fPMotors["EncoderPort"][1], + ) + self.fsModule = SwerveModule( + self.fSMotors["drivePort"], + self.fSMotors["turnPort"], + self.fSMotors["EncoderPort"][0], + self.fSMotors["EncoderPort"][1], + ) + self.apModule = SwerveModule( + self.aPMotors["drivePort"], + self.aPMotors["turnPort"], + self.aPMotors["EncoderPort"][0], + self.aPMotors["EncoderPort"][1], + ) + self.asModule = SwerveModule( + self.aSMotors["drivePort"], + self.aSMotors["turnPort"], + self.aSMotors["EncoderPort"][0], + self.aSMotors["EncoderPort"][1], + ) + + self.fsZero.setBoolean(self.fsModule.isZeroed) + self.asZero.setBoolean(self.asModule.isZeroed) + self.fpZero.setBoolean(self.fpModule.isZeroed) + self.apZero.setBoolean(self.apModule.isZeroed) From 032a4a01c36a0e79f34c6734ac69ffc82f06ff08 Mon Sep 17 00:00:00 2001 From: kredcool Date: Thu, 11 Jan 2024 16:38:51 -0500 Subject: [PATCH 04/61] all is made with reference to Maxswerve, numbers will need to be changed. this is just the init --- rio/constants/robot_hardware.yaml | 24 ++++++- rio/subsystems/swerveModule.py | 103 +++++++++++++++++++++--------- rio/subsystems/swervesubsystem.py | 5 -- 3 files changed, 96 insertions(+), 36 deletions(-) diff --git a/rio/constants/robot_hardware.yaml b/rio/constants/robot_hardware.yaml index 612a281d..55edc7ba 100644 --- a/rio/constants/robot_hardware.yaml +++ b/rio/constants/robot_hardware.yaml @@ -7,6 +7,9 @@ drivetrain: wheelRadius: 1.5 # Updated Never by Nobody # cycles: 2048, counts: 8192 per revolution encoderResolution: 2048 # Updated Never by Nobody + # TODO fix these (there in amps) + DriveMotorCurrentLimit: 50 + TurnMotorCurrentLimit: 20 FPMotors: drivePort: 1 # Updated Never by Nobody @@ -41,9 +44,28 @@ drivetrain: I: 0 # Updated Never by Nobody D: 0 # Updated Never by Nobody FF: 0 # Updated Never by Nobody + MinOutput: -1 + MaxOutput: 1 TurnPIDValues: P: 0 # Updated Never by Nobody I: 0 # Updated Never by Nobody D: 0 # Updated Never by Nobody - FF: 0 # Updated Never by Nobody \ No newline at end of file + FF: 0 # Updated Never by Nobody + MinOutput: -1 + MaxOutput: 1 + + Conversions: + # see rev's maxswervemodule code for how numbers were + # obtained https://github.com/robotpy/robotpy-rev/tree/main/examples/maxswerve + Drive: + MotorPositionFactor: 289.070303031 + EncoderVelocityFactor: 17344.2181818 + + + Turn: + EncoderPositionFactor: 6.28 + EncoderVelocityFactor: 0.104666666667 + EncoderPositionPIDMinInput: 0 + EncoderPositionPIDMaxInput: 6.28 + \ No newline at end of file diff --git a/rio/subsystems/swerveModule.py b/rio/subsystems/swerveModule.py index f70ae63f..00906cfd 100644 --- a/rio/subsystems/swerveModule.py +++ b/rio/subsystems/swerveModule.py @@ -1,15 +1,16 @@ -from rev import CANSparkMax, CANSparkMaxLowLevel +from rev import CANSparkMax, CANSparkMaxLowLevel, SparkMaxAbsoluteEncoder from constants.constants import getConstants from wpilib import DutyCycleEncoder +from wpimath.kinematics import SwerveModuleState +from wpimath.geometry import Rotation2d class SwerveModule: def __init__( self, - driveChannel, - turnChannel, - driveEncoder, - turnEncoder, + driveChannel: int, + turnChannel: int, + chassisAngularOffset: float, ) -> None: """ LMAO look at this joke code @@ -23,8 +24,12 @@ def __init__( # constants constants = getConstants("robot_hardware") - self.drivePID = constants["DrivePIDValues"] - self.turnPID = constants["TurnPIDValues"] + self.driveConstants = constants["drivetrain"] + self.drivePID = self.driveConstants["DrivePIDValues"] + self.turnPID = self.driveConstants["TurnPIDValues"] + conversions = constants["Conversions"] + self.driveConversions = conversions["Drive"] + self.turnConversions = conversions["Turn"] # motors self.driveMotor = CANSparkMax( @@ -34,47 +39,85 @@ def __init__( turnChannel, CANSparkMaxLowLevel.MotorType.kBrushless ) + # factory reset so we know whats been done to them + self.driveMotor.restoreFactoryDefaults() + self.turnMotor.restoreFactoryDefaults() + # encoders self.driveEncoder = self.driveMotor.getEncoder() - self.turnEncoder = self.turnMotor.getEncoder() + self.turnEncoder = self.turnMotor.getAbsoluteEncoder( + SparkMaxAbsoluteEncoder.Type.kDutyCycle + ) + + # converting units to meters and meters per second to better work with WPILib's swerve APIs + self.driveEncoder.setPositionConversionFactor( + self.driveConversions["MotorPositionFactor"] + ) + self.driveEncoder.setVelocityConversionFactor( + self.driveConversions["EncoderVelocityFactor"] + ) + + self.turnEncoder.setPositionConversionFactor( + self.turnConversions["EncoderPositionFactor"] + ) + self.turnEncoder.setVelocityConversionFactor( + self.turnConversions["EncoderVelocityFactor"] + ) + + # we may need to invert the turning encoder + # self.turnEncoder.setInverted(True) # PID self.drivePIDController = self.driveMotor.getPIDController() self.turnPIDController = self.turnMotor.getPIDController() + self.drivePIDController.setFeedbackDevice(self.driveEncoder) + self.turnPIDController.setFeedbackDevice(self.turnEncoder) + self.drivePIDController.setP(self.drivePID["P"]) self.drivePIDController.setI(self.drivePID["I"]) self.drivePIDController.setD(self.drivePID["D"]) self.drivePIDController.setFF(self.drivePID["FF"]) + self.drivePIDController.setOutputRange( + self.drivePID["MinOutput"], self.drivePID["MaxOutput"] + ) self.turnPIDController.setP(self.turnPID["P"]) self.turnPIDController.setI(self.turnPID["I"]) self.turnPIDController.setD(self.turnPID["D"]) self.turnPIDController.setFF(self.turnPID["FF"]) + self.turnPIDController.setOutputRange( + self.turnPID["MinOutput"], self.turnPID["MaxOutput"] + ) - # based on rev, be skeptical - def zeroEncoders(self): - """ - sets the ecoder pos to zero - maybe - """ - # this may be the wrong encoder - self.turnEncoder.setPosition(0) + # this should fix it spinning from 359 to zero the long way + self.turnPIDController.setPositionPIDWrappingEnabled(True) + self.turnPIDController.setPositionPIDWrappingMinInput( + self.turnConversions["EncoderPositionPIDMinInput"] + ) + self.turnPIDController.setPositionPIDWrappingMaxInput( + self.turnConversions["EncoderPositionPIDMaxInput"] + ) - # + # defining idle modes + self.driveMotor.setIdleMode(CANSparkMax.IdleMode.kBrake) + self.turnMotor.setIdleMode(CANSparkMax.IdleMode.kBrake) - def gotoZero(self): - """ - remember how much this sucked - last year, zero's serve maybe - """ - if not self.zero: - if self.turnEncoder.getPosition() != 0: - self.steer_motor.set(0.165) # CHANGEME + # setting limits + self.driveMotor.setSmartCurrentLimit( + self.driveConstants["DriveMotorCurrentLimt"] + ) + self.turnMotor.setSmartCurrentLimit(self.driveConstants["TurnMotorCurrentLimt"]) + + # save config, to ensure config says after brown outs + self.driveMotor.burnFlash() + self.turnMotor.burnFlash() + + # Misc + self.chassisAngularOffset = 0 + self.chassisAngularOffset = chassisAngularOffset - else: - self.turnEncoder.setPosition(0) - self.zero = True + self.desiredState = SwerveModuleState(0.0, Rotation2d()) + self.desiredState.angle = Rotation2d(self.turnEncoder.getPosition()) - else: - self.turnEncoder.setPosition(0) + self.driveEncoder.setPosition(0) diff --git a/rio/subsystems/swervesubsystem.py b/rio/subsystems/swervesubsystem.py index 4c9d5c56..d7fa96ab 100644 --- a/rio/subsystems/swervesubsystem.py +++ b/rio/subsystems/swervesubsystem.py @@ -42,8 +42,3 @@ def __init__(self) -> None: self.aSMotors["EncoderPort"][0], self.aSMotors["EncoderPort"][1], ) - - self.fsZero.setBoolean(self.fsModule.isZeroed) - self.asZero.setBoolean(self.asModule.isZeroed) - self.fpZero.setBoolean(self.fpModule.isZeroed) - self.apZero.setBoolean(self.apModule.isZeroed) From 778884488fa3f070c4e04e7ce74bfef77522b4e0 Mon Sep 17 00:00:00 2001 From: kredcool Date: Thu, 11 Jan 2024 16:40:15 -0500 Subject: [PATCH 05/61] a tiny bit of commenting --- rio/subsystems/swerveModule.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/rio/subsystems/swerveModule.py b/rio/subsystems/swerveModule.py index 00906cfd..a3a5417b 100644 --- a/rio/subsystems/swerveModule.py +++ b/rio/subsystems/swerveModule.py @@ -71,9 +71,11 @@ def __init__( self.drivePIDController = self.driveMotor.getPIDController() self.turnPIDController = self.turnMotor.getPIDController() + # feedback self.drivePIDController.setFeedbackDevice(self.driveEncoder) self.turnPIDController.setFeedbackDevice(self.turnEncoder) + # drive PIDS self.drivePIDController.setP(self.drivePID["P"]) self.drivePIDController.setI(self.drivePID["I"]) self.drivePIDController.setD(self.drivePID["D"]) @@ -82,6 +84,7 @@ def __init__( self.drivePID["MinOutput"], self.drivePID["MaxOutput"] ) + # turn PIDS self.turnPIDController.setP(self.turnPID["P"]) self.turnPIDController.setI(self.turnPID["I"]) self.turnPIDController.setD(self.turnPID["D"]) @@ -113,11 +116,13 @@ def __init__( self.driveMotor.burnFlash() self.turnMotor.burnFlash() - # Misc + # Chassis angular offset self.chassisAngularOffset = 0 self.chassisAngularOffset = chassisAngularOffset + # making the desired state self.desiredState = SwerveModuleState(0.0, Rotation2d()) self.desiredState.angle = Rotation2d(self.turnEncoder.getPosition()) + # misc self.driveEncoder.setPosition(0) From 605399ee42cf80dd71b3ae46f3e821c3e93037b9 Mon Sep 17 00:00:00 2001 From: kredcool Date: Thu, 11 Jan 2024 16:43:34 -0500 Subject: [PATCH 06/61] added getState() --- rio/subsystems/swerveModule.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/rio/subsystems/swerveModule.py b/rio/subsystems/swerveModule.py index a3a5417b..33e0fc19 100644 --- a/rio/subsystems/swerveModule.py +++ b/rio/subsystems/swerveModule.py @@ -126,3 +126,15 @@ def __init__( # misc self.driveEncoder.setPosition(0) + + def getState(self) -> SwerveModuleState: + """ + returns the current state + this is all relative to the chassis + """ + + # applies angular offset of chassis to encoder position to get a final position + return SwerveModuleState( + self.driveEncoder.getVelocity(), + Rotation2d(self.turnEncoder.getPosition() - self.chassisAngularOffset), + ) From c994daa3fa540bba69a8e45113301c6caec56702 Mon Sep 17 00:00:00 2001 From: kredcool Date: Thu, 11 Jan 2024 16:48:28 -0500 Subject: [PATCH 07/61] added getPosition() --- rio/subsystems/swerveModule.py | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/rio/subsystems/swerveModule.py b/rio/subsystems/swerveModule.py index 33e0fc19..0fb23afe 100644 --- a/rio/subsystems/swerveModule.py +++ b/rio/subsystems/swerveModule.py @@ -1,7 +1,7 @@ from rev import CANSparkMax, CANSparkMaxLowLevel, SparkMaxAbsoluteEncoder from constants.constants import getConstants from wpilib import DutyCycleEncoder -from wpimath.kinematics import SwerveModuleState +from wpimath.kinematics import SwerveModuleState, SwerveModulePosition from wpimath.geometry import Rotation2d @@ -138,3 +138,15 @@ def getState(self) -> SwerveModuleState: self.driveEncoder.getVelocity(), Rotation2d(self.turnEncoder.getPosition() - self.chassisAngularOffset), ) + + def getPosition(self) -> SwerveModulePosition: + """ + returns the position + this is all relative to the chassis + """ + + # applies angular offset of chassis to encoder position to get a final position + return SwerveModulePosition( + self.driveEncoder.getPosition(), + Rotation2d(self.turnEncoder.getPosition() - self.chassisAngularOffset), + ) From e8de9a2830d17cd58b190732dbf11f06a3777300 Mon Sep 17 00:00:00 2001 From: kredcool Date: Thu, 11 Jan 2024 16:55:12 -0500 Subject: [PATCH 08/61] added setDesiredState() --- rio/subsystems/swerveModule.py | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/rio/subsystems/swerveModule.py b/rio/subsystems/swerveModule.py index 0fb23afe..ff2cf013 100644 --- a/rio/subsystems/swerveModule.py +++ b/rio/subsystems/swerveModule.py @@ -150,3 +150,30 @@ def getPosition(self) -> SwerveModulePosition: self.driveEncoder.getPosition(), Rotation2d(self.turnEncoder.getPosition() - self.chassisAngularOffset), ) + + def setDesiredState(self, desiredState: SwerveModuleState): + """ + set a new desired state + """ + + # add chassis offset + correctedDesiredState = SwerveModuleState() + correctedDesiredState.speed = desiredState.speed + correctedDesiredState.angle = desiredState.angle + Rotation2d( + self.chassisAngularOffset + ) + + # make it avoid spinning over 90 in reference state + optimizedDesiredState = SwerveModuleState.optimize( + correctedDesiredState, Rotation2d(self.turnEncoder.getPosition()) + ) + + # turn to setpoints + self.drivePIDController.setReference( + optimizedDesiredState.speed, CANSparkMax.ControlType.kVelocity + ) + self.turnPIDController.setReference( + optimizedDesiredState.angle.radians(), CANSparkMax.ControlType.kPosition + ) + + self.desiredState = desiredState From 78eb9c2527adcd87f42c562dcbb7276fc118ed46 Mon Sep 17 00:00:00 2001 From: kredcool Date: Thu, 11 Jan 2024 16:56:38 -0500 Subject: [PATCH 09/61] added resetEncoders() --- rio/subsystems/swerveModule.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/rio/subsystems/swerveModule.py b/rio/subsystems/swerveModule.py index ff2cf013..e86492a9 100644 --- a/rio/subsystems/swerveModule.py +++ b/rio/subsystems/swerveModule.py @@ -177,3 +177,10 @@ def setDesiredState(self, desiredState: SwerveModuleState): ) self.desiredState = desiredState + + def resetEncoders(self) -> None: + """ + to quote rev as to the function of this + "-+es all the SwerveModule encoders." + """ + self.driveEncoder.setPosition(0) From 42e8f3e17b1b8ecc10f938bfe8f6420aeb5bb4f1 Mon Sep 17 00:00:00 2001 From: kredcool Date: Thu, 11 Jan 2024 18:07:05 -0500 Subject: [PATCH 10/61] requires a few more conversions to our stuff --- rio/constants/robot_hardware.yaml | 8 ++- rio/subsystems/swervesubsystem.py | 83 ++++++++++++++++++++----------- 2 files changed, 61 insertions(+), 30 deletions(-) diff --git a/rio/constants/robot_hardware.yaml b/rio/constants/robot_hardware.yaml index 55edc7ba..3c02f439 100644 --- a/rio/constants/robot_hardware.yaml +++ b/rio/constants/robot_hardware.yaml @@ -17,13 +17,15 @@ drivetrain: EncoderPorts: [1, 2] # Updated Never by Nobody EncoderReversed: False # Updated Never by Nobody Inverted: False # Updated Never by Nobody - + ChassisAngularOffset: -1.75 + APMotors: drivePort: 3 # Updated Never by Nobody turnPort: 4 # Updated Never by Nobody EncoderPorts: [3, 4] # Updated Never by Nobody EncoderReversed: False # Updated Never by Nobody Inverted: False # Updated Never by Nobody + ChassisAngularOffset: 3.14 FSMotors: drivePort: 5 # Updated Never by Nobody @@ -31,13 +33,15 @@ drivetrain: EncoderPorts: [5, 6] # Updated Never by Nobody EncoderReversed: False # Updated Never by Nobody Inverted: False # Updated Never by Nobody - + ChassisAngularOffset: 0 + ASMotors: drivePort: 7 # Updated Never by Nobody turnPort: 8 # Updated Never by Nobody EncoderPorts: [7, 8] # Updated Never by Nobody EncoderReversed: False # Updated Never by Nobody Inverted: False # Updated Never by Nobody + ChassisAngularOffset: 1.75 DrivePIDValues: P: 0 # Updated Never by Nobody diff --git a/rio/subsystems/swervesubsystem.py b/rio/subsystems/swervesubsystem.py index d7fa96ab..d96a1753 100644 --- a/rio/subsystems/swervesubsystem.py +++ b/rio/subsystems/swervesubsystem.py @@ -1,44 +1,71 @@ # rename to drivesubsystem.py when completed +# replace IMU with our gyro from commands2 import Subsystem from constants.constants import getConstants from .swerveModule import SwerveModule +from wpilib import Timer +from wpimath.filter import SlewRateLimiter class drivesubsystem(Subsystem): - def __init__(self) -> None: + def __init__(self): + super().__init__() + # constants constants = getConstants("robot_hardware") self.driveConst = constants["drivetrain"] - # modules - self.fPMotors = self.driveConst["FPMotors"] - self.fSMotors = self.driveConst["FSMotors"] - self.aPMotors = self.driveConst["APMotors"] - self.aSMotors = self.driveConst["ASMotors"] - - # create swerve modules - self.fpModule = SwerveModule( - self.fPMotors["drivePort"], - self.fPMotors["turnPort"], - self.fPMotors["EncoderPort"][0], - self.fPMotors["EncoderPort"][1], + # module constants + self.fPMotorsConst = self.driveConst["FPMotors"] + self.fSMotorsConst = self.driveConst["FSMotors"] + self.aPMotorsConst = self.driveConst["APMotors"] + self.aSMotorsConst = self.driveConst["ASMotors"] + + # defining the modules + self.fPModule = SwerveModule( + self.fPMotorsConst["drivePort"], + self.fPMotorsConst["turnPort"], + self.fPMotorsConst["ChassisAngularOffset"], ) - self.fsModule = SwerveModule( - self.fSMotors["drivePort"], - self.fSMotors["turnPort"], - self.fSMotors["EncoderPort"][0], - self.fSMotors["EncoderPort"][1], + self.fSModule = SwerveModule( + self.fSMotorsConst["drivePort"], + self.fSMotorsConst["turnPort"], + self.fSMotorsConst["ChassisAngularOffset"], ) - self.apModule = SwerveModule( - self.aPMotors["drivePort"], - self.aPMotors["turnPort"], - self.aPMotors["EncoderPort"][0], - self.aPMotors["EncoderPort"][1], + self.aPModule = SwerveModule( + self.aPMotorsConst["drivePort"], + self.aPMotorsConst["turnPort"], + self.aPMotorsConst["ChassisAngularOffset"], ) - self.asModule = SwerveModule( - self.aSMotors["drivePort"], - self.aSMotors["turnPort"], - self.aSMotors["EncoderPort"][0], - self.aSMotors["EncoderPort"][1], + self.aSModule = SwerveModule( + self.aSMotorsConst["drivePort"], + self.aSMotorsConst["turnPort"], + self.aSMotorsConst["ChassisAngularOffset"], + ) + + # The gyro sensor + self.gyro = ADIS16448_IMU() + + # Slew rate filter variables for controlling lateral acceleration + self.currentRotation = 0.0 + self.currentTranslationDir = 0.0 + self.currentTranslationMag = 0.0 + + self.magLimiter = SlewRateLimiter(self.driveConst["MagnitudeSlewRate"]) + self.rotLimiter = SlewRateLimiter(self.driveConst["RotationalSlewRate"]) + + # timer + self.prevTime = Timer.getFPGATimestamp() + + # Odometry class for tracking robot pose + self.odometry = SwerveDrive4Odometry( + DriveConstants.kDriveKinematics, + Rotation2d.fromDegrees(self.gyro.getAngle()), + ( + self.frontLeft.getPosition(), + self.frontRight.getPosition(), + self.rearLeft.getPosition(), + self.rearRight.getPosition(), + ), ) From 8e05dc485f554324e7b9af2c06560c60848108e9 Mon Sep 17 00:00:00 2001 From: kredcool Date: Thu, 11 Jan 2024 18:53:16 -0500 Subject: [PATCH 11/61] made init and periodic, need to replace DriveConstatns.kDriveKinematics --- rio/constants/robot_hardware.yaml | 6 ++++++ rio/subsystems/swerveModule.py | 7 +++++-- rio/subsystems/swervesubsystem.py | 31 ++++++++++++++++++++++++------- 3 files changed, 35 insertions(+), 9 deletions(-) diff --git a/rio/constants/robot_hardware.yaml b/rio/constants/robot_hardware.yaml index 3c02f439..1fde7b49 100644 --- a/rio/constants/robot_hardware.yaml +++ b/rio/constants/robot_hardware.yaml @@ -3,6 +3,8 @@ # Motor placement, max and min extensions # of arms, and similar should go here. +# TODO change all TODO values to be for our swerve + drivetrain: wheelRadius: 1.5 # Updated Never by Nobody # cycles: 2048, counts: 8192 per revolution @@ -10,6 +12,10 @@ drivetrain: # TODO fix these (there in amps) DriveMotorCurrentLimit: 50 TurnMotorCurrentLimit: 20 + # TODO change in % per seconds + MagnitudeSlewRate: 1.8 + # TODO change me in % per seconds + RotationalSlewRate: 2 FPMotors: drivePort: 1 # Updated Never by Nobody diff --git a/rio/subsystems/swerveModule.py b/rio/subsystems/swerveModule.py index e86492a9..b77da52a 100644 --- a/rio/subsystems/swerveModule.py +++ b/rio/subsystems/swerveModule.py @@ -1,9 +1,12 @@ +# vendor libs from rev import CANSparkMax, CANSparkMaxLowLevel, SparkMaxAbsoluteEncoder -from constants.constants import getConstants -from wpilib import DutyCycleEncoder + from wpimath.kinematics import SwerveModuleState, SwerveModulePosition from wpimath.geometry import Rotation2d +# constants +from constants.constants import getConstants + class SwerveModule: def __init__( diff --git a/rio/subsystems/swervesubsystem.py b/rio/subsystems/swervesubsystem.py index d96a1753..10d4cee4 100644 --- a/rio/subsystems/swervesubsystem.py +++ b/rio/subsystems/swervesubsystem.py @@ -1,11 +1,13 @@ # rename to drivesubsystem.py when completed -# replace IMU with our gyro from commands2 import Subsystem from constants.constants import getConstants from .swerveModule import SwerveModule from wpilib import Timer from wpimath.filter import SlewRateLimiter +from navx import AHRS +from wpimath.kinematics import SwerveDrive4Odometry +from wpimath.geometry import Rotation2d class drivesubsystem(Subsystem): @@ -45,7 +47,7 @@ def __init__(self): ) # The gyro sensor - self.gyro = ADIS16448_IMU() + self.gyro = AHRS.create_spi() # Slew rate filter variables for controlling lateral acceleration self.currentRotation = 0.0 @@ -58,14 +60,29 @@ def __init__(self): # timer self.prevTime = Timer.getFPGATimestamp() - # Odometry class for tracking robot pose + # for taking robit POS + # TODO start with this tomorrow self.odometry = SwerveDrive4Odometry( DriveConstants.kDriveKinematics, Rotation2d.fromDegrees(self.gyro.getAngle()), ( - self.frontLeft.getPosition(), - self.frontRight.getPosition(), - self.rearLeft.getPosition(), - self.rearRight.getPosition(), + self.fPModule.getPosition(), + self.fSModule.getPosition(), + self.aPModule.getPosition(), + self.aSModule.getPosition(), + ), + ) + + # TODO rest goes here + + def periodic(self) -> None: + # update the robot pos + self.odometry.update( + Rotation2d.fromDegrees(self.gyro.getPitch()), + ( + self.fPModule.getPosition(), + self.fSModule.getPosition(), + self.aPModule.getPosition(), + self.aSModule.getPosition(), ), ) From 63137913df1b22d10e0ff51bf911626f23fc7ec6 Mon Sep 17 00:00:00 2001 From: kredcool Date: Fri, 12 Jan 2024 19:01:15 -0500 Subject: [PATCH 12/61] some stuff is in the wrong places --- rio/__init__.py | 0 rio/constants.py | 44 ++++ rio/constants/mathConstant.py | 89 ++++++++ rio/constants/robot_controls.yaml | 37 +-- rio/constants/robot_hardware.yaml | 116 +++++----- rio/constants/robot_pid.yaml | 31 --- rio/mathConstant.py | 89 ++++++++ rio/robot.py | 79 ++----- rio/robot_controls.yaml | 7 + rio/robot_hardware.yaml | 89 ++++++++ rio/robot_pid.yaml | 0 rio/robotcontainer.py | 176 ++++++++------- rio/simgui-ds.json | 71 ++---- rio/simgui-window.json | 45 ++-- rio/simgui.json | 68 +----- rio/subsystems/drivesubsystem.py | 361 ++++++++++++++++++++---------- rio/subsystems/swerveModule.py | 189 ---------------- rio/subsystems/swervemodule.py | 148 ++++++++++++ rio/subsystems/swervesubsystem.py | 88 -------- rio/swerveutils.py | 91 ++++++++ 20 files changed, 1006 insertions(+), 812 deletions(-) create mode 100644 rio/__init__.py create mode 100644 rio/constants.py create mode 100644 rio/constants/mathConstant.py create mode 100644 rio/mathConstant.py mode change 100644 => 100755 rio/robot.py create mode 100644 rio/robot_controls.yaml create mode 100644 rio/robot_hardware.yaml create mode 100644 rio/robot_pid.yaml delete mode 100644 rio/subsystems/swerveModule.py create mode 100644 rio/subsystems/swervemodule.py delete mode 100644 rio/subsystems/swervesubsystem.py create mode 100644 rio/swerveutils.py diff --git a/rio/__init__.py b/rio/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/rio/constants.py b/rio/constants.py new file mode 100644 index 00000000..ffe95648 --- /dev/null +++ b/rio/constants.py @@ -0,0 +1,44 @@ +# Tidal Force Robotics +# 2022 + +import os +from sre_constants import CATEGORY_WORD +from wpilib import RobotBase +import yaml +import logging + +""" +Use this file only for storing non-changing constants. +""" + + +def load(fullPath: str): + # Try opening requested .yaml + with open(f"{fullPath}.yaml", "r") as yamlFile: + # Use yaml.safe_load to load the yaml into a dict + return yaml.safe_load(yamlFile) + + +def getConstants(identifier: str): + constants = {} + + # Clunky but it works + if RobotBase.isReal(): + path = "/home/lvuser/py/constants/" + else: + path = "constants/" + + try: + # Try opening requested .yaml + constants = load(f"{path}{identifier}") + except FileNotFoundError: + try: + # Try again but from one directory in (useful for unit testing) + constants = load(f"../{path}{identifier}") + except FileNotFoundError as e: + # If the file is not found, report it! + logging.error(f"{identifier} config not found!") + raise e + + # When all is done, return the important bits! + return constants diff --git a/rio/constants/mathConstant.py b/rio/constants/mathConstant.py new file mode 100644 index 00000000..bc0e1f8a --- /dev/null +++ b/rio/constants/mathConstant.py @@ -0,0 +1,89 @@ +""" +this is hear to make the files cleaner +and to do alot of needed math in its own file +""" + +# wpilib +from wpimath.geometry import Translation2d +from wpimath.kinematics import SwerveDrive4Kinematics +from wpimath.trajectory import TrapezoidProfileRadians + +# vendor libs +from rev import CANSparkMax + +# constants +from constants.constants import getConstants + +# misc +import math + + +class DriveConstants: + # constants + constants = getConstants("robot_hardware") + drivetrain = constants["driveTrain"] + + # allowed max speed + maxAngularSpeed = math.tau + + # Chassis config + trackWidth = drivetrain["TrackWidth"] + # Distance between centers of right and left wheels + wheelBase = drivetrain["WheelBase"] + + # Distance of front and back wheels + modulePositions = [ + Translation2d(wheelBase / 2, trackWidth / 2), + Translation2d(wheelBase / 2, -trackWidth / 2), + Translation2d(-wheelBase / 2, trackWidth / 2), + Translation2d(-wheelBase / 2, -trackWidth / 2), + ] + driveKinematics = SwerveDrive4Kinematics(*modulePositions) + + # Angular offsets of modules relative to chassis in radians + fPChassisAngularOffset = -math.pi / 2 + fSChassisAngularOffset = 0 + aPChassisAngularOffset = math.pi + aSChassisAngularOffset = math.pi / 2 + + +class ModuleConstants: + constants = getConstants("robot_hardware") + drivetrain = constants["driveTrain"] + NeoMotor = constants["NeoMotor"] + pid = constants["PID"] + + driveMotorPinionTeeth = drivetrain["driveMotorPinionTeeth"] + + # Calculations for drive motor conversion factors and feed forward + driveMotorFreeSpeedRps = NeoMotor["FreeSpeedRpm"] / 60 + WheelDiameterMeters = drivetrain["WheelDiameterMeters"] + WheelCircumferenceMeters = WheelDiameterMeters * math.pi + + driveMotorReduction = ( + drivetrain["BevelGearTeeth"] * drivetrain["firstStageSpurTeeth"] + ) / (driveMotorPinionTeeth * drivetrain["bevelPinion"]) + + driveEncoderPositionFactor = (WheelDiameterMeters * math.pi) / driveMotorReduction + driveEncoderVelocityFactor = ( + (WheelDiameterMeters * math.pi) / driveMotorReduction + ) / 60.0 + + turnEncoderPositionFactor = math.tau + turnEncoderVelocityFactor = math.tau / 60.0 + + turnEncoderPositionPIDMinInput = pid["turnEncoderPositionPIDMinInput"] # radian + turnEncoderPositionPIDMaxInput = turnEncoderPositionFactor + + driveMotorIdleMode = CANSparkMax.IdleMode.kBrake + turnMotorIdleMode = CANSparkMax.IdleMode.kBrake + + +class AutoConstants: + maxAngularSpeedRadiansPerSecond = math.pi + maxAngularSpeedRadiansPerSecondSquared = math.pi + + # Constraint for motion robot angle controller + thetaControllerConstraints = TrapezoidProfileRadians.Constraints( + maxAngularSpeedRadiansPerSecond, maxAngularSpeedRadiansPerSecondSquared + ) diff --git a/rio/constants/robot_controls.yaml b/rio/constants/robot_controls.yaml index 6296b5ef..dda89737 100644 --- a/rio/constants/robot_controls.yaml +++ b/rio/constants/robot_controls.yaml @@ -1,34 +1,7 @@ # This file defines the input -# and control scheme of the robot. -# this is orginized into control modes +# and control scheme of the robot, +# Arranged into operating "modes" -main mode: - driver: # using a stick controller - controller_port: 0 # What usb port the driver controller is on - - # Axis - ForwardAxis: 1 - SteerAxis: 2 - - # Buttons - HalfSpeedButton: 1 - DiffLock: 2 - Turn90: 3 - TurnAnti90: 4 - StartConfig: 5 - Slow: 10 - Crawl: 3 - - operator: # using an "x-box" controller - controller_port: 1 - - # Hold Buttons - Unclamp: 6 - Clamp: 5 - FindZero: 8 - hold: 3 - - # Presets/Modes - LowGoal: 1 - HighGoal: 4 - ManualMode: 7 \ No newline at end of file +driver: + ControllerPort: 0 + DeadZone: 0.05 \ No newline at end of file diff --git a/rio/constants/robot_hardware.yaml b/rio/constants/robot_hardware.yaml index 1fde7b49..a7698ae6 100644 --- a/rio/constants/robot_hardware.yaml +++ b/rio/constants/robot_hardware.yaml @@ -3,79 +3,87 @@ # Motor placement, max and min extensions # of arms, and similar should go here. -# TODO change all TODO values to be for our swerve +# rev's code will help with replacing values +# https://github.com/robotpy/robotpy-rev/tree/main/examples/maxswerve -drivetrain: - wheelRadius: 1.5 # Updated Never by Nobody - # cycles: 2048, counts: 8192 per revolution - encoderResolution: 2048 # Updated Never by Nobody - # TODO fix these (there in amps) - DriveMotorCurrentLimit: 50 - TurnMotorCurrentLimit: 20 - # TODO change in % per seconds +driveTrain: + MaxSpeedMeterPerSecond: 4.8 + + DirectionSlewRate: 1.2 MagnitudeSlewRate: 1.8 - # TODO change me in % per seconds - RotationalSlewRate: 2 + RotationalSlewRate: 2.0 + + TrackWidth: 0.6731 + WheelBase: 0.6731 + + # may need to be changed depending on if it spins the same way + turnEncoderInverted: True + GyroReversed: False + + ModuleConstants: + driveMotorPinionTeeth: 14 + BevelGearTeeth: 45 + firstStageSpurTeeth: 22 + bevelPinion: 15 + WheelDiameterMeters: 0.0762 + + # in amps + driveMotorCurrentLimit: 50 + turnMotorCurrentLimit: 20 + + autonomous: + maxSpeedMetersPerSecond: 3 + maxAccelerationMetersPerSecondSquared: 3 + + pXController: 1 + pYController: 1 + pThetaController: 1 + FPMotors: - drivePort: 1 # Updated Never by Nobody - turnPort: 2 # Updated Never by Nobody - EncoderPorts: [1, 2] # Updated Never by Nobody + driveID: 1 # Updated Never by Nobody + turnID: 2 # Updated Never by Nobody + EncoderIDs: [1, 2] # Updated Never by Nobody EncoderReversed: False # Updated Never by Nobody Inverted: False # Updated Never by Nobody - ChassisAngularOffset: -1.75 APMotors: - drivePort: 3 # Updated Never by Nobody - turnPort: 4 # Updated Never by Nobody - EncoderPorts: [3, 4] # Updated Never by Nobody + driveID: 3 # Updated Never by Nobody + turnID: 4 # Updated Never by Nobody + EncoderIDs: [3, 4] # Updated Never by Nobody EncoderReversed: False # Updated Never by Nobody Inverted: False # Updated Never by Nobody - ChassisAngularOffset: 3.14 FSMotors: - drivePort: 5 # Updated Never by Nobody - turnPort: 6 # Updated Never by Nobody - EncoderPorts: [5, 6] # Updated Never by Nobody + driveID: 5 # Updated Never by Nobody + turnID: 6 # Updated Never by Nobody + EncoderIDs: [5, 6] # Updated Never by Nobody EncoderReversed: False # Updated Never by Nobody Inverted: False # Updated Never by Nobody - ChassisAngularOffset: 0 ASMotors: - drivePort: 7 # Updated Never by Nobody - turnPort: 8 # Updated Never by Nobody - EncoderPorts: [7, 8] # Updated Never by Nobody + driveID: 7 # Updated Never by Nobody + turnID: 8 # Updated Never by Nobody + EncoderIDs: [7, 8] # Updated Never by Nobody EncoderReversed: False # Updated Never by Nobody Inverted: False # Updated Never by Nobody - ChassisAngularOffset: 1.75 - DrivePIDValues: - P: 0 # Updated Never by Nobody - I: 0 # Updated Never by Nobody - D: 0 # Updated Never by Nobody - FF: 0 # Updated Never by Nobody - MinOutput: -1 - MaxOutput: 1 +PID: + turnEncoderPositionPIDMinInput: 0 - TurnPIDValues: - P: 0 # Updated Never by Nobody - I: 0 # Updated Never by Nobody - D: 0 # Updated Never by Nobody - FF: 0 # Updated Never by Nobody - MinOutput: -1 - MaxOutput: 1 + driveP: 0.04 + driveI: 0 + driveD: 0 + driveFF: 1 + driveMinOutput: -1 + driveMaxOutput: 1 - Conversions: - # see rev's maxswervemodule code for how numbers were - # obtained https://github.com/robotpy/robotpy-rev/tree/main/examples/maxswerve - Drive: - MotorPositionFactor: 289.070303031 - EncoderVelocityFactor: 17344.2181818 - + turnP: 1 + turnI: 0 + turnD: 0 + turnFF: 0 + turnMinOutput: -1 + turnMaxOutput: 1 - Turn: - EncoderPositionFactor: 6.28 - EncoderVelocityFactor: 0.104666666667 - EncoderPositionPIDMinInput: 0 - EncoderPositionPIDMaxInput: 6.28 - \ No newline at end of file +NeoMotor: + FreeSpeedRpm: 5676 diff --git a/rio/constants/robot_pid.yaml b/rio/constants/robot_pid.yaml index 1eabd5cf..e69de29b 100644 --- a/rio/constants/robot_pid.yaml +++ b/rio/constants/robot_pid.yaml @@ -1,31 +0,0 @@ -# Constants for the robot PID - -drive: - # Diff lock - kStabilizationP: 0.011 - kStabilizationI: 0.0001 - kStabilizationD: 0 - - # TurnToAngle - kTurnP: 0.007 - kTurnI: 0.006 - kTurnD: 0.001 - - kMaxTurnRateDegPerS: 100 - kMaxTurnAccelerationDegPerSSquared: 300 - - kTurnToleranceDeg: 5 - kTurnRateToleranceDegPerS: 10 # degrees per second - -arm: - elevator: - kp: 0.075 - ki: 0 - kd: 0.1 - ff: 0 - - ladder: - kp: 0.1 - ki: 0 - kd: 0.1 - ff: 0 \ No newline at end of file diff --git a/rio/mathConstant.py b/rio/mathConstant.py new file mode 100644 index 00000000..bc0e1f8a --- /dev/null +++ b/rio/mathConstant.py @@ -0,0 +1,89 @@ +""" +this is hear to make the files cleaner +and to do alot of needed math in its own file +""" + +# wpilib +from wpimath.geometry import Translation2d +from wpimath.kinematics import SwerveDrive4Kinematics +from wpimath.trajectory import TrapezoidProfileRadians + +# vendor libs +from rev import CANSparkMax + +# constants +from constants.constants import getConstants + +# misc +import math + + +class DriveConstants: + # constants + constants = getConstants("robot_hardware") + drivetrain = constants["driveTrain"] + + # allowed max speed + maxAngularSpeed = math.tau + + # Chassis config + trackWidth = drivetrain["TrackWidth"] + # Distance between centers of right and left wheels + wheelBase = drivetrain["WheelBase"] + + # Distance of front and back wheels + modulePositions = [ + Translation2d(wheelBase / 2, trackWidth / 2), + Translation2d(wheelBase / 2, -trackWidth / 2), + Translation2d(-wheelBase / 2, trackWidth / 2), + Translation2d(-wheelBase / 2, -trackWidth / 2), + ] + driveKinematics = SwerveDrive4Kinematics(*modulePositions) + + # Angular offsets of modules relative to chassis in radians + fPChassisAngularOffset = -math.pi / 2 + fSChassisAngularOffset = 0 + aPChassisAngularOffset = math.pi + aSChassisAngularOffset = math.pi / 2 + + +class ModuleConstants: + constants = getConstants("robot_hardware") + drivetrain = constants["driveTrain"] + NeoMotor = constants["NeoMotor"] + pid = constants["PID"] + + driveMotorPinionTeeth = drivetrain["driveMotorPinionTeeth"] + + # Calculations for drive motor conversion factors and feed forward + driveMotorFreeSpeedRps = NeoMotor["FreeSpeedRpm"] / 60 + WheelDiameterMeters = drivetrain["WheelDiameterMeters"] + WheelCircumferenceMeters = WheelDiameterMeters * math.pi + + driveMotorReduction = ( + drivetrain["BevelGearTeeth"] * drivetrain["firstStageSpurTeeth"] + ) / (driveMotorPinionTeeth * drivetrain["bevelPinion"]) + + driveEncoderPositionFactor = (WheelDiameterMeters * math.pi) / driveMotorReduction + driveEncoderVelocityFactor = ( + (WheelDiameterMeters * math.pi) / driveMotorReduction + ) / 60.0 + + turnEncoderPositionFactor = math.tau + turnEncoderVelocityFactor = math.tau / 60.0 + + turnEncoderPositionPIDMinInput = pid["turnEncoderPositionPIDMinInput"] # radian + turnEncoderPositionPIDMaxInput = turnEncoderPositionFactor + + driveMotorIdleMode = CANSparkMax.IdleMode.kBrake + turnMotorIdleMode = CANSparkMax.IdleMode.kBrake + + +class AutoConstants: + maxAngularSpeedRadiansPerSecond = math.pi + maxAngularSpeedRadiansPerSecondSquared = math.pi + + # Constraint for motion robot angle controller + thetaControllerConstraints = TrapezoidProfileRadians.Constraints( + maxAngularSpeedRadiansPerSecond, maxAngularSpeedRadiansPerSecondSquared + ) diff --git a/rio/robot.py b/rio/robot.py old mode 100644 new mode 100755 index 132a3cbe..1bf9b908 --- a/rio/robot.py +++ b/rio/robot.py @@ -1,78 +1,29 @@ -#!/usr/bin/env python3 -# -# 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. -# - -import typing -import logging - -import wpilib +# vendor libs import commands2 -import commands2.cmd - -import robotcontainer +import wpilib -""" -The VM is configured to automatically run this class, and to call the functions corresponding to -each mode, as described in the TimedRobot documentation. If you change the name of this class or -the package after creating this project, you must also update the build.gradle file in the -project. -""" +# robot container +from robotcontainer import RobotContainer class UnnamedToaster(commands2.TimedCommandRobot): - """ - Our default robot class, pass it to wpilib.run - Command v2 robots are encouraged to inherit from TimedCommandRobot, which - has an implementation of robotPeriodic which runs the scheduler for you - """ - - def robotInit(self) -> None: - """ - This function is run when the robot is first started up and should be used for any - initialization code. - """ - - wpilib.CameraServer.launch() - - self.autonomousCommand: typing.Optional[commands2.Command] = None - - # Instantiate our RobotContainer. This will perform all our button bindings, and put our - # autonomous chooser on the dashboard. - self.container = robotcontainer.RobotContainer() - - def teleopInit(self) -> None: - # 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 - # this line or comment it out. - if self.autonomousCommand is not None: - self.autonomousCommand.cancel() - - def disabledInit(self) -> None: - """This function is called once each time the robot enters Disabled mode.""" - - def disabledPeriodic(self) -> None: - """This function is called periodically when disabled""" + def robotInit(self): + self.container = RobotContainer() + self.autonomousCommand = None def autonomousInit(self) -> None: - """This autonomous runs the autonomous command selected by your RobotContainer class.""" self.autonomousCommand = self.container.getAutonomousCommand() - # schedule the autonomous command (example) - if self.autonomousCommand is not None: + if self.autonomousCommand: self.autonomousCommand.schedule() - else: - logging.warning("no auto command?") - - def autonomousPeriodic(self) -> None: - """This function is called periodically during autonomous""" - def teleopPeriodic(self) -> None: - """This function is called periodically during operator control""" + def teleopInit(self) -> None: + if self.autonomousCommand: + self.autonomousCommand.cancel() def testInit(self) -> None: - # Cancels all running commands at the start of test mode commands2.CommandScheduler.getInstance().cancelAll() + + +if __name__ == "__main__": + wpilib.run(UnnamedToaster) diff --git a/rio/robot_controls.yaml b/rio/robot_controls.yaml new file mode 100644 index 00000000..dda89737 --- /dev/null +++ b/rio/robot_controls.yaml @@ -0,0 +1,7 @@ +# This file defines the input +# and control scheme of the robot, +# Arranged into operating "modes" + +driver: + ControllerPort: 0 + DeadZone: 0.05 \ No newline at end of file diff --git a/rio/robot_hardware.yaml b/rio/robot_hardware.yaml new file mode 100644 index 00000000..a7698ae6 --- /dev/null +++ b/rio/robot_hardware.yaml @@ -0,0 +1,89 @@ +# This file defines the robot's +# physical dimensions. Things like +# Motor placement, max and min extensions +# of arms, and similar should go here. + +# rev's code will help with replacing values +# https://github.com/robotpy/robotpy-rev/tree/main/examples/maxswerve + +driveTrain: + MaxSpeedMeterPerSecond: 4.8 + + DirectionSlewRate: 1.2 + MagnitudeSlewRate: 1.8 + RotationalSlewRate: 2.0 + + TrackWidth: 0.6731 + WheelBase: 0.6731 + + # may need to be changed depending on if it spins the same way + turnEncoderInverted: True + GyroReversed: False + + ModuleConstants: + driveMotorPinionTeeth: 14 + BevelGearTeeth: 45 + firstStageSpurTeeth: 22 + bevelPinion: 15 + + WheelDiameterMeters: 0.0762 + + # in amps + driveMotorCurrentLimit: 50 + turnMotorCurrentLimit: 20 + + autonomous: + maxSpeedMetersPerSecond: 3 + maxAccelerationMetersPerSecondSquared: 3 + + pXController: 1 + pYController: 1 + pThetaController: 1 + + FPMotors: + driveID: 1 # Updated Never by Nobody + turnID: 2 # Updated Never by Nobody + EncoderIDs: [1, 2] # Updated Never by Nobody + EncoderReversed: False # Updated Never by Nobody + Inverted: False # Updated Never by Nobody + + APMotors: + driveID: 3 # Updated Never by Nobody + turnID: 4 # Updated Never by Nobody + EncoderIDs: [3, 4] # Updated Never by Nobody + EncoderReversed: False # Updated Never by Nobody + Inverted: False # Updated Never by Nobody + + FSMotors: + driveID: 5 # Updated Never by Nobody + turnID: 6 # Updated Never by Nobody + EncoderIDs: [5, 6] # Updated Never by Nobody + EncoderReversed: False # Updated Never by Nobody + Inverted: False # Updated Never by Nobody + + ASMotors: + driveID: 7 # Updated Never by Nobody + turnID: 8 # Updated Never by Nobody + EncoderIDs: [7, 8] # Updated Never by Nobody + EncoderReversed: False # Updated Never by Nobody + Inverted: False # Updated Never by Nobody + +PID: + turnEncoderPositionPIDMinInput: 0 + + driveP: 0.04 + driveI: 0 + driveD: 0 + driveFF: 1 + driveMinOutput: -1 + driveMaxOutput: 1 + + turnP: 1 + turnI: 0 + turnD: 0 + turnFF: 0 + turnMinOutput: -1 + turnMaxOutput: 1 + +NeoMotor: + FreeSpeedRpm: 5676 diff --git a/rio/robot_pid.yaml b/rio/robot_pid.yaml new file mode 100644 index 00000000..e69de29b diff --git a/rio/robotcontainer.py b/rio/robotcontainer.py index c0d5d4ac..58b9b1c2 100644 --- a/rio/robotcontainer.py +++ b/rio/robotcontainer.py @@ -1,108 +1,126 @@ -import wpilib - +# commands import commands2 -import commands2.cmd -import commands2.button -# Constants -from constants.constants import getConstants +from commands2 import cmd -# Subsystems -from subsystems.drivesubsystem import DriveSubsystem +# wpilib +import wpimath +import wpilib -# Commands -from commands.FlyByWire import FlyByWire +from wpimath.controller import PIDController, ProfiledPIDControllerRadians +from wpimath.geometry import Pose2d, Rotation2d, Translation2d +from wpimath.trajectory import TrajectoryConfig, TrajectoryGenerator -# Autonomous +# subsystems +from subsystems.drivesubsystem import DriveSubsystem -# NetworkTables -from ntcore import NetworkTableInstance +# constants +from constants.mathConstant import AutoConstants, DriveConstants +from constants.constants import getConstants -# Misc -from extras.deployData import getDeployData +# misc +import math class RobotContainer: - """ - This class is where the bulk of the robot should be declared. Since Command-based is a - "declarative" paradigm, very little robot logic should actually be handled in the :class:`.Robot` - periodic methods (other than the scheduler calls). Instead, the structure of the robot (including - subsystems, commands, and button mappings) should be declared here. - - """ - - def __init__(self): - """The container for the robot. Contains subsystems, OI devices, and commands.""" - # Configure networktables - self.configureNetworktables() - - # Setup constants - self.controlConsts = getConstants("robot_controls") - self.hardConsts = getConstants("robot_hardware") - self.pidConsts = getConstants("robot_pid") - self.driverConsts = self.controlConsts["main mode"]["driver"] - self.operatorConsts = self.controlConsts["main mode"]["operator"] + def __init__(self) -> None: + # constants + # drivetrain + hardwareConstants = getConstants("robot_hardware") + self.driveConst = hardwareConstants["driveTrain"] + self.autoConst = self.driveConst["autonomous"] + + # controller + controllerConst = getConstants("robot_controls") + self.driverConst = controllerConst["driver"] # The robot's subsystems self.robotDrive = DriveSubsystem() - # The driver's controller - self.driverController = commands2.button.CommandJoystick( - self.driverConsts["controller_port"] - ) - # The operators controller - self.operatorController = commands2.button.CommandJoystick( - self.operatorConsts["controller_port"] + # The driver's controller + self.driverController = wpilib.XboxController( + self.driverConst["ControllerPort"] ) - # Configure the button bindings + # Config buttons self.configureButtonBindings() - # Setup all autonomous routines - self.configureAutonomous() - - # Default drive command + # default command self.robotDrive.setDefaultCommand( - FlyByWire( - self.robotDrive, - lambda: -self.driverController.getRawAxis( - self.driverConsts["ForwardAxis"], - ), - lambda: self.driverController.getRawAxis( - self.driverConsts["SteerAxis"], + commands2.RunCommand( + lambda: self.robotDrive.drive( + -wpimath.applyDeadband( + self.driverController.getLeftY(), self.driverConst["DeadZone"] + ), + -wpimath.applyDeadband( + self.driverController.getLeftX(), self.driverConst["DeadZone"] + ), + -wpimath.applyDeadband( + self.driverController.getRightX(), self.driverConst["DeadZone"] + ), + True, + True, ), + self.robotDrive, ) ) - def configureButtonBindings(self): - """ - Use this method to define your button->command mappings. Buttons can be created via the button - factories on commands2.button.CommandGenericHID or one of its - subclasses (commands2.button.CommandJoystick or command2.button.CommandXboxController). - """ + def configureButtonBindings(self) -> None: pass - def configureAutonomous(self): - pass + def disablePIDSubsystems(self) -> None: + """ + 'Disables all ProfiledPIDSubsystem and PIDSubsystem instances. + This should be called on robot disable to prevent integral windup.' + - rev + """ - def configureNetworktables(self): - # Configure networktables - self.nt = NetworkTableInstance.getDefault() - self.sd = self.nt.getTable("SmartDashboard") + def getAutonomousCommand(self) -> commands2.Command: + # Create config for trajectory + config = TrajectoryConfig( + self.autoConst["maxSpeedMetersPerSecond"], + self.autoConst["maxSpeedMetersPerSecondSquared"], + ) + # ensure max speed is obeyed + config.setKinematics(DriveConstants.driveKinematics) + + # An example trajectory to follow. All units in meters. + exampleTrajectory = TrajectoryGenerator.generateTrajectory( + # Start at the origin facing the +X direction + Pose2d(0, 0, Rotation2d(0)), + # Pass through these two interior waypoints, making an 's' curve path + [Translation2d(1, 1), Translation2d(2, -1)], + # End 3 meters straight ahead of where we started, facing forward + Pose2d(3, 0, Rotation2d(0)), + config, + ) - # Subtables - self.build_table = self.sd.getSubTable("BuildData") + thetaController = ProfiledPIDControllerRadians( + self.autoConst["pThetaController"], + 0, + 0, + AutoConstants.thetaControllerConstraints, + ) + thetaController.enableContinuousInput(-math.pi, math.pi) + + swerveControllerCommand = commands2.SwerveControllerCommand( + exampleTrajectory, + self.robotDrive.getPose(), + DriveConstants.driveKinematics, + PIDController(self.autoConst["pXController"], 0, 0), + PIDController(self.autoConst["pYController"], 0, 0), + thetaController, + self.robotDrive.setModuleStates(), + (self.robotDrive,), + ) - # Build data (May need to be moved to a dedicated function to be updated more than once) - data = getDeployData() - for key in data: - key_entry = self.build_table.getEntry(str(key)) - key_entry.setString(str(data[key])) + # Reset odometry to starting pose + self.robotDrive.resetOdometry(exampleTrajectory.initialPose()) - def getAutonomousCommand(self) -> commands2.Command: - """ - Use this to pass the autonomous command to the main :class:`.Robot` class. - :returns: the command to run in autonomous - """ - # return self.autoChooser.getSelected() - return None + # Run path following command, then stop at the end + return swerveControllerCommand.andThen( + cmd.run( + lambda: self.robotDrive.drive(0, 0, 0, False, False), + self.robotDrive, + ) + ) diff --git a/rio/simgui-ds.json b/rio/simgui-ds.json index c57e37c3..73cc713c 100644 --- a/rio/simgui-ds.json +++ b/rio/simgui-ds.json @@ -1,39 +1,28 @@ { - "System Joysticks": { - "window": { - "visible": false - } - }, "keyboardJoysticks": [ { "axisConfig": [ { - "decKey": 69, - "decayRate": 0.0, - "incKey": 82, - "keyRate": 0.009999999776482582 + "decKey": 65, + "incKey": 68 }, { "decKey": 87, "incKey": 83 }, { - "decKey": 65, - "incKey": 68 + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 } ], "axisCount": 3, - "buttonCount": 10, + "buttonCount": 4, "buttonKeys": [ 90, 88, 67, - 86, - 86, - 86, - 86, - 86, - 86, 86 ], "povConfig": [ @@ -53,43 +42,21 @@ { "axisConfig": [ { - "decKey": 263, - "incKey": 262 - }, - { - "decKey": 265, - "incKey": 264 + "decKey": 74, + "incKey": 76 }, { - "decKey": 265, - "incKey": 264 - }, - { - "decKey": 265, - "incKey": 264 - }, - { - "decKey": 265, - "incKey": 264 - }, - { - "decKey": 265, - "incKey": 264 + "decKey": 73, + "incKey": 75 } ], - "axisCount": 6, - "buttonCount": 10, + "axisCount": 2, + "buttonCount": 4, "buttonKeys": [ 77, 44, 46, - 47, - 86, - 86, - 86, - 86, - 86, - 86 + 47 ], "povCount": 0 }, @@ -121,15 +88,5 @@ "buttonCount": 0, "povCount": 0 } - ], - "robotJoysticks": [ - { - "guid": "Keyboard0", - "name": "Driver Joystick" - }, - { - "guid": "Keyboard1", - "name": "Operator Stick" - } ] } diff --git a/rio/simgui-window.json b/rio/simgui-window.json index a5c7242c..d76a5956 100644 --- a/rio/simgui-window.json +++ b/rio/simgui-window.json @@ -5,47 +5,30 @@ "MainWindow": { "GLOBAL": { "fps": "120", - "height": "757", + "height": "720", "maximized": "0", "style": "0", "userScale": "2", - "width": "1451", - "xpos": "170", - "ypos": "163" - } - }, - "Table": { - "0x542B5671,2": { - "Column 0 Width": "262", - "Column 1 Width": "350", - "RefScale": "13" + "width": "1280", + "xpos": "636", + "ypos": "329" } }, "Window": { - "###/SmartDashboard/Autonomous": { - "Collapsed": "0", - "Pos": "188,23", - "Size": "254,54" - }, - "###/SmartDashboard/Field": { - "Collapsed": "0", - "Pos": "4,301", - "Size": "438,242" - }, "###FMS": { "Collapsed": "0", - "Pos": "3,608", + "Pos": "5,540", "Size": "283,146" }, "###Joysticks": { "Collapsed": "0", - "Pos": "402,553", - "Size": "796,200" + "Pos": "250,465", + "Size": "796,73" }, "###NetworkTables": { "Collapsed": "0", - "Pos": "444,23", - "Size": "750,529" + "Pos": "250,277", + "Size": "750,185" }, "###NetworkTables Info": { "Collapsed": "0", @@ -54,18 +37,18 @@ }, "###Other Devices": { "Collapsed": "0", - "Pos": "1199,21", - "Size": "250,734" + "Pos": "1025,20", + "Size": "250,695" }, "###System Joysticks": { "Collapsed": "0", - "Pos": "3,383", + "Pos": "5,350", "Size": "192,218" }, "###Timing": { "Collapsed": "0", - "Pos": "4,123", - "Size": "135,173" + "Pos": "5,150", + "Size": "135,150" }, "Debug##Default": { "Collapsed": "0", diff --git a/rio/simgui.json b/rio/simgui.json index 6c3c86fa..5f9d2754 100644 --- a/rio/simgui.json +++ b/rio/simgui.json @@ -1,73 +1,7 @@ { - "HALProvider": { - "Other Devices": { - "SPARK MAX [1]": { - "header": { - "open": true - } - }, - "SPARK MAX [2]": { - "header": { - "open": true - } - }, - "SPARK MAX [3]": { - "header": { - "open": true - } - }, - "navX-Sensor[4]": { - "header": { - "open": true - } - } - } - }, "NTProvider": { "types": { - "/FMSInfo": "FMSInfo", - "/SmartDashboard/Autonomous": "String Chooser", - "/SmartDashboard/Field": "Field2d" - }, - "windows": { - "/SmartDashboard/Autonomous": { - "window": { - "visible": true - } - }, - "/SmartDashboard/Field": { - "window": { - "visible": true - } - } - } - }, - "NetworkTables": { - "Retained Values": { - "open": false - }, - "transitory": { - "SmartDashboard": { - "Autonomous": { - "open": true - }, - "BuildData": { - "open": true - }, - "Drivetrain": { - "open": true - }, - "Field": { - "open": true - }, - "Pose": { - "open": true - }, - "Test": { - "open": true - }, - "open": true - } + "/FMSInfo": "FMSInfo" } }, "NetworkTables Info": { diff --git a/rio/subsystems/drivesubsystem.py b/rio/subsystems/drivesubsystem.py index d516114a..4bd5c36f 100644 --- a/rio/subsystems/drivesubsystem.py +++ b/rio/subsystems/drivesubsystem.py @@ -1,161 +1,282 @@ -# Robot +# wpilib import wpilib -import wpilib.drive -import commands2 -from ntcore import NetworkTableInstance -from wpimath.geometry import Pose2d -from wpimath.kinematics import DifferentialDriveOdometry -from wpilib import Field2d +from wpimath.filter import SlewRateLimiter +from wpimath.geometry import Pose2d, Rotation2d +from wpimath.kinematics import ( + ChassisSpeeds, + SwerveModuleState, + SwerveDrive4Kinematics, + SwerveDrive4Odometry, +) -# Constants +# swerve stuff +import swerveutils +from .swervemodule import SwerveModule + +# constants from constants.constants import getConstants +from constants.mathConstant import DriveConstants + +# misc +import math -# Vendor Libs -from rev import CANSparkMax, CANSparkMaxLowLevel -from navx import AHRS +from commands2 import Subsystem +from typing import Tuple -class DriveSubsystem(commands2.Subsystem): +class DriveSubsystem(Subsystem): def __init__(self) -> None: super().__init__() - # Configure networktables - self.nt = NetworkTableInstance.getDefault() - self.sd = self.nt.getTable("SmartDashboard") + # constants + constants = getConstants("robot_hardware") + self.driveConsts = constants["driveTrain"] + self.autoConsts = self.driveConsts["autonomous"] - # network tables - self.nt = NetworkTableInstance.getDefault() - self.sd = self.nt.getTable("SmartDashboard") + # module constants + self.fPConsts = self.driveConsts["FPMotors"] + self.fSConsts = self.driveConsts["FSMotors"] + self.aPConsts = self.driveConsts["APMotors"] + self.aSConsts = self.driveConsts["ASMotors"] - # Hardware consts - constants = getConstants("robot_hardware") - self.driveConst = constants["drivetrain"] # All the drivetrain consts - self.leftCosnt = self.driveConst["leftMotor"] # Left specific - self.rightCosnt = self.driveConst["rightMotor"] # Right specific - - # The motors on the left side of the drive. - self.leftMotor1 = CANSparkMax( - self.leftCosnt["Motor1Port"], - CANSparkMaxLowLevel.MotorType.kBrushless, - ) - self.leftMotor2 = CANSparkMax( - self.leftCosnt["Motor2Port"], - CANSparkMaxLowLevel.MotorType.kBrushless, + # Create SwerveModules + self.fPModule = SwerveModule( + self.fPConsts["driveID"], + self.fPConsts["turnID"], + DriveConstants.fPChassisAngularOffset, ) - # Combine left motors into one group - self.leftMotors = wpilib.MotorControllerGroup( - self.leftMotor1, - self.leftMotor2, + self.fSModule = SwerveModule( + self.fSConsts["driveID"], + self.fSConsts["turnID"], + DriveConstants.fSChassisAngularOffset, ) - self.leftMotors.setInverted(self.leftCosnt["Inverted"]) - # The motors on the right side of the drive. - self.rightMotor1 = CANSparkMax( - self.rightCosnt["Motor1Port"], - CANSparkMaxLowLevel.MotorType.kBrushless, - ) - self.rightMotor2 = CANSparkMax( - self.rightCosnt["Motor2Port"], - CANSparkMaxLowLevel.MotorType.kBrushless, + self.aPModule = SwerveModule( + self.aPConsts["driveID"], + self.aPConsts["turnID"], + DriveConstants.aPChassisAngularOffset, ) - # Combine left motors into one group - self.rightMotors = wpilib.MotorControllerGroup( - self.rightMotor1, - self.rightMotor2, + self.aSModule = SwerveModule( + self.aSConsts["driveID"], + self.aSConsts["turnID"], + DriveConstants.aSChassisAngularOffset, ) - self.rightMotors.setInverted(self.rightCosnt["Inverted"]) - # self.rightMotors.setInverted(False) - - # This fixes a bug in rev firmware involving flash settings. - self.leftMotor1.setInverted(False) - self.leftMotor2.setInverted(False) - self.rightMotor1.setInverted(False) - self.rightMotor2.setInverted(False) - # The robot's drivetrain kinematics - self.drive = wpilib.drive.DifferentialDrive(self.leftMotors, self.rightMotors) + # The gyro sensor + self.gyro = wpilib.ADIS16448_IMU() - # The left-side drive encoder - self.leftEncoder = self.leftMotor1.getEncoder() + # Slew rate filter variables for controlling lateral acceleration + self.currentRotation = 0.0 + self.currentTranslationDir = 0.0 + self.currentTranslationMag = 0.0 - # The right-side drive encoder - self.rightEncoder = self.rightMotor2.getEncoder() + self.magLimiter = SlewRateLimiter(self.driveConsts["MagnitudeSlewRate"]) + self.rotLimiter = SlewRateLimiter(self.driveConsts["RotationalSlewRate"]) + self.prevTime = wpilib.Timer.getFPGATimestamp() - # PID Controllers - self.lPID = self.leftMotor1.getPIDController() - self.rPID = self.rightMotor1.getPIDController() - - # Setup the conversion factors for the motor controllers - # TODO: Because rev is rev, there are a lot of problems that need to be addressed. - # https://www.chiefdelphi.com/t/spark-max-encoder-setpositionconversionfactor-not-doing-anything/396629 - self.leftEncoder.setPositionConversionFactor( - 1 / self.driveConst["encoderConversionFactor"] + # Odometry class for tracking robot pose + self.odometry = SwerveDrive4Odometry( + DriveConstants.driveKinematics, + Rotation2d.fromDegrees(self.gyro.getAngle()), + ( + self.fPModule.getPosition(), + self.fSModule.getPosition(), + self.aPModule.getPosition(), + self.aSModule.getPosition(), + ), ) - self.rightEncoder.setPositionConversionFactor( - 1 / self.driveConst["encoderConversionFactor"] + + def periodic(self) -> None: + # Update the odometry in the periodic block + self.odometry.update( + Rotation2d.fromDegrees(self.gyro.getAngle()), + ( + self.fPModule.getPosition(), + self.fSModule.getPosition(), + self.aPModule.getPosition(), + self.aSModule.getPosition(), + ), ) - # Gyro - self.ahrs = AHRS.create_spi() # creates navx object + def getPose(self) -> Pose2d: + """Returns the currently-estimated pose of the robot. - # Robot odometry - self.field = Field2d() - self.odometry = DifferentialDriveOdometry( - self.ahrs.getRotation2d(), - self.leftEncoder.getPosition(), - self.rightEncoder.getPosition(), - ) + :returns: The pose. + """ + return self.odometry.getPose() + + def resetOdometry(self, pose: Pose2d) -> None: + """Resets the odometry to the specified pose. - # Enable braking - self.rightMotor1.setIdleMode(CANSparkMax.IdleMode.kBrake) - self.rightMotor2.setIdleMode(CANSparkMax.IdleMode.kBrake) - self.leftMotor1.setIdleMode(CANSparkMax.IdleMode.kBrake) - self.leftMotor2.setIdleMode(CANSparkMax.IdleMode.kBrake) + :param pose: The pose to which to set the odometry. - def arcadeDrive(self, fwd: float, rot: float): """ - Drives the robot using arcade controls. + self.odometry.resetPosition( + Rotation2d.fromDegrees(self.gyro.getAngle()), + ( + self.fPModule.getPosition(), + self.fSModule.getPosition(), + self.aPModule.getPosition(), + self.aSModule.getPosition(), + ), + pose, + ) - :param fwd: the commanded forward movement - :param rot: the commanded rotation + def drive( + self, + xSpeed: float, + ySpeed: float, + rot: float, + fieldRelative: bool, + rateLimit: bool, + ) -> None: + """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 rot: Angular rate of the robot. + :param fieldRelative: Whether the provided x and y speeds are relative to the + field. + :param rateLimit: Whether to enable rate limiting for smoother control. """ - self.drive.arcadeDrive(fwd, rot) - def tankDriveVolts(self, leftVolts, rightVolts): - """Control the robot's drivetrain with voltage inputs for each side.""" - # Set the voltage of the left side. - # inverting this delays the KP issue but doesn't fix it - self.leftMotors.setVoltage(leftVolts) + xSpeedCommanded = xSpeed + ySpeedCommanded = ySpeed - # Set the voltage of the right side. - self.rightMotors.setVoltage(rightVolts) + if rateLimit: + # Convert XY to polar for rate limiting + inputTranslationDir = math.atan2(ySpeed, xSpeed) + inputTranslationMag = math.hypot(xSpeed, ySpeed) - # print(f"({leftVolts}, {rightVolts})") + # Calculate the direction slew rate based on an estimate of the lateral acceleration + if self.currentTranslationMag != 0.0: + directionSlewRate = abs( + self.driveConsts["DirectionSlewRate"] / self.currentTranslationMag + ) + else: + directionSlewRate = 500.0 + # some high number that means the slew rate is effectively instantaneous - # Resets the timer for this motor's MotorSafety - self.drive.feed() + currentTime = wpilib.Timer.getFPGATimestamp() + elapsedTime = currentTime - self.prevTime + angleDif = swerveutils.angleDifference( + inputTranslationDir, self.currentTranslationDir + ) + if angleDif < 0.45 * math.pi: + self.currentTranslationDir = swerveutils.stepTowardsCircular( + self.currentTranslationDir, + inputTranslationDir, + directionSlewRate * elapsedTime, + ) + self.currentTranslationMag = self.magLimiter.calculate( + inputTranslationMag + ) - # Reset the encoders - self.resetEncoders() + elif angleDif > 0.85 * math.pi: + # some small number to avoid floating-point errors with equality checking + # keep currentTranslationDir unchanged + if self.currentTranslationMag > 1e-4: + self.currentTranslationMag = self.magLimiter.calculate(0.0) + else: + self.currentTranslationDir = swerveutils.wrapAngle( + self.currentTranslationDir + math.pi + ) + self.currentTranslationMag = self.magLimiter.calculate( + inputTranslationMag + ) - def setMaxOutput(self, maxOutput: float): - """ - Sets the max output of the drive. Useful for scaling the drive to drive more slowly. - :param maxOutput: the maximum output to which the drive will be constrained + else: + self.currentTranslationDir = swerveutils.stepTowardsCircular( + self.currentTranslationDir, + inputTranslationDir, + directionSlewRate * elapsedTime, + ) + self.currentTranslationMag = self.magLimiter.calculate(0.0) + + self.prevTime = currentTime + + xSpeedCommanded = self.currentTranslationMag * math.cos( + self.currentTranslationDir + ) + ySpeedCommanded = self.currentTranslationMag * math.sin( + self.currentTranslationDir + ) + self.currentRotation = self.rotLimiter.calculate(rot) + + else: + self.currentRotation = rot + + # Convert the commanded speeds into the correct units for the drivetrain + xSpeedDelivered = xSpeedCommanded * self.autoConsts["maxSpeedMetersPerSecond"] + ySpeedDelivered = ySpeedCommanded * self.autoConsts["maxSpeedMetersPerSecond"] + rotDelivered = self.currentRotation * DriveConstants.maxAngularSpeed + + swerveModuleStates = DriveConstants.driveKinematics.toSwerveModuleStates( + ChassisSpeeds.fromFieldRelativeSpeeds( + xSpeedDelivered, + ySpeedDelivered, + rotDelivered, + Rotation2d.fromDegrees(self.gyro.getAngle()), + ) + if fieldRelative + else ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered) + ) + fl, fr, rl, rr = SwerveDrive4Kinematics.desaturateWheelSpeeds( + swerveModuleStates, self.autoConsts["maxSpeedMetersPerSecond"] + ) + self.fPModule.setDesiredState(fl) + self.fSModule.setDesiredState(fr) + self.aPModule.setDesiredState(rl) + self.aSModule.setDesiredState(rr) + + def setX(self) -> None: + """Sets the wheels into an X formation to prevent movement.""" + self.fPModule.setDesiredState(SwerveModuleState(0, Rotation2d.fromDegrees(45))) + self.fSModule.setDesiredState(SwerveModuleState(0, Rotation2d.fromDegrees(-45))) + self.aPModule.setDesiredState(SwerveModuleState(0, Rotation2d.fromDegrees(-45))) + self.aSModule.setDesiredState(SwerveModuleState(0, Rotation2d.fromDegrees(45))) + + def setModuleStates( + self, + desiredStates: Tuple[ + SwerveModuleState, SwerveModuleState, SwerveModuleState, SwerveModuleState + ], + ) -> None: + """Sets the swerve ModuleStates. + + :param desiredStates: The desired SwerveModule states. """ - self.drive.setMaxOutput(maxOutput) + fl, fr, rl, rr = SwerveDrive4Kinematics.desaturateWheelSpeeds( + desiredStates, self.autoConsts["maxSpeedMetersPerSecond"] + ) + self.fPModule.setDesiredState(fl) + self.fSModule.setDesiredState(fr) + self.aPModule.setDesiredState(rl) + self.aSModule.setDesiredState(rr) - def resetEncoders(self): + def resetEncoders(self) -> None: """Resets the drive encoders to currently read a position of 0.""" - self.leftEncoder.setPosition(0) - self.rightEncoder.setPosition(0) + self.fPModule.resetEncoders() + self.aPModule.resetEncoders() + self.fSModule.resetEncoders() + self.aSModule.resetEncoders() - # https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/differential-drive-odometry.html#resetting-the-robot-pose - self.odometry.resetPosition( - self.ahrs.getRotation2d(), - self.leftEncoder.getPosition(), - -self.rightEncoder.getPosition(), - Pose2d(), - ) + def zeroHeading(self) -> None: + """Zeroes the heading of the robot.""" + self.gyro.reset() + + def getHeading(self) -> float: + """Returns the heading of the robot. + + :returns: the robot's heading in degrees, from -180 to 180 + """ + return Rotation2d.fromDegrees(self.gyro.getAngle()).degrees() + + def getTurnRate(self) -> float: + """Returns the turn rate of the robot. + + :returns: The turn rate of the robot, in degrees per second + """ + return self.gyro.getRate() * (-1.0 if self.driveConsts["GyroReversed"] else 1.0) diff --git a/rio/subsystems/swerveModule.py b/rio/subsystems/swerveModule.py deleted file mode 100644 index b77da52a..00000000 --- a/rio/subsystems/swerveModule.py +++ /dev/null @@ -1,189 +0,0 @@ -# vendor libs -from rev import CANSparkMax, CANSparkMaxLowLevel, SparkMaxAbsoluteEncoder - -from wpimath.kinematics import SwerveModuleState, SwerveModulePosition -from wpimath.geometry import Rotation2d - -# constants -from constants.constants import getConstants - - -class SwerveModule: - def __init__( - self, - driveChannel: int, - turnChannel: int, - chassisAngularOffset: float, - ) -> None: - """ - LMAO look at this joke code - it 'claims' to be a swerve module - - driveChannel: drive motor id - turnChannel: turn motor id - driveEncoder: encoder channel - turnEncoder: encoder channel - """ - - # constants - constants = getConstants("robot_hardware") - self.driveConstants = constants["drivetrain"] - self.drivePID = self.driveConstants["DrivePIDValues"] - self.turnPID = self.driveConstants["TurnPIDValues"] - conversions = constants["Conversions"] - self.driveConversions = conversions["Drive"] - self.turnConversions = conversions["Turn"] - - # motors - self.driveMotor = CANSparkMax( - driveChannel, CANSparkMaxLowLevel.MotorType.kBrushless - ) - self.turnMotor = CANSparkMax( - turnChannel, CANSparkMaxLowLevel.MotorType.kBrushless - ) - - # factory reset so we know whats been done to them - self.driveMotor.restoreFactoryDefaults() - self.turnMotor.restoreFactoryDefaults() - - # encoders - self.driveEncoder = self.driveMotor.getEncoder() - self.turnEncoder = self.turnMotor.getAbsoluteEncoder( - SparkMaxAbsoluteEncoder.Type.kDutyCycle - ) - - # converting units to meters and meters per second to better work with WPILib's swerve APIs - self.driveEncoder.setPositionConversionFactor( - self.driveConversions["MotorPositionFactor"] - ) - self.driveEncoder.setVelocityConversionFactor( - self.driveConversions["EncoderVelocityFactor"] - ) - - self.turnEncoder.setPositionConversionFactor( - self.turnConversions["EncoderPositionFactor"] - ) - self.turnEncoder.setVelocityConversionFactor( - self.turnConversions["EncoderVelocityFactor"] - ) - - # we may need to invert the turning encoder - # self.turnEncoder.setInverted(True) - - # PID - self.drivePIDController = self.driveMotor.getPIDController() - self.turnPIDController = self.turnMotor.getPIDController() - - # feedback - self.drivePIDController.setFeedbackDevice(self.driveEncoder) - self.turnPIDController.setFeedbackDevice(self.turnEncoder) - - # drive PIDS - self.drivePIDController.setP(self.drivePID["P"]) - self.drivePIDController.setI(self.drivePID["I"]) - self.drivePIDController.setD(self.drivePID["D"]) - self.drivePIDController.setFF(self.drivePID["FF"]) - self.drivePIDController.setOutputRange( - self.drivePID["MinOutput"], self.drivePID["MaxOutput"] - ) - - # turn PIDS - self.turnPIDController.setP(self.turnPID["P"]) - self.turnPIDController.setI(self.turnPID["I"]) - self.turnPIDController.setD(self.turnPID["D"]) - self.turnPIDController.setFF(self.turnPID["FF"]) - self.turnPIDController.setOutputRange( - self.turnPID["MinOutput"], self.turnPID["MaxOutput"] - ) - - # this should fix it spinning from 359 to zero the long way - self.turnPIDController.setPositionPIDWrappingEnabled(True) - self.turnPIDController.setPositionPIDWrappingMinInput( - self.turnConversions["EncoderPositionPIDMinInput"] - ) - self.turnPIDController.setPositionPIDWrappingMaxInput( - self.turnConversions["EncoderPositionPIDMaxInput"] - ) - - # defining idle modes - self.driveMotor.setIdleMode(CANSparkMax.IdleMode.kBrake) - self.turnMotor.setIdleMode(CANSparkMax.IdleMode.kBrake) - - # setting limits - self.driveMotor.setSmartCurrentLimit( - self.driveConstants["DriveMotorCurrentLimt"] - ) - self.turnMotor.setSmartCurrentLimit(self.driveConstants["TurnMotorCurrentLimt"]) - - # save config, to ensure config says after brown outs - self.driveMotor.burnFlash() - self.turnMotor.burnFlash() - - # Chassis angular offset - self.chassisAngularOffset = 0 - self.chassisAngularOffset = chassisAngularOffset - - # making the desired state - self.desiredState = SwerveModuleState(0.0, Rotation2d()) - self.desiredState.angle = Rotation2d(self.turnEncoder.getPosition()) - - # misc - self.driveEncoder.setPosition(0) - - def getState(self) -> SwerveModuleState: - """ - returns the current state - this is all relative to the chassis - """ - - # applies angular offset of chassis to encoder position to get a final position - return SwerveModuleState( - self.driveEncoder.getVelocity(), - Rotation2d(self.turnEncoder.getPosition() - self.chassisAngularOffset), - ) - - def getPosition(self) -> SwerveModulePosition: - """ - returns the position - this is all relative to the chassis - """ - - # applies angular offset of chassis to encoder position to get a final position - return SwerveModulePosition( - self.driveEncoder.getPosition(), - Rotation2d(self.turnEncoder.getPosition() - self.chassisAngularOffset), - ) - - def setDesiredState(self, desiredState: SwerveModuleState): - """ - set a new desired state - """ - - # add chassis offset - correctedDesiredState = SwerveModuleState() - correctedDesiredState.speed = desiredState.speed - correctedDesiredState.angle = desiredState.angle + Rotation2d( - self.chassisAngularOffset - ) - - # make it avoid spinning over 90 in reference state - optimizedDesiredState = SwerveModuleState.optimize( - correctedDesiredState, Rotation2d(self.turnEncoder.getPosition()) - ) - - # turn to setpoints - self.drivePIDController.setReference( - optimizedDesiredState.speed, CANSparkMax.ControlType.kVelocity - ) - self.turnPIDController.setReference( - optimizedDesiredState.angle.radians(), CANSparkMax.ControlType.kPosition - ) - - self.desiredState = desiredState - - def resetEncoders(self) -> None: - """ - to quote rev as to the function of this - "-+es all the SwerveModule encoders." - """ - self.driveEncoder.setPosition(0) diff --git a/rio/subsystems/swervemodule.py b/rio/subsystems/swervemodule.py new file mode 100644 index 00000000..5dedc03f --- /dev/null +++ b/rio/subsystems/swervemodule.py @@ -0,0 +1,148 @@ +# vendor libs +from rev import CANSparkMax, SparkMaxAbsoluteEncoder +from wpimath.geometry import Rotation2d +from wpimath.kinematics import SwerveModuleState, SwerveModulePosition + +# constants +from constants.mathConstant import ModuleConstants +from constants.constants import getConstants + + +class SwerveModule: + def __init__(self, driveCANId: int, turnCANId: int, chassisAngularOffset: float): + """ + A Swerve Module + """ + constants = getConstants("robot_hardware") + self.driveconsts = constants["driveTrain"] + self.pidConsts = constants["PID"] + + self.chassisAngularOffset = 0 + self.desiredState = SwerveModuleState(0.0, Rotation2d()) + + self.driveSparkMax = CANSparkMax(driveCANId, CANSparkMax.MotorType.kBrushless) + self.turnSparkMax = CANSparkMax(turnCANId, CANSparkMax.MotorType.kBrushless) + + # This helps get it to a known state for less tomfoolery + self.driveSparkMax.restoreFactoryDefaults() + self.turnSparkMax.restoreFactoryDefaults() + + self.driveEncoder = self.driveSparkMax.getEncoder() + self.turnEncoder = self.turnSparkMax.getAbsoluteEncoder( + SparkMaxAbsoluteEncoder.Type.kDutyCycle + ) + self.drivePIDController = self.driveSparkMax.getPIDController() + self.turnPIDController = self.turnSparkMax.getPIDController() + self.drivePIDController.setFeedbackDevice(self.driveEncoder) + self.turnPIDController.setFeedbackDevice(self.turnEncoder) + + # Appling position and velocity conversion factors for driving encoder + self.driveEncoder.setPositionConversionFactor( + ModuleConstants.driveEncoderPositionFactor + ) + self.driveEncoder.setVelocityConversionFactor( + ModuleConstants.driveEncoderVelocityFactor + ) + + # Appling position and velocity conversion factors for turning encoder + self.turnEncoder.setPositionConversionFactor( + ModuleConstants.turnEncoderPositionFactor + ) + self.turnEncoder.setVelocityConversionFactor( + ModuleConstants.turnEncoderVelocityFactor + ) + + self.turnEncoder.setInverted(self.driveconsts["turnEncoderInverted"]) + + # Enable PID wrapping + self.turnPIDController.setPositionPIDWrappingEnabled(True) + self.turnPIDController.setPositionPIDWrappingMinInput( + ModuleConstants.turnEncoderPositionPIDMinInput + ) + self.turnPIDController.setPositionPIDWrappingMaxInput( + ModuleConstants.turnEncoderPositionPIDMaxInput + ) + + # PIDS + self.drivePIDController.setP(self.pidConsts["driveP"]) + self.drivePIDController.setI(self.pidConsts["driveI"]) + self.drivePIDController.setD(self.pidConsts["driveD"]) + self.drivePIDController.setFF(self.pidConsts["driveFF"]) + self.drivePIDController.setOutputRange( + self.pidConsts["driveMinOutput"], self.pidConsts["driveMaxOutput"] + ) + + self.turnPIDController.setP(self.pidConsts["turnP"]) + self.turnPIDController.setI(self.pidConsts["turnI"]) + self.turnPIDController.setD(self.pidConsts["turnD"]) + self.turnPIDController.setFF(self.pidConsts["turnFF"]) + self.turnPIDController.setOutputRange( + self.pidConsts["turnMinOutput"], self.pidConsts["turnMaxOutput"] + ) + + self.driveSparkMax.setIdleMode(ModuleConstants.driveMotorIdleMode) + self.turnSparkMax.setIdleMode(ModuleConstants.turnMotorIdleMode) + self.driveSparkMax.setSmartCurrentLimit( + self.driveconsts["driveMotorCurrentLimit"] + ) + self.turnSparkMax.setSmartCurrentLimit( + self.driveconsts["turnMotorCurrentLimit"] + ) + + # Save the SPARK MAX configs + self.driveSparkMax.burnFlash() + self.turnSparkMax.burnFlash() + + self.chassisAngularOffset = chassisAngularOffset + self.desiredState.angle = Rotation2d(self.turnEncoder.getPosition()) + self.driveEncoder.setPosition(0) + + def getState(self) -> SwerveModuleState: + """ + This is relative to the chassis + """ + return SwerveModuleState( + self.driveEncoder.getVelocity(), + Rotation2d(self.turnEncoder.getPosition() - self.chassisAngularOffset), + ) + + def getPosition(self) -> SwerveModulePosition: + """ + This is relative to the chassis + """ + return SwerveModulePosition( + self.driveEncoder.getPosition(), + Rotation2d(self.turnEncoder.getPosition() - self.chassisAngularOffset), + ) + + def setDesiredState(self, desiredState: SwerveModuleState) -> None: + """ + sets a desired state + """ + # add angular offset + correctedDesiredState = SwerveModuleState() + correctedDesiredState.speed = desiredState.speed + correctedDesiredState.angle = desiredState.angle + Rotation2d( + self.chassisAngularOffset + ) + + # Optimizeation + optimizedDesiredState = SwerveModuleState.optimize( + correctedDesiredState, Rotation2d(self.turnEncoder.getPosition()) + ) + + # got to setpoints + self.drivePIDController.setReference( + optimizedDesiredState.speed, CANSparkMax.ControlType.kVelocity + ) + self.turnPIDController.setReference( + optimizedDesiredState.angle.radians(), CANSparkMax.ControlType.kPosition + ) + + self.desiredState = desiredState + + def resetEncoders(self) -> None: + """ + '-+es all the SwerveModule encoders.' - rev + """ + self.driveEncoder.setPosition(0) diff --git a/rio/subsystems/swervesubsystem.py b/rio/subsystems/swervesubsystem.py deleted file mode 100644 index 10d4cee4..00000000 --- a/rio/subsystems/swervesubsystem.py +++ /dev/null @@ -1,88 +0,0 @@ -# rename to drivesubsystem.py when completed - -from commands2 import Subsystem -from constants.constants import getConstants -from .swerveModule import SwerveModule -from wpilib import Timer -from wpimath.filter import SlewRateLimiter -from navx import AHRS -from wpimath.kinematics import SwerveDrive4Odometry -from wpimath.geometry import Rotation2d - - -class drivesubsystem(Subsystem): - def __init__(self): - super().__init__() - - # constants - constants = getConstants("robot_hardware") - self.driveConst = constants["drivetrain"] - - # module constants - self.fPMotorsConst = self.driveConst["FPMotors"] - self.fSMotorsConst = self.driveConst["FSMotors"] - self.aPMotorsConst = self.driveConst["APMotors"] - self.aSMotorsConst = self.driveConst["ASMotors"] - - # defining the modules - self.fPModule = SwerveModule( - self.fPMotorsConst["drivePort"], - self.fPMotorsConst["turnPort"], - self.fPMotorsConst["ChassisAngularOffset"], - ) - self.fSModule = SwerveModule( - self.fSMotorsConst["drivePort"], - self.fSMotorsConst["turnPort"], - self.fSMotorsConst["ChassisAngularOffset"], - ) - self.aPModule = SwerveModule( - self.aPMotorsConst["drivePort"], - self.aPMotorsConst["turnPort"], - self.aPMotorsConst["ChassisAngularOffset"], - ) - self.aSModule = SwerveModule( - self.aSMotorsConst["drivePort"], - self.aSMotorsConst["turnPort"], - self.aSMotorsConst["ChassisAngularOffset"], - ) - - # The gyro sensor - self.gyro = AHRS.create_spi() - - # Slew rate filter variables for controlling lateral acceleration - self.currentRotation = 0.0 - self.currentTranslationDir = 0.0 - self.currentTranslationMag = 0.0 - - self.magLimiter = SlewRateLimiter(self.driveConst["MagnitudeSlewRate"]) - self.rotLimiter = SlewRateLimiter(self.driveConst["RotationalSlewRate"]) - - # timer - self.prevTime = Timer.getFPGATimestamp() - - # for taking robit POS - # TODO start with this tomorrow - self.odometry = SwerveDrive4Odometry( - DriveConstants.kDriveKinematics, - Rotation2d.fromDegrees(self.gyro.getAngle()), - ( - self.fPModule.getPosition(), - self.fSModule.getPosition(), - self.aPModule.getPosition(), - self.aSModule.getPosition(), - ), - ) - - # TODO rest goes here - - def periodic(self) -> None: - # update the robot pos - self.odometry.update( - Rotation2d.fromDegrees(self.gyro.getPitch()), - ( - self.fPModule.getPosition(), - self.fSModule.getPosition(), - self.aPModule.getPosition(), - self.aSModule.getPosition(), - ), - ) diff --git a/rio/swerveutils.py b/rio/swerveutils.py new file mode 100644 index 00000000..206c18c6 --- /dev/null +++ b/rio/swerveutils.py @@ -0,0 +1,91 @@ +import math + + +def stepTowards(current: float, target: float, stepsize: float) -> float: + """ + Steps a value towards a target with a specified step size. + + :param current: The current or starting value. Can be positive or negative. + :param target: The target value the algorithm will step towards. Can be positive or negative. + :param stepsize: The maximum step size that can be taken. + + :returns: The new value for {@code current} after performing the specified step towards the specified target. + """ + + if abs(current - target) <= stepsize: + return target + + elif target < current: + return current - stepsize + + else: + return current + stepsize + + +def stepTowardsCircular(current: float, target: float, stepsize: float) -> float: + """ + Steps a value (angle) towards a target (angle) taking the shortest path with a specified step size. + + :param current: The current or starting angle (in radians). Can lie outside the 0 to 2*PI range. + :param target: The target angle (in radians) the algorithm will step towards. Can lie outside the 0 to 2*PI range. + :param stepsize: The maximum step size that can be taken (in radians). + + :returns: The new angle (in radians) for {@code current} after performing the specified step towards the specified target. + This value will always lie in the range 0 to 2*PI (exclusive). + """ + + current = wrapAngle(current) + target = wrapAngle(target) + + stepDirection = math.copysign(target - current, 1) + difference = abs(current - target) + + if difference <= stepsize: + return target + elif difference > math.pi: # does the system need to wrap over eventually? + # handle the special case where you can reach the target in one step while also wrapping + if ( + current + math.tau - target < stepsize + or target + math.tau - current < stepsize + ): + return target + else: + # this will handle wrapping gracefully + return wrapAngle(current - stepDirection * stepsize) + else: + return current + stepDirection * stepsize + + +def angleDifference(angleA: float, angleB: float) -> float: + """ + Finds the (unsigned) minimum difference between two angles including calculating across 0. + + :param angleA: An angle (in radians). + :param angleB: An angle (in radians). + + :returns: The (unsigned) minimum difference between the two angles (in radians). + """ + difference = abs(angleA - angleB) + return math.tau - difference if difference > math.pi else difference + + +def wrapAngle(angle: float) -> float: + """ + Wraps an angle until it lies within the range from 0 to 2*PI (exclusive). + + :param angle: The angle (in radians) to wrap. Can be positive or negative and can lie multiple wraps outside the output range. + + :returns: An angle (in radians) from 0 and 2*PI (exclusive). + """ + + twoPi = math.tau + + # Handle this case separately to avoid floating point errors with the floor after the division in the case below + if angle == twoPi: + return 0.0 + elif angle > twoPi: + return angle - twoPi * math.floor(angle / twoPi) + elif angle < 0.0: + return angle + twoPi * (math.floor((-angle) / twoPi) + 1) + else: + return angle From 2fbd7f92b423f84d18ee15ca971d54f9ab3cef86 Mon Sep 17 00:00:00 2001 From: kredcool Date: Fri, 12 Jan 2024 19:17:55 -0500 Subject: [PATCH 13/61] some stuff appears to be out of date so expect errno --- rio/__init__.py | 0 rio/constants.py | 44 --------------- rio/constants/mathConstant.py | 12 +++-- rio/constants/robot_hardware.yaml | 17 ------ rio/constants/robot_pid.yaml | 19 +++++++ rio/robot_controls.yaml | 7 --- rio/robot_hardware.yaml | 89 ------------------------------- rio/robot_pid.yaml | 0 rio/robotcontainer.py | 2 +- rio/subsystems/swervemodule.py | 10 ++-- 10 files changed, 35 insertions(+), 165 deletions(-) delete mode 100644 rio/__init__.py delete mode 100644 rio/constants.py delete mode 100644 rio/robot_controls.yaml delete mode 100644 rio/robot_hardware.yaml delete mode 100644 rio/robot_pid.yaml diff --git a/rio/__init__.py b/rio/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/rio/constants.py b/rio/constants.py deleted file mode 100644 index ffe95648..00000000 --- a/rio/constants.py +++ /dev/null @@ -1,44 +0,0 @@ -# Tidal Force Robotics -# 2022 - -import os -from sre_constants import CATEGORY_WORD -from wpilib import RobotBase -import yaml -import logging - -""" -Use this file only for storing non-changing constants. -""" - - -def load(fullPath: str): - # Try opening requested .yaml - with open(f"{fullPath}.yaml", "r") as yamlFile: - # Use yaml.safe_load to load the yaml into a dict - return yaml.safe_load(yamlFile) - - -def getConstants(identifier: str): - constants = {} - - # Clunky but it works - if RobotBase.isReal(): - path = "/home/lvuser/py/constants/" - else: - path = "constants/" - - try: - # Try opening requested .yaml - constants = load(f"{path}{identifier}") - except FileNotFoundError: - try: - # Try again but from one directory in (useful for unit testing) - constants = load(f"../{path}{identifier}") - except FileNotFoundError as e: - # If the file is not found, report it! - logging.error(f"{identifier} config not found!") - raise e - - # When all is done, return the important bits! - return constants diff --git a/rio/constants/mathConstant.py b/rio/constants/mathConstant.py index bc0e1f8a..89ee4a8d 100644 --- a/rio/constants/mathConstant.py +++ b/rio/constants/mathConstant.py @@ -48,10 +48,14 @@ class DriveConstants: class ModuleConstants: - constants = getConstants("robot_hardware") - drivetrain = constants["driveTrain"] - NeoMotor = constants["NeoMotor"] - pid = constants["PID"] + # constants + # hardware + hardwareConstants = getConstants("robot_hardware") + drivetrain = hardwareConstants["driveTrain"] + NeoMotor = hardwareConstants["NeoMotor"] + + # pids + pid = getConstants("robot_pid")["PID"] driveMotorPinionTeeth = drivetrain["driveMotorPinionTeeth"] diff --git a/rio/constants/robot_hardware.yaml b/rio/constants/robot_hardware.yaml index a7698ae6..b7979c06 100644 --- a/rio/constants/robot_hardware.yaml +++ b/rio/constants/robot_hardware.yaml @@ -68,22 +68,5 @@ driveTrain: EncoderReversed: False # Updated Never by Nobody Inverted: False # Updated Never by Nobody -PID: - turnEncoderPositionPIDMinInput: 0 - - driveP: 0.04 - driveI: 0 - driveD: 0 - driveFF: 1 - driveMinOutput: -1 - driveMaxOutput: 1 - - turnP: 1 - turnI: 0 - turnD: 0 - turnFF: 0 - turnMinOutput: -1 - turnMaxOutput: 1 - NeoMotor: FreeSpeedRpm: 5676 diff --git a/rio/constants/robot_pid.yaml b/rio/constants/robot_pid.yaml index e69de29b..e6f9db2b 100644 --- a/rio/constants/robot_pid.yaml +++ b/rio/constants/robot_pid.yaml @@ -0,0 +1,19 @@ +# rev's code will help with replacing values +# https://github.com/robotpy/robotpy-rev/tree/main/examples/maxswerve + +PID: + turnEncoderPositionPIDMinInput: 0 + + driveP: 0.04 + driveI: 0 + driveD: 0 + driveFF: 1 + driveMinOutput: -1 + driveMaxOutput: 1 + + turnP: 1 + turnI: 0 + turnD: 0 + turnFF: 0 + turnMinOutput: -1 + turnMaxOutput: 1 \ No newline at end of file diff --git a/rio/robot_controls.yaml b/rio/robot_controls.yaml deleted file mode 100644 index dda89737..00000000 --- a/rio/robot_controls.yaml +++ /dev/null @@ -1,7 +0,0 @@ -# This file defines the input -# and control scheme of the robot, -# Arranged into operating "modes" - -driver: - ControllerPort: 0 - DeadZone: 0.05 \ No newline at end of file diff --git a/rio/robot_hardware.yaml b/rio/robot_hardware.yaml deleted file mode 100644 index a7698ae6..00000000 --- a/rio/robot_hardware.yaml +++ /dev/null @@ -1,89 +0,0 @@ -# This file defines the robot's -# physical dimensions. Things like -# Motor placement, max and min extensions -# of arms, and similar should go here. - -# rev's code will help with replacing values -# https://github.com/robotpy/robotpy-rev/tree/main/examples/maxswerve - -driveTrain: - MaxSpeedMeterPerSecond: 4.8 - - DirectionSlewRate: 1.2 - MagnitudeSlewRate: 1.8 - RotationalSlewRate: 2.0 - - TrackWidth: 0.6731 - WheelBase: 0.6731 - - # may need to be changed depending on if it spins the same way - turnEncoderInverted: True - GyroReversed: False - - ModuleConstants: - driveMotorPinionTeeth: 14 - BevelGearTeeth: 45 - firstStageSpurTeeth: 22 - bevelPinion: 15 - - WheelDiameterMeters: 0.0762 - - # in amps - driveMotorCurrentLimit: 50 - turnMotorCurrentLimit: 20 - - autonomous: - maxSpeedMetersPerSecond: 3 - maxAccelerationMetersPerSecondSquared: 3 - - pXController: 1 - pYController: 1 - pThetaController: 1 - - FPMotors: - driveID: 1 # Updated Never by Nobody - turnID: 2 # Updated Never by Nobody - EncoderIDs: [1, 2] # Updated Never by Nobody - EncoderReversed: False # Updated Never by Nobody - Inverted: False # Updated Never by Nobody - - APMotors: - driveID: 3 # Updated Never by Nobody - turnID: 4 # Updated Never by Nobody - EncoderIDs: [3, 4] # Updated Never by Nobody - EncoderReversed: False # Updated Never by Nobody - Inverted: False # Updated Never by Nobody - - FSMotors: - driveID: 5 # Updated Never by Nobody - turnID: 6 # Updated Never by Nobody - EncoderIDs: [5, 6] # Updated Never by Nobody - EncoderReversed: False # Updated Never by Nobody - Inverted: False # Updated Never by Nobody - - ASMotors: - driveID: 7 # Updated Never by Nobody - turnID: 8 # Updated Never by Nobody - EncoderIDs: [7, 8] # Updated Never by Nobody - EncoderReversed: False # Updated Never by Nobody - Inverted: False # Updated Never by Nobody - -PID: - turnEncoderPositionPIDMinInput: 0 - - driveP: 0.04 - driveI: 0 - driveD: 0 - driveFF: 1 - driveMinOutput: -1 - driveMaxOutput: 1 - - turnP: 1 - turnI: 0 - turnD: 0 - turnFF: 0 - turnMinOutput: -1 - turnMaxOutput: 1 - -NeoMotor: - FreeSpeedRpm: 5676 diff --git a/rio/robot_pid.yaml b/rio/robot_pid.yaml deleted file mode 100644 index e69de29b..00000000 diff --git a/rio/robotcontainer.py b/rio/robotcontainer.py index 58b9b1c2..c7c64514 100644 --- a/rio/robotcontainer.py +++ b/rio/robotcontainer.py @@ -79,7 +79,7 @@ def getAutonomousCommand(self) -> commands2.Command: # Create config for trajectory config = TrajectoryConfig( self.autoConst["maxSpeedMetersPerSecond"], - self.autoConst["maxSpeedMetersPerSecondSquared"], + self.autoConst["maxAccelerationMetersPerSecondSquared"], ) # ensure max speed is obeyed config.setKinematics(DriveConstants.driveKinematics) diff --git a/rio/subsystems/swervemodule.py b/rio/subsystems/swervemodule.py index 5dedc03f..5b18a8c5 100644 --- a/rio/subsystems/swervemodule.py +++ b/rio/subsystems/swervemodule.py @@ -13,9 +13,13 @@ def __init__(self, driveCANId: int, turnCANId: int, chassisAngularOffset: float) """ A Swerve Module """ - constants = getConstants("robot_hardware") - self.driveconsts = constants["driveTrain"] - self.pidConsts = constants["PID"] + # constants + # hardware + hardwareConstants = getConstants("robot_hardware") + self.driveconsts = hardwareConstants["driveTrain"] + + # PID + self.pidConsts = getConstants("robot_pid")["PID"] self.chassisAngularOffset = 0 self.desiredState = SwerveModuleState(0.0, Rotation2d()) From 21666aa058c30091f01a1f73728079be1b6130dd Mon Sep 17 00:00:00 2001 From: kredcool Date: Fri, 12 Jan 2024 19:28:30 -0500 Subject: [PATCH 14/61] i forget something idk what --- rio/robotcontainer.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rio/robotcontainer.py b/rio/robotcontainer.py index c7c64514..968c8441 100644 --- a/rio/robotcontainer.py +++ b/rio/robotcontainer.py @@ -1,7 +1,7 @@ # commands import commands2 -from commands2 import cmd +from commands2 import cmd, swerveControllerCommand # wpilib import wpimath @@ -103,7 +103,7 @@ def getAutonomousCommand(self) -> commands2.Command: ) thetaController.enableContinuousInput(-math.pi, math.pi) - swerveControllerCommand = commands2.SwerveControllerCommand( + swerveControllerCommand = commands2.swerveControllerCommand( exampleTrajectory, self.robotDrive.getPose(), DriveConstants.driveKinematics, From 9359da9de96543e485619db1cf9aba944bff6683 Mon Sep 17 00:00:00 2001 From: kredcool Date: Sat, 13 Jan 2024 12:38:33 -0500 Subject: [PATCH 15/61] adds the ottoman empire, no I won't explain --- rio/ottomanEmpire/.gitignore | 24 + rio/ottomanEmpire/.readthedocs.yml | 13 + rio/ottomanEmpire/LICENSE | 24 + rio/ottomanEmpire/README.md | 7 + rio/ottomanEmpire/__init__.py | 0 rio/ottomanEmpire/bursa/__init__.py | 117 ++ rio/ottomanEmpire/bursa/button/__init__.py | 19 + .../bursa/button/commandgenerichid.py | 190 +++ .../bursa/button/commandjoystick.py | 205 +++ .../bursa/button/commandps4controller.py | 251 ++++ .../bursa/button/commandxboxcontroller.py | 237 +++ .../bursa/button/joystickbutton.py | 20 + .../bursa/button/networkbutton.py | 127 ++ rio/ottomanEmpire/bursa/button/povbutton.py | 21 + rio/ottomanEmpire/bursa/button/trigger.py | 267 ++++ rio/ottomanEmpire/bursa/cmd.py | 191 +++ rio/ottomanEmpire/bursa/command.py | 557 +++++++ rio/ottomanEmpire/bursa/commandgroup.py | 21 + rio/ottomanEmpire/bursa/commandscheduler.py | 475 ++++++ rio/ottomanEmpire/bursa/conditionalcommand.py | 61 + rio/ottomanEmpire/bursa/functionalcommand.py | 52 + rio/ottomanEmpire/bursa/instantcommand.py | 29 + rio/ottomanEmpire/bursa/notifiercommand.py | 41 + .../bursa/parallelcommandgroup.py | 87 ++ .../bursa/paralleldeadlinegroup.py | 98 ++ rio/ottomanEmpire/bursa/parallelracegroup.py | 82 ++ rio/ottomanEmpire/bursa/perpetualcommand.py | 46 + rio/ottomanEmpire/bursa/pidcommand.py | 74 + rio/ottomanEmpire/bursa/pidsubsystem.py | 99 ++ rio/ottomanEmpire/bursa/printcommand.py | 18 + rio/ottomanEmpire/bursa/proxycommand.py | 90 ++ .../bursa/proxyschedulecommand.py | 41 + rio/ottomanEmpire/bursa/repeatcommand.py | 55 + rio/ottomanEmpire/bursa/runcommand.py | 25 + rio/ottomanEmpire/bursa/schedulecommand.py | 30 + rio/ottomanEmpire/bursa/selectcommand.py | 71 + .../bursa/sequentialcommandgroup.py | 90 ++ rio/ottomanEmpire/bursa/startendcommand.py | 33 + rio/ottomanEmpire/bursa/subsystem.py | 180 +++ .../bursa/swervecontrollercommand.py | 293 ++++ rio/ottomanEmpire/bursa/timedcommandrobot.py | 15 + .../bursa/trapezoidprofilesubsystem.py | 75 + rio/ottomanEmpire/bursa/util.py | 24 + rio/ottomanEmpire/bursa/waitcommand.py | 33 + rio/ottomanEmpire/bursa/waituntilcommand.py | 74 + rio/ottomanEmpire/bursa/wrappercommand.py | 80 + rio/ottomanEmpire/docs/.gitignore | 11 + rio/ottomanEmpire/docs/Makefile | 183 +++ rio/ottomanEmpire/docs/api.rst | 24 + rio/ottomanEmpire/docs/conf.py | 158 ++ rio/ottomanEmpire/docs/index.rst | 11 + rio/ottomanEmpire/docs/requirements.txt | 5 + rio/ottomanEmpire/docs/setup.py | 23 + rio/ottomanEmpire/setup.py | 23 + .../tests/compositiontestbase.py | 154 ++ rio/ottomanEmpire/tests/conftest.py | 20 + rio/ottomanEmpire/tests/requirements.txt | 1 + rio/ottomanEmpire/tests/run_tests.py | 12 + .../tests/test_command_decorators.py | 222 +++ .../tests/test_command_requirements.py | 59 + .../tests/test_command_schedule.py | 90 ++ .../tests/test_commandgroup_error.py | 38 + .../tests/test_conditional_command.py | 55 + .../tests/test_default_command.py | 73 + .../tests/test_functional_command.py | 36 + .../tests/test_instant_command.py | 21 + rio/ottomanEmpire/tests/test_networkbutton.py | 29 + .../tests/test_notifier_command.py | 23 + .../tests/test_parallelcommandgroup.py | 117 ++ .../tests/test_paralleldeadlinegroup.py | 119 ++ .../tests/test_parallelracegroup.py | 183 +++ .../tests/test_perpetualcommand.py | 18 + rio/ottomanEmpire/tests/test_pidcommand.py | 114 ++ rio/ottomanEmpire/tests/test_printcommand.py | 17 + rio/ottomanEmpire/tests/test_proxycommand.py | 40 + rio/ottomanEmpire/tests/test_repeatcommand.py | 69 + .../tests/test_robotdisabledcommand.py | 157 ++ rio/ottomanEmpire/tests/test_runcommand.py | 22 + .../tests/test_schedulecommand.py | 36 + rio/ottomanEmpire/tests/test_scheduler.py | 67 + .../tests/test_schedulingrecursion.py | 165 +++ rio/ottomanEmpire/tests/test_selectcommand.py | 94 ++ .../tests/test_sequentialcommandgroup.py | 114 ++ .../tests/test_startendcommand.py | 30 + .../tests/test_swervecontrollercommand.py | 1304 +++++++++++++++++ rio/ottomanEmpire/tests/test_trigger.py | 219 +++ rio/ottomanEmpire/tests/test_waitcommand.py | 50 + .../tests/test_waituntilcommand.py | 22 + rio/ottomanEmpire/tests/util.py | 331 +++++ rio/ottomanEmpire/workflows/dist.yml | 36 + rio/robotcontainer.py | 7 +- 91 files changed, 9262 insertions(+), 2 deletions(-) create mode 100644 rio/ottomanEmpire/.gitignore create mode 100644 rio/ottomanEmpire/.readthedocs.yml create mode 100644 rio/ottomanEmpire/LICENSE create mode 100644 rio/ottomanEmpire/README.md create mode 100644 rio/ottomanEmpire/__init__.py create mode 100644 rio/ottomanEmpire/bursa/__init__.py create mode 100644 rio/ottomanEmpire/bursa/button/__init__.py create mode 100644 rio/ottomanEmpire/bursa/button/commandgenerichid.py create mode 100644 rio/ottomanEmpire/bursa/button/commandjoystick.py create mode 100644 rio/ottomanEmpire/bursa/button/commandps4controller.py create mode 100644 rio/ottomanEmpire/bursa/button/commandxboxcontroller.py create mode 100644 rio/ottomanEmpire/bursa/button/joystickbutton.py create mode 100644 rio/ottomanEmpire/bursa/button/networkbutton.py create mode 100644 rio/ottomanEmpire/bursa/button/povbutton.py create mode 100644 rio/ottomanEmpire/bursa/button/trigger.py create mode 100644 rio/ottomanEmpire/bursa/cmd.py create mode 100644 rio/ottomanEmpire/bursa/command.py create mode 100644 rio/ottomanEmpire/bursa/commandgroup.py create mode 100644 rio/ottomanEmpire/bursa/commandscheduler.py create mode 100644 rio/ottomanEmpire/bursa/conditionalcommand.py create mode 100644 rio/ottomanEmpire/bursa/functionalcommand.py create mode 100644 rio/ottomanEmpire/bursa/instantcommand.py create mode 100644 rio/ottomanEmpire/bursa/notifiercommand.py create mode 100644 rio/ottomanEmpire/bursa/parallelcommandgroup.py create mode 100644 rio/ottomanEmpire/bursa/paralleldeadlinegroup.py create mode 100644 rio/ottomanEmpire/bursa/parallelracegroup.py create mode 100644 rio/ottomanEmpire/bursa/perpetualcommand.py create mode 100644 rio/ottomanEmpire/bursa/pidcommand.py create mode 100644 rio/ottomanEmpire/bursa/pidsubsystem.py create mode 100644 rio/ottomanEmpire/bursa/printcommand.py create mode 100644 rio/ottomanEmpire/bursa/proxycommand.py create mode 100644 rio/ottomanEmpire/bursa/proxyschedulecommand.py create mode 100644 rio/ottomanEmpire/bursa/repeatcommand.py create mode 100644 rio/ottomanEmpire/bursa/runcommand.py create mode 100644 rio/ottomanEmpire/bursa/schedulecommand.py create mode 100644 rio/ottomanEmpire/bursa/selectcommand.py create mode 100644 rio/ottomanEmpire/bursa/sequentialcommandgroup.py create mode 100644 rio/ottomanEmpire/bursa/startendcommand.py create mode 100644 rio/ottomanEmpire/bursa/subsystem.py create mode 100644 rio/ottomanEmpire/bursa/swervecontrollercommand.py create mode 100644 rio/ottomanEmpire/bursa/timedcommandrobot.py create mode 100644 rio/ottomanEmpire/bursa/trapezoidprofilesubsystem.py create mode 100644 rio/ottomanEmpire/bursa/util.py create mode 100644 rio/ottomanEmpire/bursa/waitcommand.py create mode 100644 rio/ottomanEmpire/bursa/waituntilcommand.py create mode 100644 rio/ottomanEmpire/bursa/wrappercommand.py create mode 100644 rio/ottomanEmpire/docs/.gitignore create mode 100644 rio/ottomanEmpire/docs/Makefile create mode 100644 rio/ottomanEmpire/docs/api.rst create mode 100644 rio/ottomanEmpire/docs/conf.py create mode 100644 rio/ottomanEmpire/docs/index.rst create mode 100644 rio/ottomanEmpire/docs/requirements.txt create mode 100644 rio/ottomanEmpire/docs/setup.py create mode 100644 rio/ottomanEmpire/setup.py create mode 100644 rio/ottomanEmpire/tests/compositiontestbase.py create mode 100644 rio/ottomanEmpire/tests/conftest.py create mode 100644 rio/ottomanEmpire/tests/requirements.txt create mode 100755 rio/ottomanEmpire/tests/run_tests.py create mode 100644 rio/ottomanEmpire/tests/test_command_decorators.py create mode 100644 rio/ottomanEmpire/tests/test_command_requirements.py create mode 100644 rio/ottomanEmpire/tests/test_command_schedule.py create mode 100644 rio/ottomanEmpire/tests/test_commandgroup_error.py create mode 100644 rio/ottomanEmpire/tests/test_conditional_command.py create mode 100644 rio/ottomanEmpire/tests/test_default_command.py create mode 100644 rio/ottomanEmpire/tests/test_functional_command.py create mode 100644 rio/ottomanEmpire/tests/test_instant_command.py create mode 100644 rio/ottomanEmpire/tests/test_networkbutton.py create mode 100644 rio/ottomanEmpire/tests/test_notifier_command.py create mode 100644 rio/ottomanEmpire/tests/test_parallelcommandgroup.py create mode 100644 rio/ottomanEmpire/tests/test_paralleldeadlinegroup.py create mode 100644 rio/ottomanEmpire/tests/test_parallelracegroup.py create mode 100644 rio/ottomanEmpire/tests/test_perpetualcommand.py create mode 100644 rio/ottomanEmpire/tests/test_pidcommand.py create mode 100644 rio/ottomanEmpire/tests/test_printcommand.py create mode 100644 rio/ottomanEmpire/tests/test_proxycommand.py create mode 100644 rio/ottomanEmpire/tests/test_repeatcommand.py create mode 100644 rio/ottomanEmpire/tests/test_robotdisabledcommand.py create mode 100644 rio/ottomanEmpire/tests/test_runcommand.py create mode 100644 rio/ottomanEmpire/tests/test_schedulecommand.py create mode 100644 rio/ottomanEmpire/tests/test_scheduler.py create mode 100644 rio/ottomanEmpire/tests/test_schedulingrecursion.py create mode 100644 rio/ottomanEmpire/tests/test_selectcommand.py create mode 100644 rio/ottomanEmpire/tests/test_sequentialcommandgroup.py create mode 100644 rio/ottomanEmpire/tests/test_startendcommand.py create mode 100644 rio/ottomanEmpire/tests/test_swervecontrollercommand.py create mode 100644 rio/ottomanEmpire/tests/test_trigger.py create mode 100644 rio/ottomanEmpire/tests/test_waitcommand.py create mode 100644 rio/ottomanEmpire/tests/test_waituntilcommand.py create mode 100644 rio/ottomanEmpire/tests/util.py create mode 100644 rio/ottomanEmpire/workflows/dist.yml diff --git a/rio/ottomanEmpire/.gitignore b/rio/ottomanEmpire/.gitignore new file mode 100644 index 00000000..37cf260e --- /dev/null +++ b/rio/ottomanEmpire/.gitignore @@ -0,0 +1,24 @@ + +*.py[ciod] +*.egg-info + +*.dll +*.so +*.dylib +py.typed + +/build +/dist + +/.vscode + +/wpilib/buttons/rpy-include +/wpilib/buttons/_init_commands_v1_buttons.py +/wpilib/buttons/pkgcfg.py + +/commands2/_init_impl.py +/commands2/include +/commands2/lib +/commands2/rpy-include +/commands2/pkgcfg.py +/commands2/version.py diff --git a/rio/ottomanEmpire/.readthedocs.yml b/rio/ottomanEmpire/.readthedocs.yml new file mode 100644 index 00000000..c0a2ee18 --- /dev/null +++ b/rio/ottomanEmpire/.readthedocs.yml @@ -0,0 +1,13 @@ +version: 2 + +sphinx: + configuration: docs/conf.py + +build: + os: ubuntu-22.04 + tools: + python: "3.11" + +python: + install: + - requirements: docs/requirements.txt diff --git a/rio/ottomanEmpire/LICENSE b/rio/ottomanEmpire/LICENSE new file mode 100644 index 00000000..3d5a824c --- /dev/null +++ b/rio/ottomanEmpire/LICENSE @@ -0,0 +1,24 @@ +Copyright (c) 2009-2021 FIRST and other WPILib contributors +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of FIRST, WPILib, nor the names of other WPILib + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND +ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR +PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/rio/ottomanEmpire/README.md b/rio/ottomanEmpire/README.md new file mode 100644 index 00000000..4af1fccb --- /dev/null +++ b/rio/ottomanEmpire/README.md @@ -0,0 +1,7 @@ +robotpy-commands-v2 +=================== + +Python wrappers around a modified version of the new WPILib commands library. + +* Documentation @ https://robotpy.readthedocs.io/projects/commands-v2 +* Examples @ https://github.com/robotpy/examples/tree/main/commands-v2 diff --git a/rio/ottomanEmpire/__init__.py b/rio/ottomanEmpire/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/rio/ottomanEmpire/bursa/__init__.py b/rio/ottomanEmpire/bursa/__init__.py new file mode 100644 index 00000000..52efd58b --- /dev/null +++ b/rio/ottomanEmpire/bursa/__init__.py @@ -0,0 +1,117 @@ +from .button import Trigger +from .command import Command, InterruptionBehavior + +from . import cmd + +# from .cmd import ( +# deadline, +# either, +# none, +# parallel, +# print_, +# race, +# repeatingSequence, +# run, +# runEnd, +# runOnce, +# select, +# sequence, +# startEnd, +# waitSeconds, +# waitUntil, +# ) +from .commandgroup import CommandGroup, IllegalCommandUse +from .commandscheduler import CommandScheduler +from .conditionalcommand import ConditionalCommand +from .functionalcommand import FunctionalCommand +from .instantcommand import InstantCommand +from .notifiercommand import NotifierCommand +from .parallelcommandgroup import ParallelCommandGroup +from .paralleldeadlinegroup import ParallelDeadlineGroup +from .parallelracegroup import ParallelRaceGroup +from .perpetualcommand import PerpetualCommand +from .pidcommand import PIDCommand +from .pidsubsystem import PIDSubsystem +from .printcommand import PrintCommand +from .proxycommand import ProxyCommand +from .proxyschedulecommand import ProxyScheduleCommand +from .repeatcommand import RepeatCommand +from .runcommand import RunCommand +from .schedulecommand import ScheduleCommand +from .selectcommand import SelectCommand +from .sequentialcommandgroup import SequentialCommandGroup +from .startendcommand import StartEndCommand +from .subsystem import Subsystem +from .swervecontrollercommand import SwerveControllerCommand +from .timedcommandrobot import TimedCommandRobot +from .trapezoidprofilesubsystem import TrapezoidProfileSubsystem +from .waitcommand import WaitCommand +from .waituntilcommand import WaitUntilCommand +from .wrappercommand import WrapperCommand + +__all__ = [ + "cmd", + "Command", + "CommandGroup", + "CommandScheduler", + "ConditionalCommand", + "FunctionalCommand", + "IllegalCommandUse", + "InstantCommand", + "InterruptionBehavior", + "NotifierCommand", + "ParallelCommandGroup", + "ParallelDeadlineGroup", + "ParallelRaceGroup", + "PerpetualCommand", + "PIDCommand", + "PIDSubsystem", + "PrintCommand", + "ProxyCommand", + "ProxyScheduleCommand", + "RepeatCommand", + "RunCommand", + "ScheduleCommand", + "SelectCommand", + "SequentialCommandGroup", + "StartEndCommand", + "Subsystem", + "SwerveControllerCommand", + "TimedCommandRobot", + "TrapezoidProfileSubsystem", + "WaitCommand", + "WaitUntilCommand", + "WrapperCommand", + # "none", + # "runOnce", + # "run", + # "startEnd", + # "runEnd", + # "print_", + # "waitSeconds", + # "waitUntil", + # "either", + # "select", + # "sequence", + # "repeatingSequence", + # "parallel", + # "race", + # "deadline", + "Trigger", # was here in 2023 +] + + +def __getattr__(attr): + if attr == "SubsystemBase": + import warnings + + warnings.warn("SubsystemBase is deprecated", DeprecationWarning, stacklevel=2) + return Subsystem + + if attr == "CommandBase": + import warnings + + warnings.warn("CommandBase is deprecated", DeprecationWarning, stacklevel=2) + return Command + + raise AttributeError("module {!r} has no attribute " "{!r}".format(__name__, attr)) diff --git a/rio/ottomanEmpire/bursa/button/__init__.py b/rio/ottomanEmpire/bursa/button/__init__.py new file mode 100644 index 00000000..f6947397 --- /dev/null +++ b/rio/ottomanEmpire/bursa/button/__init__.py @@ -0,0 +1,19 @@ +from .commandgenerichid import CommandGenericHID +from .commandjoystick import CommandJoystick +from .commandps4controller import CommandPS4Controller +from .commandxboxcontroller import CommandXboxController +from .joystickbutton import JoystickButton +from .networkbutton import NetworkButton +from .povbutton import POVButton +from .trigger import Trigger + +__all__ = [ + "Trigger", + "CommandGenericHID", + "CommandJoystick", + "CommandPS4Controller", + "CommandXboxController", + "JoystickButton", + "NetworkButton", + "POVButton", +] diff --git a/rio/ottomanEmpire/bursa/button/commandgenerichid.py b/rio/ottomanEmpire/bursa/button/commandgenerichid.py new file mode 100644 index 00000000..1512fd15 --- /dev/null +++ b/rio/ottomanEmpire/bursa/button/commandgenerichid.py @@ -0,0 +1,190 @@ +from typing import Optional + +from wpilib.event import EventLoop +from wpilib.interfaces import GenericHID + +from ..commandscheduler import CommandScheduler +from .trigger import Trigger + + +class CommandGenericHID: + """ + A version of GenericHID with Trigger factories for command-based. + """ + + def __init__(self, port: int): + """ + Construct an instance of a device. + + :param port: The port on the Driver Station that the device is plugged into. + """ + self._hid = GenericHID(port) + + def getHID(self) -> GenericHID: + """ + Get the underlying GenericHID object. + """ + return self._hid + + def button(self, button: int, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around this button's digital signal. + + :param button: The button index + :param loop: the event loop instance to attache the event to. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getRawButtonPressed(button)) + + def pov( + self, angle: int, *, pov: int = 0, loop: Optional[EventLoop] = None + ) -> Trigger: + """ + Constructs a Trigger instance based around this angle of a POV on the HID. + + The POV angles start at 0 in the up direction, and increase clockwise (e.g. right is 90, + upper-left is 315). + + :param angle: POV angle in degrees, or -1 for the center / not pressed. + :param pov: index of the POV to read (starting at 0). Defaults to 0. + :param loop: the event loop instance to attach the event to. Defaults to {@link + CommandScheduler#getDefaultButtonLoop() the default command scheduler button loop}. + :returns: a Trigger instance based around this angle of a POV on the HID. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getPOV(pov) == angle) + + def povUp(self) -> Trigger: + """ + Constructs a Trigger instance based around the 0 degree angle (up) of the default (index 0) POV + on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command + scheduler button loop}. + + :returns: a Trigger instance based around the 0 degree angle of a POV on the HID. + """ + return self.pov(0) + + def povUpRight(self) -> Trigger: + """ + Constructs a Trigger instance based around the 45 degree angle (right up) of the default (index + 0) POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default + command scheduler button loop}. + + :returns: a Trigger instance based around the 45 degree angle of a POV on the HID. + """ + return self.pov(45) + + def povRight(self) -> Trigger: + """ + Constructs a Trigger instance based around the 90 degree angle (right) of the default (index 0) + POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command + scheduler button loop}. + + :returns: a Trigger instance based around the 90 degree angle of a POV on the HID. + """ + return self.pov(90) + + def povDownRight(self) -> Trigger: + """ + Constructs a Trigger instance based around the 135 degree angle (right down) of the default + (index 0) POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the + default command scheduler button loop}. + + :returns: a Trigger instance based around the 135 degree angle of a POV on the HID. + """ + return self.pov(135) + + def povDown(self) -> Trigger: + """ + Constructs a Trigger instance based around the 180 degree angle (down) of the default (index 0) + POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command + scheduler button loop}. + + :returns: a Trigger instance based around the 180 degree angle of a POV on the HID. + """ + return self.pov(180) + + def povDownLeft(self) -> Trigger: + """ + Constructs a Trigger instance based around the 225 degree angle (down left) of the default + (index 0) POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the + default command scheduler button loop}. + + :returns: a Trigger instance based around the 225 degree angle of a POV on the HID. + """ + return self.pov(225) + + def povLeft(self) -> Trigger: + """ + Constructs a Trigger instance based around the 270 degree angle (left) of the default (index 0) + POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command + scheduler button loop}. + + :returns: a Trigger instance based around the 270 degree angle of a POV on the HID. + """ + return self.pov(270) + + def povUpLeft(self) -> Trigger: + """ + Constructs a Trigger instance based around the 315 degree angle (left up) of the default (index + 0) POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default + command scheduler button loop}. + + :returns: a Trigger instance based around the 315 degree angle of a POV on the HID. + """ + return self.pov(315) + + def povCenter(self) -> Trigger: + """ + Constructs a Trigger instance based around the center (not pressed) position of the default + (index 0) POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the + default command scheduler button loop}. + + :returns: a Trigger instance based around the center position of a POV on the HID. + """ + return self.pov(-1) + + def axisLessThan( + self, axis: int, threshold: float, loop: Optional[EventLoop] = None + ) -> Trigger: + """ + Constructs a Trigger instance that is true when the axis value is less than {@code threshold}, + attached to the given loop. + + :param axis: The axis to read, starting at 0 + :param threshold: The value below which this trigger should return true. + :param loop: the event loop instance to attach the trigger to + :returns: a Trigger instance that is true when the axis value is less than the provided + threshold. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getRawAxis(axis) < threshold) + + def axisGreaterThan( + self, axis: int, threshold: float, loop: Optional[EventLoop] = None + ) -> Trigger: + """ + Constructs a Trigger instance that is true when the axis value is greater than {@code + threshold}, attached to the given loop. + + :param axis: The axis to read, starting at 0 + :param threshold: The value above which this trigger should return true. + :param loop: the event loop instance to attach the trigger to. + :returns: a Trigger instance that is true when the axis value is greater than the provided + threshold. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getRawAxis(axis) > threshold) + + def getRawAxis(self, axis: int) -> float: + """ + Get the value of the axis. + + :param axis: The axis to read, starting at 0. + :returns: The value of the axis. + """ + return self._hid.getRawAxis(axis) diff --git a/rio/ottomanEmpire/bursa/button/commandjoystick.py b/rio/ottomanEmpire/bursa/button/commandjoystick.py new file mode 100644 index 00000000..f5f28c51 --- /dev/null +++ b/rio/ottomanEmpire/bursa/button/commandjoystick.py @@ -0,0 +1,205 @@ +from typing import Optional + +from wpilib import Joystick +from wpilib.event import EventLoop + +from ..commandscheduler import CommandScheduler +from .commandgenerichid import CommandGenericHID +from .trigger import Trigger + + +class CommandJoystick(CommandGenericHID): + """ + A version of Joystick with Trigger factories for command-based. + """ + + _hid: Joystick + + def __init__(self, port: int): + """ + Construct an instance of a controller. + + :param port: The port index on the Driver Station that the controller is plugged into. + """ + + super().__init__(port) + self._hid = Joystick(port) + + def getHID(self): + """ + Get the underlying GenericHID object. + + :returns: the wrapped GenericHID object + """ + return self._hid + + def trigger(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the trigger button's digital signal. + + :param loop: the event loop instance to attach the event to. + :returns: an event instance representing the trigger button's digital signal attached to the + given loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getTrigger()) + + def top(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the top button's digital signal. + + :param loop: the event loop instance to attach the event to. + :returns: an event instance representing the top button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getTop()) + + def setXChannel(self, channel: int): + """ + Set the channel associated with the X axis. + + :param channel: The channel to set the axis to. + """ + self._hid.setXChannel(channel) + + def setYChannel(self, channel: int): + """ + Set the channel associated with the Y axis. + + :param channel: The channel to set the axis to. + """ + self._hid.setYChannel(channel) + + def setZChannel(self, channel: int): + """ + Set the channel associated with the Z axis. + + :param channel: The channel to set the axis to. + """ + self._hid.setZChannel(channel) + + def setThrottleChannel(self, channel: int): + """ + Set the channel associated with the throttle axis. + + :param channel: The channel to set the axis to. + """ + self._hid.setThrottleChannel(channel) + + def setTwistChannel(self, channel: int): + """ + Set the channel associated with the twist axis. + + :param channel: The channel to set the axis to. + """ + self._hid.setTwistChannel(channel) + + def getXChannel(self) -> int: + """ + Get the channel currently associated with the X axis. + + :returns: The channel for the axis. + """ + return self._hid.getXChannel() + + def getYChannel(self) -> int: + """ + Get the channel currently associated with the Y axis. + + :returns: The channel for the axis. + """ + return self._hid.getYChannel() + + def getZChannel(self) -> int: + """ + Get the channel currently associated with the Z axis. + + :returns: The channel for the axis. + """ + return self._hid.getZChannel() + + def getThrottleChannel(self) -> int: + """ + Get the channel currently associated with the throttle axis. + + :returns: The channel for the axis. + """ + return self._hid.getThrottleChannel() + + def getTwistChannel(self) -> int: + """ + Get the channel currently associated with the twist axis. + + :returns: The channel for the axis. + """ + return self._hid.getTwistChannel() + + def getX(self) -> float: + """ + Get the x position of the HID. + + :returns: the x position + """ + return self._hid.getX() + + def getY(self) -> float: + """ + Get the y position of the HID. + + :returns: the y position + """ + return self._hid.getY() + + def getZ(self) -> float: + """ + Get the z position of the HID. + + :returns: the z position + """ + return self._hid.getZ() + + def getTwist(self) -> float: + """ + Get the twist value of the current joystick. This depends on the mapping of the joystick + connected to the current port. + + :returns: The Twist value of the joystick. + """ + return self._hid.getTwist() + + def getThrottle(self) -> float: + """ + Get the throttle value of the current joystick. This depends on the mapping of the joystick + connected to the current port. + + :returns: The Throttle value of the joystick. + """ + return self._hid.getThrottle() + + def getMagnitude(self) -> float: + """ + Get the magnitude of the direction vector formed by the joystick's current position relative to + its origin. + + :returns: The magnitude of the direction vector + """ + return self._hid.getMagnitude() + + def getDirectionRadians(self) -> float: + """ + Get the direction of the vector formed by the joystick and its origin in radians. + + :returns: The direction of the vector in radians + """ + return self._hid.getDirectionRadians() + + def getDirectionDegrees(self) -> float: + """ + Get the direction of the vector formed by the joystick and its origin in degrees. + + :returns: The direction of the vector in degrees + """ + return self._hid.getDirectionDegrees() diff --git a/rio/ottomanEmpire/bursa/button/commandps4controller.py b/rio/ottomanEmpire/bursa/button/commandps4controller.py new file mode 100644 index 00000000..9b2da01d --- /dev/null +++ b/rio/ottomanEmpire/bursa/button/commandps4controller.py @@ -0,0 +1,251 @@ +from typing import Optional + +from wpilib import PS4Controller +from wpilib.event import EventLoop + +from ..commandscheduler import CommandScheduler +from .commandgenerichid import CommandGenericHID +from .trigger import Trigger + + +class CommandPS4Controller(CommandGenericHID): + """ + A version of PS4Controller with Trigger factories for command-based. + """ + + _hid: PS4Controller + + def __init__(self, port: int): + """ + Construct an instance of a device. + + :param port: The port index on the Driver Station that the device is plugged into. + """ + super().__init__(port) + self._hid = PS4Controller(port) + + def getHID(self): + """ + Get the underlying GenericHID object. + + :returns: the wrapped GenericHID object + """ + return self._hid + + def L2(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the L2 button's digital signal. + + :param loop: the event loop instance to attach the event to. + :returns: an event instance representing the L2 button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getL2Button()) + + def R2(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the R2 button's digital signal. + + :param loop: the event loop instance to attach the event to. + :returns: an event instance representing the R2 button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getR2Button()) + + def L1(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the L1 button's digital signal. + + :param loop: the event loop instance to attach the event to. + :returns: an event instance representing the L1 button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getL1Button()) + + def R1(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the R1 button's digital signal. + + :param loop: the event loop instance to attach the event to. + :returns: an event instance representing the R1 button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getR1Button()) + + def L3(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the L3 button's digital signal. + + :param loop: the event loop instance to attach the event to. + :returns: an event instance representing the L3 button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getL3Button()) + + def R3(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the R3 button's digital signal. + + :param loop: the event loop instance to attach the event to. + :returns: an event instance representing the R3 button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getR3Button()) + + def square(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the square button's digital signal. + + :param loop: the event loop instance to attach the event to. + :returns: an event instance representing the square button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getSquareButton()) + + def cross(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the cross button's digital signal. + + :param loop: the event loop instance to attach the event to. + :returns: an event instance representing the cross button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getCrossButton()) + + def triangle(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the triangle button's digital signal. + + :param loop: the event loop instance to attach the event to. + :returns: an event instance representing the triangle button's digital signal attached to the + given loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getTriangleButton()) + + def circle(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the circle button's digital signal. + + :param loop: the event loop instance to attach the event to. + :returns: an event instance representing the circle button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getCircleButton()) + + def share(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the share button's digital signal. + + :param loop: the event loop instance to attach the event to. + :returns: an event instance representing the share button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getShareButton()) + + def PS(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the PS button's digital signal. + + :param loop: the event loop instance to attach the event to. + :returns: an event instance representing the PS button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getPSButton()) + + def options(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the options button's digital signal. + + :param loop: the event loop instance to attach the event to. + :returns: an event instance representing the options button's digital signal attached to the + given loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getOptionsButton()) + + def touchpad(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the touchpad's digital signal. + + :param loop: the event loop instance to attach the event to. + :returns: an event instance representing the touchpad's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getTouchpad()) + + def getLeftX(self) -> float: + """ + Get the X axis value of left side of the controller. + + :returns: the axis value. + """ + return self._hid.getLeftX() + + def getRightX(self) -> float: + """ + Get the X axis value of right side of the controller. + + :returns: the axis value. + """ + return self._hid.getRightX() + + def getLeftY(self) -> float: + """ + Get the Y axis value of left side of the controller. + + :returns: the axis value. + """ + return self._hid.getLeftY() + + def getRightY(self) -> float: + """ + Get the Y axis value of right side of the controller. + + :returns: the axis value. + """ + return self._hid.getRightY() + + def getL2Axis(self) -> float: + """ + Get the L2 axis value of the controller. Note that this axis is bound to the range of [0, 1] as + opposed to the usual [-1, 1]. + + :returns: the axis value. + """ + return self._hid.getL2Axis() + + def getR2Axis(self) -> float: + """ + Get the R2 axis value of the controller. Note that this axis is bound to the range of [0, 1] as + opposed to the usual [-1, 1]. + + :returns: the axis value. + """ + return self._hid.getR2Axis() diff --git a/rio/ottomanEmpire/bursa/button/commandxboxcontroller.py b/rio/ottomanEmpire/bursa/button/commandxboxcontroller.py new file mode 100644 index 00000000..2c613f4b --- /dev/null +++ b/rio/ottomanEmpire/bursa/button/commandxboxcontroller.py @@ -0,0 +1,237 @@ +from typing import Optional + +from wpilib import XboxController +from wpilib.event import EventLoop + +from ..commandscheduler import CommandScheduler +from .commandgenerichid import CommandGenericHID +from .trigger import Trigger + + +class CommandXboxController(CommandGenericHID): + """ + A version of XboxController with Trigger factories for command-based. + """ + + _hid: XboxController + + def __init__(self, port: int): + """ + Construct an instance of a controller. + + :param port: The port index on the Driver Station that the controller is plugged into. + """ + super().__init__(port) + self._hid = XboxController(port) + + def getHID(self): + """ + Get the underlying GenericHID object. + + :returns: the wrapped GenericHID object + """ + return self._hid + + def leftBumper(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the left bumper's digital signal. + + :param loop: the event loop instance to attach the event to. + :returns: an event instance representing the right bumper's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getLeftBumper()) + + def rightBumper(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the right bumper's digital signal. + + :param loop: the event loop instance to attach the event to. + :returns: an event instance representing the left bumper's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getRightBumper()) + + def leftStick(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the left stick button's digital signal. + + :param loop: the event loop instance to attach the event to. + :returns: an event instance representing the left stick button's digital signal attached to the + given loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getLeftStickButton()) + + def rightStick(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the right stick button's digital signal. + + :param loop: the event loop instance to attach the event to. + :returns: an event instance representing the right stick button's digital signal attached to the + given loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getRightStickButton()) + + def a(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the A button's digital signal. + + :param loop: the event loop instance to attach the event to. + :returns: an event instance representing the A button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getAButton()) + + def b(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the B button's digital signal. + + :param loop: the event loop instance to attach the event to. + :returns: an event instance representing the B button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getBButton()) + + def x(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the X button's digital signal. + + :param loop: the event loop instance to attach the event to. + :returns: an event instance representing the X button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getXButton()) + + def y(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the Y button's digital signal. + + :param loop: the event loop instance to attach the event to. + :returns: an event instance representing the Y button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getYButton()) + + def start(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the start button's digital signal. + + :param loop: the event loop instance to attach the event to. + :returns: an event instance representing the start button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getStartButton()) + + def back(self, loop: Optional[EventLoop] = None) -> Trigger: + """ + Constructs an event instance around the back button's digital signal. + + :param loop: the event loop instance to attach the event to. + :returns: an event instance representing the back button's digital signal attached to the given + loop. + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getBackButton()) + + def leftTrigger( + self, threshold: float = 0.5, loop: Optional[EventLoop] = None + ) -> Trigger: + """ + Constructs a Trigger instance around the axis value of the left trigger. The returned trigger + will be true when the axis value is greater than {@code threshold}. + + :param threshold: the minimum axis value for the returned Trigger to be true. This value + should be in the range [0, 1] where 0 is the unpressed state of the axis. + :param loop: the event loop instance to attach the Trigger to. + :returns: a Trigger instance that is true when the left trigger's axis exceeds the provided + threshold, attached to the given event loop + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getLeftTriggerAxis() > threshold) + + def rightTrigger( + self, threshold: float = 0.5, loop: Optional[EventLoop] = None + ) -> Trigger: + """ + Constructs a Trigger instance around the axis value of the right trigger. The returned trigger + will be true when the axis value is greater than {@code threshold}. + + :param threshold: the minimum axis value for the returned Trigger to be true. This value + should be in the range [0, 1] where 0 is the unpressed state of the axis. + :param loop: the event loop instance to attach the Trigger to. + :returns: a Trigger instance that is true when the right trigger's axis exceeds the provided + threshold, attached to the given event loop + """ + if loop is None: + loop = CommandScheduler.getInstance().getDefaultButtonLoop() + return Trigger(loop, lambda: self._hid.getRightTriggerAxis() > threshold) + + def getLeftX(self) -> float: + """ + Get the X axis value of left side of the controller. + + :returns: The axis value. + """ + return self._hid.getLeftX() + + def getRightX(self) -> float: + """ + Get the X axis value of right side of the controller. + + :returns: The axis value. + """ + return self._hid.getRightX() + + def getLeftY(self) -> float: + """ + Get the Y axis value of left side of the controller. + + :returns: The axis value. + """ + return self._hid.getLeftY() + + def getRightY(self) -> float: + """ + Get the Y axis value of right side of the controller. + + :returns: The axis value. + """ + return self._hid.getRightY() + + def getLeftTriggerAxis(self) -> float: + """ + Get the left trigger (LT) axis value of the controller. Note that this axis is bound to the + range of [0, 1] as opposed to the usual [-1, 1]. + + :returns: The axis value. + """ + return self._hid.getLeftTriggerAxis() + + def getRightTriggerAxis(self) -> float: + """ + Get the right trigger (RT) axis value of the controller. Note that this axis is bound to the + range of [0, 1] as opposed to the usual [-1, 1]. + + :returns: The axis value. + """ + return self._hid.getRightTriggerAxis() diff --git a/rio/ottomanEmpire/bursa/button/joystickbutton.py b/rio/ottomanEmpire/bursa/button/joystickbutton.py new file mode 100644 index 00000000..2e0e107c --- /dev/null +++ b/rio/ottomanEmpire/bursa/button/joystickbutton.py @@ -0,0 +1,20 @@ +from wpilib.interfaces import GenericHID + +from .trigger import Trigger + + +class JoystickButton(Trigger): + """ + A Button that gets its state from a GenericHID. + + This class is provided by the NewCommands VendorDep + """ + + def __init__(self, joystick: GenericHID, buttonNumber: int): + """ + Creates a joystick button for triggering commands. + + :param joystick: The GenericHID object that has the button (e.g. Joystick, KinectStick, etc) + :param buttonNumber: The button number (see GenericHID#getRawButton(int) + """ + super().__init__(lambda: joystick.getRawButton(buttonNumber)) diff --git a/rio/ottomanEmpire/bursa/button/networkbutton.py b/rio/ottomanEmpire/bursa/button/networkbutton.py new file mode 100644 index 00000000..c6c6602c --- /dev/null +++ b/rio/ottomanEmpire/bursa/button/networkbutton.py @@ -0,0 +1,127 @@ +from typing import overload + +from ntcore import BooleanSubscriber, BooleanTopic, NetworkTable, NetworkTableInstance + +from ..util import format_args_kwargs +from .trigger import Trigger + + +class NetworkButton(Trigger): + """ + A Button that uses a NetworkTable boolean field. + + This class is provided by the NewCommands VendorDep + """ + + @overload + def __init__(self, topic: BooleanTopic) -> None: + """ + Creates a NetworkButton that commands can be bound to. + + :param topic: The boolean topic that contains the value. + """ + pass + + @overload + def __init__(self, sub: BooleanSubscriber) -> None: + """ + Creates a NetworkButton that commands can be bound to. + + :param sub: The boolean subscriber that provides the value. + """ + pass + + @overload + def __init__(self, table: NetworkTable, field: str) -> None: + """ + Creates a NetworkButton that commands can be bound to. + + :param table: The table where the networktable value is located. + :param field: The field that is the value. + """ + pass + + @overload + def __init__(self, table: str, field: str) -> None: + """ + Creates a NetworkButton that commands can be bound to. + + :param table: The table where the networktable value is located. + :param field: The field that is the value. + """ + pass + + @overload + def __init__(self, inst: NetworkTableInstance, table: str, field: str) -> None: + """ + Creates a NetworkButton that commands can be bound to. + + :param inst: The NetworkTable instance to use + :param table: The table where the networktable value is located. + :param field: the field that is the value. + """ + pass + + def __init__(self, *args, **kwargs) -> None: + def init_sub(sub: BooleanSubscriber): + return super(NetworkButton, self).__init__( + lambda: sub.getTopic().getInstance().isConnected() and sub.get() + ) + + def init_topic(topic: BooleanTopic): + init_sub(topic.subscribe(False)) + + def init_table_field(table: NetworkTable, field: str): + init_topic(table.getBooleanTopic(field)) + + def init_inst_table_field(inst: NetworkTableInstance, table: str, field: str): + init_table_field(inst.getTable(table), field) + + def init_str_table_field(table: str, field: str): + init_inst_table_field(NetworkTableInstance.getDefault(), table, field) + + num_args = len(args) + len(kwargs) + + if num_args == 1: + if "topic" in kwargs: + return init_topic(kwargs["topic"]) + if "sub" in kwargs: + return init_sub(kwargs["sub"]) + if isinstance(args[0], BooleanTopic): + return init_topic(args[0]) + if isinstance(args[0], BooleanSubscriber): + return init_sub(args[0]) + elif num_args == 2: + table, field, *_ = args + (None, None) + if "table" in kwargs: + table = kwargs["table"] + if "field" in kwargs: + field = kwargs["field"] + if table is not None and field is not None: + if isinstance(table, NetworkTable): + return init_table_field(table, field) + if isinstance(table, str): + return init_str_table_field(table, field) + elif num_args == 3: + inst, table, field, *_ = args + (None, None, None) + if "inst" in kwargs: + inst = kwargs["inst"] + if "table" in kwargs: + table = kwargs["table"] + if "field" in kwargs: + field = kwargs["field"] + if inst is not None and table is not None and field is not None: + return init_inst_table_field(inst, table, field) + + raise TypeError( + f""" +TypeError: NetworkButton(): incompatible function arguments. The following argument types are supported: + 1. (self: NetworkButton, topic: BooleanTopic) + 2. (self: NetworkButton, sub: BooleanSubscriber) + 3. (self: NetworkButton, table: NetworkTable, field: str) + 4. (self: NetworkButton, table: str, field: str) + 5. (self: NetworkButton, inst: NetworkTableInstance, table: str, field: str) + +Invoked with: {format_args_kwargs(self, *args, **kwargs)} +""" + ) diff --git a/rio/ottomanEmpire/bursa/button/povbutton.py b/rio/ottomanEmpire/bursa/button/povbutton.py new file mode 100644 index 00000000..8e0c06cd --- /dev/null +++ b/rio/ottomanEmpire/bursa/button/povbutton.py @@ -0,0 +1,21 @@ +from wpilib.interfaces import GenericHID + +from .trigger import Trigger + + +class POVButton(Trigger): + """ + A Button that gets its state from a POV on a GenericHID. + + This class is provided by the NewCommands VendorDep + """ + + def __init__(self, joystick: GenericHID, angle: int, povNumber: int = 0): + """ + Creates a POV button for triggering commands. + + :param joystick: The GenericHID object that has the POV + :param angle: The desired angle in degrees (e.g. 90, 270) + :param povNumber: The POV number (see GenericHID#getPOV(int)) + """ + super().__init__(lambda: joystick.getPOV(povNumber) == angle) diff --git a/rio/ottomanEmpire/bursa/button/trigger.py b/rio/ottomanEmpire/bursa/button/trigger.py new file mode 100644 index 00000000..d9f88329 --- /dev/null +++ b/rio/ottomanEmpire/bursa/button/trigger.py @@ -0,0 +1,267 @@ +from types import SimpleNamespace +from typing import Callable, overload + +from typing_extensions import Self +from wpilib.event import EventLoop +from wpimath.filter import Debouncer + +from ..command import Command +from ..commandscheduler import CommandScheduler +from ..util import format_args_kwargs + + +class Trigger: + """ + This class provides an easy way to link commands to conditions. + + It is very easy to link a button to a command. For instance, you could link the trigger button + of a joystick to a "score" command. + """ + + _loop: EventLoop + _condition: Callable[[], bool] + + @overload + def __init__(self, condition: Callable[[], bool] = lambda: False): + """ + Creates a new trigger based on the given condition. + + Polled by the default scheduler button loop. + + :param condition: the condition represented by this trigger + """ + ... + + @overload + def __init__(self, loop: EventLoop, condition: Callable[[], bool]): + """ + Creates a new trigger based on the given condition. + + :param loop: The loop instance that polls this trigger. + :param condition: the condition represented by this trigger + """ + ... + + def __init__(self, *args, **kwargs): + def init_loop_condition(loop: EventLoop, condition: Callable[[], bool]): + self._loop = loop + self._condition = condition + + def init_condition(condition: Callable[[], bool]): + init_loop_condition( + CommandScheduler.getInstance().getDefaultButtonLoop(), condition + ) + + num_args = len(args) + len(kwargs) + + if num_args == 0: + return init_condition(lambda: False) + elif num_args == 1 and len(kwargs) == 1: + if "condition" in kwargs: + return init_condition(kwargs["condition"]) + elif num_args == 1 and len(args) == 1: + if callable(args[0]): + return init_condition(args[0]) + elif num_args == 2: + loop, condition, *_ = args + (None, None) + if "loop" in kwargs: + loop = kwargs["loop"] + if "condition" in kwargs: + condition = kwargs["condition"] + if loop is not None and condition is not None: + return init_loop_condition(loop, condition) + + raise TypeError( + f""" +TypeError: Trigger(): incompatible function arguments. The following argument types are supported: + 1. (self: Trigger) + 2. (self: Trigger, condition: () -> bool) + 3. (self: Trigger, loop: EventLoop, condition: () -> bool) + +Invoked with: {format_args_kwargs(self, *args, **kwargs)} +""" + ) + + def onTrue(self, command: Command) -> Self: + """ + Starts the given command whenever the condition changes from `False` to `True`. + + :param command: the command to start + :returns: this trigger, so calls can be chained + """ + + @self._loop.bind + def _(state=SimpleNamespace(pressed_last=self._condition())): + pressed = self._condition() + if not state.pressed_last and pressed: + command.schedule() + state.pressed_last = pressed + + return self + + def onFalse(self, command: Command) -> Self: + """ + Starts the given command whenever the condition changes from `True` to `False`. + + :param command: the command to start + :returns: this trigger, so calls can be chained + """ + + @self._loop.bind + def _(state=SimpleNamespace(pressed_last=self._condition())): + pressed = self._condition() + if state.pressed_last and not pressed: + command.schedule() + state.pressed_last = pressed + + return self + + def whileTrue(self, command: Command) -> Self: + """ + Starts the given command when the condition changes to `True` and cancels it when the condition + changes to `False`. + + Doesn't re-start the command if it ends while the condition is still `True`. If the command + should restart, see RepeatCommand. + + :param command: the command to start + :returns: this trigger, so calls can be chained + """ + + @self._loop.bind + def _(state=SimpleNamespace(pressed_last=self._condition())): + pressed = self._condition() + if not state.pressed_last and pressed: + command.schedule() + elif state.pressed_last and not pressed: + command.cancel() + state.pressed_last = pressed + + return self + + def whileFalse(self, command: Command) -> Self: + """ + Starts the given command when the condition changes to `False` and cancels it when the + condition changes to `True`. + + Doesn't re-start the command if it ends while the condition is still `False`. If the command + should restart, see RepeatCommand. + + :param command: the command to start + :returns: this trigger, so calls can be chained + """ + + @self._loop.bind + def _(state=SimpleNamespace(pressed_last=self._condition())): + pressed = self._condition() + if state.pressed_last and not pressed: + command.schedule() + elif not state.pressed_last and pressed: + command.cancel() + state.pressed_last = pressed + + return self + + def toggleOnTrue(self, command: Command) -> Self: + """ + Toggles a command when the condition changes from `False` to `True`. + + :param command: the command to toggle + :returns: this trigger, so calls can be chained + """ + + @self._loop.bind + def _(state=SimpleNamespace(pressed_last=self._condition())): + pressed = self._condition() + if not state.pressed_last and pressed: + if command.isScheduled(): + command.cancel() + else: + command.schedule() + state.pressed_last = pressed + + return self + + def toggleOnFalse(self, command: Command) -> Self: + """ + Toggles a command when the condition changes from `True` to `False`. + + :param command: the command to toggle + :returns: this trigger, so calls can be chained + """ + + @self._loop.bind + def _(state=SimpleNamespace(pressed_last=self._condition())): + pressed = self._condition() + if state.pressed_last and not pressed: + if command.isScheduled(): + command.cancel() + else: + command.schedule() + state.pressed_last = pressed + + return self + + def __call__(self) -> bool: + return self._condition() + + def getAsBoolean(self) -> bool: + return self._condition() + + def __bool__(self) -> bool: + return self._condition() + + def __and__(self, other: Callable[[], bool]) -> "Trigger": + return Trigger(lambda: self() and other()) + + def and_(self, other: Callable[[], bool]) -> "Trigger": + """ + Composes two triggers with logical AND. + + :param trigger: the condition to compose with + :returns: A trigger which is active when both component triggers are active. + """ + return self & other + + def __or__(self, other: Callable[[], bool]) -> "Trigger": + return Trigger(lambda: self() or other()) + + def or_(self, other: Callable[[], bool]) -> "Trigger": + """ + Composes two triggers with logical OR. + + :param trigger: the condition to compose with + :returns: A trigger which is active when either component trigger is active. + """ + return self | other + + def __invert__(self) -> "Trigger": + return Trigger(lambda: not self()) + + def negate(self) -> "Trigger": + """ + Creates a new trigger that is active when this trigger is inactive, i.e. that acts as the + negation of this trigger. + + :returns: the negated trigger + """ + return ~self + + def not_(self) -> "Trigger": + return ~self + + def debounce( + self, + seconds: float, + debounce_type: Debouncer.DebounceType = Debouncer.DebounceType.kRising, + ) -> "Trigger": + """ + Creates a new debounced trigger from this trigger - it will become active when this trigger has + been active for longer than the specified period. + + :param seconds: The debounce period. + :param type: The debounce type. + :returns: The debounced trigger. + """ + debouncer = Debouncer(seconds, debounce_type) + return Trigger(lambda: debouncer.calculate(self())) diff --git a/rio/ottomanEmpire/bursa/cmd.py b/rio/ottomanEmpire/bursa/cmd.py new file mode 100644 index 00000000..62a2e24c --- /dev/null +++ b/rio/ottomanEmpire/bursa/cmd.py @@ -0,0 +1,191 @@ +from typing import Any, Callable, Dict, Hashable + +from .command import Command +from .conditionalcommand import ConditionalCommand +from .functionalcommand import FunctionalCommand +from .instantcommand import InstantCommand +from .parallelcommandgroup import ParallelCommandGroup +from .paralleldeadlinegroup import ParallelDeadlineGroup +from .parallelracegroup import ParallelRaceGroup +from .printcommand import PrintCommand +from .runcommand import RunCommand +from .selectcommand import SelectCommand +from .sequentialcommandgroup import SequentialCommandGroup +from .startendcommand import StartEndCommand +from .subsystem import Subsystem +from .waitcommand import WaitCommand +from .waituntilcommand import WaitUntilCommand + +# Is this whole file just to avoid the `new` keyword in Java? + + +def none() -> Command: + """ + Constructs a command that does nothing, finishing immediately. + + :returns: the command + """ + return InstantCommand() + + +def runOnce(action: Callable[[], Any], *requirements: Subsystem) -> Command: + """ + Constructs a command that runs an action once and finishes. + + :param action: the action to run + :param requirements: subsystems the action requires + :returns: the command + """ + return InstantCommand(action, *requirements) + + +def run(action: Callable[[], Any], *requirements: Subsystem) -> Command: + """ + Constructs a command that runs an action every iteration until interrupted. + + :param action: the action to run + :param requirements: subsystems the action requires + :returns: the command + """ + return RunCommand(action, *requirements) + + +def startEnd( + run: Callable[[], Any], end: Callable[[], Any], *requirements: Subsystem +) -> Command: + """ + Constructs a command that runs an action once and another action when the command is + interrupted. + + :param start: the action to run on start + :param end: the action to run on interrupt + :param requirements: subsystems the action requires + :returns: the command + """ + return StartEndCommand(run, lambda: end(), *requirements) + + +def runEnd( + run: Callable[[], Any], end: Callable[[], Any], *requirements: Subsystem +) -> Command: + """ + Constructs a command that runs an action every iteration until interrupted, and then runs a + second action. + + :param run: the action to run every iteration + :param end: the action to run on interrupt + :param requirements: subsystems the action requires + :returns: the command + """ + return FunctionalCommand( + lambda: None, run, lambda interrupted: end(), lambda: False, *requirements + ) + + +def print_(message: str) -> Command: + """ + Constructs a command that prints a message and finishes. + + :param message: the message to print + :returns: the command + """ + return PrintCommand(message) + + +def waitSeconds(seconds: float) -> Command: + """ + Constructs a command that does nothing, finishing after a specified duration. + + :param seconds: after how long the command finishes + :returns: the command + """ + return WaitCommand(seconds) + + +def waitUntil(condition: Callable[[], bool]) -> Command: + """ + Constructs a command that does nothing, finishing once a condition becomes true. + + :param condition: the condition + :returns: the command + """ + return WaitUntilCommand(condition) + + +def either(onTrue: Command, onFalse: Command, selector: Callable[[], bool]) -> Command: + """ + Runs one of two commands, based on the boolean selector function. + + :param onTrue: the command to run if the selector function returns true + :param onFalse: the command to run if the selector function returns false + :param selector: the selector function + :returns: the command + """ + return ConditionalCommand(onTrue, onFalse, selector) + + +def select( + commands: Dict[Hashable, Command], selector: Callable[[], Hashable] +) -> Command: + """ + Runs one of several commands, based on the selector function. + + :param selector: the selector function + :param commands: map of commands to select from + :returns: the command + """ + return SelectCommand(commands, selector) + + +def sequence(*commands: Command) -> Command: + """ + Runs a group of commands in series, one after the other. + + :param commands: the commands to include + :returns: the command group + """ + return SequentialCommandGroup(*commands) + + +def repeatingSequence(*commands: Command) -> Command: + """ + Runs a group of commands in series, one after the other. Once the last command ends, the group + is restarted. + + :param commands: the commands to include + :returns: the command group + """ + return sequence(*commands).repeatedly() + + +def parallel(*commands: Command) -> Command: + """ + Runs a group of commands at the same time. Ends once all commands in the group finish. + + :param commands: the commands to include + :returns: the command + """ + return ParallelCommandGroup(*commands) + + +def race(*commands: Command) -> Command: + """ + Runs a group of commands at the same time. Ends once any command in the group finishes, and + cancels the others. + + :param commands: the commands to include + :returns: the command group + """ + return ParallelRaceGroup(*commands) + + +def deadline(deadline: Command, *commands: Command) -> Command: + """ + Runs a group of commands at the same time. Ends once a specific command finishes, and cancels + the others. + + :param deadline: the deadline command + :param commands: the commands to include + :returns: the command group + """ + return ParallelDeadlineGroup(deadline, *commands) diff --git a/rio/ottomanEmpire/bursa/command.py b/rio/ottomanEmpire/bursa/command.py new file mode 100644 index 00000000..e8b0546f --- /dev/null +++ b/rio/ottomanEmpire/bursa/command.py @@ -0,0 +1,557 @@ +# Don't import stuff from the package here, it'll cause a circular import + + +from __future__ import annotations + +from enum import Enum +from typing import TYPE_CHECKING, Any, Callable, Set + +from typing_extensions import Self, TypeAlias + +if TYPE_CHECKING: + from .instantcommand import InstantCommand + from .subsystem import Subsystem + from .parallelracegroup import ParallelRaceGroup + from .sequentialcommandgroup import SequentialCommandGroup + from .paralleldeadlinegroup import ParallelDeadlineGroup + from .parallelcommandgroup import ParallelCommandGroup + from .perpetualcommand import PerpetualCommand + from .repeatcommand import RepeatCommand + from .proxycommand import ProxyCommand + from .conditionalcommand import ConditionalCommand + from .wrappercommand import WrapperCommand + +from wpiutil import Sendable, SendableRegistry, SendableBuilder + + +class InterruptionBehavior(Enum): + """ + An enum describing the command's behavior when another command with a shared requirement is + scheduled. + """ + + kCancelIncoming = 0 + """ + This command ends, #end(boolean) end(true) is called, and the incoming command is + scheduled normally. + + This is the default behavior. + """ + + kCancelSelf = 1 + """ This command continues, and the incoming command is not scheduled.""" + + +class Command(Sendable): + """ + A state machine representing a complete action to be performed by the robot. Commands are run by + the CommandScheduler, and can be composed into CommandGroups to allow users to build + complicated multistep actions without the need to roll the state machine logic themselves. + + Commands are run synchronously from the main robot loop; no multithreading is used, unless + specified explicitly from the command implementation. + + This class is provided by the NewCommands VendorDep + """ + + InterruptionBehavior: TypeAlias = ( + InterruptionBehavior # type alias for 2023 location + ) + + requirements: Set[Subsystem] + + def __new__(cls, *args, **kwargs) -> Self: + instance = super().__new__( + cls, + ) + super().__init__(instance) + SendableRegistry.add(instance, cls.__name__) + instance.requirements = set() + return instance + + def __init__(self): + pass + + def initialize(self): + """The initial subroutine of a command. Called once when the command is initially scheduled.""" + pass + + def execute(self): + """The main body of a command. Called repeatedly while the command is scheduled.""" + pass + + def isFinished(self) -> bool: + """ + Whether the command has finished. Once a command finishes, the scheduler will call its end() + method and un-schedule it. + + :returns: whether the command has finished. + """ + return False + + def end(self, interrupted: bool): + """ + The action to take when the command ends. Called when either the command finishes normally, or + when it interrupted/canceled. + + Do not schedule commands here that share requirements with this command. Use {@link + #andThen(Command...)} instead. + + :param interrupted: whether the command was interrupted/canceled + """ + pass + + def getRequirements(self) -> Set[Subsystem]: + """ + Specifies the set of subsystems used by this command. Two commands cannot use the same + subsystem at the same time. If another command is scheduled that shares a requirement, {@link + #getInterruptionBehavior()} will be checked and followed. If no subsystems are required, return + an empty set. + + Note: it is recommended that user implementations contain the requirements as a field, and + return that field here, rather than allocating a new set every time this is called. + + :returns: the set of subsystems that are required + """ + return self.requirements + + def addRequirements(self, *requirements: Subsystem): + """ + Adds the specified subsystems to the requirements of the command. The scheduler will prevent + two commands that require the same subsystem from being scheduled simultaneously. + + Note that the scheduler determines the requirements of a command when it is scheduled, so + this method should normally be called from the command's constructor. + + :param requirements: the requirements to add + """ + self.requirements.update(requirements) + + def runsWhenDisabled(self) -> bool: + """ + Whether the given command should run when the robot is disabled. Override to return true if the + command should run when disabled. + + :returns: whether the command should run when the robot is disabled + """ + return False + + def schedule(self, interruptible: bool = True) -> None: + """Schedules this command.""" + from .commandscheduler import CommandScheduler + + CommandScheduler.getInstance().schedule(self) + + def cancel(self) -> None: + """ + Cancels this command. Will call #end(boolean) end(true). Commands will be canceled + regardless of InterruptionBehavior interruption behavior. + """ + from .commandscheduler import CommandScheduler + + CommandScheduler.getInstance().cancel(self) + + def isScheduled(self) -> bool: + """ + Whether the command is currently scheduled. Note that this does not detect whether the command + is in a composition, only whether it is directly being run by the scheduler. + + :returns: Whether the command is scheduled. + """ + from .commandscheduler import CommandScheduler + + return CommandScheduler.getInstance().isScheduled(self) + + def hasRequirement(self, requirement: Subsystem) -> bool: + """ + Whether the command requires a given subsystem. + + :param requirement: the subsystem to inquire about + :returns: whether the subsystem is required + """ + return requirement in self.requirements + + def getInterruptionBehavior(self) -> InterruptionBehavior: + """ + How the command behaves when another command with a shared requirement is scheduled. + + :returns: a variant of InterruptionBehavior, defaulting to {@link InterruptionBehavior#kCancelSelf kCancelSelf}. + """ + return InterruptionBehavior.kCancelSelf + + def withTimeout(self, seconds: float) -> ParallelRaceGroup: + """ + Decorates this command with a timeout. If the specified timeout is exceeded before the command + finishes normally, the command will be interrupted and un-scheduled. Note that the timeout only + applies to the command returned by this method; the calling command is not itself changed. + + Note: This decorator works by adding this command to a composition. The command the + decorator was called on cannot be scheduled independently or be added to a different + composition (namely, decorators), unless it is manually cleared from the list of composed + commands with CommandScheduler#removeComposedCommand(Command). The command composition + returned from this method can be further decorated without issue. + + :param seconds: the timeout duration + :returns: the command with the timeout added + """ + from .waitcommand import WaitCommand + + return self.raceWith(WaitCommand(seconds)) + + def until(self, condition: Callable[[], bool]) -> ParallelRaceGroup: + """ + Decorates this command with an interrupt condition. If the specified condition becomes true + before the command finishes normally, the command will be interrupted and un-scheduled. + + Note: This decorator works by adding this command to a composition. The command the + decorator was called on cannot be scheduled independently or be added to a different + composition (namely, decorators), unless it is manually cleared from the list of composed + commands with CommandScheduler#removeComposedCommand(Command). The command composition + returned from this method can be further decorated without issue. + + :param condition: the interrupt condition + :returns: the command with the interrupt condition added + """ + from .waituntilcommand import WaitUntilCommand + + return self.raceWith(WaitUntilCommand(condition)) + + def onlyWhile(self, condition: Callable[[], bool]) -> ParallelRaceGroup: + """ + Decorates this command with a run condition. If the specified condition becomes false before + the command finishes normally, the command will be interrupted and un-scheduled. + + Note: This decorator works by adding this command to a composition. The command the + decorator was called on cannot be scheduled independently or be added to a different + composition (namely, decorators), unless it is manually cleared from the list of composed + commands with CommandScheduler#removeComposedCommand(Command). The command composition + returned from this method can be further decorated without issue. + + :param condition: the interrupt condition + :returns: the command with the interrupt condition added + """ + return self.until(lambda: not condition()) + + def withInterrupt(self, condition: Callable[[], bool]) -> ParallelRaceGroup: + """ + Decorates this command with an interrupt condition. If the specified condition becomes true + before the command finishes normally, the command will be interrupted and un-scheduled. Note + that this only applies to the command returned by this method; the calling command is not + itself changed. + + Note: This decorator works by adding this command to a composition. The command the + decorator was called on cannot be scheduled independently or be added to a different + composition (namely, decorators), unless it is manually cleared from the list of composed + commands with CommandScheduler#removeComposedCommand(Command). The command composition + returned from this method can be further decorated without issue. + + :param condition: the interrupt condition + :returns: the command with the interrupt condition added + @deprecated Replace with #until(BooleanSupplier) + """ + return self.until(condition) + + def beforeStarting(self, before: Command) -> SequentialCommandGroup: + """ + Decorates this command with another command to run before this command starts. + + Note: This decorator works by adding this command to a composition. The command the + decorator was called on cannot be scheduled independently or be added to a different + composition (namely, decorators), unless it is manually cleared from the list of composed + commands with CommandScheduler#removeComposedCommand(Command). The command composition + returned from this method can be further decorated without issue. + + :param before: the command to run before this one + :returns: the decorated command + """ + from .sequentialcommandgroup import SequentialCommandGroup + + return SequentialCommandGroup(before, self) + + def andThen(self, *next: Command) -> SequentialCommandGroup: + """ + Decorates this command with a set of commands to run after it in sequence. Often more + convenient/less-verbose than constructing a new SequentialCommandGroup explicitly. + + Note: This decorator works by adding this command to a composition. The command the + decorator was called on cannot be scheduled independently or be added to a different + composition (namely, decorators), unless it is manually cleared from the list of composed + commands with CommandScheduler#removeComposedCommand(Command). The command composition + returned from this method can be further decorated without issue. + + :param next: the commands to run next + :returns: the decorated command + """ + from .sequentialcommandgroup import SequentialCommandGroup + + return SequentialCommandGroup(self, *next) + + def deadlineWith(self, *parallel: Command) -> ParallelDeadlineGroup: + """ + Decorates this command with a set of commands to run parallel to it, ending when the calling + command ends and interrupting all the others. Often more convenient/less-verbose than + constructing a new ParallelDeadlineGroup explicitly. + + Note: This decorator works by adding this command to a composition. The command the + decorator was called on cannot be scheduled independently or be added to a different + composition (namely, decorators), unless it is manually cleared from the list of composed + commands with CommandScheduler#removeComposedCommand(Command). The command composition + returned from this method can be further decorated without issue. + + :param parallel: the commands to run in parallel + :returns: the decorated command + """ + from .paralleldeadlinegroup import ParallelDeadlineGroup + + return ParallelDeadlineGroup(self, *parallel) + + def alongWith(self, *parallel: Command) -> ParallelCommandGroup: + """ + Decorates this command with a set of commands to run parallel to it, ending when the last + command ends. Often more convenient/less-verbose than constructing a new {@link + ParallelCommandGroup} explicitly. + + Note: This decorator works by adding this command to a composition. The command the + decorator was called on cannot be scheduled independently or be added to a different + composition (namely, decorators), unless it is manually cleared from the list of composed + commands with CommandScheduler#removeComposedCommand(Command). The command composition + returned from this method can be further decorated without issue. + + :param parallel: the commands to run in parallel + :returns: the decorated command + """ + from .parallelcommandgroup import ParallelCommandGroup + + return ParallelCommandGroup(self, *parallel) + + def raceWith(self, *parallel: Command) -> ParallelRaceGroup: + """ + Decorates this command with a set of commands to run parallel to it, ending when the first + command ends. Often more convenient/less-verbose than constructing a new {@link + ParallelRaceGroup} explicitly. + + Note: This decorator works by adding this command to a composition. The command the + decorator was called on cannot be scheduled independently or be added to a different + composition (namely, decorators), unless it is manually cleared from the list of composed + commands with CommandScheduler#removeComposedCommand(Command). The command composition + returned from this method can be further decorated without issue. + + :param parallel: the commands to run in parallel + :returns: the decorated command + """ + from .parallelracegroup import ParallelRaceGroup + + return ParallelRaceGroup(self, *parallel) + + def perpetually(self) -> PerpetualCommand: + """ + Decorates this command to run perpetually, ignoring its ordinary end conditions. The decorated + command can still be interrupted or canceled. + + Note: This decorator works by adding this command to a composition. The command the + decorator was called on cannot be scheduled independently or be added to a different + composition (namely, decorators), unless it is manually cleared from the list of composed + commands with CommandScheduler#removeComposedCommand(Command). The command composition + returned from this method can be further decorated without issue. + + :returns: the decorated command + @deprecated PerpetualCommand violates the assumption that execute() doesn't get called after + isFinished() returns true -- an assumption that should be valid. This was unsafe/undefined + behavior from the start, and RepeatCommand provides an easy way to achieve similar end + results with slightly different (and safe) semantics. + """ + from .perpetualcommand import PerpetualCommand + + return PerpetualCommand(self) + + def repeatedly(self) -> RepeatCommand: + """ + Decorates this command to run repeatedly, restarting it when it ends, until this command is + interrupted. The decorated command can still be canceled. + + Note: This decorator works by adding this command to a composition. The command the + decorator was called on cannot be scheduled independently or be added to a different + composition (namely, decorators), unless it is manually cleared from the list of composed + commands with CommandScheduler#removeComposedCommand(Command). The command composition + returned from this method can be further decorated without issue. + + :returns: the decorated command + """ + from .repeatcommand import RepeatCommand + + return RepeatCommand(self) + + def asProxy(self) -> ProxyCommand: + """ + Decorates this command to run "by proxy" by wrapping it in a ProxyCommand. This is + useful for "forking off" from command compositions when the user does not wish to extend the + command's requirements to the entire command composition. + + :returns: the decorated command + """ + from .proxycommand import ProxyCommand + + return ProxyCommand(self) + + def unless(self, condition: Callable[[], bool]) -> ConditionalCommand: + """ + Decorates this command to only run if this condition is not met. If the command is already + running and the condition changes to true, the command will not stop running. The requirements + of this command will be kept for the new conditional command. + + Note: This decorator works by adding this command to a composition. The command the + decorator was called on cannot be scheduled independently or be added to a different + composition (namely, decorators), unless it is manually cleared from the list of composed + commands with CommandScheduler#removeComposedCommand(Command). The command composition + returned from this method can be further decorated without issue. + + :param condition: the condition that will prevent the command from running + :returns: the decorated command + """ + from .conditionalcommand import ConditionalCommand + from .instantcommand import InstantCommand + + return ConditionalCommand(InstantCommand(), self, condition) + + def onlyIf(self, condition: Callable[[], bool]) -> ConditionalCommand: + """ + Decorates this command to only run if this condition is met. If the command is already running + and the condition changes to false, the command will not stop running. The requirements of this + command will be kept for the new conditional command. + + Note: This decorator works by adding this command to a composition. The command the + decorator was called on cannot be scheduled independently or be added to a different + composition (namely, decorators), unless it is manually cleared from the list of composed + commands with CommandScheduler#removeComposedCommand(Command). The command composition + returned from this method can be further decorated without issue. + + :param condition: the condition that will allow the command to run + :returns: the decorated command + """ + return self.unless(lambda: not condition()) + + def ignoringDisable(self, doesRunWhenDisabled: bool) -> WrapperCommand: + """ + Decorates this command to run or stop when disabled. + + :param doesRunWhenDisabled: true to run when disabled. + :returns: the decorated command + """ + from .wrappercommand import WrapperCommand + + class W(WrapperCommand): + def runsWhenDisabled(self) -> bool: + return doesRunWhenDisabled + + return W(self) + + def withInterruptBehavior(self, behavior: InterruptionBehavior) -> WrapperCommand: + """ + Decorates this command to have a different InterruptionBehavior interruption behavior. + + :param interruptBehavior: the desired interrupt behavior + :returns: the decorated command + """ + from .wrappercommand import WrapperCommand + + class W(WrapperCommand): + def getInterruptionBehavior(self) -> InterruptionBehavior: + return behavior + + return W(self) + + def finallyDo(self, end: Callable[[bool], Any]) -> WrapperCommand: + """ + Decorates this command with a lambda to call on interrupt or end, following the command's + inherent #end(boolean) method. + + :param end: a lambda accepting a boolean parameter specifying whether the command was + interrupted. + :return: the decorated command + """ + from .wrappercommand import WrapperCommand + + class W(WrapperCommand): + def end(self, interrupted: bool) -> None: + self._command.end(interrupted) + end(interrupted) + + return W(self) + + def handleInterrupt(self, handler: Callable[[], Any]) -> WrapperCommand: + """ + Decorates this command with a lambda to call on interrupt, following the command's inherent + #end(boolean) method. + + :param handler: a lambda to run when the command is interrupted + :returns: the decorated command + """ + return self.finallyDo(lambda interrupted: handler() if interrupted else None) + + def getName(self) -> str: + """ + Gets the name of this Command. + + :returns: Name + """ + return SendableRegistry.getName(self) + + def setName(self, name: str): + """ + Sets the name of this Command. + + :param name: Name + """ + SendableRegistry.setName(self, name) + + def withName(self, name: str) -> WrapperCommand: + """ + Decorates this command with a name. + + :param name: the name of the command + :returns: the decorated command + """ + from .wrappercommand import WrapperCommand + + wrapper = WrapperCommand(self) + wrapper.setName(name) + return wrapper + + def initSendable(self, builder: SendableBuilder) -> None: + from .commandscheduler import CommandScheduler + + builder.setSmartDashboardType("Command") + builder.addStringProperty( + ".name", + self.getName, + lambda _: None, + ) + + def on_set(value: bool) -> None: + if value: + if not self.isScheduled(): + self.schedule() + else: + if self.isScheduled(): + self.cancel() + + builder.addBooleanProperty( + "running", + self.isScheduled, + on_set, + ) + builder.addBooleanProperty( + ".isParented", + lambda: CommandScheduler.getInstance().isComposed(self), + lambda _: None, + ) + builder.addStringProperty( + "interruptBehavior", + lambda: self.getInterruptionBehavior().name, + lambda _: None, + ) + builder.addBooleanProperty( + "runsWhenDisabled", + self.runsWhenDisabled, + lambda _: None, + ) diff --git a/rio/ottomanEmpire/bursa/commandgroup.py b/rio/ottomanEmpire/bursa/commandgroup.py new file mode 100644 index 00000000..8a00f242 --- /dev/null +++ b/rio/ottomanEmpire/bursa/commandgroup.py @@ -0,0 +1,21 @@ +from __future__ import annotations + +from .command import Command + + +class IllegalCommandUse(Exception): + pass + + +class CommandGroup(Command): + """ + A base for CommandGroups. + """ + + def addCommands(self, *commands: Command): + """ + Adds the given commands to the command group. + + :param commands: The commands to add. + """ + raise NotImplementedError diff --git a/rio/ottomanEmpire/bursa/commandscheduler.py b/rio/ottomanEmpire/bursa/commandscheduler.py new file mode 100644 index 00000000..ef51150c --- /dev/null +++ b/rio/ottomanEmpire/bursa/commandscheduler.py @@ -0,0 +1,475 @@ +from __future__ import annotations + +from typing import Any, Callable, Dict, Iterable, List, Optional, Set, Union + +import hal +from typing_extensions import Self +from wpilib import RobotBase, RobotState, TimedRobot, Watchdog +from wpilib.event import EventLoop + +from .command import Command, InterruptionBehavior +from .commandgroup import * +from .subsystem import Subsystem + + +class CommandScheduler: + """ + The scheduler responsible for running Commands. A Command-based robot should call {@link + CommandScheduler#run()} on the singleton instance in its periodic block in order to run commands + synchronously from the main loop. Subsystems should be registered with the scheduler using {@link + CommandScheduler#registerSubsystem(Subsystem...)} in order for their Subsystem#periodic() + methods to be called and for their default commands to be scheduled. + """ + + _instance: Optional[CommandScheduler] = None + + def __new__(cls) -> CommandScheduler: + if cls._instance is None: + return super().__new__(cls) + return cls._instance + + @staticmethod + def getInstance() -> "CommandScheduler": + """ + Returns the Scheduler instance. + + :returns: the instance + """ + return CommandScheduler() + + @staticmethod + def resetInstance() -> None: + """ + Resets the scheduler instance, which is useful for testing purposes. This should not be + called by user code. + """ + inst = CommandScheduler._instance + if inst: + inst._defaultButtonLoop.clear() + CommandScheduler._instance = None + + def __init__(self) -> None: + """ + Gets the scheduler instance. + """ + if CommandScheduler._instance is not None: + return + CommandScheduler._instance = self + self._composedCommands: Set[Command] = set() + self._scheduledCommands: Dict[Command, None] = {} + self._requirements: Dict[Subsystem, Command] = {} + self._subsystems: Dict[Subsystem, Optional[Command]] = {} + + self._defaultButtonLoop = EventLoop() + self.setActiveButtonLoop(self._defaultButtonLoop) + + self._disabled = False + + self._initActions: List[Callable[[Command], None]] = [] + self._executeActions: List[Callable[[Command], None]] = [] + self._interruptActions: List[Callable[[Command], None]] = [] + self._finishActions: List[Callable[[Command], None]] = [] + + self._inRunLoop = False + self._toSchedule: Dict[Command, None] = {} + self._toCancel: Dict[Command, None] = {} + + self._watchdog = Watchdog(TimedRobot.kDefaultPeriod, lambda: None) + + hal.report( + hal.tResourceType.kResourceType_Command.value, + hal.tInstances.kCommand2_Scheduler.value, + ) + + def setPeriod(self, period: float) -> None: + """ + Changes the period of the loop overrun watchdog. This should be kept in sync with the + TimedRobot period. + + :param period: Period in seconds. + """ + self._watchdog.setTimeout(period) + + def getDefaultButtonLoop(self) -> EventLoop: + """ + Get the default button poll. + + :returns: a reference to the default EventLoop object polling buttons. + """ + return self._defaultButtonLoop + + def getActiveButtonLoop(self) -> EventLoop: + """ + Get the active button poll. + + :returns: a reference to the current EventLoop object polling buttons. + """ + return self._activeButtonLoop + + def setActiveButtonLoop(self, loop: EventLoop) -> None: + """ + Replace the button poll with another one. + + :param loop: the new button polling loop object. + """ + self._activeButtonLoop = loop + + def initCommand(self, command: Command, *requirements: Subsystem) -> None: + """ + Initializes a given command, adds its requirements to the list, and performs the init actions. + + :param command: The command to initialize + :param requirements: The command requirements + """ + self._scheduledCommands[command] = None + for requirement in requirements: + self._requirements[requirement] = command + command.initialize() + for action in self._initActions: + action(command) + # self._watchdog.addEpoch() + + def schedule(self, *commands) -> None: + """ + Schedules a command for execution. Does nothing if the command is already scheduled. If a + command's requirements are not available, it will only be started if all the commands currently + using those requirements have been scheduled as interruptible. If this is the case, they will + be interrupted and the command will be scheduled. + + :param commands: the commands to schedule. + """ + if len(commands) > 1: + for command in commands: + self.schedule(command) + return + + command = commands[0] + + if command is None: + # DriverStation.reportWarning("CommandScheduler tried to schedule a null command!", True) + return + + if self._inRunLoop: + self._toSchedule[command] = None + return + + if command in self.getComposedCommands(): + raise IllegalCommandUse( + "A command that is part of a CommandGroup cannot be independently scheduled" + ) + + if self._disabled: + return + + if RobotState.isDisabled() and not command.runsWhenDisabled(): + return + + if self.isScheduled(command): + return + + requirements = command.getRequirements() + + if self._requirements.keys().isdisjoint(requirements): + self.initCommand(command, *requirements) + else: + for requirement in requirements: + requiringCommand = self.requiring(requirement) + if ( + requiringCommand is not None + and requiringCommand.getInterruptionBehavior() + == InterruptionBehavior.kCancelIncoming + ): + return + + for requirement in requirements: + requiringCommand = self.requiring(requirement) + if requiringCommand is not None: + self.cancel(requiringCommand) + + self.initCommand(command, *requirements) + + def run(self): + """ + Runs a single iteration of the scheduler. The execution occurs in the following order: + + Subsystem periodic methods are called. + + Button bindings are polled, and new commands are scheduled from them. + + Currently-scheduled commands are executed. + + End conditions are checked on currently-scheduled commands, and commands that are finished + have their end methods called and are removed. + + Any subsystems not being used as requirements have their default methods started. + """ + if self._disabled: + return + self._watchdog.reset() + + for subsystem in self._subsystems: + subsystem.periodic() + if RobotBase.isSimulation(): + subsystem.simulationPeriodic() + # self._watchdog.addEpoch() + + loopCache = self._activeButtonLoop + loopCache.poll() + self._watchdog.addEpoch("buttons.run()") + + self._inRunLoop = True + + for command in self._scheduledCommands.copy(): + if not command.runsWhenDisabled() and RobotState.isDisabled(): + command.end(True) + for action in self._interruptActions: + action(command) + for requirement in command.getRequirements(): + self._requirements.pop(requirement) + self._scheduledCommands.pop(command) + continue + + command.execute() + for action in self._executeActions: + action(command) + # self._watchdog.addEpoch() + if command.isFinished(): + command.end(False) + for action in self._finishActions: + action(command) + self._scheduledCommands.pop(command) + for requirement in command.getRequirements(): + self._requirements.pop(requirement) + + self._inRunLoop = False + + for command in self._toSchedule: + self.schedule(command) + + for command in self._toCancel: + self.cancel(command) + + self._toSchedule.clear() + self._toCancel.clear() + + for subsystem, command in self._subsystems.items(): + if subsystem not in self._requirements and command is not None: + self.schedule(command) + + self._watchdog.disable() + if self._watchdog.isExpired(): + self._watchdog.printEpochs() + + def registerSubsystem(self, *subsystems: Subsystem) -> None: + """ + Registers subsystems with the scheduler. This must be called for the subsystem's periodic block + to run when the scheduler is run, and for the subsystem's default command to be scheduled. It + is recommended to call this from the constructor of your subsystem implementations. + + :param subsystems: the subsystem to register + """ + for subsystem in subsystems: + if subsystem in self._subsystems: + # DriverStation.reportWarning("Tried to register an already-registered subsystem", True) + continue + self._subsystems[subsystem] = None + + def unregisterSubsystem(self, *subsystems: Subsystem) -> None: + """ + Un-registers subsystems with the scheduler. The subsystem will no longer have its periodic + block called, and will not have its default command scheduled. + + :param subsystems: the subsystem to un-register + """ + for subsystem in subsystems: + self._subsystems.pop(subsystem) + + def setDefaultCommand(self, subsystem: Subsystem, defaultCommand: Command) -> None: + """ + Sets the default command for a subsystem. Registers that subsystem if it is not already + registered. Default commands will run whenever there is no other command currently scheduled + that requires the subsystem. Default commands should be written to never end (i.e. their {@link + Command#isFinished()} method should return false), as they would simply be re-scheduled if they + do. Default commands must also require their subsystem. + + :param subsystem: the subsystem whose default command will be set + :param defaultCommand: the default command to associate with the subsystem + """ + self.requireNotComposed([defaultCommand]) + if subsystem not in defaultCommand.getRequirements(): + raise IllegalCommandUse("Default commands must require their subsystem!") + if ( + defaultCommand.getInterruptionBehavior() + != InterruptionBehavior.kCancelIncoming + ): + # DriverStation.reportWarning("Registering a non-interruptible default command\nThis will likely prevent any other commands from requiring this subsystem.", True) + pass + self._subsystems[subsystem] = defaultCommand + + def removeDefaultCommand(self, subsystem: Subsystem) -> None: + """ + Removes the default command for a subsystem. The current default command will run until another + command is scheduled that requires the subsystem, at which point the current default command + will not be re-scheduled. + + :param subsystem: the subsystem whose default command will be removed + """ + self._subsystems[subsystem] = None + + def getDefaultCommand(self, subsystem: Subsystem) -> Optional[Command]: + """ + Gets the default command associated with this subsystem. Null if this subsystem has no default + command associated with it. + + :param subsystem: the subsystem to inquire about + :returns: the default command associated with the subsystem + """ + return self._subsystems[subsystem] + + def cancel(self, *commands: Command) -> None: + """ + Cancels commands. The scheduler will only call Command#end(boolean) method of the + canceled command with {@code true}, indicating they were canceled (as opposed to finishing + normally). + + Commands will be canceled regardless of InterruptionBehavior interruption behavior. + + :param commands: the commands to cancel + """ + if self._inRunLoop: + for command in commands: + self._toCancel[command] = None + return + + for command in commands: + if not self.isScheduled(command): + continue + + self._scheduledCommands.pop(command) + for requirement in command.getRequirements(): + del self._requirements[requirement] + command.end(True) + for action in self._interruptActions: + action(command) + + def cancelAll(self) -> None: + """Cancels all commands that are currently scheduled.""" + self.cancel(*self._scheduledCommands) + + def isScheduled(self, *commands: Command) -> bool: + """ + Whether the given commands are running. Note that this only works on commands that are directly + scheduled by the scheduler; it will not work on commands inside compositions, as the scheduler + does not see them. + + :param commands: the command to query + :returns: whether the command is currently scheduled + """ + return all(command in self._scheduledCommands for command in commands) + + def requiring(self, subsystem: Subsystem) -> Optional[Command]: + """ + Returns the command currently requiring a given subsystem. None if no command is currently + requiring the subsystem + + :param subsystem: the subsystem to be inquired about + :returns: the command currently requiring the subsystem, or None if no command is currently + scheduled + """ + return self._requirements.get(subsystem) + + def disable(self) -> None: + """Disables the command scheduler.""" + self._disabled = True + + def enable(self) -> None: + """Enables the command scheduler.""" + self._disabled = False + + def onCommandInitialize(self, action: Callable[[Command], Any]) -> None: + """ + Adds an action to perform on the initialization of any command by the scheduler. + + :param action: the action to perform + """ + self._initActions.append(action) + + def onCommandExecute(self, action: Callable[[Command], Any]) -> None: + """ + Adds an action to perform on the execution of any command by the scheduler. + + :param action: the action to perform + """ + self._executeActions.append(action) + + def onCommandInterrupt(self, action: Callable[[Command], Any]) -> None: + """ + Adds an action to perform on the interruption of any command by the scheduler. + + :param action: the action to perform + """ + self._interruptActions.append(action) + + def onCommandFinish(self, action: Callable[[Command], Any]) -> None: + """ + Adds an action to perform on the finishing of any command by the scheduler. + + :param action: the action to perform + """ + self._finishActions.append(action) + + def registerComposedCommands(self, commands: Iterable[Command]) -> None: + """ + Register commands as composed. An exception will be thrown if these commands are scheduled + directly or added to a composition. + + :param commands: the commands to register + @throws IllegalArgumentException if the given commands have already been composed. + """ + self.requireNotComposed(commands) + self._composedCommands.update(commands) + + def clearComposedCommands(self) -> None: + """ + Clears the list of composed commands, allowing all commands to be freely used again. + + WARNING: Using this haphazardly can result in unexpected/undesirable behavior. Do not use + this unless you fully understand what you are doing. + """ + self._composedCommands.clear() + + def removeComposedCommands(self, commands: Iterable[Command]) -> None: + """ + Removes a single command from the list of composed commands, allowing it to be freely used + again. + + WARNING: Using this haphazardly can result in unexpected/undesirable behavior. Do not use + this unless you fully understand what you are doing. + + :param command: the command to remove from the list of grouped commands + """ + self._composedCommands.difference_update(commands) + + def requireNotComposed(self, commands: Iterable[Command]) -> None: + """ + Requires that the specified command hasn't been already added to a composition. + + :param commands: The commands to check + @throws IllegalArgumentException if the given commands have already been composed. + """ + if not self._composedCommands.isdisjoint(commands): + raise IllegalCommandUse( + "Commands that have been composed may not be added to another composition or scheduled individually" + ) + + def isComposed(self, command: Command) -> bool: + """ + Check if the given command has been composed. + + :param command: The command to check + :returns: true if composed + """ + return command in self.getComposedCommands() + + def getComposedCommands(self) -> Set[Command]: + return self._composedCommands diff --git a/rio/ottomanEmpire/bursa/conditionalcommand.py b/rio/ottomanEmpire/bursa/conditionalcommand.py new file mode 100644 index 00000000..b26760de --- /dev/null +++ b/rio/ottomanEmpire/bursa/conditionalcommand.py @@ -0,0 +1,61 @@ +from __future__ import annotations + +from typing import Callable + +from .command import Command +from .commandgroup import * +from .commandscheduler import CommandScheduler + + +class ConditionalCommand(Command): + """ + A command composition that runs one of two commands, depending on the value of the given + condition when this command is initialized. + + The rules for command compositions apply: command instances that are passed to it cannot be + added to any other composition or scheduled individually, and the composition requires all + subsystems its components require. + + This class is provided by the NewCommands VendorDep""" + + selectedCommand: Command + + def __init__( + self, onTrue: Command, onFalse: Command, condition: Callable[[], bool] + ): + """ + Creates a new ConditionalCommand. + + :param onTrue: the command to run if the condition is true + :param onFalse: the command to run if the condition is false + :param condition: the condition to determine which command to run""" + super().__init__() + + CommandScheduler.getInstance().registerComposedCommands([onTrue, onFalse]) + + self.onTrue = onTrue + self.onFalse = onFalse + self.condition = condition + + self.addRequirements(*onTrue.getRequirements()) + self.addRequirements(*onFalse.getRequirements()) + + def initialize(self): + if self.condition(): + self.selectedCommand = self.onTrue + else: + self.selectedCommand = self.onFalse + + self.selectedCommand.initialize() + + def execute(self): + self.selectedCommand.execute() + + def isFinished(self) -> bool: + return self.selectedCommand.isFinished() + + def end(self, interrupted: bool): + self.selectedCommand.end(interrupted) + + def runsWhenDisabled(self) -> bool: + return self.onTrue.runsWhenDisabled() and self.onFalse.runsWhenDisabled() diff --git a/rio/ottomanEmpire/bursa/functionalcommand.py b/rio/ottomanEmpire/bursa/functionalcommand.py new file mode 100644 index 00000000..9498568e --- /dev/null +++ b/rio/ottomanEmpire/bursa/functionalcommand.py @@ -0,0 +1,52 @@ +from __future__ import annotations + +from typing import Any, Callable + +from .command import Command +from .commandgroup import * +from .subsystem import Subsystem + + +class FunctionalCommand(Command): + """ + A command that allows the user to pass in functions for each of the basic command methods through + the constructor. Useful for inline definitions of complex commands - note, however, that if a + command is beyond a certain complexity it is usually better practice to write a proper class for + it than to inline it.""" + + def __init__( + self, + onInit: Callable[[], Any], + onExecute: Callable[[], Any], + onEnd: Callable[[bool], Any], + isFinished: Callable[[], bool], + *requirements: Subsystem, + ): + """ + Creates a new FunctionalCommand. + + :param onInit: the function to run on command initialization + :param onExecute: the function to run on command execution + :param onEnd: the function to run on command end + :param isFinished: the function that determines whether the command has finished + :param requirements: the subsystems required by this command""" + super().__init__() + + self._onInit = onInit + self._onExecute = onExecute + self._onEnd = onEnd + self._isFinished = isFinished + + self.addRequirements(*requirements) + + def initialize(self): + self._onInit() + + def execute(self): + self._onExecute() + + def end(self, interrupted: bool): + self._onEnd(interrupted) + + def isFinished(self) -> bool: + return self._isFinished() diff --git a/rio/ottomanEmpire/bursa/instantcommand.py b/rio/ottomanEmpire/bursa/instantcommand.py new file mode 100644 index 00000000..6d20a2e6 --- /dev/null +++ b/rio/ottomanEmpire/bursa/instantcommand.py @@ -0,0 +1,29 @@ +from __future__ import annotations + +from typing import Callable, Optional + +from .functionalcommand import FunctionalCommand +from .subsystem import Subsystem + + +class InstantCommand(FunctionalCommand): + """ + A Command that runs instantly; it will initialize, execute once, and end on the same iteration of + the scheduler. Users can either pass in a Runnable and a set of requirements, or else subclass + this command if desired.""" + + def __init__( + self, toRun: Optional[Callable[[], None]] = None, *requirements: Subsystem + ): + """ + Creates a new InstantCommand that runs the given Runnable with the given requirements. + + :param toRun: the Runnable to run + :param requirements: the subsystems required by this command""" + super().__init__( + toRun or (lambda: None), + lambda: None, + lambda _: None, + lambda: True, + *requirements, + ) diff --git a/rio/ottomanEmpire/bursa/notifiercommand.py b/rio/ottomanEmpire/bursa/notifiercommand.py new file mode 100644 index 00000000..0a6e57e8 --- /dev/null +++ b/rio/ottomanEmpire/bursa/notifiercommand.py @@ -0,0 +1,41 @@ +from __future__ import annotations + +from typing import Any, Callable + +from wpilib import Notifier + +from .command import Command +from .commandgroup import * +from .subsystem import Subsystem + + +class NotifierCommand(Command): + """ + A command that starts a notifier to run the given runnable periodically in a separate thread. Has + no end condition as-is; either subclass it or use Command#withTimeout(double) or {@link + Command#until(java.util.function.BooleanSupplier)} to give it one. + + WARNING: Do not use this class unless you are confident in your ability to make the executed + code thread-safe. If you do not know what "thread-safe" means, that is a good sign that you + should not use this class.""" + + def __init__( + self, toRun: Callable[[], Any], period: float, *requirements: Subsystem + ): + """ + Creates a new NotifierCommand. + + :param toRun: the runnable for the notifier to run + :param period: the period at which the notifier should run, in seconds + :param requirements: the subsystems required by this command""" + super().__init__() + + self.notifier = Notifier(toRun) + self.period = period + self.addRequirements(*requirements) + + def initialize(self): + self.notifier.startPeriodic(self.period) + + def end(self, interrupted: bool): + self.notifier.stop() diff --git a/rio/ottomanEmpire/bursa/parallelcommandgroup.py b/rio/ottomanEmpire/bursa/parallelcommandgroup.py new file mode 100644 index 00000000..e3ad01dd --- /dev/null +++ b/rio/ottomanEmpire/bursa/parallelcommandgroup.py @@ -0,0 +1,87 @@ +from __future__ import annotations + +from typing import Dict + +from commands2.command import Command, InterruptionBehavior + +from .command import Command, InterruptionBehavior +from .commandgroup import * +from .commandscheduler import CommandScheduler +from .util import flatten_args_commands + + +class ParallelCommandGroup(CommandGroup): + """ + A command composition that runs a set of commands in parallel, ending when the last command ends. + + The rules for command compositions apply: command instances that are passed to it cannot be + added to any other composition or scheduled individually, and the composition requires all + subsystems its components require.""" + + def __init__(self, *commands: Command): + """ + Creates a new ParallelCommandGroup. The given commands will be executed simultaneously. The + command composition will finish when the last command finishes. If the composition is + interrupted, only the commands that are still running will be interrupted. + + :param commands: the commands to include in this composition.""" + super().__init__() + self._commands: Dict[Command, bool] = {} + self._runsWhenDisabled = True + self._interruptBehavior = InterruptionBehavior.kCancelIncoming + self.addCommands(*commands) + + def addCommands(self, *commands: Command): + commands = flatten_args_commands(commands) + if True in self._commands.values(): + raise IllegalCommandUse( + "Commands cannot be added to a composition while it is running" + ) + + CommandScheduler.getInstance().registerComposedCommands(commands) + + for command in commands: + if not command.getRequirements().isdisjoint(self.requirements): + raise IllegalCommandUse( + "Multiple comands in a parallel composition cannot require the same subsystems." + ) + + self._commands[command] = False + self.requirements.update(command.getRequirements()) + self._runsWhenDisabled = ( + self._runsWhenDisabled and command.runsWhenDisabled() + ) + + if command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf: + self._interruptBehavior = InterruptionBehavior.kCancelSelf + + def initialize(self): + for command in self._commands: + command.initialize() + self._commands[command] = True + + def execute(self): + for command, isRunning in self._commands.items(): + if not isRunning: + continue + command.execute() + if command.isFinished(): + command.end(False) + self._commands[command] = False + + def end(self, interrupted: bool): + if interrupted: + for command, isRunning in self._commands.items(): + if not isRunning: + continue + command.end(True) + self._commands[command] = False + + def isFinished(self) -> bool: + return True not in self._commands.values() + + def runsWhenDisabled(self) -> bool: + return self._runsWhenDisabled + + def getInterruptionBehavior(self) -> InterruptionBehavior: + return self._interruptBehavior diff --git a/rio/ottomanEmpire/bursa/paralleldeadlinegroup.py b/rio/ottomanEmpire/bursa/paralleldeadlinegroup.py new file mode 100644 index 00000000..eb064f49 --- /dev/null +++ b/rio/ottomanEmpire/bursa/paralleldeadlinegroup.py @@ -0,0 +1,98 @@ +from __future__ import annotations + +from typing import Dict + +from commands2.command import Command, InterruptionBehavior + +from .command import Command, InterruptionBehavior +from .commandgroup import * +from .commandscheduler import CommandScheduler +from .util import flatten_args_commands + + +class ParallelDeadlineGroup(CommandGroup): + """ + A command composition that runs one of a selection of commands, either using a selector and a key + to command mapping, or a supplier that returns the command directly at runtime. + + The rules for command compositions apply: command instances that are passed to it cannot be + added to any other composition or scheduled individually, and the composition requires all + subsystems its components require.""" + + def __init__(self, deadline: Command, *commands: Command): + """ + Creates a new SelectCommand. + + :param commands: the map of commands to choose from + :param selector: the selector to determine which command to run""" + super().__init__() + self._commands: Dict[Command, bool] = {} + self._runsWhenDisabled = True + self._finished = True + self._deadline = deadline + self._interruptBehavior = InterruptionBehavior.kCancelIncoming + self.addCommands(*commands) + if deadline not in self._commands: + self.addCommands(deadline) + + def setDeadline(self, deadline: Command): + if deadline not in self._commands: + self.addCommands(deadline) + self._deadline = deadline + + def addCommands(self, *commands: Command): + commands = flatten_args_commands(commands) + if not self._finished: + raise IllegalCommandUse( + "Commands cannot be added to a composition while it is running" + ) + + CommandScheduler.getInstance().registerComposedCommands(commands) + + for command in commands: + if not command.getRequirements().isdisjoint(self.requirements): + raise IllegalCommandUse( + "Multiple comands in a parallel composition cannot require the same subsystems." + ) + + self._commands[command] = False + self.requirements.update(command.getRequirements()) + self._runsWhenDisabled = ( + self._runsWhenDisabled and command.runsWhenDisabled() + ) + + if command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf: + self._interruptBehavior = InterruptionBehavior.kCancelSelf + + def initialize(self): + for command in self._commands: + command.initialize() + self._commands[command] = True + self._finished = False + + def execute(self): + for command, isRunning in self._commands.items(): + if not isRunning: + continue + command.execute() + if command.isFinished(): + command.end(False) + self._commands[command] = False + if command == self._deadline: + self._finished = True + + def end(self, interrupted: bool): + for command, isRunning in self._commands.items(): + if not isRunning: + continue + command.end(True) + self._commands[command] = False + + def isFinished(self) -> bool: + return self._finished + + def runsWhenDisabled(self) -> bool: + return self._runsWhenDisabled + + def getInterruptionBehavior(self) -> InterruptionBehavior: + return self._interruptBehavior diff --git a/rio/ottomanEmpire/bursa/parallelracegroup.py b/rio/ottomanEmpire/bursa/parallelracegroup.py new file mode 100644 index 00000000..3b66c14a --- /dev/null +++ b/rio/ottomanEmpire/bursa/parallelracegroup.py @@ -0,0 +1,82 @@ +from __future__ import annotations + +from typing import Set + +from commands2.command import Command, InterruptionBehavior + +from .command import Command, InterruptionBehavior +from .commandgroup import * +from .commandscheduler import CommandScheduler +from .util import flatten_args_commands + + +class ParallelRaceGroup(CommandGroup): + """ + A composition that runs a set of commands in parallel, ending when any one of the commands ends + and interrupting all the others. + + The rules for command compositions apply: command instances that are passed to it cannot be + added to any other composition or scheduled individually, and the composition requires all + subsystems its components require.""" + + def __init__(self, *commands: Command): + """ + Creates a new ParallelCommandRace. The given commands will be executed simultaneously, and will + "race to the finish" - the first command to finish ends the entire command, with all other + commands being interrupted. + + :param commands: the commands to include in this composition.""" + super().__init__() + self._commands: Set[Command] = set() + self._runsWhenDisabled = True + self._interruptBehavior = InterruptionBehavior.kCancelIncoming + self._finished = True + self.addCommands(*commands) + + def addCommands(self, *commands: Command): + commands = flatten_args_commands(commands) + if not self._finished: + raise IllegalCommandUse( + "Commands cannot be added to a composition while it is running" + ) + + CommandScheduler.getInstance().registerComposedCommands(commands) + + for command in commands: + if not command.getRequirements().isdisjoint(self.requirements): + raise IllegalCommandUse( + "Multiple comands in a parallel composition cannot require the same subsystems." + ) + + self._commands.add(command) + self.requirements.update(command.getRequirements()) + self._runsWhenDisabled = ( + self._runsWhenDisabled and command.runsWhenDisabled() + ) + + if command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf: + self._interruptBehavior = InterruptionBehavior.kCancelSelf + + def initialize(self): + self._finished = False + for command in self._commands: + command.initialize() + + def execute(self): + for command in self._commands: + command.execute() + if command.isFinished(): + self._finished = True + + def end(self, interrupted: bool): + for command in self._commands: + command.end(not command.isFinished()) + + def isFinished(self) -> bool: + return self._finished + + def runsWhenDisabled(self) -> bool: + return self._runsWhenDisabled + + def getInterruptionBehavior(self) -> InterruptionBehavior: + return self._interruptBehavior diff --git a/rio/ottomanEmpire/bursa/perpetualcommand.py b/rio/ottomanEmpire/bursa/perpetualcommand.py new file mode 100644 index 00000000..1e535e34 --- /dev/null +++ b/rio/ottomanEmpire/bursa/perpetualcommand.py @@ -0,0 +1,46 @@ +from __future__ import annotations + +from .command import Command +from .commandgroup import * +from .commandscheduler import CommandScheduler + + +class PerpetualCommand(Command): + """ + A command that runs another command in perpetuity, ignoring that command's end conditions. While + this class does not extend CommandGroupBase, it is still considered a composition, as it + allows one to compose another command within it; the command instances that are passed to it + cannot be added to any other groups, or scheduled individually. + + As a rule, CommandGroups require the union of the requirements of their component commands. + + This class is provided by the NewCommands VendorDep + + @deprecated PerpetualCommand violates the assumption that execute() doesn't get called after + isFinished() returns true -- an assumption that should be valid. This was unsafe/undefined + behavior from the start, and RepeatCommand provides an easy way to achieve similar end + results with slightly different (and safe) semantics.""" + + def __init__(self, command: Command): + """ + Creates a new PerpetualCommand. Will run another command in perpetuity, ignoring that command's + end conditions, unless this command itself is interrupted. + + :param command: the command to run perpetually""" + super().__init__() + + CommandScheduler.getInstance().registerComposedCommands([command]) + self._command = command + self.addRequirements(*command.getRequirements()) + + def initialize(self): + self._command.initialize() + + def execute(self): + self._command.execute + + def end(self, interrupted: bool): + self._command.end(interrupted) + + def runsWhenDisabled(self) -> bool: + return self._command.runsWhenDisabled() diff --git a/rio/ottomanEmpire/bursa/pidcommand.py b/rio/ottomanEmpire/bursa/pidcommand.py new file mode 100644 index 00000000..339764c0 --- /dev/null +++ b/rio/ottomanEmpire/bursa/pidcommand.py @@ -0,0 +1,74 @@ +# 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. +from __future__ import annotations + +from typing import Any, Callable, Union + +from .command import Command +from .subsystem import Subsystem + +from wpimath.controller import PIDController + + +class PIDCommand(Command): + """ + A command that controls an output with a PIDController. Runs forever by default - to add + exit conditions and/or other behavior, subclass this class. The controller calculation and output + are performed synchronously in the command's execute() method. + """ + + def __init__( + self, + controller: PIDController, + measurementSource: Callable[[], float], + setpoint: Union[Callable[[], float], float, int], + useOutput: Callable[[float], Any], + *requirements: Subsystem, + ): + """ + Creates a new PIDCommand, which controls the given output with a PIDController. + + :param controller: the controller that controls the output. + :param measurementSource: the measurement of the process variable + :param setpoint: the controller's setpoint (either a function that returns a) + number or a number + :param useOutput: the controller's output + :param requirements: the subsystems required by this command + """ + super().__init__() + + self._controller = controller + self._useOutput = useOutput + self._measurement = measurementSource + + if isinstance(setpoint, (float, int)): + setpoint = float(setpoint) + self._setpoint = lambda: setpoint + elif callable(setpoint): + self._setpoint = setpoint + else: + raise ValueError( + f"invalid setpoint (must be callable or number; got {type(setpoint)})" + ) + + self.addRequirements(*requirements) + + def initialize(self): + self._controller.reset() + + def execute(self): + self._useOutput( + self._controller.calculate(self._measurement(), self._setpoint()) + ) + + def end(self, interrupted): + self._useOutput(0) + + def getController(self): + """ + Returns the PIDController used by the command. + + :return: The PIDController + """ + return self._controller diff --git a/rio/ottomanEmpire/bursa/pidsubsystem.py b/rio/ottomanEmpire/bursa/pidsubsystem.py new file mode 100644 index 00000000..c63469a3 --- /dev/null +++ b/rio/ottomanEmpire/bursa/pidsubsystem.py @@ -0,0 +1,99 @@ +# 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. +from __future__ import annotations + +from wpimath.controller import PIDController + +from .subsystem import Subsystem + + +class PIDSubsystem(Subsystem): + """ + A subsystem that uses a {@link PIDController} to control an output. The + controller is run synchronously from the subsystem's periodic() method. + """ + + def __init__(self, controller: PIDController, initial_position: float = 0.0): + """ + Creates a new PIDSubsystem. + + :param controller: The PIDController to use. + :param initial_position: The initial setpoint of the subsystem. + """ + super().__init__() + + self._controller = controller + self.setSetpoint(initial_position) + self.addChild("PID Controller", self._controller) + self._enabled = False + + def periodic(self): + """ + Executes the PID control logic during each periodic update. + + This method is called synchronously from the subsystem's periodic() method. + """ + if self._enabled: + self.useOutput( + self._controller.calculate(self.getMeasurement()), self.getSetpoint() + ) + + def getController(self) -> PIDController: + """ + Returns the PIDController used by the subsystem. + + :return: The PIDController. + """ + return self._controller + + def setSetpoint(self, setpoint: float): + """ + Sets the setpoint for the subsystem. + + :param setpoint: The setpoint for the subsystem. + """ + self._controller.setSetpoint(setpoint) + + def getSetpoint(self) -> float: + """ + Returns the current setpoint of the subsystem. + + :return: The current setpoint. + """ + return self._controller.getSetpoint() + + def useOutput(self, output: float, setpoint: float): + """ + Uses the output from the PIDController. + + :param output: The output of the PIDController. + :param setpoint: The setpoint of the PIDController (for feedforward). + """ + raise NotImplementedError("Subclasses must implement this method") + + def getMeasurement(self) -> float: + """ + Returns the measurement of the process variable used by the PIDController. + + :return: The measurement of the process variable. + """ + raise NotImplementedError("Subclasses must implement this method") + + def enable(self): + """Enables the PID control. Resets the controller.""" + self._enabled = True + self._controller.reset() + + def disable(self): + """Disables the PID control. Sets output to zero.""" + self._enabled = False + self.useOutput(0, 0) + + def isEnabled(self) -> bool: + """ + Returns whether the controller is enabled. + + :return: Whether the controller is enabled. + """ + return self._enabled diff --git a/rio/ottomanEmpire/bursa/printcommand.py b/rio/ottomanEmpire/bursa/printcommand.py new file mode 100644 index 00000000..082a8c7f --- /dev/null +++ b/rio/ottomanEmpire/bursa/printcommand.py @@ -0,0 +1,18 @@ +from __future__ import annotations + +from .instantcommand import InstantCommand + + +class PrintCommand(InstantCommand): + """ + A command that prints a string when initialized.""" + + def __init__(self, message: str): + """ + Creates a new a PrintCommand. + + :param message: the message to print""" + super().__init__(lambda: print(message)) + + def runsWhenDisabled(self) -> bool: + return True diff --git a/rio/ottomanEmpire/bursa/proxycommand.py b/rio/ottomanEmpire/bursa/proxycommand.py new file mode 100644 index 00000000..630e7b52 --- /dev/null +++ b/rio/ottomanEmpire/bursa/proxycommand.py @@ -0,0 +1,90 @@ +from __future__ import annotations + +from typing import Callable, overload + +from .command import Command +from .commandgroup import * +from .util import format_args_kwargs + + +class ProxyCommand(Command): + """ + Schedules the given command when this command is initialized, and ends when it ends. Useful for + forking off from CommandGroups. If this command is interrupted, it will cancel the command. + """ + + _supplier: Callable[[], Command] + + @overload + def __init__(self, supplier: Callable[[], Command]): + """ + Creates a new ProxyCommand that schedules the supplied command when initialized, and ends when + it is no longer scheduled. Useful for lazily creating commands at runtime. + + :param supplier: the command supplier""" + ... + + @overload + def __init__(self, command: Command): + """ + Creates a new ProxyCommand that schedules the given command when initialized, and ends when it + is no longer scheduled. + + :param command: the command to run by proxy""" + ... + + def __init__(self, *args, **kwargs): + super().__init__() + + def init_supplier(supplier: Callable[[], Command]): + self._supplier = supplier + + def init_command(command: Command): + self._supplier = lambda: command + + num_args = len(args) + len(kwargs) + + if num_args == 1 and len(kwargs) == 1: + if "supplier" in kwargs: + return init_supplier(kwargs["supplier"]) + elif "command" in kwargs: + return init_command(kwargs["command"]) + elif num_args == 1 and len(args) == 1: + if isinstance(args[0], Command): + return init_command(args[0]) + elif callable(args[0]): + return init_supplier(args[0]) + + raise TypeError( + f""" +TypeError: ProxyCommand(): incompatible function arguments. The following argument types are supported: + 1. (self: ProxyCommand, supplier: () -> Command) + 2. (self: ProxyCommand, command: Command) + +Invoked with: {format_args_kwargs(self, *args, **kwargs)} +""" + ) + + def initialize(self): + self._command = self._supplier() + self._command.schedule() + + def end(self, interrupted: bool): + if interrupted and self._command is not None: + self._command.cancel() + self._command = None + + def execute(self): + pass + + def isFinished(self) -> bool: + return self._command is None or not self._command.isScheduled() + + def runsWhenDisabled(self) -> bool: + """ + Whether the given command should run when the robot is disabled. Override to return true if the + command should run when disabled. + + :returns: true. Otherwise, this proxy would cancel commands that do run when disabled. + """ + return True diff --git a/rio/ottomanEmpire/bursa/proxyschedulecommand.py b/rio/ottomanEmpire/bursa/proxyschedulecommand.py new file mode 100644 index 00000000..23b9d8a4 --- /dev/null +++ b/rio/ottomanEmpire/bursa/proxyschedulecommand.py @@ -0,0 +1,41 @@ +from __future__ import annotations + +from .command import Command +from .commandgroup import * + + +class ProxyScheduleCommand(Command): + """ + Schedules the given commands when this command is initialized, and ends when all the commands are + no longer scheduled. Useful for forking off from CommandGroups. If this command is interrupted, + it will cancel all the commands. + """ + + def __init__(self, *toSchedule: Command): + """ + Creates a new ProxyScheduleCommand that schedules the given commands when initialized, and ends + when they are all no longer scheduled. + + :param toSchedule: the commands to schedule + @deprecated Replace with ProxyCommand, composing multiple of them in a {@link + ParallelRaceGroup} if needed.""" + super().__init__() + self.toSchedule = set(toSchedule) + self._finished = False + + def initialize(self): + for command in self.toSchedule: + command.schedule() + + def end(self, interrupted: bool): + if interrupted: + for command in self.toSchedule: + command.cancel() + + def execute(self): + self._finished = True + for command in self.toSchedule: + self._finished = self._finished and not command.isScheduled() + + def isFinished(self) -> bool: + return self._finished diff --git a/rio/ottomanEmpire/bursa/repeatcommand.py b/rio/ottomanEmpire/bursa/repeatcommand.py new file mode 100644 index 00000000..dc7dc991 --- /dev/null +++ b/rio/ottomanEmpire/bursa/repeatcommand.py @@ -0,0 +1,55 @@ +from __future__ import annotations + +from commands2.command import InterruptionBehavior + +from .command import Command +from .commandgroup import * + + +class RepeatCommand(Command): + """ + A command that runs another command repeatedly, restarting it when it ends, until this command is + interrupted. Command instances that are passed to it cannot be added to any other groups, or + scheduled individually. + + The rules for command compositions apply: command instances that are passed to it cannot be + added to any other composition or scheduled individually,and the composition requires all + subsystems its components require.""" + + def __init__(self, command: Command): + """ + Creates a new RepeatCommand. Will run another command repeatedly, restarting it whenever it + ends, until this command is interrupted. + + :param command: the command to run repeatedly""" + super().__init__() + self._command = command + + def initialize(self): + self._ended = False + self._command.initialize() + + def execute(self): + if self._ended: + self._ended = False + self._command.initialize() + + self._command.execute() + + if self._command.isFinished(): + self._command.end(False) + self._ended = True + + def isFinished(self) -> bool: + return False + + def end(self, interrupted: bool): + if not self._ended: + self._command.end(interrupted) + self._ended = True + + def runsWhenDisabled(self) -> bool: + return self._command.runsWhenDisabled() + + def getInterruptionBehavior(self) -> InterruptionBehavior: + return self._command.getInterruptionBehavior() diff --git a/rio/ottomanEmpire/bursa/runcommand.py b/rio/ottomanEmpire/bursa/runcommand.py new file mode 100644 index 00000000..231567f5 --- /dev/null +++ b/rio/ottomanEmpire/bursa/runcommand.py @@ -0,0 +1,25 @@ +from __future__ import annotations + +from typing import Any, Callable + +from .commandgroup import * +from .functionalcommand import FunctionalCommand +from .subsystem import Subsystem + + +class RunCommand(FunctionalCommand): + """ + A command that runs a Runnable continuously. Has no end condition as-is; either subclass it or + use Command#withTimeout(double) or Command#until(BooleanSupplier) to give it one. + If you only wish to execute a Runnable once, use InstantCommand.""" + + def __init__(self, toRun: Callable[[], Any], *requirements: Subsystem): + """ + Creates a new RunCommand. The Runnable will be run continuously until the command ends. Does + not run when disabled. + + :param toRun: the Runnable to run + :param requirements: the subsystems to require""" + super().__init__( + lambda: None, toRun, lambda interrupted: None, lambda: False, *requirements + ) diff --git a/rio/ottomanEmpire/bursa/schedulecommand.py b/rio/ottomanEmpire/bursa/schedulecommand.py new file mode 100644 index 00000000..1d987e6d --- /dev/null +++ b/rio/ottomanEmpire/bursa/schedulecommand.py @@ -0,0 +1,30 @@ +from __future__ import annotations + +from .command import Command +from .commandgroup import * + + +class ScheduleCommand(Command): + """ + Schedules the given commands when this command is initialized. Useful for forking off from + CommandGroups. Note that if run from a composition, the composition will not know about the + status of the scheduled commands, and will treat this command as finishing instantly. + """ + + def __init__(self, *commands: Command): + """ + Creates a new ScheduleCommand that schedules the given commands when initialized. + + :param toSchedule: the commands to schedule""" + super().__init__() + self.toSchedule = set(commands) + + def initialize(self): + for command in self.toSchedule: + command.schedule() + + def isFinished(self) -> bool: + return True + + def runsWhenDisabled(self) -> bool: + return True diff --git a/rio/ottomanEmpire/bursa/selectcommand.py b/rio/ottomanEmpire/bursa/selectcommand.py new file mode 100644 index 00000000..56710dcd --- /dev/null +++ b/rio/ottomanEmpire/bursa/selectcommand.py @@ -0,0 +1,71 @@ +from __future__ import annotations + +from typing import Callable, Dict, Hashable + +from commands2.command import InterruptionBehavior + +from .command import Command, InterruptionBehavior +from .commandgroup import * +from .commandscheduler import CommandScheduler +from .printcommand import PrintCommand + + +class SelectCommand(Command): + """ + A command composition that runs one of a selection of commands, either using a selector and a key + to command mapping, or a supplier that returns the command directly at runtime. + + The rules for command compositions apply: command instances that are passed to it cannot be + added to any other composition or scheduled individually, and the composition requires all + subsystems its components require.""" + + def __init__( + self, + commands: Dict[Hashable, Command], + selector: Callable[[], Hashable], + ): + """ + Creates a new SelectCommand. + + :param commands: the map of commands to choose from + :param selector: the selector to determine which command to run""" + super().__init__() + + self._commands = commands + self._selector = selector + + CommandScheduler.getInstance().registerComposedCommands(commands.values()) + + self._runsWhenDisabled = True + self._interruptBehavior = InterruptionBehavior.kCancelIncoming + for command in commands.values(): + self.addRequirements(*command.getRequirements()) + self._runsWhenDisabled = ( + self._runsWhenDisabled and command.runsWhenDisabled() + ) + if command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf: + self._interruptBehavior = InterruptionBehavior.kCancelSelf + + def initialize(self): + if self._selector() not in self._commands: + self._selectedCommand = PrintCommand( + "SelectCommand selector value does not correspond to any command!" + ) + return + self._selectedCommand = self._commands[self._selector()] + self._selectedCommand.initialize() + + def execute(self): + self._selectedCommand.execute() + + def end(self, interrupted: bool): + self._selectedCommand.end(interrupted) + + def isFinished(self) -> bool: + return self._selectedCommand.isFinished() + + def runsWhenDisabled(self) -> bool: + return self._runsWhenDisabled + + def getInterruptionBehavior(self) -> InterruptionBehavior: + return self._interruptBehavior diff --git a/rio/ottomanEmpire/bursa/sequentialcommandgroup.py b/rio/ottomanEmpire/bursa/sequentialcommandgroup.py new file mode 100644 index 00000000..ef6984d4 --- /dev/null +++ b/rio/ottomanEmpire/bursa/sequentialcommandgroup.py @@ -0,0 +1,90 @@ +from __future__ import annotations + +from typing import List + +from commands2.command import Command, InterruptionBehavior + +from .command import Command, InterruptionBehavior +from .commandgroup import * +from .commandscheduler import CommandScheduler +from .util import flatten_args_commands + + +class SequentialCommandGroup(CommandGroup): + """ + A command composition that runs a list of commands in sequence. + + The rules for command compositions apply: command instances that are passed to it cannot be + added to any other composition or scheduled individually, and the composition requires all + subsystems its components require.""" + + def __init__(self, *commands: Command): + """ + Creates a new SequentialCommandGroup. The given commands will be run sequentially, with the + composition finishing when the last command finishes. + + :param commands: the commands to include in this composition.""" + super().__init__() + self._commands: List[Command] = [] + self._runsWhenDisabled = True + self._interruptBehavior = InterruptionBehavior.kCancelIncoming + self._currentCommandIndex = -1 + self.addCommands(*commands) + + def addCommands(self, *commands: Command): + commands = flatten_args_commands(commands) + if self._currentCommandIndex != -1: + raise IllegalCommandUse( + "Commands cannot be added to a composition while it is running" + ) + + CommandScheduler.getInstance().registerComposedCommands(commands) + + for command in commands: + self._commands.append(command) + self.requirements.update(command.getRequirements()) + self._runsWhenDisabled = ( + self._runsWhenDisabled and command.runsWhenDisabled() + ) + if command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf: + self._interruptBehavior = InterruptionBehavior.kCancelSelf + + def initialize(self): + self._currentCommandIndex = 0 + if self._commands: + self._commands[0].initialize() + + def execute(self): + if not self._commands: + return + + currentCommand = self._commands[self._currentCommandIndex] + + currentCommand.execute() + if currentCommand.isFinished(): + currentCommand.end(False) + self._currentCommandIndex += 1 + if self._currentCommandIndex < len(self._commands): + self._commands[self._currentCommandIndex].initialize() + + def end(self, interrupted: bool): + if not interrupted: + return + if not self._commands: + return + if not self._currentCommandIndex > -1: + return + if not self._currentCommandIndex < len(self._commands): + return + + self._commands[self._currentCommandIndex].end(True) + self._currentCommandIndex = -1 + + def isFinished(self) -> bool: + return self._currentCommandIndex == len(self._commands) + + def runsWhenDisabled(self) -> bool: + return self._runsWhenDisabled + + def getInterruptionBehavior(self) -> InterruptionBehavior: + return self._interruptBehavior diff --git a/rio/ottomanEmpire/bursa/startendcommand.py b/rio/ottomanEmpire/bursa/startendcommand.py new file mode 100644 index 00000000..1ea896af --- /dev/null +++ b/rio/ottomanEmpire/bursa/startendcommand.py @@ -0,0 +1,33 @@ +from __future__ import annotations + +from typing import Any, Callable + +from .commandgroup import * +from .functionalcommand import FunctionalCommand +from .subsystem import Subsystem + + +class StartEndCommand(FunctionalCommand): + """ + A command that runs a given runnable when it is initialized, and another runnable when it ends. + Useful for running and then stopping a motor, or extending and then retracting a solenoid. Has no + end condition as-is; either subclass it or use Command#withTimeout(double) or {@link + Command#until(java.util.function.BooleanSupplier)} to give it one. + """ + + def __init__( + self, + onInit: Callable[[], Any], + onEnd: Callable[[], Any], + *requirements: Subsystem, + ): + """ + Creates a new StartEndCommand. Will run the given runnables when the command starts and when it + ends. + + :param onInit: the Runnable to run on command init + :param onEnd: the Runnable to run on command end + :param requirements: the subsystems required by this command""" + super().__init__( + onInit, lambda: None, lambda _: onEnd(), lambda: False, *requirements + ) diff --git a/rio/ottomanEmpire/bursa/subsystem.py b/rio/ottomanEmpire/bursa/subsystem.py new file mode 100644 index 00000000..a3ba81ca --- /dev/null +++ b/rio/ottomanEmpire/bursa/subsystem.py @@ -0,0 +1,180 @@ +# Don't import stuff from the package here, it'll cause a circular import + +from __future__ import annotations + +from typing import TYPE_CHECKING, Callable, Optional + +if TYPE_CHECKING: + from .command import Command + from .commandscheduler import CommandScheduler + +from wpiutil import Sendable, SendableBuilder, SendableRegistry + + +class Subsystem(Sendable): + """ + A robot subsystem. Subsystems are the basic unit of robot organization in the Command-based + framework; they encapsulate low-level hardware objects (motor controllers, sensors, etc.) and + provide methods through which they can be used by Commands. Subsystems are used by the + CommandScheduler's resource management system to ensure multiple robot actions are not + "fighting" over the same hardware; Commands that use a subsystem should include that subsystem in + their Command#getRequirements() method, and resources used within a subsystem should + generally remain encapsulated and not be shared by other parts of the robot. + + Subsystems must be registered with the scheduler with the {@link + CommandScheduler#registerSubsystem(Subsystem...)} method in order for the {@link + Subsystem#periodic()} method to be called. It is recommended that this method be called from the + constructor of users' Subsystem implementations. The SubsystemBase class offers a simple + base for user implementations that handles this. + """ + + def __new__(cls, *arg, **kwargs) -> "Subsystem": + instance = super().__new__(cls) + super().__init__(instance) + SendableRegistry.addLW(instance, cls.__name__, cls.__name__) + # add to the scheduler + from .commandscheduler import CommandScheduler + + CommandScheduler.getInstance().registerSubsystem(instance) + return instance + + def __init__(self) -> None: + pass + + def periodic(self) -> None: + """ + This method is called periodically by the CommandScheduler. Useful for updating + subsystem-specific state that you don't want to offload to a Command. Teams should try + to be consistent within their own codebases about which responsibilities will be handled by + Commands, and which will be handled here.""" + pass + + def simulationPeriodic(self) -> None: + """ + This method is called periodically by the CommandScheduler. Useful for updating + subsystem-specific state that needs to be maintained for simulations, such as for updating + edu.wpi.first.wpilibj.simulation classes and setting simulated sensor readings. + """ + pass + + def setDefaultCommand(self, command: Command) -> None: + """ + Sets the default Command of the subsystem. The default command will be automatically + scheduled when no other commands are scheduled that require the subsystem. Default commands + should generally not end on their own, i.e. their Command#isFinished() method should + always return false. Will automatically register this subsystem with the {@link + CommandScheduler}. + + :param defaultCommand: the default command to associate with this subsystem""" + from .commandscheduler import CommandScheduler + + CommandScheduler.getInstance().setDefaultCommand(self, command) + + def removeDefaultCommand(self) -> None: + """ + Removes the default command for the subsystem. This will not cancel the default command if it + is currently running.""" + CommandScheduler.getInstance().removeDefaultCommand(self) + + def getDefaultCommand(self) -> Optional[Command]: + """ + Gets the default command for this subsystem. Returns null if no default command is currently + associated with the subsystem. + + :returns: the default command associated with this subsystem""" + from .commandscheduler import CommandScheduler + + return CommandScheduler.getInstance().getDefaultCommand(self) + + def getCurrentCommand(self) -> Optional[Command]: + """ + Returns the command currently running on this subsystem. Returns null if no command is + currently scheduled that requires this subsystem. + + :returns: the scheduled command currently requiring this subsystem""" + from .commandscheduler import CommandScheduler + + return CommandScheduler.getInstance().requiring(self) + + def runOnce(self, action: Callable[[], None]) -> Command: + """ + Constructs a command that runs an action once and finishes. Requires this subsystem. + + :param action: the action to run + :return: the command""" + from .cmd import runOnce + + return runOnce(action, self) + + def run(self, action: Callable[[], None]) -> Command: + """ + Constructs a command that runs an action every iteration until interrupted. Requires this + subsystem. + + :param action: the action to run + :returns: the command""" + from .cmd import run + + return run(action, self) + + def startEnd(self, start: Callable[[], None], end: Callable[[], None]) -> Command: + """ + Constructs a command that runs an action once and another action when the command is + interrupted. Requires this subsystem. + + :param start: the action to run on start + :param end: the action to run on interrupt + :returns: the command""" + from .cmd import startEnd + + return startEnd(start, end, self) + + def runEnd(self, run: Callable[[], None], end: Callable[[], None]) -> Command: + """ + Constructs a command that runs an action every iteration until interrupted, and then runs a + second action. Requires this subsystem. + + :param run: the action to run every iteration + :param end: the action to run on interrupt + :returns: the command""" + from .cmd import runEnd + + return runEnd(run, end, self) + + def getName(self) -> str: + return SendableRegistry.getName(self) + + def setName(self, name: str) -> None: + SendableRegistry.setName(self, name) + + def getSubsystem(self) -> str: + return SendableRegistry.getSubsystem(self) + + def addChild(self, name: str, child: Sendable) -> None: + SendableRegistry.addLW(child, self.getSubsystem(), name) + + def initSendable(self, builder: SendableBuilder) -> None: + builder.setSmartDashboardType("Subsystem") + + builder.addBooleanProperty( + ".hasDefault", lambda: self.getDefaultCommand() is not None, lambda _: None + ) + + def get_default(): + command = self.getDefaultCommand() + if command is not None: + return command.getName() + return "none" + + builder.addStringProperty(".default", get_default, lambda _: None) + builder.addBooleanProperty( + ".hasCommand", lambda: self.getCurrentCommand() is not None, lambda _: None + ) + + def get_current(): + command = self.getCurrentCommand() + if command is not None: + return command.getName() + return "none" + + builder.addStringProperty(".command", get_current, lambda _: None) diff --git a/rio/ottomanEmpire/bursa/swervecontrollercommand.py b/rio/ottomanEmpire/bursa/swervecontrollercommand.py new file mode 100644 index 00000000..10cf15d0 --- /dev/null +++ b/rio/ottomanEmpire/bursa/swervecontrollercommand.py @@ -0,0 +1,293 @@ +# 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. +from __future__ import annotations +from typing import Callable, Optional, Union, Tuple +from typing_extensions import overload + +# from overtake import overtake +from wpimath.controller import ( + HolonomicDriveController, + PIDController, + ProfiledPIDControllerRadians, +) +from wpimath.geometry import Pose2d, Rotation2d +from wpimath.kinematics import ( + SwerveDrive2Kinematics, + SwerveDrive3Kinematics, + SwerveDrive4Kinematics, + SwerveDrive6Kinematics, + SwerveModuleState, +) +from wpimath.trajectory import Trajectory +from wpilib import Timer + +from .command import Command +from .subsystem import Subsystem + + +class SwerveControllerCommand(Command): + """ + A command that uses two PID controllers (PIDController) and a ProfiledPIDController + (ProfiledPIDController) to follow a trajectory (Trajectory) with a swerve drive. + + This command outputs the raw desired Swerve Module States (SwerveModuleState) in an + array. The desired wheel and module rotation velocities should be taken from those and used in + velocity PIDs. + + The robot angle controller does not follow the angle given by the trajectory but rather goes + to the angle given in the final state of the trajectory. + """ + + @overload + def __init__( + self, + trajectory: Trajectory, + pose: Callable[[], Pose2d], + kinematics: Union[ + SwerveDrive2Kinematics, + SwerveDrive3Kinematics, + SwerveDrive4Kinematics, + SwerveDrive6Kinematics, + ], + xController: PIDController, + yController: PIDController, + thetaController: ProfiledPIDControllerRadians, + desiredRotation: Callable[[], Rotation2d], + outputModuleStates: Callable[[SwerveModuleState], None], + requirements: Tuple[Subsystem], + ) -> None: + """ + Constructs a new SwerveControllerCommand that when executed will follow the + provided trajectory. This command will not return output voltages but + rather raw module states from the position controllers which need to be put + into a velocity PID. + + Note: The controllers will *not* set the outputVolts to zero upon + completion of the path- this is left to the user, since it is not + appropriate for paths with nonstationary endstates. + :param trajectory: The trajectory to follow. + :param pose: A function that supplies the robot pose - use one of the odometry classes to + provide this. + :param kinematics: The kinematics for the robot drivetrain. Can be kinematics for 2/3/4/6 + SwerveKinematics. + :param outputModuleStates: The raw output module states from the position controllers. + :param requirements: The subsystems to require. + :param xController: The Trajectory Tracker PID controller + for the robot's x position. + :param yController: The Trajectory Tracker PID controller + for the robot's y position. + :param thetaController: The Trajectory Tracker PID controller + for angle for the robot. + :param desiredRotation: The angle that the drivetrain should be + facing. This is sampled at each time step. + """ + super().__init__() + self._trajectory = trajectory + self._pose = pose + self._kinematics = kinematics + self._outputModuleStates = outputModuleStates + self._controller = HolonomicDriveController( + xController, yController, thetaController + ) + self._desiredRotation = desiredRotation + self._timer = Timer() + self.addRequirements(requirements) # type: ignore + + @overload + def __init__( + self, + trajectory: Trajectory, + pose: Callable[[], Pose2d], + kinematics: Union[ + SwerveDrive2Kinematics, + SwerveDrive3Kinematics, + SwerveDrive4Kinematics, + SwerveDrive6Kinematics, + ], + xController: PIDController, + yController: PIDController, + thetaController: ProfiledPIDControllerRadians, + outputModuleStates: Callable[[SwerveModuleState], None], + requirements: Tuple[Subsystem], + ) -> None: + """ + Constructs a new SwerveControllerCommand that when executed will follow the + provided trajectory. This command will not return output voltages but + rather raw module states from the position controllers which need to be put + into a velocity PID. + + Note: The controllers will *not* set the outputVolts to zero upon + completion of the path- this is left to the user, since it is not + appropriate for paths with nonstationary endstates. + + Note 2: The final rotation of the robot will be set to the rotation of + the final pose in the trajectory. The robot will not follow the rotations + from the poses at each timestep. If alternate rotation behavior is desired, + the other constructor with a supplier for rotation should be used. + + :param trajectory: The trajectory to follow. + :param pose: A function that supplies the robot pose - use one of the odometry classes to + provide this. + :param kinematics: The kinematics for the robot drivetrain. Can be kinematics for 2/3/4/6 + SwerveKinematics. + :param xController: The Trajectory Tracker PID controller + for the robot's x position. + :param yController: The Trajectory Tracker PID controller + for the robot's y position. + :param thetaController: The Trajectory Tracker PID controller + for angle for the robot. + :param outputModuleStates: The raw output module states from the position controllers. + :param requirements: The subsystems to require. + """ + super().__init__() + self._trajectory = trajectory + self._pose = pose + self._kinematics = kinematics + self._outputModuleStates = outputModuleStates + self._controller = HolonomicDriveController( + xController, yController, thetaController + ) + self._desiredRotation = self._trajectory.states()[-1].pose.rotation + self._timer = Timer() + self.addRequirements(requirements) # type:ignore + + @overload + def __init__( + self, + trajectory: Trajectory, + pose: Callable[[], Pose2d], + kinematics: Union[ + SwerveDrive2Kinematics, + SwerveDrive3Kinematics, + SwerveDrive4Kinematics, + SwerveDrive6Kinematics, + ], + controller: HolonomicDriveController, + desiredRotation: Callable[[], Rotation2d], + outputModuleStates: Callable[[SwerveModuleState], None], + requirements: Tuple[Subsystem], + ) -> None: + """ + Constructs a new SwerveControllerCommand that when executed will follow the + provided trajectory. This command will not return output voltages but + rather raw module states from the position controllers which need to be put + into a velocity PID. + + Note: The controllers will *not* set the outputVolts to zero upon + completion of the path- this is left to the user, since it is not + appropriate for paths with nonstationary endstates. + + :param trajectory: The trajectory to follow. + :param pose: A function that supplies the robot pose - use one of the odometry classes to + provide this. + :param kinematics: The kinematics for the robot drivetrain. Can be kinematics for 2/3/4/6 + SwerveKinematics. + :param controller: The HolonomicDriveController for the drivetrain. + :param desiredRotation: The angle that the drivetrain should be + facing. This is sampled at each time step. + :param outputModuleStates: The raw output module states from the position controllers. + :param requirements: The subsystems to require. + + """ + super().__init__() + self._trajectory = trajectory + self._pose = pose + self._kinematics = kinematics + self._outputModuleStates = outputModuleStates + self._controller = controller + self._desiredRotation = desiredRotation + self._timer = Timer() + self.addRequirements(requirements) # type:ignore + + @overload + def __init__( + self, + trajectory: Trajectory, + pose: Callable[[], Pose2d], + kinematics: Union[ + SwerveDrive2Kinematics, + SwerveDrive3Kinematics, + SwerveDrive4Kinematics, + SwerveDrive6Kinematics, + ], + controller: HolonomicDriveController, + outputModuleStates: Callable[[SwerveModuleState], None], + requirements: Tuple[Subsystem], + ) -> None: + """ + Constructs a new SwerveControllerCommand that when executed will follow the + provided trajectory. This command will not return output voltages but + rather raw module states from the position controllers which need to be put + into a velocity PID. + + Note: The controllers will *not* set the outputVolts to zero upon + completion of the path- this is left to the user, since it is not + appropriate for paths with nonstationary endstates. + + Note 2: The final rotation of the robot will be set to the rotation of + the final pose in the trajectory. The robot will not follow the rotations + from the poses at each timestep. If alternate rotation behavior is desired, + the other constructor with a supplier for rotation should be used. + + :param trajectory: The trajectory to follow. + :param pose: A function that supplies the robot pose - use one of the odometry classes to + provide this. + :param kinematics: The kinematics for the robot drivetrain. Can be kinematics for 2/3/4/6 + SwerveKinematics. + :param controller: The HolonomicDriveController for the drivetrain. + :param outputModuleStates: The raw output module states from the position controllers. + :param requirements: The subsystems to require. + + """ + super().__init__() + self._trajectory = trajectory + self._pose = pose + self._kinematics = kinematics + self._outputModuleStates = outputModuleStates + self._controller = controller + self._desiredRotation = self._trajectory.states()[-1].pose.rotation + self._timer = Timer() + self.addRequirements(requirements) # type: ignore + + # @overtake(runtime_type_checker="beartype") + def __init__( + self, + trajectory: Trajectory, + pose: Callable[[], Pose2d], + kinematics: Union[ + SwerveDrive2Kinematics, + SwerveDrive3Kinematics, + SwerveDrive4Kinematics, + SwerveDrive6Kinematics, + ], + outputModuleStates: Callable[[SwerveModuleState], None], + requirements: Tuple[Subsystem], + *, + controller: Optional[HolonomicDriveController] = None, + xController: Optional[PIDController] = None, + yController: Optional[PIDController] = None, + thetaController: Optional[ProfiledPIDControllerRadians] = None, + desiredRotation: Optional[Callable[[], Rotation2d]] = None, + ): + ... + + def initialize(self): + self._timer.restart() + + def execute(self): + curTime = self._timer.get() + desiredState = self._trajectory.sample(curTime) + + targetChassisSpeeds = self._controller.calculate( + self._pose(), desiredState, self._desiredRotation() + ) + targetModuleStates = self._kinematics.toSwerveModuleStates(targetChassisSpeeds) + + self._outputModuleStates(targetModuleStates) + + def end(self, interrupted): + self._timer.stop() + + def isFinished(self): + return self._timer.hasElapsed(self._trajectory.totalTime()) diff --git a/rio/ottomanEmpire/bursa/timedcommandrobot.py b/rio/ottomanEmpire/bursa/timedcommandrobot.py new file mode 100644 index 00000000..4a4b3bc5 --- /dev/null +++ b/rio/ottomanEmpire/bursa/timedcommandrobot.py @@ -0,0 +1,15 @@ +from wpilib import TimedRobot + +from .commandscheduler import CommandScheduler + +seconds = float + + +class TimedCommandRobot(TimedRobot): + kSchedulerOffset = 0.005 + + def __init__(self, period: seconds = TimedRobot.kDefaultPeriod / 1000) -> None: + super().__init__(period) + self.addPeriodic( + CommandScheduler.getInstance().run, period, self.kSchedulerOffset + ) diff --git a/rio/ottomanEmpire/bursa/trapezoidprofilesubsystem.py b/rio/ottomanEmpire/bursa/trapezoidprofilesubsystem.py new file mode 100644 index 00000000..2b40fd53 --- /dev/null +++ b/rio/ottomanEmpire/bursa/trapezoidprofilesubsystem.py @@ -0,0 +1,75 @@ +# 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. +from __future__ import annotations + +from typing import Union + +from .subsystem import Subsystem +from wpimath.trajectory import TrapezoidProfile + + +class TrapezoidProfileSubsystem(Subsystem): + """ + A subsystem that generates and runs trapezoidal motion profiles automatically. The user specifies + how to use the current state of the motion profile by overriding the `useState` method. + """ + + def __init__( + self, + constraints: TrapezoidProfile.Constraints, + initial_position: float = 0.0, + period: float = 0.02, + ): + """ + Creates a new TrapezoidProfileSubsystem. + + :param constraints: The constraints (maximum velocity and acceleration) for the profiles. + :param initial_position: The initial position of the controlled mechanism when the subsystem is constructed. + :param period: The period of the main robot loop, in seconds. + """ + self._profile = TrapezoidProfile(constraints) + self._state = TrapezoidProfile.State(initial_position, 0) + self.setGoal(initial_position) + self._period = period + self._enabled = True + + def periodic(self): + """ + Executes the TrapezoidProfileSubsystem logic during each periodic update. + + This method is called synchronously from the subsystem's periodic() method. + """ + self._state = self._profile.calculate(self._period, self._goal, self._state) + if self._enabled: + self.useState(self._state) + + def setGoal(self, goal: Union[TrapezoidProfile.State, float]): + """ + Sets the goal state for the subsystem. Goal velocity assumed to be zero. + + :param goal: The goal position for the subsystem's motion profile. The goal + can either be a `TrapezoidProfile.State` or `float`. If float is provided, + the assumed velocity for the goal will be 0. + """ + # If we got a float, instantiate the state + if isinstance(goal, (float, int)): + goal = TrapezoidProfile.State(goal, 0) + + self._goal = goal + + def enable(self): + """Enable the TrapezoidProfileSubsystem's output.""" + self._enabled = True + + def disable(self): + """Disable the TrapezoidProfileSubsystem's output.""" + self._enabled = False + + def useState(self, state: TrapezoidProfile.State): + """ + Users should override this to consume the current state of the motion profile. + + :param state: The current state of the motion profile. + """ + raise NotImplementedError("Subclasses must implement this method") diff --git a/rio/ottomanEmpire/bursa/util.py b/rio/ottomanEmpire/bursa/util.py new file mode 100644 index 00000000..617a5762 --- /dev/null +++ b/rio/ottomanEmpire/bursa/util.py @@ -0,0 +1,24 @@ +from __future__ import annotations + +from typing import Iterable, List, Tuple, Union + +from .command import Command + + +def flatten_args_commands( + *commands: Union[Command, Iterable[Command]] +) -> Tuple[Command, ...]: + flattened_commands: List[Command] = [] + for command in commands: + if isinstance(command, Command): + flattened_commands.append(command) + elif isinstance(command, Iterable): + flattened_commands.extend(flatten_args_commands(*command)) + return tuple(flattened_commands) + + +def format_args_kwargs(*args, **kwargs) -> str: + return ", ".join( + [repr(arg) for arg in args] + + [f"{key}={repr(value)}" for key, value in kwargs.items()] + ) diff --git a/rio/ottomanEmpire/bursa/waitcommand.py b/rio/ottomanEmpire/bursa/waitcommand.py new file mode 100644 index 00000000..4f7ed04d --- /dev/null +++ b/rio/ottomanEmpire/bursa/waitcommand.py @@ -0,0 +1,33 @@ +from __future__ import annotations + +from wpilib import Timer + +from .command import Command +from .commandgroup import * + + +class WaitCommand(Command): + """ + A command that does nothing but takes a specified amount of time to finish.""" + + def __init__(self, seconds: float): + """ + Creates a new WaitCommand. This command will do nothing, and end after the specified duration. + + :param seconds: the time to wait, in seconds""" + super().__init__() + self._duration = seconds + self._timer = Timer() + + def initialize(self): + self._timer.reset() + self._timer.start() + + def end(self, interrupted: bool): + self._timer.stop() + + def isFinished(self) -> bool: + return self._timer.hasElapsed(self._duration) + + def runsWhenDisabled(self) -> bool: + return True diff --git a/rio/ottomanEmpire/bursa/waituntilcommand.py b/rio/ottomanEmpire/bursa/waituntilcommand.py new file mode 100644 index 00000000..34839c9f --- /dev/null +++ b/rio/ottomanEmpire/bursa/waituntilcommand.py @@ -0,0 +1,74 @@ +from __future__ import annotations + +from typing import Callable, overload + +from wpilib import Timer + +from .command import Command +from .util import format_args_kwargs + + +class WaitUntilCommand(Command): + """ + A command that does nothing but ends after a specified match time or condition. Useful for + CommandGroups.""" + + _condition: Callable[[], bool] + + @overload + def __init__(self, condition: Callable[[], bool]): + """ + Creates a new WaitUntilCommand that ends after a given condition becomes true. + + :param condition: the condition to determine when to end""" + ... + + @overload + def __init__(self, time: float): + """ + Creates a new WaitUntilCommand that ends after a given match time. + + NOTE: The match timer used for this command is UNOFFICIAL. Using this command does NOT + guarantee that the time at which the action is performed will be judged to be legal by the + referees. When in doubt, add a safety factor or time the action manually. + + :param time: the match time at which to end, in seconds""" + ... + + def __init__(self, *args, **kwargs): + super().__init__() + + def init_condition(condition: Callable[[], bool]) -> None: + self._condition = condition + + def init_time(time: float) -> None: + self._condition = lambda: Timer.getMatchTime() - time > 0 + + num_args = len(args) + len(kwargs) + + if num_args == 1 and len(kwargs) == 1: + if "condition" in kwargs: + return init_condition(kwargs["condition"]) + elif "time" in kwargs: + return init_time(kwargs["time"]) + elif num_args == 1 and len(args) == 1: + if isinstance(args[0], float): + return init_time(args[0]) + elif callable(args[0]): + return init_condition(args[0]) + + raise TypeError( + f""" +TypeError: WaitUntilCommand(): incompatible function arguments. The following argument types are supported: + 1. (self: WaitUntilCommand, condition: () -> bool) + 2. (self: WaitUntilCommand, time: float) + +Invoked with: {format_args_kwargs(self, *args, **kwargs)} +""" + ) + + def isFinished(self) -> bool: + return self._condition() + + def runsWhenDisabled(self) -> bool: + return True diff --git a/rio/ottomanEmpire/bursa/wrappercommand.py b/rio/ottomanEmpire/bursa/wrappercommand.py new file mode 100644 index 00000000..43c50372 --- /dev/null +++ b/rio/ottomanEmpire/bursa/wrappercommand.py @@ -0,0 +1,80 @@ +from __future__ import annotations + +from typing import Set + +from .command import Command, InterruptionBehavior +from .commandgroup import * +from .commandscheduler import CommandScheduler + + +class WrapperCommand(Command): + """ + A class used internally to wrap commands while overriding a specific method; all other methods + will call through to the wrapped command. + + The rules for command compositions apply: command instances that are passed to it cannot be + added to any other composition or scheduled individually, and the composition requires all + subsystems its components require.""" + + def __init__(self, command: Command): + """ + Wrap a command. + + :param command: the command being wrapped. Trying to directly schedule this command or add it to + a composition will throw an exception.""" + super().__init__() + + CommandScheduler.getInstance().registerComposedCommands([command]) + self._command = command + self.setName(self._command.getName()) + + def initialize(self): + """The initial subroutine of a command. Called once when the command is initially scheduled.""" + self._command.initialize() + + def execute(self): + """The main body of a command. Called repeatedly while the command is scheduled.""" + self._command.execute() + + def end(self, interrupted: bool): + """ + The action to take when the command ends. Called when either the command finishes normally, or + when it interrupted/canceled. + + Do not schedule commands here that share requirements with this command. Use {@link + #andThen(Command...)} instead. + + :param interrupted: whether the command was interrupted/canceled""" + self._command.end(interrupted) + + def isFinished(self) -> bool: + """ + Whether the command has finished. Once a command finishes, the scheduler will call its end() + method and un-schedule it. + + :returns: whether the command has finished.""" + return self._command.isFinished() + + def getRequirements(self) -> Set: + """ + Specifies the set of subsystems used by this command. Two commands cannot use the same + subsystem at the same time. If the command is scheduled as interruptible and another command is + scheduled that shares a requirement, the command will be interrupted. Else, the command will + not be scheduled. If no subsystems are required, return an empty set. + + Note: it is recommended that user implementations contain the requirements as a field, and + return that field here, rather than allocating a new set every time this is called. + + :returns: the set of subsystems that are required""" + return self._command.getRequirements() + + def runsWhenDisabled(self) -> bool: + """ + Whether the given command should run when the robot is disabled. Override to return true if the + command should run when disabled. + + :returns: whether the command should run when the robot is disabled""" + return self._command.runsWhenDisabled() + + def getInterruptionBehavior(self) -> InterruptionBehavior: + return self._command.getInterruptionBehavior() diff --git a/rio/ottomanEmpire/docs/.gitignore b/rio/ottomanEmpire/docs/.gitignore new file mode 100644 index 00000000..4080f00f --- /dev/null +++ b/rio/ottomanEmpire/docs/.gitignore @@ -0,0 +1,11 @@ + + +# Doc-related stuffs +/_build +/_sidebar.rst.inc +/_sidebar.toml + +/commands2.rst +/commands2 +/commands2.button.rst +/commands2.button \ No newline at end of file diff --git a/rio/ottomanEmpire/docs/Makefile b/rio/ottomanEmpire/docs/Makefile new file mode 100644 index 00000000..b17322b3 --- /dev/null +++ b/rio/ottomanEmpire/docs/Makefile @@ -0,0 +1,183 @@ +# Makefile for Sphinx documentation +# + +# You can set these variables from the command line. +SPHINXOPTS = +SPHINXBUILD = sphinx-build +PAPER = +BUILDDIR = _build + +# User-friendly check for sphinx-build +ifeq ($(shell which $(SPHINXBUILD) >/dev/null 2>&1; echo $$?), 1) +$(error The '$(SPHINXBUILD)' command was not found. Make sure you have Sphinx installed, then set the SPHINXBUILD environment variable to point to the full path of the '$(SPHINXBUILD)' executable. Alternatively you can add the directory with the executable to your PATH. If you don't have Sphinx installed, grab it from http://sphinx-doc.org/) +endif + +# Internal variables. +PAPEROPT_a4 = -D latex_paper_size=a4 +PAPEROPT_letter = -D latex_paper_size=letter +ALLSPHINXOPTS = -d $(BUILDDIR)/doctrees $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) . +# the i18n builder cannot share the environment and doctrees with the others +I18NSPHINXOPTS = $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) . + +.PHONY: help clean html dirhtml singlehtml pickle json htmlhelp qthelp devhelp epub latex latexpdf text man changes linkcheck doctest gettext + +help: + @echo "Please use \`make ' where is one of" + @echo " html to make standalone HTML files" + @echo " dirhtml to make HTML files named index.html in directories" + @echo " singlehtml to make a single large HTML file" + @echo " pickle to make pickle files" + @echo " json to make JSON files" + @echo " htmlhelp to make HTML files and a HTML help project" + @echo " qthelp to make HTML files and a qthelp project" + @echo " devhelp to make HTML files and a Devhelp project" + @echo " epub to make an epub" + @echo " latex to make LaTeX files, you can set PAPER=a4 or PAPER=letter" + @echo " latexpdf to make LaTeX files and run them through pdflatex" + @echo " latexpdfja to make LaTeX files and run them through platex/dvipdfmx" + @echo " text to make text files" + @echo " man to make manual pages" + @echo " texinfo to make Texinfo files" + @echo " info to make Texinfo files and run them through makeinfo" + @echo " gettext to make PO message catalogs" + @echo " changes to make an overview of all changed/added/deprecated items" + @echo " xml to make Docutils-native XML files" + @echo " pseudoxml to make pseudoxml-XML files for display purposes" + @echo " linkcheck to check all external links for integrity" + @echo " doctest to run all doctests embedded in the documentation (if enabled)" + +clean: + rm -rf $(BUILDDIR)/* + rm -rf __pycache__ + for i in commands2.buttons \ + commands2; do \ + rm -f $$i.rst; \ + rm -rf $$i; \ + done + +html: + $(SPHINXBUILD) -b html $(ALLSPHINXOPTS) $(BUILDDIR)/html + @echo + @echo "Build finished. The HTML pages are in $(BUILDDIR)/html." + +dirhtml: + $(SPHINXBUILD) -b dirhtml $(ALLSPHINXOPTS) $(BUILDDIR)/dirhtml + @echo + @echo "Build finished. The HTML pages are in $(BUILDDIR)/dirhtml." + +singlehtml: + $(SPHINXBUILD) -b singlehtml $(ALLSPHINXOPTS) $(BUILDDIR)/singlehtml + @echo + @echo "Build finished. The HTML page is in $(BUILDDIR)/singlehtml." + +pickle: + $(SPHINXBUILD) -b pickle $(ALLSPHINXOPTS) $(BUILDDIR)/pickle + @echo + @echo "Build finished; now you can process the pickle files." + +json: + $(SPHINXBUILD) -b json $(ALLSPHINXOPTS) $(BUILDDIR)/json + @echo + @echo "Build finished; now you can process the JSON files." + +htmlhelp: + $(SPHINXBUILD) -b htmlhelp $(ALLSPHINXOPTS) $(BUILDDIR)/htmlhelp + @echo + @echo "Build finished; now you can run HTML Help Workshop with the" \ + ".hhp project file in $(BUILDDIR)/htmlhelp." + +qthelp: + $(SPHINXBUILD) -b qthelp $(ALLSPHINXOPTS) $(BUILDDIR)/qthelp + @echo + @echo "Build finished; now you can run "qcollectiongenerator" with the" \ + ".qhcp project file in $(BUILDDIR)/qthelp, like this:" + @echo "# qcollectiongenerator $(BUILDDIR)/qthelp/RobotPyWPILib.qhcp" + @echo "To view the help file:" + @echo "# assistant -collectionFile $(BUILDDIR)/qthelp/RobotPyWPILib.qhc" + +devhelp: + $(SPHINXBUILD) -b devhelp $(ALLSPHINXOPTS) $(BUILDDIR)/devhelp + @echo + @echo "Build finished." + @echo "To view the help file:" + @echo "# mkdir -p $$HOME/.local/share/devhelp/RobotPyWPILib" + @echo "# ln -s $(BUILDDIR)/devhelp $$HOME/.local/share/devhelp/RobotPyWPILib" + @echo "# devhelp" + +epub: + $(SPHINXBUILD) -b epub $(ALLSPHINXOPTS) $(BUILDDIR)/epub + @echo + @echo "Build finished. The epub file is in $(BUILDDIR)/epub." + +latex: + $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex + @echo + @echo "Build finished; the LaTeX files are in $(BUILDDIR)/latex." + @echo "Run \`make' in that directory to run these through (pdf)latex" \ + "(use \`make latexpdf' here to do that automatically)." + +latexpdf: + $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex + @echo "Running LaTeX files through pdflatex..." + $(MAKE) -C $(BUILDDIR)/latex all-pdf + @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." + +latexpdfja: + $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex + @echo "Running LaTeX files through platex and dvipdfmx..." + $(MAKE) -C $(BUILDDIR)/latex all-pdf-ja + @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." + +text: + $(SPHINXBUILD) -b text $(ALLSPHINXOPTS) $(BUILDDIR)/text + @echo + @echo "Build finished. The text files are in $(BUILDDIR)/text." + +man: + $(SPHINXBUILD) -b man $(ALLSPHINXOPTS) $(BUILDDIR)/man + @echo + @echo "Build finished. The manual pages are in $(BUILDDIR)/man." + +texinfo: + $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo + @echo + @echo "Build finished. The Texinfo files are in $(BUILDDIR)/texinfo." + @echo "Run \`make' in that directory to run these through makeinfo" \ + "(use \`make info' here to do that automatically)." + +info: + $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo + @echo "Running Texinfo files through makeinfo..." + make -C $(BUILDDIR)/texinfo info + @echo "makeinfo finished; the Info files are in $(BUILDDIR)/texinfo." + +gettext: + $(SPHINXBUILD) -b gettext $(I18NSPHINXOPTS) $(BUILDDIR)/locale + @echo + @echo "Build finished. The message catalogs are in $(BUILDDIR)/locale." + +changes: + $(SPHINXBUILD) -b changes $(ALLSPHINXOPTS) $(BUILDDIR)/changes + @echo + @echo "The overview file is in $(BUILDDIR)/changes." + +linkcheck: + $(SPHINXBUILD) -b linkcheck $(ALLSPHINXOPTS) $(BUILDDIR)/linkcheck + @echo + @echo "Link check complete; look for any errors in the above output " \ + "or in $(BUILDDIR)/linkcheck/output.txt." + +doctest: + $(SPHINXBUILD) -b doctest $(ALLSPHINXOPTS) $(BUILDDIR)/doctest + @echo "Testing of doctests in the sources finished, look at the " \ + "results in $(BUILDDIR)/doctest/output.txt." + +xml: + $(SPHINXBUILD) -b xml $(ALLSPHINXOPTS) $(BUILDDIR)/xml + @echo + @echo "Build finished. The XML files are in $(BUILDDIR)/xml." + +pseudoxml: + $(SPHINXBUILD) -b pseudoxml $(ALLSPHINXOPTS) $(BUILDDIR)/pseudoxml + @echo + @echo "Build finished. The pseudo-XML files are in $(BUILDDIR)/pseudoxml." diff --git a/rio/ottomanEmpire/docs/api.rst b/rio/ottomanEmpire/docs/api.rst new file mode 100644 index 00000000..faf88f70 --- /dev/null +++ b/rio/ottomanEmpire/docs/api.rst @@ -0,0 +1,24 @@ + +.. _command_v2_api: + +Commands V2 API +--------------- + +Objects in this package allow you to implement a robot using the +latest version of WPILib's Command-based programming. Command +based programming is a design pattern to help you organize your +robot programs, by organizing your robot program into components +based on :class:`.Command` and :class:`.Subsystem` + +Each one of the objects in the Command framework has detailed +documentation available. If you need more information, for examples, +tutorials, and other detailed information on programming your robot +using this pattern, we recommend that you consult the Java version of the +`FRC Control System documentation `_ + + +.. toctree:: + + commands2 + commands2.button + commands2.cmd diff --git a/rio/ottomanEmpire/docs/conf.py b/rio/ottomanEmpire/docs/conf.py new file mode 100644 index 00000000..0f2e83a8 --- /dev/null +++ b/rio/ottomanEmpire/docs/conf.py @@ -0,0 +1,158 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +# +# Imports +# + +import os +from os.path import abspath, dirname + +# Project must be built+installed to generate docs +import commands2 + +# -- RTD configuration ------------------------------------------------ + +# on_rtd is whether we are on readthedocs.org, this line of code grabbed from docs.readthedocs.org +on_rtd = os.environ.get("READTHEDOCS", None) == "True" + +# This is used for linking and such so we link to the thing we're building +rtd_version = os.environ.get("READTHEDOCS_VERSION", "latest") +if rtd_version not in ["stable", "latest"]: + rtd_version = "stable" + +# -- General configuration ------------------------------------------------ + + +# Add any Sphinx extension module names here, as strings. They can be +# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom +# ones. +extensions = [ + "sphinx.ext.autodoc", + "sphinx.ext.autosummary", + "sphinx.ext.intersphinx", + "sphinx.ext.viewcode", + "robotpy_sphinx.all", +] + +# Add any paths that contain templates here, relative to this directory. +templates_path = ["_templates"] + +# The suffix of source filenames. +source_suffix = ".rst" + +# The master toctree document. +master_doc = "index" + +# General information about the project. +project = "RobotPy Commands v2" +copyright = "2021, RobotPy development team" + +# The version info for the project you're documenting, acts as replacement for +# |version| and |release|, also used in various other places throughout the +# built documents. +# +# The short X.Y version. +version = ".".join(commands2.__version__.split(".")[:2]) +# The full version, including alpha/beta/rc tags. +release = commands2.__version__ + +autoclass_content = "both" + +intersphinx_mapping = { + "networktables": ( + f"https://robotpy.readthedocs.io/projects/pynetworktables/en/{rtd_version}/", + None, + ), + "wpilib": ( + f"https://robotpy.readthedocs.io/projects/wpilib/en/{rtd_version}/", + None, + ), + "wpimath": ( + f"https://robotpy.readthedocs.io/projects/wpimath/en/{rtd_version}/", + None, + ), +} + +# List of patterns, relative to source directory, that match files and +# directories to ignore when looking for source files. +exclude_patterns = ["_build"] + +# The name of the Pygments (syntax highlighting) style to use. +pygments_style = "sphinx" + +# -- Options for HTML output ---------------------------------------------- + +if not on_rtd: # only import and set the theme if we're building docs locally + import sphinx_rtd_theme + + html_theme = "sphinx_rtd_theme" + html_theme_path = [sphinx_rtd_theme.get_html_theme_path()] +else: + html_theme = "default" + +# Output file base name for HTML help builder. +htmlhelp_basename = "RobotPyCommandDoc" + + +# -- Options for LaTeX output --------------------------------------------- + +latex_elements = {} + +# Grouping the document tree into LaTeX files. List of tuples +# (source start file, target name, title, +# author, documentclass [howto, manual, or own class]). +latex_documents = [ + ( + "index", + "RobotPyCommandV2Doc.tex", + "RobotPy CommandV2 Documentation", + "RobotPy development team", + "manual", + ) +] + +# -- Options for Texinfo output ------------------------------------------- + +# Grouping the document tree into Texinfo files. List of tuples +# (source start file, target name, title, author, +# dir menu entry, description, category) +texinfo_documents = [ + ( + "index", + "RobotPyCommandV2Doc", + "RobotPy CommandV2 Documentation", + "RobotPy development team", + "RobotPyCommandV2Doc", + "One line description of project.", + "Miscellaneous", + ) +] + +# -- Options for Epub output ---------------------------------------------- + +# Bibliographic Dublin Core info. +epub_title = "RobotPy CommandV2" +epub_author = "RobotPy development team" +epub_publisher = "RobotPy development team" +epub_copyright = "2021, RobotPy development team" + +# A list of files that should not be packed into the epub file. +epub_exclude_files = ["search.html"] + +# -- Custom Document processing ---------------------------------------------- + +from robotpy_sphinx.regen import gen_package +from robotpy_sphinx.sidebar import generate_sidebar + +generate_sidebar( + globals(), + "commandv2", + "https://raw.githubusercontent.com/robotpy/docs-sidebar/master/sidebar.toml", +) + +root = abspath(dirname(__file__)) + +gen_package(root, "commands2") +gen_package(root, "commands2.button") +gen_package(root, "commands2.cmd") diff --git a/rio/ottomanEmpire/docs/index.rst b/rio/ottomanEmpire/docs/index.rst new file mode 100644 index 00000000..cd7b4269 --- /dev/null +++ b/rio/ottomanEmpire/docs/index.rst @@ -0,0 +1,11 @@ +Commands V2 +=========== + +.. include:: _sidebar.rst.inc + +Indices and tables +================== + +* :ref:`genindex` +* :ref:`modindex` +* :ref:`search` \ No newline at end of file diff --git a/rio/ottomanEmpire/docs/requirements.txt b/rio/ottomanEmpire/docs/requirements.txt new file mode 100644 index 00000000..10a9f290 --- /dev/null +++ b/rio/ottomanEmpire/docs/requirements.txt @@ -0,0 +1,5 @@ +# This file is intended for use on readthedocs +sphinx +sphinx-rtd-theme +robotpy-sphinx-plugin +-e docs \ No newline at end of file diff --git a/rio/ottomanEmpire/docs/setup.py b/rio/ottomanEmpire/docs/setup.py new file mode 100644 index 00000000..1786ba89 --- /dev/null +++ b/rio/ottomanEmpire/docs/setup.py @@ -0,0 +1,23 @@ +# this is a dirty hack to convince readthedocs to install a specific +# version of our software. We assume that this will always be triggered +# on a specific version + +from setuptools import setup + +try: + from setuptools_scm import get_version +except ImportError: + import subprocess + + def get_version(*args, **kwargs): + return subprocess.check_output(["git", "describe", "--tags"], encoding="utf-8") + + +package = "robotpy-commands-v2" +version = get_version(root="..", relative_to=__file__) + +setup( + name="dummy-package", + version="1.0", + install_requires=["%s==%s" % (package, version)], +) diff --git a/rio/ottomanEmpire/setup.py b/rio/ottomanEmpire/setup.py new file mode 100644 index 00000000..99b6e032 --- /dev/null +++ b/rio/ottomanEmpire/setup.py @@ -0,0 +1,23 @@ +#!/usr/bin/env python3 + +import setuptools + +setuptools.setup( + name="robotpy-commands-v2", + use_scm_version=True, + setup_requires=["setuptools_scm"], + author="RobotPy Development Team", + author_email="robotpy@googlegroups.com", + description="WPILib command framework v2", + url="https://github.com/robotpy/robotpy-commands-v2", + packages=["commands2"], + install_requires=[ + "wpilib<2025,>=2024.0.0b2", + "typing_extensions>=4.1.0,<5", + "overtake~=0.4.0", + "beartype~=0.16.4", + ], + license="BSD-3-Clause", + python_requires=">=3.8", + include_package_data=True, +) diff --git a/rio/ottomanEmpire/tests/compositiontestbase.py b/rio/ottomanEmpire/tests/compositiontestbase.py new file mode 100644 index 00000000..ca1bfad4 --- /dev/null +++ b/rio/ottomanEmpire/tests/compositiontestbase.py @@ -0,0 +1,154 @@ +from typing import Generic, TypeVar + +import commands2 +import pytest + +# T = TypeVar("T", bound=commands2.Command) +# T = commands2.Command + +from util import * + +if not IS_OLD_COMMANDS: + + class SingleCompositionTestBase: + def composeSingle(self, member: commands2.Command): + raise NotImplementedError + + @pytest.mark.parametrize( + "interruptionBehavior", + [ + commands2.InterruptionBehavior.kCancelSelf, + commands2.InterruptionBehavior.kCancelIncoming, + ], + ) + def test_interruptible( + self, interruptionBehavior: commands2.InterruptionBehavior + ): + command = self.composeSingle( + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + interruptionBehavior + ) + ) + assert command.getInterruptionBehavior() == interruptionBehavior + + @pytest.mark.parametrize("runsWhenDisabled", [True, False]) + def test_runWhenDisabled(self, runsWhenDisabled: bool): + command = self.composeSingle( + commands2.WaitUntilCommand(lambda: False).ignoringDisable( + runsWhenDisabled + ) + ) + assert command.runsWhenDisabled() == runsWhenDisabled + + class MultiCompositionTestBase(SingleCompositionTestBase): + def compose(self, *members: commands2.Command): + raise NotImplementedError + + def composeSingle(self, member: commands2.Command): + return self.compose(member) + + @pytest.mark.parametrize( + "expected,command1,command2,command3", + [ + pytest.param( + commands2.InterruptionBehavior.kCancelSelf, + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelSelf + ), + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelSelf + ), + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelSelf + ), + id="AllCancelSelf", + ), + pytest.param( + commands2.InterruptionBehavior.kCancelIncoming, + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelIncoming + ), + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelIncoming + ), + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelIncoming + ), + id="AllCancelIncoming", + ), + pytest.param( + commands2.InterruptionBehavior.kCancelSelf, + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelSelf + ), + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelSelf + ), + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelIncoming + ), + id="TwoCancelSelfOneIncoming", + ), + pytest.param( + commands2.InterruptionBehavior.kCancelSelf, + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelIncoming + ), + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelIncoming + ), + commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( + commands2.InterruptionBehavior.kCancelSelf + ), + id="TwoCancelIncomingOneSelf", + ), + ], + ) + def test_interruptible(self, expected, command1, command2, command3): + command = self.compose(command1, command2, command3) + assert command.getInterruptionBehavior() == expected + + @pytest.mark.parametrize( + "expected,command1,command2,command3", + [ + pytest.param( + False, + commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), + commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), + commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), + id="AllFalse", + ), + pytest.param( + True, + commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), + commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), + commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), + id="AllTrue", + ), + pytest.param( + False, + commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), + commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), + commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), + id="TwoTrueOneFalse", + ), + pytest.param( + False, + commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), + commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), + commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), + id="TwoFalseOneTrue", + ), + ], + ) + def test_runWhenDisabled(self, expected, command1, command2, command3): + command = self.compose(command1, command2, command3) + assert command.runsWhenDisabled() == expected + +else: + + class SingleCompositionTestBase: + ... + + class MultiCompositionTestBase: + ... diff --git a/rio/ottomanEmpire/tests/conftest.py b/rio/ottomanEmpire/tests/conftest.py new file mode 100644 index 00000000..96f18263 --- /dev/null +++ b/rio/ottomanEmpire/tests/conftest.py @@ -0,0 +1,20 @@ +import commands2 +import pytest +from ntcore import NetworkTableInstance +from wpilib.simulation import DriverStationSim + + +@pytest.fixture(autouse=True) +def scheduler(): + commands2.CommandScheduler.resetInstance() + DriverStationSim.setEnabled(True) + DriverStationSim.notifyNewData() + return commands2.CommandScheduler.getInstance() + + +@pytest.fixture() +def nt_instance(): + inst = NetworkTableInstance.create() + inst.startLocal() + yield inst + inst.stopLocal() diff --git a/rio/ottomanEmpire/tests/requirements.txt b/rio/ottomanEmpire/tests/requirements.txt new file mode 100644 index 00000000..e079f8a6 --- /dev/null +++ b/rio/ottomanEmpire/tests/requirements.txt @@ -0,0 +1 @@ +pytest diff --git a/rio/ottomanEmpire/tests/run_tests.py b/rio/ottomanEmpire/tests/run_tests.py new file mode 100755 index 00000000..62696f8c --- /dev/null +++ b/rio/ottomanEmpire/tests/run_tests.py @@ -0,0 +1,12 @@ +#!/usr/bin/env python3 + +import os +import subprocess +import sys +from os.path import abspath, dirname + +if __name__ == "__main__": + root = abspath(dirname(__file__)) + os.chdir(root) + + subprocess.check_call([sys.executable, "-m", "pytest"]) diff --git a/rio/ottomanEmpire/tests/test_command_decorators.py b/rio/ottomanEmpire/tests/test_command_decorators.py new file mode 100644 index 00000000..9c89ff9c --- /dev/null +++ b/rio/ottomanEmpire/tests/test_command_decorators.py @@ -0,0 +1,222 @@ +from typing import TYPE_CHECKING + +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import commands2 +import pytest + + +def test_timeout(scheduler: commands2.CommandScheduler): + with ManualSimTime() as sim: + command1 = commands2.WaitCommand(1) + timeout = command1.withTimeout(2) + scheduler.schedule(timeout) + scheduler.run() + assert not command1.isScheduled() + assert timeout.isScheduled() + sim.step(3) + scheduler.run() + assert not timeout.isScheduled() + + +def test_until(scheduler: commands2.CommandScheduler): + condition = OOBoolean(False) + command = commands2.WaitCommand(10).until(condition) + scheduler.schedule(command) + scheduler.run() + assert command.isScheduled() + condition.set(True) + scheduler.run() + assert not command.isScheduled() + + +@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") +def test_only_while(scheduler: commands2.CommandScheduler): + condition = OOBoolean(True) + command = commands2.WaitCommand(10).onlyWhile(condition) + scheduler.schedule(command) + scheduler.run() + assert command.isScheduled() + condition.set(False) + scheduler.run() + assert not command.isScheduled() + + +@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") +def test_ignoringDisable(scheduler: commands2.CommandScheduler): + command = commands2.RunCommand(lambda: None).ignoringDisable(True) + DriverStationSim.setEnabled(False) + DriverStationSim.notifyNewData() + scheduler.schedule(command) + scheduler.run() + assert command.isScheduled() + + +def test_beforeStarting(scheduler: commands2.CommandScheduler): + condition = OOBoolean(False) + condition.set(False) + command = commands2.InstantCommand() + scheduler.schedule( + command.beforeStarting(commands2.InstantCommand(lambda: condition.set(True))) + ) + assert condition == True + + +@pytest.mark.skip +def test_andThenLambda(scheduler: commands2.CommandScheduler): + ... + + +def test_andThen(scheduler: commands2.CommandScheduler): + condition = OOBoolean(False) + condition.set(False) + command1 = commands2.InstantCommand() + command2 = commands2.InstantCommand(lambda: condition.set(True)) + scheduler.schedule(command1.andThen(command2)) + assert condition == False + scheduler.run() + assert condition == True + + +def test_deadlineWith(scheduler: commands2.CommandScheduler): + condition = OOBoolean(False) + condition.set(False) + + dictator = commands2.WaitUntilCommand(condition) + endsBefore = commands2.InstantCommand() + endsAfter = commands2.WaitUntilCommand(lambda: False) + + group = dictator.deadlineWith(endsBefore, endsAfter) + + scheduler.schedule(group) + scheduler.run() + assert group.isScheduled() + condition.set(True) + scheduler.run() + assert not group.isScheduled() + + +def test_alongWith(scheduler: commands2.CommandScheduler): + condition = OOBoolean() + condition.set(False) + + command1 = commands2.WaitUntilCommand(condition) + command2 = commands2.InstantCommand() + + group = command1.alongWith(command2) + + scheduler.schedule(group) + scheduler.run() + assert group.isScheduled() + condition.set(True) + scheduler.run() + assert not group.isScheduled() + + +def test_raceWith(scheduler: commands2.CommandScheduler): + command1 = commands2.WaitUntilCommand(lambda: False) + command2 = commands2.InstantCommand() + + group = command1.raceWith(command2) + + scheduler.schedule(group) + scheduler.run() + assert not group.isScheduled() + + +def test_perpetually(scheduler: commands2.CommandScheduler): + command = commands2.InstantCommand() + perpetual = command.perpetually() + scheduler.schedule(perpetual) + scheduler.run() + scheduler.run() + scheduler.run() + assert perpetual.isScheduled() + + +@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") +def test_unless(scheduler: commands2.CommandScheduler): + unlessCondition = OOBoolean(True) + hasRunCondition = OOBoolean(False) + + command = commands2.InstantCommand(lambda: hasRunCondition.set(True)).unless( + unlessCondition + ) + + scheduler.schedule(command) + scheduler.run() + assert hasRunCondition == False + unlessCondition.set(False) + scheduler.schedule(command) + scheduler.run() + assert hasRunCondition == True + + +@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") +def test_onlyIf(scheduler: commands2.CommandScheduler): + onlyIfCondition = OOBoolean(False) + hasRunCondition = OOBoolean(False) + + command = commands2.InstantCommand(lambda: hasRunCondition.set(True)).onlyIf( + onlyIfCondition + ) + + scheduler.schedule(command) + scheduler.run() + assert hasRunCondition == False + onlyIfCondition.set(True) + scheduler.schedule(command) + scheduler.run() + assert hasRunCondition == True + + +@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") +def test_finallyDo(scheduler: commands2.CommandScheduler): + first = OOInteger(0) + second = OOInteger(0) + + command = commands2.FunctionalCommand( + lambda: None, + lambda: None, + lambda interrupted: first.incrementAndGet() if not interrupted else None, + lambda: True, + ).finallyDo(lambda interrupted: second.addAndGet(1 + first())) + + scheduler.schedule(command) + assert first == 0 + assert second == 0 + scheduler.run() + assert first == 1 + assert second == 2 + + +@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") +def test_handleInterrupt(scheduler: commands2.CommandScheduler): + first = OOInteger(0) + second = OOInteger(0) + + command = commands2.FunctionalCommand( + lambda: None, + lambda: None, + lambda interrupted: first.incrementAndGet() if interrupted else None, + lambda: False, + ).handleInterrupt(lambda: second.addAndGet(1 + first())) + + scheduler.schedule(command) + scheduler.run() + assert first == 0 + assert second == 0 + scheduler.cancel(command) + assert first == 1 + assert second == 2 + + +@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") +def test_withName(scheduler: commands2.CommandScheduler): + command = commands2.InstantCommand() + name = "Named" + named = command.withName(name) + assert named.getName() == name diff --git a/rio/ottomanEmpire/tests/test_command_requirements.py b/rio/ottomanEmpire/tests/test_command_requirements.py new file mode 100644 index 00000000..0215e0bc --- /dev/null +++ b/rio/ottomanEmpire/tests/test_command_requirements.py @@ -0,0 +1,59 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_requirementInterrupt(scheduler: commands2.CommandScheduler): + requirement = commands2.Subsystem() + interrupted = commands2.Command() + interrupted.addRequirements(requirement) + interrupter = commands2.Command() + interrupter.addRequirements(requirement) + start_spying_on(interrupted) + start_spying_on(interrupter) + + scheduler.schedule(interrupted) + scheduler.run() + scheduler.schedule(interrupter) + scheduler.run() + + verify(interrupted).initialize() + verify(interrupted).execute() + verify(interrupted).end(True) + + verify(interrupter).initialize() + verify(interrupter).execute() + + assert not interrupted.isScheduled() + assert interrupter.isScheduled() + + +@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") +def test_requirementUninterruptible(scheduler: commands2.CommandScheduler): + requirement = commands2.Subsystem() + notInterrupted = commands2.RunCommand( + lambda: None, requirement + ).withInterruptBehavior(commands2.InterruptionBehavior.kCancelIncoming) + interrupter = commands2.Command() + interrupter.addRequirements(requirement) + start_spying_on(notInterrupted) + + scheduler.schedule(notInterrupted) + scheduler.schedule(interrupter) + + assert scheduler.isScheduled(notInterrupted) + assert not scheduler.isScheduled(interrupter) + + +def test_defaultCommandRequirementError(scheduler: commands2.CommandScheduler): + system = commands2.Subsystem() + missingRequirement = commands2.WaitUntilCommand(lambda: False) + + with pytest.raises(commands2.IllegalCommandUse): + scheduler.setDefaultCommand(system, missingRequirement) diff --git a/rio/ottomanEmpire/tests/test_command_schedule.py b/rio/ottomanEmpire/tests/test_command_schedule.py new file mode 100644 index 00000000..62d21823 --- /dev/null +++ b/rio/ottomanEmpire/tests/test_command_schedule.py @@ -0,0 +1,90 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_instantSchedule(scheduler: commands2.CommandScheduler): + command = commands2.Command() + command.isFinished = lambda: True + start_spying_on(command) + + scheduler.schedule(command) + assert scheduler.isScheduled(command) + verify(command).initialize() + + scheduler.run() + + verify(command).execute() + verify(command).end(False) + assert not scheduler.isScheduled(command) + + +def test_singleIterationSchedule(scheduler: commands2.CommandScheduler): + command = commands2.Command() + start_spying_on(command) + + scheduler.schedule(command) + assert scheduler.isScheduled(command) + + scheduler.run() + command.isFinished = lambda: True + scheduler.run() + + verify(command).initialize() + verify(command, times(2)).execute() + verify(command).end(False) + assert not scheduler.isScheduled(command) + + +def test_multiSchedule(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + command3 = commands2.Command() + + scheduler.schedule(command1, command2, command3) + assert scheduler.isScheduled(command1, command2, command3) + scheduler.run() + assert scheduler.isScheduled(command1, command2, command3) + + command1.isFinished = lambda: True + scheduler.run() + assert scheduler.isScheduled(command2, command3) + assert not scheduler.isScheduled(command1) + + command2.isFinished = lambda: True + scheduler.run() + assert scheduler.isScheduled(command3) + assert not scheduler.isScheduled(command1, command2) + + command3.isFinished = lambda: True + scheduler.run() + assert not scheduler.isScheduled(command1, command2, command3) + + +def test_schedulerCancel(scheduler: commands2.CommandScheduler): + command = commands2.Command() + start_spying_on(command) + + scheduler.schedule(command) + + scheduler.run() + scheduler.cancel(command) + scheduler.run() + + verify(command).execute() + verify(command).end(True) + verify(command, never()).end(False) + + assert not scheduler.isScheduled(command) + + +def test_notScheduledCancel(scheduler: commands2.CommandScheduler): + command = commands2.Command() + + scheduler.cancel(command) diff --git a/rio/ottomanEmpire/tests/test_commandgroup_error.py b/rio/ottomanEmpire/tests/test_commandgroup_error.py new file mode 100644 index 00000000..36008f1a --- /dev/null +++ b/rio/ottomanEmpire/tests/test_commandgroup_error.py @@ -0,0 +1,38 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_commandInMultipleGroups(): + command1 = commands2.Command() + command2 = commands2.Command() + + commands2.ParallelCommandGroup(command1, command2) + with pytest.raises(commands2.IllegalCommandUse): + commands2.ParallelCommandGroup(command1, command2) + + +def test_commandInGroupExternallyScheduled(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + + commands2.ParallelCommandGroup(command1, command2) + + with pytest.raises(commands2.IllegalCommandUse): + scheduler.schedule(command1) + + +@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") +def test_redecoratedCommandError(scheduler: commands2.CommandScheduler): + command = commands2.InstantCommand() + command.withTimeout(10).until(lambda: False) + with pytest.raises(commands2.IllegalCommandUse): + command.withTimeout(10) + scheduler.removeComposedCommands([command]) + command.withTimeout(10) diff --git a/rio/ottomanEmpire/tests/test_conditional_command.py b/rio/ottomanEmpire/tests/test_conditional_command.py new file mode 100644 index 00000000..caeff2da --- /dev/null +++ b/rio/ottomanEmpire/tests/test_conditional_command.py @@ -0,0 +1,55 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_conditionalCommand(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command1.isFinished = lambda: True + command2 = commands2.Command() + + start_spying_on(command1) + start_spying_on(command2) + + conditionalCommand = commands2.ConditionalCommand(command1, command2, lambda: True) + + scheduler.schedule(conditionalCommand) + scheduler.run() + + verify(command1).initialize() + verify(command1).execute() + verify(command1).end(False) + + verify(command2, never()).initialize() + verify(command2, never()).execute() + verify(command2, never()).end(False) + + +def test_conditionalCommandRequirement(scheduler: commands2.CommandScheduler): + system1 = commands2.Subsystem() + system2 = commands2.Subsystem() + system3 = commands2.Subsystem() + + command1 = commands2.Command() + command1.addRequirements(system1, system2) + command2 = commands2.Command() + command2.addRequirements(system3) + + start_spying_on(command1) + start_spying_on(command2) + + conditionalCommand = commands2.ConditionalCommand(command1, command2, lambda: True) + + scheduler.schedule(conditionalCommand) + scheduler.schedule(commands2.InstantCommand(lambda: None, system3)) + + assert not scheduler.isScheduled(conditionalCommand) + + assert command1.end.called_with(True) + assert not command2.end.called_with(True) diff --git a/rio/ottomanEmpire/tests/test_default_command.py b/rio/ottomanEmpire/tests/test_default_command.py new file mode 100644 index 00000000..211ecae4 --- /dev/null +++ b/rio/ottomanEmpire/tests/test_default_command.py @@ -0,0 +1,73 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_defaultCommandSchedule(scheduler: commands2.CommandScheduler): + hasDefaultCommand = commands2.Subsystem() + + defaultCommand = commands2.Command() + defaultCommand.addRequirements(hasDefaultCommand) + + scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand) + scheduler.run() + + assert scheduler.isScheduled(defaultCommand) + + +def test_defaultCommandInterruptResume(scheduler: commands2.CommandScheduler): + hasDefaultCommand = commands2.Subsystem() + + defaultCommand = commands2.Command() + defaultCommand.addRequirements(hasDefaultCommand) + + interrupter = commands2.Command() + interrupter.addRequirements(hasDefaultCommand) + + scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand) + scheduler.run() + scheduler.schedule(interrupter) + + assert not scheduler.isScheduled(defaultCommand) + assert scheduler.isScheduled(interrupter) + + scheduler.cancel(interrupter) + scheduler.run() + + assert scheduler.isScheduled(defaultCommand) + assert not scheduler.isScheduled(interrupter) + + +def test_defaultCommandDisableResume(scheduler: commands2.CommandScheduler): + hasDefaultCommand = commands2.Subsystem() + + defaultCommand = commands2.Command() + defaultCommand.addRequirements(hasDefaultCommand) + defaultCommand.runsWhenDisabled = lambda: False + + start_spying_on(defaultCommand) + + scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand) + scheduler.run() + + assert scheduler.isScheduled(defaultCommand) + + DriverStationSim.setEnabled(False) + DriverStationSim.notifyNewData() + scheduler.run() + + assert not scheduler.isScheduled(defaultCommand) + + DriverStationSim.setEnabled(True) + DriverStationSim.notifyNewData() + scheduler.run() + + assert scheduler.isScheduled(defaultCommand) + + assert defaultCommand.end.called_with(True) diff --git a/rio/ottomanEmpire/tests/test_functional_command.py b/rio/ottomanEmpire/tests/test_functional_command.py new file mode 100644 index 00000000..c6481cab --- /dev/null +++ b/rio/ottomanEmpire/tests/test_functional_command.py @@ -0,0 +1,36 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_functionalCommandSchedule(scheduler: commands2.CommandScheduler): + cond1 = OOBoolean() + cond2 = OOBoolean() + cond3 = OOBoolean() + cond4 = OOBoolean() + + command = commands2.FunctionalCommand( + lambda: cond1.set(True), + lambda: cond2.set(True), + lambda _: cond3.set(True), + lambda: cond4.get(), + ) + + scheduler.schedule(command) + scheduler.run() + + assert scheduler.isScheduled(command) + + cond4.set(True) + scheduler.run() + + assert not scheduler.isScheduled(command) + assert cond1 + assert cond2 + assert cond3 diff --git a/rio/ottomanEmpire/tests/test_instant_command.py b/rio/ottomanEmpire/tests/test_instant_command.py new file mode 100644 index 00000000..96d3089e --- /dev/null +++ b/rio/ottomanEmpire/tests/test_instant_command.py @@ -0,0 +1,21 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_instantCommandSchedule(scheduler: commands2.CommandScheduler): + cond = OOBoolean() + + command = commands2.InstantCommand(lambda: cond.set(True)) + + scheduler.schedule(command) + scheduler.run() + + assert cond + assert not scheduler.isScheduled(command) diff --git a/rio/ottomanEmpire/tests/test_networkbutton.py b/rio/ottomanEmpire/tests/test_networkbutton.py new file mode 100644 index 00000000..d49f687d --- /dev/null +++ b/rio/ottomanEmpire/tests/test_networkbutton.py @@ -0,0 +1,29 @@ +from typing import TYPE_CHECKING + +import commands2 +from ntcore import NetworkTableInstance +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + + +def test_networkbutton( + scheduler: commands2.CommandScheduler, nt_instance: NetworkTableInstance +): + # command = commands2.Command() + command = commands2.Command() + start_spying_on(command) + + pub = nt_instance.getTable("TestTable").getBooleanTopic("Test").publish() + + button = commands2.button.NetworkButton(nt_instance, "TestTable", "Test") + + pub.set(False) + button.onTrue(command) + scheduler.run() + assert command.schedule.times_called == 0 + pub.set(True) + scheduler.run() + scheduler.run() + verify(command).schedule() diff --git a/rio/ottomanEmpire/tests/test_notifier_command.py b/rio/ottomanEmpire/tests/test_notifier_command.py new file mode 100644 index 00000000..3022c20c --- /dev/null +++ b/rio/ottomanEmpire/tests/test_notifier_command.py @@ -0,0 +1,23 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +@pytest.mark.skip(reason="NotifierCommand is broken") +def test_notifierCommandScheduler(scheduler: commands2.CommandScheduler): + with ManualSimTime() as sim: + counter = OOInteger(0) + command = commands2.NotifierCommand(counter.incrementAndGet, 0.01) + + scheduler.schedule(command) + for i in range(5): + sim.step(0.005) + scheduler.cancel(command) + + assert counter == 2 diff --git a/rio/ottomanEmpire/tests/test_parallelcommandgroup.py b/rio/ottomanEmpire/tests/test_parallelcommandgroup.py new file mode 100644 index 00000000..04355af7 --- /dev/null +++ b/rio/ottomanEmpire/tests/test_parallelcommandgroup.py @@ -0,0 +1,117 @@ +from typing import TYPE_CHECKING + +import commands2 +from compositiontestbase import MultiCompositionTestBase # type: ignore +from util import * # type: ignore + +# from tests.compositiontestbase import T + +if TYPE_CHECKING: + from .util import * + from .compositiontestbase import MultiCompositionTestBase + +import pytest + + +class TestParallelCommandGroupComposition(MultiCompositionTestBase): + def compose(self, *members: commands2.Command): + return commands2.ParallelCommandGroup(*members) + + +def test_parallelGroupSchedule(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + + start_spying_on(command1) + start_spying_on(command2) + + group = commands2.ParallelCommandGroup(command1, command2) + + scheduler.schedule(group) + + verify(command1).initialize() + verify(command2).initialize() + + command1.isFinished = lambda: True + scheduler.run() + command2.isFinished = lambda: True + scheduler.run() + + verify(command1).execute() + verify(command1).end(False) + verify(command2, times(2)).execute() + verify(command2).end(False) + + assert not scheduler.isScheduled(group) + + +def test_parallelGroupInterrupt(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + + start_spying_on(command1) + start_spying_on(command2) + + group = commands2.ParallelCommandGroup(command1, command2) + + scheduler.schedule(group) + + command1.isFinished = lambda: True + scheduler.run() + scheduler.run() + scheduler.cancel(group) + + verify(command1).execute() + verify(command1).end(False) + verify(command1, never()).end(True) + + verify(command2, times(2)).execute() + verify(command2, never()).end(False) + verify(command2).end(True) + + assert not scheduler.isScheduled(group) + + +def test_notScheduledCancel(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + + group = commands2.ParallelCommandGroup(command1, command2) + + scheduler.cancel(group) + + +def test_parallelGroupRequirement(scheduler: commands2.CommandScheduler): + system1 = commands2.Subsystem() + system2 = commands2.Subsystem() + system3 = commands2.Subsystem() + system4 = commands2.Subsystem() + + command1 = commands2.Command() + command1.addRequirements(system1, system2) + command2 = commands2.Command() + command2.addRequirements(system3) + command3 = commands2.Command() + command3.addRequirements(system3, system4) + + group = commands2.ParallelCommandGroup(command1, command2) + + scheduler.schedule(group) + scheduler.schedule(command3) + + assert not scheduler.isScheduled(group) + assert scheduler.isScheduled(command3) + + +def test_parallelGroupRequirementError(): + system1 = commands2.Subsystem() + system2 = commands2.Subsystem() + system3 = commands2.Subsystem() + + command1 = commands2.Command() + command1.addRequirements(system1, system2) + command2 = commands2.Command() + command2.addRequirements(system2, system3) + + with pytest.raises(commands2.IllegalCommandUse): + commands2.ParallelCommandGroup(command1, command2) diff --git a/rio/ottomanEmpire/tests/test_paralleldeadlinegroup.py b/rio/ottomanEmpire/tests/test_paralleldeadlinegroup.py new file mode 100644 index 00000000..9136509b --- /dev/null +++ b/rio/ottomanEmpire/tests/test_paralleldeadlinegroup.py @@ -0,0 +1,119 @@ +from typing import TYPE_CHECKING + +import commands2 +from compositiontestbase import MultiCompositionTestBase # type: ignore +from util import * # type: ignore + +# from tests.compositiontestbase import T + +if TYPE_CHECKING: + from .util import * + from .compositiontestbase import MultiCompositionTestBase + +import pytest + + +class TestParallelDeadlineGroupComposition(MultiCompositionTestBase): + def compose(self, *members: commands2.Command): + return commands2.ParallelDeadlineGroup(members[0], *members[1:]) + + +def test_parallelDeadlineSchedule(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + command2.isFinished = lambda: True + command3 = commands2.Command() + + start_spying_on(command1) + start_spying_on(command2) + start_spying_on(command3) + + group = commands2.ParallelDeadlineGroup(command1, command2, command3) + + scheduler.schedule(group) + scheduler.run() + + assert scheduler.isScheduled(group) + + command1.isFinished = lambda: True + scheduler.run() + + assert not scheduler.isScheduled(group) + + verify(command2).initialize() + verify(command2).execute() + verify(command2).end(False) + verify(command2, never()).end(True) + + verify(command1).initialize() + verify(command1, times(2)).execute() + verify(command1).end(False) + verify(command1, never()).end(True) + + verify(command3).initialize() + verify(command3, times(2)).execute() + verify(command3, never()).end(False) + verify(command3).end(True) + + +def test_parallelDeadlineInterrupt(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + command2.isFinished = lambda: True + + start_spying_on(command1) + start_spying_on(command2) + + group = commands2.ParallelDeadlineGroup(command1, command2) + + scheduler.schedule(group) + + scheduler.run() + scheduler.run() + scheduler.cancel(group) + + verify(command1, times(2)).execute() + verify(command1, never()).end(False) + verify(command1).end(True) + + verify(command2).execute() + verify(command2).end(False) + verify(command2, never()).end(True) + + assert not scheduler.isScheduled(group) + + +def test_parallelDeadlineRequirement(scheduler: commands2.CommandScheduler): + system1 = commands2.Subsystem() + system2 = commands2.Subsystem() + system3 = commands2.Subsystem() + system4 = commands2.Subsystem() + + command1 = commands2.Command() + command1.addRequirements(system1, system2) + command2 = commands2.Command() + command2.addRequirements(system3) + command3 = commands2.Command() + command3.addRequirements(system3, system4) + + group = commands2.ParallelDeadlineGroup(command1, command2) + + scheduler.schedule(group) + scheduler.schedule(command3) + + assert not scheduler.isScheduled(group) + assert scheduler.isScheduled(command3) + + +def test_parallelDeadlineRequirementError(scheduler: commands2.CommandScheduler): + system1 = commands2.Subsystem() + system2 = commands2.Subsystem() + system3 = commands2.Subsystem() + + command1 = commands2.Command() + command1.addRequirements(system1, system2) + command2 = commands2.Command() + command2.addRequirements(system2, system3) + + with pytest.raises(commands2.IllegalCommandUse): + commands2.ParallelDeadlineGroup(command1, command2) diff --git a/rio/ottomanEmpire/tests/test_parallelracegroup.py b/rio/ottomanEmpire/tests/test_parallelracegroup.py new file mode 100644 index 00000000..58e2ace4 --- /dev/null +++ b/rio/ottomanEmpire/tests/test_parallelracegroup.py @@ -0,0 +1,183 @@ +from typing import TYPE_CHECKING + +import commands2 +from compositiontestbase import MultiCompositionTestBase # type: ignore +from util import * # type: ignore + +# from tests.compositiontestbase import T + +if TYPE_CHECKING: + from .util import * + from .compositiontestbase import MultiCompositionTestBase + +import pytest + + +class TestParallelRaceGroupComposition(MultiCompositionTestBase): + def compose(self, *members: commands2.Command): + return commands2.ParallelRaceGroup(*members) + + +def test_parallelRaceSchedule(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + + start_spying_on(command1) + start_spying_on(command2) + + group = commands2.ParallelRaceGroup(command1, command2) + + scheduler.schedule(group) + + verify(command1).initialize() + verify(command2).initialize() + + command1.isFinished = lambda: True + scheduler.run() + command2.isFinished = lambda: True + scheduler.run() + + verify(command1).execute() + verify(command1).end(False) + verify(command2).execute() + verify(command2).end(True) + verify(command2, never()).end(False) + + assert not scheduler.isScheduled(group) + + +def test_parallelRaceInterrupt(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + + start_spying_on(command1) + start_spying_on(command2) + + group = commands2.ParallelRaceGroup(command1, command2) + + scheduler.schedule(group) + + scheduler.run() + scheduler.run() + scheduler.cancel(group) + + verify(command1, times(2)).execute() + verify(command1, never()).end(False) + verify(command1).end(True) + + verify(command2, times(2)).execute() + verify(command2, never()).end(False) + verify(command2).end(True) + + assert not scheduler.isScheduled(group) + + +def test_notScheduledCancel(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + + group = commands2.ParallelRaceGroup(command1, command2) + + scheduler.cancel(group) + + +def test_parallelRaceRequirement(scheduler: commands2.CommandScheduler): + system1 = commands2.Subsystem() + system2 = commands2.Subsystem() + system3 = commands2.Subsystem() + system4 = commands2.Subsystem() + + command1 = commands2.Command() + command1.addRequirements(system1, system2) + command2 = commands2.Command() + command2.addRequirements(system3) + command3 = commands2.Command() + command3.addRequirements(system3, system4) + + group = commands2.ParallelRaceGroup(command1, command2) + + scheduler.schedule(group) + scheduler.schedule(command3) + + assert not scheduler.isScheduled(group) + assert scheduler.isScheduled(command3) + + +def test_parallelRaceRequirementError(): + system1 = commands2.Subsystem() + system2 = commands2.Subsystem() + system3 = commands2.Subsystem() + + command1 = commands2.Command() + command1.addRequirements(system1, system2) + command2 = commands2.Command() + command2.addRequirements(system2, system3) + + with pytest.raises(commands2.IllegalCommandUse): + commands2.ParallelRaceGroup(command1, command2) + + +def test_parallelRaceOnlyCallsEndOnce(scheduler: commands2.CommandScheduler): + system1 = commands2.Subsystem() + system2 = commands2.Subsystem() + + command1 = commands2.Command() + command1.addRequirements(system1) + command2 = commands2.Command() + command2.addRequirements(system2) + command3 = commands2.Command() + + group1 = commands2.SequentialCommandGroup(command1, command2) + group2 = commands2.ParallelRaceGroup(group1, command3) + + scheduler.schedule(group2) + scheduler.run() + command1.isFinished = lambda: True + scheduler.run() + command2.isFinished = lambda: True + scheduler.run() + assert not scheduler.isScheduled(group2) + + +def test_parallelRaceScheduleTwiceTest(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + + start_spying_on(command1) + start_spying_on(command2) + + group = commands2.ParallelRaceGroup(command1, command2) + + scheduler.schedule(group) + + verify(command1).initialize() + verify(command2).initialize() + + command1.isFinished = lambda: True + scheduler.run() + command2.isFinished = lambda: True + scheduler.run() + + verify(command1).execute() + verify(command1).end(False) + verify(command2).execute() + verify(command2).end(True) + verify(command2, never()).end(False) + + assert not scheduler.isScheduled(group) + + reset(command1) + reset(command2) + + scheduler.schedule(group) + + verify(command1).initialize() + verify(command2).initialize() + + scheduler.run() + scheduler.run() + assert scheduler.isScheduled(group) + command2.isFinished = lambda: True + scheduler.run() + + assert not scheduler.isScheduled(group) diff --git a/rio/ottomanEmpire/tests/test_perpetualcommand.py b/rio/ottomanEmpire/tests/test_perpetualcommand.py new file mode 100644 index 00000000..55284ff1 --- /dev/null +++ b/rio/ottomanEmpire/tests/test_perpetualcommand.py @@ -0,0 +1,18 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_perpetualCommandSchedule(scheduler: commands2.CommandScheduler): + command = commands2.PerpetualCommand(commands2.InstantCommand()) + + scheduler.schedule(command) + scheduler.run() + + assert scheduler.isScheduled(command) diff --git a/rio/ottomanEmpire/tests/test_pidcommand.py b/rio/ottomanEmpire/tests/test_pidcommand.py new file mode 100644 index 00000000..5ba51650 --- /dev/null +++ b/rio/ottomanEmpire/tests/test_pidcommand.py @@ -0,0 +1,114 @@ +from typing import TYPE_CHECKING + +from util import * # type: ignore +import wpimath.controller as controller +import commands2 + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_pidCommandSupplier(scheduler: commands2.CommandScheduler): + with ManualSimTime() as sim: + output_float = OOFloat(0.0) + measurement_source = OOFloat(5.0) + setpoint_source = OOFloat(2.0) + pid_controller = controller.PIDController(0.1, 0.01, 0.001) + system = commands2.Subsystem() + pidCommand = commands2.PIDCommand( + pid_controller, + measurement_source, + setpoint_source, + output_float.set, + system, + ) + start_spying_on(pidCommand) + scheduler.schedule(pidCommand) + scheduler.run() + sim.step(1) + scheduler.run() + + assert scheduler.isScheduled(pidCommand) + + assert not pidCommand._controller.atSetpoint() + + # Tell the pid command we're at our setpoint through the controller + measurement_source.set(setpoint_source()) + + sim.step(2) + + scheduler.run() + + # Should be measuring error of 0 now + assert pidCommand._controller.atSetpoint() + + +def test_pidCommandScalar(scheduler: commands2.CommandScheduler): + with ManualSimTime() as sim: + output_float = OOFloat(0.0) + measurement_source = OOFloat(5.0) + setpoint_source = 2.0 + pid_controller = controller.PIDController(0.1, 0.01, 0.001) + system = commands2.Subsystem() + pidCommand = commands2.PIDCommand( + pid_controller, + measurement_source, + setpoint_source, + output_float.set, + system, + ) + start_spying_on(pidCommand) + scheduler.schedule(pidCommand) + scheduler.run() + sim.step(1) + scheduler.run() + + assert scheduler.isScheduled(pidCommand) + + assert not pidCommand._controller.atSetpoint() + + # Tell the pid command we're at our setpoint through the controller + measurement_source.set(setpoint_source) + + sim.step(2) + + scheduler.run() + + # Should be measuring error of 0 now + assert pidCommand._controller.atSetpoint() + + +def test_withTimeout(scheduler: commands2.CommandScheduler): + with ManualSimTime() as sim: + output_float = OOFloat(0.0) + measurement_source = OOFloat(5.0) + setpoint_source = OOFloat(2.0) + pid_controller = controller.PIDController(0.1, 0.01, 0.001) + system = commands2.Subsystem() + command1 = commands2.PIDCommand( + pid_controller, + measurement_source, + setpoint_source, + output_float.set, + system, + ) + start_spying_on(command1) + + timeout = command1.withTimeout(2) + + scheduler.schedule(timeout) + scheduler.run() + + verify(command1).initialize() + verify(command1).execute() + assert not scheduler.isScheduled(command1) + assert scheduler.isScheduled(timeout) + + sim.step(3) + scheduler.run() + + verify(command1).end(True) + verify(command1, never()).end(False) + assert not scheduler.isScheduled(timeout) diff --git a/rio/ottomanEmpire/tests/test_printcommand.py b/rio/ottomanEmpire/tests/test_printcommand.py new file mode 100644 index 00000000..6d28cad6 --- /dev/null +++ b/rio/ottomanEmpire/tests/test_printcommand.py @@ -0,0 +1,17 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_printCommandSchedule(capsys, scheduler: commands2.CommandScheduler): + command = commands2.PrintCommand("Test!") + scheduler.schedule(command) + scheduler.run() + assert not scheduler.isScheduled(command) + assert capsys.readouterr().out == "Test!\n" diff --git a/rio/ottomanEmpire/tests/test_proxycommand.py b/rio/ottomanEmpire/tests/test_proxycommand.py new file mode 100644 index 00000000..5f1add10 --- /dev/null +++ b/rio/ottomanEmpire/tests/test_proxycommand.py @@ -0,0 +1,40 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") +def test_proxyCommandSchedule(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + start_spying_on(command1) + + scheduleCommand = commands2.ProxyCommand(command1) + + scheduler.schedule(scheduleCommand) + + verify(command1).schedule() + + +@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") +def test_proxyCommandEnd(scheduler: commands2.CommandScheduler): + cond = OOBoolean() + + command = commands2.WaitUntilCommand(cond) + + scheduleCommand = commands2.ProxyCommand(command) + + scheduler.schedule(scheduleCommand) + scheduler.run() + + assert scheduler.isScheduled(scheduleCommand) + + cond.set(True) + scheduler.run() + scheduler.run() + assert not scheduler.isScheduled(scheduleCommand) diff --git a/rio/ottomanEmpire/tests/test_repeatcommand.py b/rio/ottomanEmpire/tests/test_repeatcommand.py new file mode 100644 index 00000000..e2ff3be9 --- /dev/null +++ b/rio/ottomanEmpire/tests/test_repeatcommand.py @@ -0,0 +1,69 @@ +from typing import TYPE_CHECKING + +import commands2 +from compositiontestbase import SingleCompositionTestBase # type: ignore +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + from .compositiontestbase import SingleCompositionTestBase + +import pytest + + +class RepeatCommandCompositionTest(SingleCompositionTestBase): + def composeSingle(self, member): + return member.repeatedly() + + +def test_callsMethodsCorrectly(scheduler: commands2.CommandScheduler): + command = commands2.Command() + repeated = command.repeatedly() + + start_spying_on(command) + + assert command.initialize.times_called == 0 + assert command.execute.times_called == 0 + assert command.isFinished.times_called == 0 + assert command.end.times_called == 0 + + scheduler.schedule(repeated) + assert command.initialize.times_called == 1 + assert command.execute.times_called == 0 + assert command.isFinished.times_called == 0 + assert command.end.times_called == 0 + + command.isFinished = lambda: False + + scheduler.run() + assert command.initialize.times_called == 1 + assert command.execute.times_called == 1 + assert command.isFinished.times_called == 1 + assert command.end.times_called == 0 + + command.isFinished = lambda: True + scheduler.run() + assert command.initialize.times_called == 1 + assert command.execute.times_called == 2 + assert command.isFinished.times_called == 2 + assert command.end.times_called == 1 + + command.isFinished = lambda: False + scheduler.run() + assert command.initialize.times_called == 2 + assert command.execute.times_called == 3 + assert command.isFinished.times_called == 3 + assert command.end.times_called == 1 + + command.isFinished = lambda: True + scheduler.run() + assert command.initialize.times_called == 2 + assert command.execute.times_called == 4 + assert command.isFinished.times_called == 4 + assert command.end.times_called == 2 + + scheduler.cancel(repeated) + assert command.initialize.times_called == 2 + assert command.execute.times_called == 4 + assert command.isFinished.times_called == 4 + assert command.end.times_called == 2 diff --git a/rio/ottomanEmpire/tests/test_robotdisabledcommand.py b/rio/ottomanEmpire/tests/test_robotdisabledcommand.py new file mode 100644 index 00000000..d34915f3 --- /dev/null +++ b/rio/ottomanEmpire/tests/test_robotdisabledcommand.py @@ -0,0 +1,157 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +if IS_OLD_COMMANDS: + import commands2.cmd + +import pytest +from wpilib import RobotState + + +def test_robotDisabledCommandCancel(scheduler: commands2.CommandScheduler): + command = commands2.Command() + scheduler.schedule(command) + assert scheduler.isScheduled(command) + DriverStationSim.setEnabled(False) + DriverStationSim.notifyNewData() + scheduler.run() + assert not scheduler.isScheduled(command) + DriverStationSim.setEnabled(True) + DriverStationSim.notifyNewData() + + +def test_runWhenDisabled(scheduler: commands2.CommandScheduler): + command = commands2.Command() + command.runsWhenDisabled = lambda: True + + scheduler.schedule(command) + + assert scheduler.isScheduled(command) + + DriverStationSim.setEnabled(False) + DriverStationSim.notifyNewData() + + scheduler.run() + + assert scheduler.isScheduled(command) + + +def test_sequentialGroupRunWhenDisabled(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command1.runsWhenDisabled = lambda: True + command2 = commands2.Command() + command2.runsWhenDisabled = lambda: True + command3 = commands2.Command() + command3.runsWhenDisabled = lambda: True + command4 = commands2.Command() + command4.runsWhenDisabled = lambda: False + + runWhenDisabled = commands2.SequentialCommandGroup(command1, command2) + dontRunWhenDisabled = commands2.SequentialCommandGroup(command3, command4) + + scheduler.schedule(runWhenDisabled) + scheduler.schedule(dontRunWhenDisabled) + + DriverStationSim.setEnabled(False) + DriverStationSim.notifyNewData() + + scheduler.run() + + assert scheduler.isScheduled(runWhenDisabled) + assert not scheduler.isScheduled(dontRunWhenDisabled) + + +def test_parallelGroupRunWhenDisabled(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command1.runsWhenDisabled = lambda: True + command2 = commands2.Command() + command2.runsWhenDisabled = lambda: True + command3 = commands2.Command() + command3.runsWhenDisabled = lambda: True + command4 = commands2.Command() + command4.runsWhenDisabled = lambda: False + + runWhenDisabled = commands2.ParallelCommandGroup(command1, command2) + dontRunWhenDisabled = commands2.ParallelCommandGroup(command3, command4) + + scheduler.schedule(runWhenDisabled) + scheduler.schedule(dontRunWhenDisabled) + + DriverStationSim.setEnabled(False) + DriverStationSim.notifyNewData() + + scheduler.run() + + assert scheduler.isScheduled(runWhenDisabled) + assert not scheduler.isScheduled(dontRunWhenDisabled) + + +def test_conditionalRunWhenDisabled(scheduler: commands2.CommandScheduler): + DriverStationSim.setEnabled(False) + DriverStationSim.notifyNewData() + + command1 = commands2.Command() + command1.runsWhenDisabled = lambda: True + command2 = commands2.Command() + command2.runsWhenDisabled = lambda: True + command3 = commands2.Command() + command3.runsWhenDisabled = lambda: True + command4 = commands2.Command() + command4.runsWhenDisabled = lambda: False + + runWhenDisabled = commands2.ConditionalCommand(command1, command2, lambda: True) + dontRunWhenDisabled = commands2.ConditionalCommand(command3, command4, lambda: True) + + scheduler.schedule(runWhenDisabled, dontRunWhenDisabled) + + assert scheduler.isScheduled(runWhenDisabled) + assert not scheduler.isScheduled(dontRunWhenDisabled) + + +def test_selectRunWhenDisabled(scheduler: commands2.CommandScheduler): + DriverStationSim.setEnabled(False) + DriverStationSim.notifyNewData() + + command1 = commands2.Command() + command1.runsWhenDisabled = lambda: True + command2 = commands2.Command() + command2.runsWhenDisabled = lambda: True + command3 = commands2.Command() + command3.runsWhenDisabled = lambda: True + command4 = commands2.Command() + command4.runsWhenDisabled = lambda: False + + runWhenDisabled = commands2.SelectCommand({1: command1, 2: command2}, lambda: 1) + dontRunWhenDisabled = commands2.SelectCommand({1: command3, 2: command4}, lambda: 1) + + scheduler.schedule(runWhenDisabled, dontRunWhenDisabled) + assert scheduler.isScheduled(runWhenDisabled) + assert not scheduler.isScheduled(dontRunWhenDisabled) + + +def test_parallelConditionalRunWhenDisabledTest(scheduler: commands2.CommandScheduler): + DriverStationSim.setEnabled(False) + DriverStationSim.notifyNewData() + + command1 = commands2.Command() + command1.runsWhenDisabled = lambda: True + command2 = commands2.Command() + command2.runsWhenDisabled = lambda: True + command3 = commands2.Command() + command3.runsWhenDisabled = lambda: True + command4 = commands2.Command() + command4.runsWhenDisabled = lambda: False + + runWhenDisabled = commands2.ConditionalCommand(command1, command2, lambda: True) + dontRunWhenDisabled = commands2.ConditionalCommand(command3, command4, lambda: True) + + parallel = commands2.cmd.parallel(runWhenDisabled, dontRunWhenDisabled) + + scheduler.schedule(parallel) + + assert not scheduler.isScheduled(runWhenDisabled) diff --git a/rio/ottomanEmpire/tests/test_runcommand.py b/rio/ottomanEmpire/tests/test_runcommand.py new file mode 100644 index 00000000..6bd4fcff --- /dev/null +++ b/rio/ottomanEmpire/tests/test_runcommand.py @@ -0,0 +1,22 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_runCommandSchedule(scheduler: commands2.CommandScheduler): + counter = OOInteger(0) + + command = commands2.RunCommand(counter.incrementAndGet) + + scheduler.schedule(command) + scheduler.run() + scheduler.run() + scheduler.run() + + assert counter == 3 diff --git a/rio/ottomanEmpire/tests/test_schedulecommand.py b/rio/ottomanEmpire/tests/test_schedulecommand.py new file mode 100644 index 00000000..401f4e0d --- /dev/null +++ b/rio/ottomanEmpire/tests/test_schedulecommand.py @@ -0,0 +1,36 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_scheduleCommandSchedule(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + + start_spying_on(command1) + start_spying_on(command2) + + scheduleCommand = commands2.ScheduleCommand(command1, command2) + + scheduler.schedule(scheduleCommand) + + verify(command1).schedule() + verify(command2).schedule() + + +def test_scheduleCommandDruingRun(scheduler: commands2.CommandScheduler): + toSchedule = commands2.InstantCommand() + scheduleCommand = commands2.ScheduleCommand(toSchedule) + group = commands2.SequentialCommandGroup( + commands2.InstantCommand(), scheduleCommand + ) + + scheduler.schedule(group) + scheduler.schedule(commands2.RunCommand(lambda: None)) + scheduler.run() diff --git a/rio/ottomanEmpire/tests/test_scheduler.py b/rio/ottomanEmpire/tests/test_scheduler.py new file mode 100644 index 00000000..4ee7f32c --- /dev/null +++ b/rio/ottomanEmpire/tests/test_scheduler.py @@ -0,0 +1,67 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_schedulerLambdaTestNoInterrupt(scheduler: commands2.CommandScheduler): + counter = OOInteger() + + scheduler.onCommandInitialize(lambda _: counter.incrementAndGet()) + scheduler.onCommandExecute(lambda _: counter.incrementAndGet()) + scheduler.onCommandFinish(lambda _: counter.incrementAndGet()) + + scheduler.schedule(commands2.InstantCommand()) + scheduler.run() + + assert counter == 3 + + +def test_schedulerInterruptLambda(scheduler: commands2.CommandScheduler): + counter = OOInteger() + + scheduler.onCommandInterrupt(lambda _: counter.incrementAndGet()) + + command = commands2.WaitCommand(10) + + scheduler.schedule(command) + scheduler.cancel(command) + + assert counter == 1 + + +def test_unregisterSubsystem(scheduler: commands2.CommandScheduler): + system = commands2.Subsystem() + scheduler.registerSubsystem(system) + scheduler.unregisterSubsystem(system) + + +def test_schedulerCancelAll(scheduler: commands2.CommandScheduler): + counter = OOInteger() + + scheduler.onCommandInterrupt(lambda _: counter.incrementAndGet()) + + command = commands2.WaitCommand(10) + command2 = commands2.WaitCommand(10) + + scheduler.schedule(command) + scheduler.schedule(command2) + scheduler.cancelAll() + + assert counter == 2 + + +def test_scheduleScheduledNoOp(scheduler: commands2.CommandScheduler): + counter = OOInteger() + + command = commands2.cmd.startEnd(counter.incrementAndGet, lambda: None) + + scheduler.schedule(command) + scheduler.schedule(command) + + assert counter == 1 diff --git a/rio/ottomanEmpire/tests/test_schedulingrecursion.py b/rio/ottomanEmpire/tests/test_schedulingrecursion.py new file mode 100644 index 00000000..ff6230dc --- /dev/null +++ b/rio/ottomanEmpire/tests/test_schedulingrecursion.py @@ -0,0 +1,165 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +@pytest.mark.parametrize( + "interruptionBehavior", + [ + commands2.InterruptionBehavior.kCancelIncoming, + commands2.InterruptionBehavior.kCancelSelf, + ], +) +def test_cancelFromInitialize( + interruptionBehavior: commands2.InterruptionBehavior, + scheduler: commands2.CommandScheduler, +): + hasOtherRun = OOBoolean() + requirement = commands2.Subsystem() + + selfCancels = commands2.Command() + selfCancels.addRequirements(requirement) + selfCancels.getInterruptionBehavior = lambda: interruptionBehavior + selfCancels.initialize = lambda: scheduler.cancel(selfCancels) + + other = commands2.RunCommand(lambda: hasOtherRun.set(True), requirement) + + scheduler.schedule(selfCancels) + scheduler.run() + scheduler.schedule(other) + + assert not scheduler.isScheduled(selfCancels) + assert scheduler.isScheduled(other) + scheduler.run() + assert hasOtherRun == True + + +@pytest.mark.parametrize( + "interruptionBehavior", + [ + commands2.InterruptionBehavior.kCancelIncoming, + commands2.InterruptionBehavior.kCancelSelf, + ], +) +def test_defaultCommandGetsRescheduledAfterSelfCanceling( + interruptionBehavior: commands2.InterruptionBehavior, + scheduler: commands2.CommandScheduler, +): + hasOtherRun = OOBoolean() + requirement = commands2.Subsystem() + + selfCancels = commands2.Command() + selfCancels.addRequirements(requirement) + selfCancels.getInterruptionBehavior = lambda: interruptionBehavior + selfCancels.initialize = lambda: scheduler.cancel(selfCancels) + + other = commands2.RunCommand(lambda: hasOtherRun.set(True), requirement) + scheduler.setDefaultCommand(requirement, other) + + scheduler.schedule(selfCancels) + scheduler.run() + + scheduler.run() + assert not scheduler.isScheduled(selfCancels) + assert scheduler.isScheduled(other) + scheduler.run() + assert hasOtherRun == True + + +def test_cancelFromEnd(scheduler: commands2.CommandScheduler): + counter = OOInteger() + selfCancels = commands2.Command() + + @patch_via_decorator(selfCancels) + def end(self, interrupted): + counter.incrementAndGet() + scheduler.cancel(self) + + scheduler.schedule(selfCancels) + + scheduler.cancel(selfCancels) + assert counter == 1 + assert not scheduler.isScheduled(selfCancels) + + +def test_scheduleFromEnd(scheduler: commands2.CommandScheduler): + counter = OOInteger() + requirement = commands2.Subsystem() + other = commands2.InstantCommand(lambda: None, requirement) + + selfCancels = commands2.Command() + selfCancels.addRequirements(requirement) + + @patch_via_decorator(selfCancels) + def end(self, interrupted): + counter.incrementAndGet() + scheduler.schedule(other) + + scheduler.schedule(selfCancels) + + scheduler.cancel(selfCancels) + assert counter == 1 + assert not scheduler.isScheduled(selfCancels) + + +def test_scheduleFromEndInterrupt(scheduler: commands2.CommandScheduler): + counter = OOInteger() + requirement = commands2.Subsystem() + other = commands2.InstantCommand(lambda: None, requirement) + + selfCancels = commands2.Command() + selfCancels.addRequirements(requirement) + + @patch_via_decorator(selfCancels) + def end(self, interrupted): + counter.incrementAndGet() + scheduler.schedule(other) + + scheduler.schedule(selfCancels) + + scheduler.schedule(other) + assert counter == 1 + assert not scheduler.isScheduled(selfCancels) + assert scheduler.isScheduled(other) + + +@pytest.mark.parametrize( + "interruptionBehavior", + [ + commands2.InterruptionBehavior.kCancelIncoming, + commands2.InterruptionBehavior.kCancelSelf, + ], +) +def test_scheduleInitializeFromDefaultCommand( + interruptionBehavior: commands2.InterruptionBehavior, + scheduler: commands2.CommandScheduler, +): + counter = OOInteger() + requirement = commands2.Subsystem() + other = commands2.InstantCommand(lambda: None, requirement).withInterruptBehavior( + interruptionBehavior + ) + + defaultCommand = commands2.Command() + defaultCommand.addRequirements(requirement) + + @patch_via_decorator(defaultCommand) + def initialize(self): + counter.incrementAndGet() + scheduler.schedule(other) + + scheduler.setDefaultCommand(requirement, defaultCommand) + + scheduler.run() + scheduler.run() + scheduler.run() + + assert counter == 3 + assert not scheduler.isScheduled(defaultCommand) + assert scheduler.isScheduled(other) diff --git a/rio/ottomanEmpire/tests/test_selectcommand.py b/rio/ottomanEmpire/tests/test_selectcommand.py new file mode 100644 index 00000000..3ceb2a72 --- /dev/null +++ b/rio/ottomanEmpire/tests/test_selectcommand.py @@ -0,0 +1,94 @@ +from typing import TYPE_CHECKING + +import commands2 +from compositiontestbase import MultiCompositionTestBase # type: ignore +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + from .compositiontestbase import MultiCompositionTestBase + +import pytest + + +class TestSelectCommandComposition(MultiCompositionTestBase): + def compose(self, *members: commands2.Command): + return commands2.SelectCommand(dict(enumerate(members)), lambda: 0) + + +def test_selectCommand(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + command3 = commands2.Command() + + start_spying_on(command1) + start_spying_on(command2) + start_spying_on(command3) + + command1.isFinished = lambda: True + + selectCommand = commands2.SelectCommand( + {"one": command1, "two": command2, "three": command3}, lambda: "one" + ) + + scheduler.schedule(selectCommand) + scheduler.run() + + verify(command1).initialize() + verify(command1).execute() + verify(command1).end(False) + + verify(command2, never()).initialize() + verify(command2, never()).execute() + verify(command2, never()).end(False) + + verify(command3, never()).initialize() + verify(command3, never()).execute() + verify(command3, never()).end(False) + + +def test_selectCommandInvalidKey(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + command3 = commands2.Command() + + start_spying_on(command1) + start_spying_on(command2) + start_spying_on(command3) + + command1.isFinished = lambda: True + + selectCommand = commands2.SelectCommand( + {"one": command1, "two": command2, "three": command3}, lambda: "four" + ) + + scheduler.schedule(selectCommand) + + +def test_selectCommandRequirement(scheduler: commands2.CommandScheduler): + system1 = commands2.Subsystem() + system2 = commands2.Subsystem() + system3 = commands2.Subsystem() + system4 = commands2.Subsystem() + + command1 = commands2.Command() + command1.addRequirements(system1, system2) + command2 = commands2.Command() + command2.addRequirements(system3) + command3 = commands2.Command() + command3.addRequirements(system3, system4) + + start_spying_on(command1) + start_spying_on(command2) + start_spying_on(command3) + + selectCommand = commands2.SelectCommand( + {"one": command1, "two": command2, "three": command3}, lambda: "one" + ) + + scheduler.schedule(selectCommand) + scheduler.schedule(commands2.InstantCommand(lambda: None, system3)) + + verify(command1).end(interrupted=True) + verify(command2, never()).end(interrupted=True) + verify(command3, never()).end(interrupted=True) diff --git a/rio/ottomanEmpire/tests/test_sequentialcommandgroup.py b/rio/ottomanEmpire/tests/test_sequentialcommandgroup.py new file mode 100644 index 00000000..a6ecfb96 --- /dev/null +++ b/rio/ottomanEmpire/tests/test_sequentialcommandgroup.py @@ -0,0 +1,114 @@ +from typing import TYPE_CHECKING + +import commands2 +from compositiontestbase import MultiCompositionTestBase # type: ignore +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + from .compositiontestbase import MultiCompositionTestBase + +import pytest + + +class TestSequentialCommandGroupComposition(MultiCompositionTestBase): + def compose(self, *members: commands2.Command): + return commands2.SequentialCommandGroup(*members) + + +def test_sequntialGroupSchedule(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + + start_spying_on(command1) + start_spying_on(command2) + + group = commands2.SequentialCommandGroup(command1, command2) + + scheduler.schedule(group) + + verify(command1).initialize() + verify(command2, never()).initialize() + + command1.isFinished = lambda: True + scheduler.run() + + verify(command1).execute() + verify(command1).end(False) + verify(command2).initialize() + verify(command2, never()).execute() + verify(command2, never()).end(False) + + command2.isFinished = lambda: True + scheduler.run() + + verify(command1).execute() + verify(command1).end(False) + verify(command2).execute() + verify(command2).end(False) + + assert not scheduler.isScheduled(group) + + +def test_sequentialGroupInterrupt(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + command3 = commands2.Command() + + start_spying_on(command1) + start_spying_on(command2) + start_spying_on(command3) + + group = commands2.SequentialCommandGroup(command1, command2, command3) + + scheduler.schedule(group) + + command1.isFinished = lambda: True + scheduler.run() + scheduler.cancel(group) + scheduler.run() + + verify(command1).execute() + verify(command1, never()).end(True) + verify(command1).end(False) + verify(command2, never()).execute() + verify(command2).end(True) + verify(command3, never()).initialize() + verify(command3, never()).execute() + + # assert command3.end.times_called == 0 + verify(command3, never()).end(True) + verify(command3, never()).end(False) + + assert not scheduler.isScheduled(group) + + +def test_notScheduledCancel(scheduler: commands2.CommandScheduler): + command1 = commands2.Command() + command2 = commands2.Command() + + group = commands2.SequentialCommandGroup(command1, command2) + + scheduler.cancel(group) + + +def test_sequentialGroupRequirement(scheduler: commands2.CommandScheduler): + system1 = commands2.Subsystem() + system2 = commands2.Subsystem() + system3 = commands2.Subsystem() + system4 = commands2.Subsystem() + + command1 = commands2.InstantCommand() + command1.addRequirements(system1, system2) + command2 = commands2.InstantCommand() + command2.addRequirements(system3) + command3 = commands2.InstantCommand() + command3.addRequirements(system3, system4) + + group = commands2.SequentialCommandGroup(command1, command2) + + scheduler.schedule(group) + scheduler.schedule(command3) + + assert not scheduler.isScheduled(group) + assert scheduler.isScheduled(command3) diff --git a/rio/ottomanEmpire/tests/test_startendcommand.py b/rio/ottomanEmpire/tests/test_startendcommand.py new file mode 100644 index 00000000..700412f7 --- /dev/null +++ b/rio/ottomanEmpire/tests/test_startendcommand.py @@ -0,0 +1,30 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_startEndCommandSchedule(scheduler: commands2.CommandScheduler): + cond1 = OOBoolean(False) + cond2 = OOBoolean(False) + + command = commands2.StartEndCommand( + lambda: cond1.set(True), + lambda: cond2.set(True), + ) + + scheduler.schedule(command) + scheduler.run() + + assert scheduler.isScheduled(command) + + scheduler.cancel(command) + + assert not scheduler.isScheduled(command) + assert cond1 == True + assert cond2 == True diff --git a/rio/ottomanEmpire/tests/test_swervecontrollercommand.py b/rio/ottomanEmpire/tests/test_swervecontrollercommand.py new file mode 100644 index 00000000..b94a47f7 --- /dev/null +++ b/rio/ottomanEmpire/tests/test_swervecontrollercommand.py @@ -0,0 +1,1304 @@ +# 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. + +from typing import TYPE_CHECKING, List, Tuple +import math + +import wpimath.controller as controller +import wpimath.trajectory as trajectory +import wpimath.geometry as geometry +import wpimath.kinematics as kinematics +from wpilib import Timer + +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + +import commands2 + +TWO = int(2) +THREE = int(3) +FOUR = int(4) +SIX = int(6) +MISMATCHED_KINEMATICS_AND_ODOMETRY = int(101) +INCOMPLETE_PID_CLASSES = int(102) + + +class SwerveControllerCommandTestDataFixtures: + def __init__(self, selector: int): + self._timer = Timer() + self._angle: geometry.Rotation2d = geometry.Rotation2d(0) + + self._kxTolerance = 1 / 12.0 + self._kyTolerance = 1 / 12.0 + self._kAngularTolerance = 1 / 12.0 + self._kWheelBase = 0.5 + self._kTrackWidth = 0.5 + + # The module positions and states start empty and will be populated below in the selector + # self._modulePositions: Tuple[kinematics.SwerveModulePosition] = [] + self._modulePositions: Tuple[kinematics.SwerveModulePosition] = [] + self._moduleStates: Tuple[kinematics.SwerveModuleState] = [] + + # Setup PID controllers, but if an error test case is requested, make sure it provides + # data that should break the command instantiation + if selector != INCOMPLETE_PID_CLASSES: + self._xController = controller.PIDController(0.6, 0, 0) + self._yController = controller.PIDController(0.6, 0, 0) + constraints = trajectory.TrapezoidProfileRadians.Constraints( + 3 * math.pi, math.pi + ) + self._rotationController = controller.ProfiledPIDControllerRadians( + 1, 0, 0, constraints + ) + + self._holonomic = controller.HolonomicDriveController( + self._xController, self._yController, self._rotationController + ) + else: + self._xController = None + self._yController = controller.PIDController(0.6, 0, 0) + constraints = trajectory.TrapezoidProfileRadians.Constraints( + 3 * math.pi, math.pi + ) + self._rotationController = controller.ProfiledPIDControllerRadians( + 1, 0, 0, constraints + ) + self._holonomic = None + + if (selector == TWO) or (selector == INCOMPLETE_PID_CLASSES): + self._kinematics = kinematics.SwerveDrive2Kinematics( + geometry.Translation2d(self._kWheelBase / 2, self._kTrackWidth / 2), + geometry.Translation2d(self._kWheelBase / 2, -self._kTrackWidth / 2), + ) + + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + + self._odometry = kinematics.SwerveDrive2Odometry( + self._kinematics, + self._angle, + self._modulePositions, + geometry.Pose2d(0, 0, geometry.Rotation2d(0)), + ) + elif selector == THREE: + self._kinematics = kinematics.SwerveDrive3Kinematics( + geometry.Translation2d(self._kWheelBase / 2, self._kTrackWidth / 2), + geometry.Translation2d(self._kWheelBase / 2, -self._kTrackWidth / 2), + geometry.Translation2d(self._kWheelBase / 2, self._kTrackWidth / 2), + ) + + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + + self._odometry = kinematics.SwerveDrive3Odometry( + self._kinematics, + self._angle, + self._modulePositions, + geometry.Pose2d(0, 0, geometry.Rotation2d(0)), + ) + elif selector == FOUR: + self._kinematics = kinematics.SwerveDrive4Kinematics( + geometry.Translation2d(self._kWheelBase / 2, self._kTrackWidth / 2), + geometry.Translation2d(self._kWheelBase / 2, -self._kTrackWidth / 2), + geometry.Translation2d(self._kWheelBase / 2, self._kTrackWidth / 2), + geometry.Translation2d(self._kWheelBase / 2, -self._kTrackWidth / 2), + ) + + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + + self._odometry = kinematics.SwerveDrive4Odometry( + self._kinematics, + self._angle, + self._modulePositions, + geometry.Pose2d(0, 0, geometry.Rotation2d(0)), + ) + elif selector == SIX: + self._kinematics = kinematics.SwerveDrive6Kinematics( + geometry.Translation2d(self._kWheelBase / 2, self._kTrackWidth / 2), + geometry.Translation2d(self._kWheelBase / 2, -self._kTrackWidth / 2), + geometry.Translation2d(self._kWheelBase / 2, self._kTrackWidth / 2), + geometry.Translation2d(self._kWheelBase / 2, -self._kTrackWidth / 2), + geometry.Translation2d(self._kWheelBase / 2, self._kTrackWidth / 2), + geometry.Translation2d(self._kWheelBase / 2, -self._kTrackWidth / 2), + ) + + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + + self._odometry = kinematics.SwerveDrive6Odometry( + self._kinematics, + self._angle, + self._modulePositions, + geometry.Pose2d(0, 0, geometry.Rotation2d(0)), + ) + elif selector == MISMATCHED_KINEMATICS_AND_ODOMETRY: + self._kinematics = kinematics.SwerveDrive2Kinematics( + geometry.Translation2d(self._kWheelBase / 2, self._kTrackWidth / 2), + geometry.Translation2d(self._kWheelBase / 2, -self._kTrackWidth / 2), + ) + + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) + + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + self._modulePositions.append( + kinematics.SwerveModulePosition(0, self._angle) + ) + + self._odometry = kinematics.SwerveDrive6Odometry( + self._kinematics, + self._angle, + self._modulePositions, + geometry.Pose2d(0, 0, geometry.Rotation2d(0)), + ) + + def setModuleStates(self, states: List[kinematics.SwerveModuleState]) -> None: + self._moduleStates = states + + def getRobotPose(self) -> geometry.Pose2d: + self._odometry.update(self._angle, self._modulePositions) + + return self._odometry.getPose() + + def getRotationHeadingZero(self) -> geometry.Rotation2d: + return geometry.Rotation2d() + + +@pytest.fixture() +def get_swerve_controller_data() -> SwerveControllerCommandTestDataFixtures: + def _method(selector): + return SwerveControllerCommandTestDataFixtures(selector) + + return _method + + +def test_SwerveControllerMismatchedKinematicsAndOdometry( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + with pytest.raises(TypeError): + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(MISMATCHED_KINEMATICS_AND_ODOMETRY) + ) + + +def test_SwerveControllerIncompletePID( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + with pytest.raises(Exception): + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(INCOMPLETE_PID_CLASSES) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data.setModuleStates, + fixture_data._xController, + fixture_data._yController, + fixture_data._rotationController, + fixture_data.getRotationHeadingZero, + subsystem, + ) + + +def test_SwerveControllerCommand2Holonomic( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(TWO) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data._holonomic, + fixture_data.setModuleStates, + subsystem, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + sim.step(0.005) + + fixture_data._timer.stop() + command.end(True) + + assert end_state.pose.X() == pytest.approx( + fixture_data.getRobotPose().X(), fixture_data._kxTolerance + ) + assert end_state.pose.Y() == pytest.approx( + fixture_data.getRobotPose().Y(), fixture_data._kyTolerance + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) + + +def test_SwerveControllerCommand2HolonomicWithRotation( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(TWO) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data._holonomic, + fixture_data.getRotationHeadingZero, + fixture_data.setModuleStates, + subsystem, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + sim.step(0.005) + + fixture_data._timer.stop() + command.end(True) + + assert end_state.pose.X() == pytest.approx( + fixture_data.getRobotPose().X(), fixture_data._kxTolerance + ) + assert end_state.pose.Y() == pytest.approx( + fixture_data.getRobotPose().Y(), fixture_data._kyTolerance + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) + + +def test_SwerveControllerCommand2PID( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(TWO) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data._xController, + fixture_data._yController, + fixture_data._rotationController, + fixture_data.setModuleStates, + subsystem, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + sim.step(0.005) + + fixture_data._timer.stop() + command.end(True) + + assert end_state.pose.X() == pytest.approx( + fixture_data.getRobotPose().X(), fixture_data._kxTolerance + ) + assert end_state.pose.Y() == pytest.approx( + fixture_data.getRobotPose().Y(), fixture_data._kyTolerance + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) + + +def test_SwerveControllerCommand2PIDWithRotation( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(TWO) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data._xController, + fixture_data._yController, + fixture_data._rotationController, + fixture_data.getRotationHeadingZero, + fixture_data.setModuleStates, + subsystem, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + sim.step(0.005) + + fixture_data._timer.stop() + command.end(True) + + assert end_state.pose.X() == pytest.approx( + fixture_data.getRobotPose().X(), fixture_data._kxTolerance + ) + assert end_state.pose.Y() == pytest.approx( + fixture_data.getRobotPose().Y(), fixture_data._kyTolerance + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) + + +def test_SwerveControllerCommand3Holonomic( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(THREE) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data._holonomic, + fixture_data.setModuleStates, + subsystem, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + sim.step(0.005) + + fixture_data._timer.stop() + command.end(True) + + assert end_state.pose.X() == pytest.approx( + fixture_data.getRobotPose().X(), fixture_data._kxTolerance + ) + assert end_state.pose.Y() == pytest.approx( + fixture_data.getRobotPose().Y(), fixture_data._kyTolerance + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) + + +def test_SwerveControllerCommand3HolonomicWithRotation( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(THREE) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data._holonomic, + fixture_data.getRotationHeadingZero, + fixture_data.setModuleStates, + subsystem, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + sim.step(0.005) + + fixture_data._timer.stop() + command.end(True) + + assert end_state.pose.X() == pytest.approx( + fixture_data.getRobotPose().X(), fixture_data._kxTolerance + ) + assert end_state.pose.Y() == pytest.approx( + fixture_data.getRobotPose().Y(), fixture_data._kyTolerance + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) + + +def test_SwerveControllerCommand3PID( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(THREE) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data._xController, + fixture_data._yController, + fixture_data._rotationController, + fixture_data.setModuleStates, + subsystem, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + sim.step(0.005) + + fixture_data._timer.stop() + command.end(True) + + assert end_state.pose.X() == pytest.approx( + fixture_data.getRobotPose().X(), fixture_data._kxTolerance + ) + assert end_state.pose.Y() == pytest.approx( + fixture_data.getRobotPose().Y(), fixture_data._kyTolerance + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) + + +def test_SwerveControllerCommand3PIDWithRotation( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(THREE) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data._xController, + fixture_data._yController, + fixture_data._rotationController, + fixture_data.getRotationHeadingZero, + fixture_data.setModuleStates, + subsystem, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + sim.step(0.005) + + fixture_data._timer.stop() + command.end(True) + + assert end_state.pose.X() == pytest.approx( + fixture_data.getRobotPose().X(), fixture_data._kxTolerance + ) + assert end_state.pose.Y() == pytest.approx( + fixture_data.getRobotPose().Y(), fixture_data._kyTolerance + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) + + +def test_SwerveControllerCommand4Holonomic( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(FOUR) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data._holonomic, + fixture_data.setModuleStates, + subsystem, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + sim.step(0.005) + + fixture_data._timer.stop() + command.end(True) + + assert end_state.pose.X() == pytest.approx( + fixture_data.getRobotPose().X(), fixture_data._kxTolerance + ) + assert end_state.pose.Y() == pytest.approx( + fixture_data.getRobotPose().Y(), fixture_data._kyTolerance + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) + + +def test_SwerveControllerCommand4HolonomicWithRotation( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(FOUR) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data._holonomic, + fixture_data.getRotationHeadingZero, + fixture_data.setModuleStates, + subsystem, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + sim.step(0.005) + + fixture_data._timer.stop() + command.end(True) + + assert end_state.pose.X() == pytest.approx( + fixture_data.getRobotPose().X(), fixture_data._kxTolerance + ) + assert end_state.pose.Y() == pytest.approx( + fixture_data.getRobotPose().Y(), fixture_data._kyTolerance + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) + + +def test_SwerveControllerCommand4PID( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(FOUR) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data._xController, + fixture_data._yController, + fixture_data._rotationController, + fixture_data.setModuleStates, + subsystem, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + sim.step(0.005) + + fixture_data._timer.stop() + command.end(True) + + assert end_state.pose.X() == pytest.approx( + fixture_data.getRobotPose().X(), fixture_data._kxTolerance + ) + assert end_state.pose.Y() == pytest.approx( + fixture_data.getRobotPose().Y(), fixture_data._kyTolerance + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) + + +def test_SwerveControllerCommand4PIDWithRotation( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(FOUR) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data._xController, + fixture_data._yController, + fixture_data._rotationController, + fixture_data.getRotationHeadingZero, + fixture_data.setModuleStates, + subsystem, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + sim.step(0.005) + + fixture_data._timer.stop() + command.end(True) + + assert end_state.pose.X() == pytest.approx( + fixture_data.getRobotPose().X(), fixture_data._kxTolerance + ) + assert end_state.pose.Y() == pytest.approx( + fixture_data.getRobotPose().Y(), fixture_data._kyTolerance + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) + + +def test_SwerveControllerCommand6Holonomic( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(SIX) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data._holonomic, + fixture_data.setModuleStates, + subsystem, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + sim.step(0.005) + + fixture_data._timer.stop() + command.end(True) + + assert end_state.pose.X() == pytest.approx( + fixture_data.getRobotPose().X(), fixture_data._kxTolerance + ) + assert end_state.pose.Y() == pytest.approx( + fixture_data.getRobotPose().Y(), fixture_data._kyTolerance + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) + + +def test_SwerveControllerCommand6HolonomicWithRotation( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(SIX) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data._holonomic, + fixture_data.getRotationHeadingZero, + fixture_data.setModuleStates, + subsystem, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + sim.step(0.005) + + fixture_data._timer.stop() + command.end(True) + + assert end_state.pose.X() == pytest.approx( + fixture_data.getRobotPose().X(), fixture_data._kxTolerance + ) + assert end_state.pose.Y() == pytest.approx( + fixture_data.getRobotPose().Y(), fixture_data._kyTolerance + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) + + +def test_SwerveControllerCommand6PID( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(SIX) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data._xController, + fixture_data._yController, + fixture_data._rotationController, + fixture_data.setModuleStates, + subsystem, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + sim.step(0.005) + + fixture_data._timer.stop() + command.end(True) + + assert end_state.pose.X() == pytest.approx( + fixture_data.getRobotPose().X(), fixture_data._kxTolerance + ) + assert end_state.pose.Y() == pytest.approx( + fixture_data.getRobotPose().Y(), fixture_data._kyTolerance + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) + + +def test_SwerveControllerCommand6PIDWithRotation( + scheduler: commands2.CommandScheduler, get_swerve_controller_data +): + with ManualSimTime() as sim: + subsystem = commands2.Subsystem() + waypoints: List[geometry.Pose2d] = [] + waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) + waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) + traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) + new_trajectory: trajectory.Trajectory = ( + trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) + ) + end_state = new_trajectory.sample(new_trajectory.totalTime()) + + fixture_data: SwerveControllerCommandTestDataFixtures = ( + get_swerve_controller_data(SIX) + ) + + command = commands2.SwerveControllerCommand( + new_trajectory, + fixture_data.getRobotPose, + fixture_data._kinematics, + fixture_data._xController, + fixture_data._yController, + fixture_data._rotationController, + fixture_data.getRotationHeadingZero, + fixture_data.setModuleStates, + subsystem, + ) + + fixture_data._timer.restart() + + command.initialize() + + while not command.isFinished(): + command.execute() + fixture_data._angle = new_trajectory.sample( + fixture_data._timer.get() + ).pose.rotation() + + for i in range(0, len(fixture_data._modulePositions)): + fixture_data._modulePositions[i].distance += ( + fixture_data._moduleStates[i].speed * 0.005 + ) + fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ + i + ].angle + + sim.step(0.005) + + fixture_data._timer.stop() + command.end(True) + + assert end_state.pose.X() == pytest.approx( + fixture_data.getRobotPose().X(), fixture_data._kxTolerance + ) + assert end_state.pose.Y() == pytest.approx( + fixture_data.getRobotPose().Y(), fixture_data._kyTolerance + ) + assert end_state.pose.rotation().radians() == pytest.approx( + fixture_data.getRobotPose().rotation().radians(), + fixture_data._kAngularTolerance, + ) diff --git a/rio/ottomanEmpire/tests/test_trigger.py b/rio/ottomanEmpire/tests/test_trigger.py new file mode 100644 index 00000000..876ae9da --- /dev/null +++ b/rio/ottomanEmpire/tests/test_trigger.py @@ -0,0 +1,219 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore +from wpilib.simulation import stepTiming + +if TYPE_CHECKING: + from .util import * + + +def test_onTrue(scheduler: commands2.CommandScheduler): + finished = OOBoolean(False) + command1 = commands2.WaitUntilCommand(finished) + + button = InternalButton() + button.setPressed(False) + button.onTrue(command1) + scheduler.run() + assert not command1.isScheduled() + button.setPressed(True) + scheduler.run() + assert command1.isScheduled() + finished.set(True) + scheduler.run() + assert not command1.isScheduled() + + +def test_onFalse(scheduler: commands2.CommandScheduler): + finished = OOBoolean(False) + command1 = commands2.WaitUntilCommand(finished) + + button = InternalButton() + button.setPressed(True) + button.onFalse(command1) + scheduler.run() + assert not command1.isScheduled() + button.setPressed(False) + scheduler.run() + assert command1.isScheduled() + finished.set(True) + scheduler.run() + assert not command1.isScheduled() + + +def test_whileTrueRepeatedly(scheduler: commands2.CommandScheduler): + inits = OOInteger(0) + counter = OOInteger(0) + + command1 = commands2.FunctionalCommand( + inits.incrementAndGet, + lambda: None, + lambda _: None, + lambda: counter.incrementAndGet() % 2 == 0, + ).repeatedly() + + button = InternalButton() + button.setPressed(False) + button.whileTrue(command1) + scheduler.run() + assert inits == 0 + button.setPressed(True) + scheduler.run() + assert inits == 1 + scheduler.run() + assert inits == 1 + scheduler.run() + assert inits == 2 + button.setPressed(False) + scheduler.run() + assert inits == 2 + + +def test_whileTrueLambdaRunCommand(scheduler: commands2.CommandScheduler): + counter = OOInteger(0) + + command1 = commands2.RunCommand(counter.incrementAndGet) + + button = InternalButton() + button.setPressed(False) + button.whileTrue(command1) + scheduler.run() + assert counter == 0 + button.setPressed(True) + scheduler.run() + assert counter == 1 + scheduler.run() + assert counter == 2 + button.setPressed(False) + scheduler.run() + assert counter == 2 + + +def test_whileTrueOnce(scheduler: commands2.CommandScheduler): + startCounter = OOInteger(0) + endCounter = OOInteger(0) + + command1 = commands2.StartEndCommand( + startCounter.incrementAndGet, endCounter.incrementAndGet + ) + + button = InternalButton() + button.setPressed(False) + button.whileTrue(command1) + scheduler.run() + assert startCounter == 0 + assert endCounter == 0 + button.setPressed(True) + scheduler.run() + scheduler.run() + assert startCounter == 1 + assert endCounter == 0 + button.setPressed(False) + scheduler.run() + assert startCounter == 1 + assert endCounter == 1 + + +def test_toggleOnTrue(scheduler: commands2.CommandScheduler): + startCounter = OOInteger(0) + endCounter = OOInteger(0) + + command1 = commands2.StartEndCommand( + startCounter.incrementAndGet, endCounter.incrementAndGet + ) + + button = InternalButton() + button.setPressed(False) + button.toggleOnTrue(command1) + scheduler.run() + assert startCounter == 0 + assert endCounter == 0 + button.setPressed(True) + scheduler.run() + scheduler.run() + assert startCounter == 1 + assert endCounter == 0 + button.setPressed(False) + scheduler.run() + assert startCounter == 1 + assert endCounter == 0 + button.setPressed(True) + scheduler.run() + assert startCounter == 1 + assert endCounter == 1 + + +def test_cancelWhenActive(scheduler: commands2.CommandScheduler): + startCounter = OOInteger(0) + endCounter = OOInteger(0) + + button = InternalButton() + command1 = commands2.StartEndCommand( + startCounter.incrementAndGet, endCounter.incrementAndGet + ).until(button) + + button.setPressed(False) + command1.schedule() + scheduler.run() + assert startCounter == 1 + assert endCounter == 0 + button.setPressed(True) + scheduler.run() + assert startCounter == 1 + assert endCounter == 1 + scheduler.run() + assert startCounter == 1 + assert endCounter == 1 + + +def test_triggerComposition(): + button1 = InternalButton() + button2 = InternalButton() + + button1.setPressed(True) + button2.setPressed(False) + + assert button1.and_(button2).getAsBoolean() == False + assert button1.or_(button2)() == True + assert bool(button1.negate()) == False + assert (button1 & ~button2)() == True + + +def test_triggerCompositionSupplier(): + button1 = InternalButton() + supplier = lambda: False + + button1.setPressed(True) + + assert button1.and_(supplier)() == False + assert button1.or_(supplier)() == True + + +def test_debounce(scheduler: commands2.CommandScheduler): + command = commands2.Command() + start_spying_on(command) + + button = InternalButton() + debounced = button.debounce(0.1) + + debounced.onTrue(command) + + button.setPressed(True) + scheduler.run() + + verify(command, never()).schedule() + + stepTiming(0.3) + + button.setPressed(True) + scheduler.run() + verify(command).schedule() + + +def test_booleanSupplier(): + button = InternalButton() + + assert button() == False + button.setPressed(True) + assert button() == True diff --git a/rio/ottomanEmpire/tests/test_waitcommand.py b/rio/ottomanEmpire/tests/test_waitcommand.py new file mode 100644 index 00000000..125a2e31 --- /dev/null +++ b/rio/ottomanEmpire/tests/test_waitcommand.py @@ -0,0 +1,50 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_waitCommand(scheduler: commands2.CommandScheduler): + with ManualSimTime() as sim: + waitCommand = commands2.WaitCommand(2) + + scheduler.schedule(waitCommand) + scheduler.run() + sim.step(1) + scheduler.run() + + assert scheduler.isScheduled(waitCommand) + + sim.step(2) + + scheduler.run() + + assert not scheduler.isScheduled(waitCommand) + + +def test_withTimeout(scheduler: commands2.CommandScheduler): + with ManualSimTime() as sim: + command1 = commands2.Command() + start_spying_on(command1) + + timeout = command1.withTimeout(2) + + scheduler.schedule(timeout) + scheduler.run() + + verify(command1).initialize() + verify(command1).execute() + assert not scheduler.isScheduled(command1) + assert scheduler.isScheduled(timeout) + + sim.step(3) + scheduler.run() + + verify(command1).end(True) + verify(command1, never()).end(False) + assert not scheduler.isScheduled(timeout) diff --git a/rio/ottomanEmpire/tests/test_waituntilcommand.py b/rio/ottomanEmpire/tests/test_waituntilcommand.py new file mode 100644 index 00000000..2762a1f6 --- /dev/null +++ b/rio/ottomanEmpire/tests/test_waituntilcommand.py @@ -0,0 +1,22 @@ +from typing import TYPE_CHECKING + +import commands2 +from util import * # type: ignore + +if TYPE_CHECKING: + from .util import * + +import pytest + + +def test_waitUntil(scheduler: commands2.CommandScheduler): + condition = OOBoolean() + + command = commands2.WaitUntilCommand(condition) + + scheduler.schedule(command) + scheduler.run() + assert scheduler.isScheduled(command) + condition.set(True) + scheduler.run() + assert not scheduler.isScheduled(command) diff --git a/rio/ottomanEmpire/tests/util.py b/rio/ottomanEmpire/tests/util.py new file mode 100644 index 00000000..0c1a835f --- /dev/null +++ b/rio/ottomanEmpire/tests/util.py @@ -0,0 +1,331 @@ +from typing import Any, Dict, TypeVar, Type + +import inspect + +import commands2 +from wpilib.simulation import DriverStationSim, pauseTiming, resumeTiming, stepTiming + + +Y = TypeVar("Y") + + +def full_subclass_of(cls: Type[Y]) -> Type[Y]: + # Pybind classes can't be monkeypatched. + # This generates a subclass with every method filled out + # so that it can be monkeypatched. + retlist = [] + clsname = cls.__name__ # + "_Subclass" + classdef = f"class {clsname}(cls):\n" + for name in dir(cls): + # for name in set(dir(cls)): + value = getattr(cls, name) + if callable(value) and not inspect.isclass(value) and not name.startswith("_"): + classdef += f" def {name}(self, *args, **kwargs):\n" + classdef += f" return super().{name}(*args, **kwargs)\n" + classdef += " ...\n" + classdef += f"retlist.append({clsname})\n" + print(classdef) + exec(classdef, globals(), locals()) + return retlist[0] + + +################### +# Compat for wrapped commands +IS_OLD_COMMANDS = False +try: + if commands2.__version__ == "2023.4.3.0": # type: ignore + IS_OLD_COMMANDS = True +except AttributeError: + pass + +if IS_OLD_COMMANDS: + # not needed for pure but works in pure + import commands2.button + + # In Java, Trigger is in the same package as Button + # I did rexport it in commands so using + # the incorrect `commands2.Trigger` instead of `commands2.button.Trigger` works + commands2.button.Trigger = commands2.Trigger + + # I moved this so its not a nested Enum. + # The old one is still there for compat + commands2.InterruptionBehavior = commands2.Command.InterruptionBehavior + + commands2.Command = commands2.CommandBase + + for name in dir(commands2): + if name == "CommandScheduler": + continue + value = getattr(commands2, name) + if inspect.isclass(value): + setattr(commands2, name, full_subclass_of(value)) + + commands2.IllegalCommandUse = RuntimeError + + +################### + + +class ManualSimTime: + def __enter__(self) -> "ManualSimTime": + pauseTiming() + return self + + def __exit__(self, *args): + resumeTiming() + + def step(self, delta: float): + stepTiming(delta) + + +class OOInteger: + def __init__(self, value: int = 0) -> None: + self.value = value + + def get(self) -> int: + return self.value + + def set(self, value: int): + self.value = value + + def incrementAndGet(self) -> int: + self.value += 1 + return self.value + + def addAndGet(self, value: int) -> int: + self.value += value + return self.value + + def __eq__(self, value: float) -> bool: + return self.value == value + + def __lt__(self, value: float) -> bool: + return self.value < value + + def __call__(self) -> int: + return self.value + + +class OOBoolean: + def __init__(self, value: bool = False) -> None: + self.value = value + + def get(self) -> bool: + return self.value + + def set(self, value: bool): + self.value = value + + def __eq__(self, value: object) -> bool: + return self.value == value + + def __bool__(self) -> bool: + return self.value + + def __call__(self) -> bool: + return self.value + + +class InternalButton(commands2.button.Trigger): + def __init__(self): + super().__init__(self.isPressed) + self.pressed = False + + def isPressed(self) -> bool: + return self.pressed + + def setPressed(self, value: bool): + self.pressed = value + + def __call__(self) -> bool: + return self.pressed + + +class OOFloat: + def __init__(self, value: float = 0.0) -> None: + self.value = value + + def get(self) -> float: + return self.value + + def set(self, value: float): + self.value = value + + def incrementAndGet(self) -> float: + self.value += 1 + return self.value + + def addAndGet(self, value: float) -> float: + self.value += value + return self.value + + def __eq__(self, value: float) -> bool: + return self.value == value + + def __lt__(self, value: float) -> bool: + return self.value < value + + def __call__(self) -> float: + return self.value + + def __name__(self) -> str: + return "OOFloat" + + +########################################## +# Fakito Framework + + +def _get_all_args_as_kwargs(method, *args, **kwargs) -> Dict[str, Any]: + try: + import inspect + + method_args = inspect.getcallargs(method, *args, **kwargs) + + method_arg_names = list(inspect.signature(method).parameters.keys()) + + for idx, arg in enumerate(args): + method_args[method_arg_names[idx]] = arg + + try: + del method_args["self"] + except KeyError: + pass + return method_args + except TypeError: + # Pybind methods can't be inspected + # The exact args/kwargs that are passed in are checked instead + r = {} + for idx, arg in enumerate(args): + r[idx] = arg + r.update(kwargs) + return r + + +class MethodWrapper: + def __init__(self, method): + self.method = method + self.og_method = method + self.times_called = 0 + self.call_log = [] + + def __call__(self, *args, **kwargs): + self.times_called += 1 + method_args = _get_all_args_as_kwargs(self.method, *args, **kwargs) + self.call_log.append(method_args) + return self.method(*args, **kwargs) + + def called_with(self, *args, **kwargs): + return _get_all_args_as_kwargs(self.method, *args, **kwargs) in self.call_log + + def times_called_with(self, *args, **kwargs): + return self.call_log.count( + _get_all_args_as_kwargs(self.method, *args, **kwargs) + ) + + +def start_spying_on(obj: Any) -> None: + """ + Mocks all methods on an object, so that that call info can be used in asserts. + + Example: + ``` + obj = SomeClass() + start_spying_on(obj) + obj.method() + obj.method = lambda: None # supports monkeypatching + assert obj.method.times_called == 2 + assert obj.method.called_with(arg1=1, arg2=2) + assert obj.method.times_called_with(arg1=1, arg2=2) == 2 + ``` + """ + + for name in dir(obj): + value = getattr(obj, name) + if callable(value) and not inspect.isclass(value) and not name.startswith("_"): + setattr(obj, name, MethodWrapper(value)) + + if not hasattr(obj.__class__, "_is_being_spied_on"): + try: + old_setattr = obj.__class__.__setattr__ + except AttributeError: + old_setattr = object.__setattr__ + + def _setattr(self, name, value): + if name in dir(self): + existing_value = getattr(self, name) + if isinstance(existing_value, MethodWrapper): + existing_value.method = value + return + old_setattr(self, name, value) + + obj.__class__.__setattr__ = _setattr + obj.__class__._is_being_spied_on = True + + +# fakito verify +def reset(obj: Any) -> None: + """ + Resets the call log of all mocked methods on an object. + Also restores all monkeypatched methods. + """ + for name in dir(obj): + value = getattr(obj, name) + if isinstance(value, MethodWrapper): + value.method = value.og_method + value.times_called = 0 + value.call_log = [] + + +class times: + def __init__(self, times: int) -> None: + self.times = times + + +def never() -> times: + return times(0) + + +class _verify: + def __init__(self, obj: Any, times: times = times(1)): + self.obj = obj + self.times = times.times + + def __getattribute__(self, name: str) -> Any: + def self_dot(name: str): + return super(_verify, self).__getattribute__(name) + + def times_string(times: int) -> str: + if times == 1: + return "1 time" + else: + return f"{times} times" + + def check(*args, **kwargs): + __tracebackhide__ = True + # import code + # code.interact(local={**globals(), **locals()}) + method = getattr(self_dot("obj"), name) + # method = getattr(self1.obj, name) + assert method.times_called_with(*args, **kwargs) == self_dot( + "times" + ), f"Expected {name} to be called {times_string(self_dot('times'))} with {args} {kwargs}, but was called {times_string(method.times_called_with(*args, **kwargs))}" + + return check + + +T = TypeVar("T") + + +def verify(obj: T, times: times = times(1)) -> T: + # import code + # code.interact(local={**globals(), **locals()}) + return _verify(obj, times) # type: ignore + + +def patch_via_decorator(obj: Any): + def decorator(method): + setattr(obj, method.__name__, method.__get__(obj, obj.__class__)) + return method + + return decorator diff --git a/rio/ottomanEmpire/workflows/dist.yml b/rio/ottomanEmpire/workflows/dist.yml new file mode 100644 index 00000000..abd40c7c --- /dev/null +++ b/rio/ottomanEmpire/workflows/dist.yml @@ -0,0 +1,36 @@ +--- +name: dist + +on: + pull_request: + push: + branches: + - main + tags: + - '*' + +jobs: + ci: + uses: robotpy/build-actions/.github/workflows/package-pure.yml@v2024 + with: + enable_sphinx_check: false + secrets: + META_REPO_ACCESS_TOKEN: ${{ secrets.REPO_ACCESS_TOKEN }} + PYPI_API_TOKEN: ${{ secrets.PYPI_PASSWORD }} + + check-mypy: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: actions/setup-python@v4 + with: + python-version: "3.12" + - name: Install requirements + run: | + pip --disable-pip-version-check install mypy setuptools wheel setuptools_scm + pip --disable-pip-version-check install --no-build-isolation -e . + - name: Run mypy + uses: liskin/gh-problem-matcher-wrap@v2 + with: + linters: mypy + run: mypy --show-column-numbers commands2 diff --git a/rio/robotcontainer.py b/rio/robotcontainer.py index 968c8441..1657d3e8 100644 --- a/rio/robotcontainer.py +++ b/rio/robotcontainer.py @@ -1,7 +1,7 @@ # commands import commands2 -from commands2 import cmd, swerveControllerCommand +from commands2 import cmd # wpilib import wpimath @@ -18,6 +18,9 @@ from constants.mathConstant import AutoConstants, DriveConstants from constants.constants import getConstants +# yes its the enitire ottoman empire +from ottomanEmpire.bursa.swervecontrollercommand import SwerveControllerCommand + # misc import math @@ -103,7 +106,7 @@ def getAutonomousCommand(self) -> commands2.Command: ) thetaController.enableContinuousInput(-math.pi, math.pi) - swerveControllerCommand = commands2.swerveControllerCommand( + swerveControllerCommand = SwerveControllerCommand( exampleTrajectory, self.robotDrive.getPose(), DriveConstants.driveKinematics, From 09b4de07b0eb11d65d84b34eb69065ec2193b6e5 Mon Sep 17 00:00:00 2001 From: kredcool Date: Sat, 13 Jan 2024 12:42:12 -0500 Subject: [PATCH 16/61] this run's --- rio/robotcontainer.py | 50 ++----------------------------------------- 1 file changed, 2 insertions(+), 48 deletions(-) diff --git a/rio/robotcontainer.py b/rio/robotcontainer.py index 1657d3e8..9fcd6b66 100644 --- a/rio/robotcontainer.py +++ b/rio/robotcontainer.py @@ -79,51 +79,5 @@ def disablePIDSubsystems(self) -> None: """ def getAutonomousCommand(self) -> commands2.Command: - # Create config for trajectory - config = TrajectoryConfig( - self.autoConst["maxSpeedMetersPerSecond"], - self.autoConst["maxAccelerationMetersPerSecondSquared"], - ) - # ensure max speed is obeyed - config.setKinematics(DriveConstants.driveKinematics) - - # An example trajectory to follow. All units in meters. - exampleTrajectory = TrajectoryGenerator.generateTrajectory( - # Start at the origin facing the +X direction - Pose2d(0, 0, Rotation2d(0)), - # Pass through these two interior waypoints, making an 's' curve path - [Translation2d(1, 1), Translation2d(2, -1)], - # End 3 meters straight ahead of where we started, facing forward - Pose2d(3, 0, Rotation2d(0)), - config, - ) - - thetaController = ProfiledPIDControllerRadians( - self.autoConst["pThetaController"], - 0, - 0, - AutoConstants.thetaControllerConstraints, - ) - thetaController.enableContinuousInput(-math.pi, math.pi) - - swerveControllerCommand = SwerveControllerCommand( - exampleTrajectory, - self.robotDrive.getPose(), - DriveConstants.driveKinematics, - PIDController(self.autoConst["pXController"], 0, 0), - PIDController(self.autoConst["pYController"], 0, 0), - thetaController, - self.robotDrive.setModuleStates(), - (self.robotDrive,), - ) - - # Reset odometry to starting pose - self.robotDrive.resetOdometry(exampleTrajectory.initialPose()) - - # Run path following command, then stop at the end - return swerveControllerCommand.andThen( - cmd.run( - lambda: self.robotDrive.drive(0, 0, 0, False, False), - self.robotDrive, - ) - ) + # rev has stuff here but I don't want to debug it + pass From 8f4d5684d3aad1690d3bfd14754dadc3a3a668a3 Mon Sep 17 00:00:00 2001 From: kredcool Date: Sat, 13 Jan 2024 14:17:38 -0500 Subject: [PATCH 17/61] its now a git submodule, the seg fault is still a very real error but it should********* be gone by the end of the weekend if robotpy people get themselves in gear. TLDR added git submodule to fix being ahead of robotpy --- .gitmodules | 4 + rio/Pipfile.lock | 108 +- rio/ottomanEmpire/.gitignore | 24 - rio/ottomanEmpire/.readthedocs.yml | 13 - rio/ottomanEmpire/LICENSE | 24 - rio/ottomanEmpire/README.md | 7 - rio/ottomanEmpire/__init__.py | 0 rio/ottomanEmpire/bursa/__init__.py | 117 -- rio/ottomanEmpire/bursa/button/__init__.py | 19 - .../bursa/button/commandgenerichid.py | 190 --- .../bursa/button/commandjoystick.py | 205 --- .../bursa/button/commandps4controller.py | 251 ---- .../bursa/button/commandxboxcontroller.py | 237 --- .../bursa/button/joystickbutton.py | 20 - .../bursa/button/networkbutton.py | 127 -- rio/ottomanEmpire/bursa/button/povbutton.py | 21 - rio/ottomanEmpire/bursa/button/trigger.py | 267 ---- rio/ottomanEmpire/bursa/cmd.py | 191 --- rio/ottomanEmpire/bursa/command.py | 557 ------- rio/ottomanEmpire/bursa/commandgroup.py | 21 - rio/ottomanEmpire/bursa/commandscheduler.py | 475 ------ rio/ottomanEmpire/bursa/conditionalcommand.py | 61 - rio/ottomanEmpire/bursa/functionalcommand.py | 52 - rio/ottomanEmpire/bursa/instantcommand.py | 29 - rio/ottomanEmpire/bursa/notifiercommand.py | 41 - .../bursa/parallelcommandgroup.py | 87 -- .../bursa/paralleldeadlinegroup.py | 98 -- rio/ottomanEmpire/bursa/parallelracegroup.py | 82 -- rio/ottomanEmpire/bursa/perpetualcommand.py | 46 - rio/ottomanEmpire/bursa/pidcommand.py | 74 - rio/ottomanEmpire/bursa/pidsubsystem.py | 99 -- rio/ottomanEmpire/bursa/printcommand.py | 18 - rio/ottomanEmpire/bursa/proxycommand.py | 90 -- .../bursa/proxyschedulecommand.py | 41 - rio/ottomanEmpire/bursa/repeatcommand.py | 55 - rio/ottomanEmpire/bursa/runcommand.py | 25 - rio/ottomanEmpire/bursa/schedulecommand.py | 30 - rio/ottomanEmpire/bursa/selectcommand.py | 71 - .../bursa/sequentialcommandgroup.py | 90 -- rio/ottomanEmpire/bursa/startendcommand.py | 33 - rio/ottomanEmpire/bursa/subsystem.py | 180 --- .../bursa/swervecontrollercommand.py | 293 ---- rio/ottomanEmpire/bursa/timedcommandrobot.py | 15 - .../bursa/trapezoidprofilesubsystem.py | 75 - rio/ottomanEmpire/bursa/util.py | 24 - rio/ottomanEmpire/bursa/waitcommand.py | 33 - rio/ottomanEmpire/bursa/waituntilcommand.py | 74 - rio/ottomanEmpire/bursa/wrappercommand.py | 80 - rio/ottomanEmpire/docs/.gitignore | 11 - rio/ottomanEmpire/docs/Makefile | 183 --- rio/ottomanEmpire/docs/api.rst | 24 - rio/ottomanEmpire/docs/conf.py | 158 -- rio/ottomanEmpire/docs/index.rst | 11 - rio/ottomanEmpire/docs/requirements.txt | 5 - rio/ottomanEmpire/docs/setup.py | 23 - rio/ottomanEmpire/setup.py | 23 - .../tests/compositiontestbase.py | 154 -- rio/ottomanEmpire/tests/conftest.py | 20 - rio/ottomanEmpire/tests/requirements.txt | 1 - rio/ottomanEmpire/tests/run_tests.py | 12 - .../tests/test_command_decorators.py | 222 --- .../tests/test_command_requirements.py | 59 - .../tests/test_command_schedule.py | 90 -- .../tests/test_commandgroup_error.py | 38 - .../tests/test_conditional_command.py | 55 - .../tests/test_default_command.py | 73 - .../tests/test_functional_command.py | 36 - .../tests/test_instant_command.py | 21 - rio/ottomanEmpire/tests/test_networkbutton.py | 29 - .../tests/test_notifier_command.py | 23 - .../tests/test_parallelcommandgroup.py | 117 -- .../tests/test_paralleldeadlinegroup.py | 119 -- .../tests/test_parallelracegroup.py | 183 --- .../tests/test_perpetualcommand.py | 18 - rio/ottomanEmpire/tests/test_pidcommand.py | 114 -- rio/ottomanEmpire/tests/test_printcommand.py | 17 - rio/ottomanEmpire/tests/test_proxycommand.py | 40 - rio/ottomanEmpire/tests/test_repeatcommand.py | 69 - .../tests/test_robotdisabledcommand.py | 157 -- rio/ottomanEmpire/tests/test_runcommand.py | 22 - .../tests/test_schedulecommand.py | 36 - rio/ottomanEmpire/tests/test_scheduler.py | 67 - .../tests/test_schedulingrecursion.py | 165 --- rio/ottomanEmpire/tests/test_selectcommand.py | 94 -- .../tests/test_sequentialcommandgroup.py | 114 -- .../tests/test_startendcommand.py | 30 - .../tests/test_swervecontrollercommand.py | 1304 ----------------- rio/ottomanEmpire/tests/test_trigger.py | 219 --- rio/ottomanEmpire/tests/test_waitcommand.py | 50 - .../tests/test_waituntilcommand.py | 22 - rio/ottomanEmpire/tests/util.py | 331 ----- rio/ottomanEmpire/workflows/dist.yml | 36 - rio/ottomanempire | 1 + rio/robotcontainer.py | 2 +- 94 files changed, 62 insertions(+), 9310 deletions(-) create mode 100644 .gitmodules delete mode 100644 rio/ottomanEmpire/.gitignore delete mode 100644 rio/ottomanEmpire/.readthedocs.yml delete mode 100644 rio/ottomanEmpire/LICENSE delete mode 100644 rio/ottomanEmpire/README.md delete mode 100644 rio/ottomanEmpire/__init__.py delete mode 100644 rio/ottomanEmpire/bursa/__init__.py delete mode 100644 rio/ottomanEmpire/bursa/button/__init__.py delete mode 100644 rio/ottomanEmpire/bursa/button/commandgenerichid.py delete mode 100644 rio/ottomanEmpire/bursa/button/commandjoystick.py delete mode 100644 rio/ottomanEmpire/bursa/button/commandps4controller.py delete mode 100644 rio/ottomanEmpire/bursa/button/commandxboxcontroller.py delete mode 100644 rio/ottomanEmpire/bursa/button/joystickbutton.py delete mode 100644 rio/ottomanEmpire/bursa/button/networkbutton.py delete mode 100644 rio/ottomanEmpire/bursa/button/povbutton.py delete mode 100644 rio/ottomanEmpire/bursa/button/trigger.py delete mode 100644 rio/ottomanEmpire/bursa/cmd.py delete mode 100644 rio/ottomanEmpire/bursa/command.py delete mode 100644 rio/ottomanEmpire/bursa/commandgroup.py delete mode 100644 rio/ottomanEmpire/bursa/commandscheduler.py delete mode 100644 rio/ottomanEmpire/bursa/conditionalcommand.py delete mode 100644 rio/ottomanEmpire/bursa/functionalcommand.py delete mode 100644 rio/ottomanEmpire/bursa/instantcommand.py delete mode 100644 rio/ottomanEmpire/bursa/notifiercommand.py delete mode 100644 rio/ottomanEmpire/bursa/parallelcommandgroup.py delete mode 100644 rio/ottomanEmpire/bursa/paralleldeadlinegroup.py delete mode 100644 rio/ottomanEmpire/bursa/parallelracegroup.py delete mode 100644 rio/ottomanEmpire/bursa/perpetualcommand.py delete mode 100644 rio/ottomanEmpire/bursa/pidcommand.py delete mode 100644 rio/ottomanEmpire/bursa/pidsubsystem.py delete mode 100644 rio/ottomanEmpire/bursa/printcommand.py delete mode 100644 rio/ottomanEmpire/bursa/proxycommand.py delete mode 100644 rio/ottomanEmpire/bursa/proxyschedulecommand.py delete mode 100644 rio/ottomanEmpire/bursa/repeatcommand.py delete mode 100644 rio/ottomanEmpire/bursa/runcommand.py delete mode 100644 rio/ottomanEmpire/bursa/schedulecommand.py delete mode 100644 rio/ottomanEmpire/bursa/selectcommand.py delete mode 100644 rio/ottomanEmpire/bursa/sequentialcommandgroup.py delete mode 100644 rio/ottomanEmpire/bursa/startendcommand.py delete mode 100644 rio/ottomanEmpire/bursa/subsystem.py delete mode 100644 rio/ottomanEmpire/bursa/swervecontrollercommand.py delete mode 100644 rio/ottomanEmpire/bursa/timedcommandrobot.py delete mode 100644 rio/ottomanEmpire/bursa/trapezoidprofilesubsystem.py delete mode 100644 rio/ottomanEmpire/bursa/util.py delete mode 100644 rio/ottomanEmpire/bursa/waitcommand.py delete mode 100644 rio/ottomanEmpire/bursa/waituntilcommand.py delete mode 100644 rio/ottomanEmpire/bursa/wrappercommand.py delete mode 100644 rio/ottomanEmpire/docs/.gitignore delete mode 100644 rio/ottomanEmpire/docs/Makefile delete mode 100644 rio/ottomanEmpire/docs/api.rst delete mode 100644 rio/ottomanEmpire/docs/conf.py delete mode 100644 rio/ottomanEmpire/docs/index.rst delete mode 100644 rio/ottomanEmpire/docs/requirements.txt delete mode 100644 rio/ottomanEmpire/docs/setup.py delete mode 100644 rio/ottomanEmpire/setup.py delete mode 100644 rio/ottomanEmpire/tests/compositiontestbase.py delete mode 100644 rio/ottomanEmpire/tests/conftest.py delete mode 100644 rio/ottomanEmpire/tests/requirements.txt delete mode 100755 rio/ottomanEmpire/tests/run_tests.py delete mode 100644 rio/ottomanEmpire/tests/test_command_decorators.py delete mode 100644 rio/ottomanEmpire/tests/test_command_requirements.py delete mode 100644 rio/ottomanEmpire/tests/test_command_schedule.py delete mode 100644 rio/ottomanEmpire/tests/test_commandgroup_error.py delete mode 100644 rio/ottomanEmpire/tests/test_conditional_command.py delete mode 100644 rio/ottomanEmpire/tests/test_default_command.py delete mode 100644 rio/ottomanEmpire/tests/test_functional_command.py delete mode 100644 rio/ottomanEmpire/tests/test_instant_command.py delete mode 100644 rio/ottomanEmpire/tests/test_networkbutton.py delete mode 100644 rio/ottomanEmpire/tests/test_notifier_command.py delete mode 100644 rio/ottomanEmpire/tests/test_parallelcommandgroup.py delete mode 100644 rio/ottomanEmpire/tests/test_paralleldeadlinegroup.py delete mode 100644 rio/ottomanEmpire/tests/test_parallelracegroup.py delete mode 100644 rio/ottomanEmpire/tests/test_perpetualcommand.py delete mode 100644 rio/ottomanEmpire/tests/test_pidcommand.py delete mode 100644 rio/ottomanEmpire/tests/test_printcommand.py delete mode 100644 rio/ottomanEmpire/tests/test_proxycommand.py delete mode 100644 rio/ottomanEmpire/tests/test_repeatcommand.py delete mode 100644 rio/ottomanEmpire/tests/test_robotdisabledcommand.py delete mode 100644 rio/ottomanEmpire/tests/test_runcommand.py delete mode 100644 rio/ottomanEmpire/tests/test_schedulecommand.py delete mode 100644 rio/ottomanEmpire/tests/test_scheduler.py delete mode 100644 rio/ottomanEmpire/tests/test_schedulingrecursion.py delete mode 100644 rio/ottomanEmpire/tests/test_selectcommand.py delete mode 100644 rio/ottomanEmpire/tests/test_sequentialcommandgroup.py delete mode 100644 rio/ottomanEmpire/tests/test_startendcommand.py delete mode 100644 rio/ottomanEmpire/tests/test_swervecontrollercommand.py delete mode 100644 rio/ottomanEmpire/tests/test_trigger.py delete mode 100644 rio/ottomanEmpire/tests/test_waitcommand.py delete mode 100644 rio/ottomanEmpire/tests/test_waituntilcommand.py delete mode 100644 rio/ottomanEmpire/tests/util.py delete mode 100644 rio/ottomanEmpire/workflows/dist.yml create mode 160000 rio/ottomanempire diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 00000000..cd63c869 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,4 @@ +[submodule "rio/ottomanempire"] + path = rio/ottomanempire + url = https://github.com/lospugs/robotpy-commands-v2.git + branch = swervecontrollercommand diff --git a/rio/Pipfile.lock b/rio/Pipfile.lock index 43f1ce91..5d9f7a10 100644 --- a/rio/Pipfile.lock +++ b/rio/Pipfile.lock @@ -172,7 +172,6 @@ "sha256:ec345caf15ae2c61540812500979e92f2989c6b6d4d13d21bdc82908043b3265" ], "index": "pypi", - "markers": "python_version >= '3.8'", "version": "==24.1a1" }, "cffi": { @@ -583,50 +582,59 @@ }, "pyyaml": { "hashes": [ - "sha256:01b45c0191e6d66c470b6cf1b9531a771a83c1c4208272ead47a3ae4f2f603bf", - "sha256:0283c35a6a9fbf047493e3a0ce8d79ef5030852c51e9d911a27badfde0605293", - "sha256:055d937d65826939cb044fc8c9b08889e8c743fdc6a32b33e2390f66013e449b", - "sha256:07751360502caac1c067a8132d150cf3d61339af5691fe9e87803040dbc5db57", - "sha256:0b4624f379dab24d3725ffde76559cff63d9ec94e1736b556dacdfebe5ab6d4b", - "sha256:0ce82d761c532fe4ec3f87fc45688bdd3a4c1dc5e0b4a19814b9009a29baefd4", - "sha256:1e4747bc279b4f613a09eb64bba2ba602d8a6664c6ce6396a4d0cd413a50ce07", - "sha256:213c60cd50106436cc818accf5baa1aba61c0189ff610f64f4a3e8c6726218ba", - "sha256:231710d57adfd809ef5d34183b8ed1eeae3f76459c18fb4a0b373ad56bedcdd9", - "sha256:277a0ef2981ca40581a47093e9e2d13b3f1fbbeffae064c1d21bfceba2030287", - "sha256:2cd5df3de48857ed0544b34e2d40e9fac445930039f3cfe4bcc592a1f836d513", - "sha256:40527857252b61eacd1d9af500c3337ba8deb8fc298940291486c465c8b46ec0", - "sha256:432557aa2c09802be39460360ddffd48156e30721f5e8d917f01d31694216782", - "sha256:473f9edb243cb1935ab5a084eb238d842fb8f404ed2193a915d1784b5a6b5fc0", - "sha256:48c346915c114f5fdb3ead70312bd042a953a8ce5c7106d5bfb1a5254e47da92", - "sha256:50602afada6d6cbfad699b0c7bb50d5ccffa7e46a3d738092afddc1f9758427f", - "sha256:68fb519c14306fec9720a2a5b45bc9f0c8d1b9c72adf45c37baedfcd949c35a2", - "sha256:77f396e6ef4c73fdc33a9157446466f1cff553d979bd00ecb64385760c6babdc", - "sha256:81957921f441d50af23654aa6c5e5eaf9b06aba7f0a19c18a538dc7ef291c5a1", - "sha256:819b3830a1543db06c4d4b865e70ded25be52a2e0631ccd2f6a47a2822f2fd7c", - "sha256:897b80890765f037df3403d22bab41627ca8811ae55e9a722fd0392850ec4d86", - "sha256:98c4d36e99714e55cfbaaee6dd5badbc9a1ec339ebfc3b1f52e293aee6bb71a4", - "sha256:9df7ed3b3d2e0ecfe09e14741b857df43adb5a3ddadc919a2d94fbdf78fea53c", - "sha256:9fa600030013c4de8165339db93d182b9431076eb98eb40ee068700c9c813e34", - "sha256:a80a78046a72361de73f8f395f1f1e49f956c6be882eed58505a15f3e430962b", - "sha256:afa17f5bc4d1b10afd4466fd3a44dc0e245382deca5b3c353d8b757f9e3ecb8d", - "sha256:b3d267842bf12586ba6c734f89d1f5b871df0273157918b0ccefa29deb05c21c", - "sha256:b5b9eccad747aabaaffbc6064800670f0c297e52c12754eb1d976c57e4f74dcb", - "sha256:bfaef573a63ba8923503d27530362590ff4f576c626d86a9fed95822a8255fd7", - "sha256:c5687b8d43cf58545ade1fe3e055f70eac7a5a1a0bf42824308d868289a95737", - "sha256:cba8c411ef271aa037d7357a2bc8f9ee8b58b9965831d9e51baf703280dc73d3", - "sha256:d15a181d1ecd0d4270dc32edb46f7cb7733c7c508857278d3d378d14d606db2d", - "sha256:d4b0ba9512519522b118090257be113b9468d804b19d63c71dbcf4a48fa32358", - "sha256:d4db7c7aef085872ef65a8fd7d6d09a14ae91f691dec3e87ee5ee0539d516f53", - "sha256:d4eccecf9adf6fbcc6861a38015c2a64f38b9d94838ac1810a9023a0609e1b78", - "sha256:d67d839ede4ed1b28a4e8909735fc992a923cdb84e618544973d7dfc71540803", - "sha256:daf496c58a8c52083df09b80c860005194014c3698698d1a57cbcfa182142a3a", - "sha256:dbad0e9d368bb989f4515da330b88a057617d16b6a8245084f1b05400f24609f", - "sha256:e61ceaab6f49fb8bdfaa0f92c4b57bcfbea54c09277b1b4f7ac376bfb7a7c174", - "sha256:f84fbc98b019fef2ee9a1cb3ce93e3187a6df0b2538a651bfb890254ba9f90b5" + "sha256:04ac92ad1925b2cff1db0cfebffb6ffc43457495c9b3c39d3fcae417d7125dc5", + "sha256:062582fca9fabdd2c8b54a3ef1c978d786e0f6b3a1510e0ac93ef59e0ddae2bc", + "sha256:0d3304d8c0adc42be59c5f8a4d9e3d7379e6955ad754aa9d6ab7a398b59dd1df", + "sha256:1635fd110e8d85d55237ab316b5b011de701ea0f29d07611174a1b42f1444741", + "sha256:184c5108a2aca3c5b3d3bf9395d50893a7ab82a38004c8f61c258d4428e80206", + "sha256:18aeb1bf9a78867dc38b259769503436b7c72f7a1f1f4c93ff9a17de54319b27", + "sha256:1d4c7e777c441b20e32f52bd377e0c409713e8bb1386e1099c2415f26e479595", + "sha256:1e2722cc9fbb45d9b87631ac70924c11d3a401b2d7f410cc0e3bbf249f2dca62", + "sha256:1fe35611261b29bd1de0070f0b2f47cb6ff71fa6595c077e42bd0c419fa27b98", + "sha256:28c119d996beec18c05208a8bd78cbe4007878c6dd15091efb73a30e90539696", + "sha256:326c013efe8048858a6d312ddd31d56e468118ad4cdeda36c719bf5bb6192290", + "sha256:40df9b996c2b73138957fe23a16a4f0ba614f4c0efce1e9406a184b6d07fa3a9", + "sha256:42f8152b8dbc4fe7d96729ec2b99c7097d656dc1213a3229ca5383f973a5ed6d", + "sha256:49a183be227561de579b4a36efbb21b3eab9651dd81b1858589f796549873dd6", + "sha256:4fb147e7a67ef577a588a0e2c17b6db51dda102c71de36f8549b6816a96e1867", + "sha256:50550eb667afee136e9a77d6dc71ae76a44df8b3e51e41b77f6de2932bfe0f47", + "sha256:510c9deebc5c0225e8c96813043e62b680ba2f9c50a08d3724c7f28a747d1486", + "sha256:5773183b6446b2c99bb77e77595dd486303b4faab2b086e7b17bc6bef28865f6", + "sha256:596106435fa6ad000c2991a98fa58eeb8656ef2325d7e158344fb33864ed87e3", + "sha256:6965a7bc3cf88e5a1c3bd2e0b5c22f8d677dc88a455344035f03399034eb3007", + "sha256:69b023b2b4daa7548bcfbd4aa3da05b3a74b772db9e23b982788168117739938", + "sha256:6c22bec3fbe2524cde73d7ada88f6566758a8f7227bfbf93a408a9d86bcc12a0", + "sha256:704219a11b772aea0d8ecd7058d0082713c3562b4e271b849ad7dc4a5c90c13c", + "sha256:7e07cbde391ba96ab58e532ff4803f79c4129397514e1413a7dc761ccd755735", + "sha256:81e0b275a9ecc9c0c0c07b4b90ba548307583c125f54d5b6946cfee6360c733d", + "sha256:855fb52b0dc35af121542a76b9a84f8d1cd886ea97c84703eaa6d88e37a2ad28", + "sha256:8d4e9c88387b0f5c7d5f281e55304de64cf7f9c0021a3525bd3b1c542da3b0e4", + "sha256:9046c58c4395dff28dd494285c82ba00b546adfc7ef001486fbf0324bc174fba", + "sha256:9eb6caa9a297fc2c2fb8862bc5370d0303ddba53ba97e71f08023b6cd73d16a8", + "sha256:a0cd17c15d3bb3fa06978b4e8958dcdc6e0174ccea823003a106c7d4d7899ac5", + "sha256:afd7e57eddb1a54f0f1a974bc4391af8bcce0b444685d936840f125cf046d5bd", + "sha256:b1275ad35a5d18c62a7220633c913e1b42d44b46ee12554e5fd39c70a243d6a3", + "sha256:b786eecbdf8499b9ca1d697215862083bd6d2a99965554781d0d8d1ad31e13a0", + "sha256:ba336e390cd8e4d1739f42dfe9bb83a3cc2e80f567d8805e11b46f4a943f5515", + "sha256:baa90d3f661d43131ca170712d903e6295d1f7a0f595074f151c0aed377c9b9c", + "sha256:bc1bf2925a1ecd43da378f4db9e4f799775d6367bdb94671027b73b393a7c42c", + "sha256:bd4af7373a854424dabd882decdc5579653d7868b8fb26dc7d0e99f823aa5924", + "sha256:bf07ee2fef7014951eeb99f56f39c9bb4af143d8aa3c21b1677805985307da34", + "sha256:bfdf460b1736c775f2ba9f6a92bca30bc2095067b8a9d77876d1fad6cc3b4a43", + "sha256:c8098ddcc2a85b61647b2590f825f3db38891662cfc2fc776415143f599bb859", + "sha256:d2b04aac4d386b172d5b9692e2d2da8de7bfb6c387fa4f801fbf6fb2e6ba4673", + "sha256:d483d2cdf104e7c9fa60c544d92981f12ad66a457afae824d146093b8c294c54", + "sha256:d858aa552c999bc8a8d57426ed01e40bef403cd8ccdd0fc5f6f04a00414cac2a", + "sha256:e7d73685e87afe9f3b36c799222440d6cf362062f78be1013661b00c5c6f678b", + "sha256:f003ed9ad21d6a4713f0a9b5a7a0a79e08dd0f221aff4525a2be4c346ee60aab", + "sha256:f22ac1c3cac4dbc50079e965eba2c1058622631e526bd9afd45fedd49ba781fa", + "sha256:faca3bdcf85b2fc05d06ff3fbc1f83e1391b3e724afa3feba7d13eeab355484c", + "sha256:fca0e3a251908a499833aa292323f32437106001d436eca0e6e7833256674585", + "sha256:fd1592b3fdf65fff2ad0004b5e363300ef59ced41c2e6b3a99d4089fa8c5435d", + "sha256:fd66fc5d0da6d9815ba2cebeb4205f95818ff4b79c3ebe268e75d961704af52f" ], "index": "pypi", - "markers": "python_version >= '3.6'", - "version": "==6.0" + "version": "==6.0.1" }, "robotpy": { "extras": [ @@ -639,7 +647,7 @@ "sha256:8b7b36a949ea7f3d0504b23654040211df6f29bd04846043b464671d9c4025e5", "sha256:b3d5c8420bbb9953af996f82f8cb28b78e02c3815f019ded094589f9854c0074" ], - "markers": "python_version < '3.13' and python_version >= '3.8'", + "index": "pypi", "version": "==2024.0.0b4.post1" }, "robotpy-cli": { @@ -701,11 +709,11 @@ }, "robotpy-installer": { "hashes": [ - "sha256:302987a3e64580dae0f2e21969aa61fa2f59114c077d2fe011629f641dee514b", - "sha256:9b93cfba473716c6b899d8f5c550ba64f52ae328f393fb00df3b086762b34142" + "sha256:07f9d1342db914a9eed17d42fda5b082381c5736143ef5c0f46ce160934bbce3", + "sha256:38eb26dbbe32076f5cf95b995190678f041048353c0dc8c8e6a1a70c6a23f226" ], "markers": "platform_machine != 'roborio' and platform_machine != 'armv7l' and platform_machine != 'aarch64'", - "version": "==2024.0.2" + "version": "==2024.0.5" }, "robotpy-navx": { "hashes": [ @@ -837,9 +845,6 @@ "version": "==4.9.0" }, "wpilib": { - "extras": [ - "all" - ], "hashes": [ "sha256:21f4c8b5d295ead0c4b52a82fcc42077ebc201438eee0cb8fa713831526711af", "sha256:3c36adff768e246220bccf56db8bdcac4f635f9f0e63b911c41084d6787eae3d", @@ -857,7 +862,7 @@ "sha256:f1a04fc08157b81e6115824790f508a8f32f4d8e4f50c89a54d9346ca9af0eb1", "sha256:f381dc85c1c7787e89730059a4e33172268d7efa699f2e8f9b0ca72733b38303" ], - "markers": "python_version >= '3.8'", + "index": "pypi", "version": "==2024.0.0b4.post1" }, "yarl": { @@ -1081,7 +1086,6 @@ "sha256:ec345caf15ae2c61540812500979e92f2989c6b6d4d13d21bdc82908043b3265" ], "index": "pypi", - "markers": "python_version >= '3.8'", "version": "==24.1a1" }, "click": { diff --git a/rio/ottomanEmpire/.gitignore b/rio/ottomanEmpire/.gitignore deleted file mode 100644 index 37cf260e..00000000 --- a/rio/ottomanEmpire/.gitignore +++ /dev/null @@ -1,24 +0,0 @@ - -*.py[ciod] -*.egg-info - -*.dll -*.so -*.dylib -py.typed - -/build -/dist - -/.vscode - -/wpilib/buttons/rpy-include -/wpilib/buttons/_init_commands_v1_buttons.py -/wpilib/buttons/pkgcfg.py - -/commands2/_init_impl.py -/commands2/include -/commands2/lib -/commands2/rpy-include -/commands2/pkgcfg.py -/commands2/version.py diff --git a/rio/ottomanEmpire/.readthedocs.yml b/rio/ottomanEmpire/.readthedocs.yml deleted file mode 100644 index c0a2ee18..00000000 --- a/rio/ottomanEmpire/.readthedocs.yml +++ /dev/null @@ -1,13 +0,0 @@ -version: 2 - -sphinx: - configuration: docs/conf.py - -build: - os: ubuntu-22.04 - tools: - python: "3.11" - -python: - install: - - requirements: docs/requirements.txt diff --git a/rio/ottomanEmpire/LICENSE b/rio/ottomanEmpire/LICENSE deleted file mode 100644 index 3d5a824c..00000000 --- a/rio/ottomanEmpire/LICENSE +++ /dev/null @@ -1,24 +0,0 @@ -Copyright (c) 2009-2021 FIRST and other WPILib contributors -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. - * Neither the name of FIRST, WPILib, nor the names of other WPILib - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND -ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR -PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR -ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/rio/ottomanEmpire/README.md b/rio/ottomanEmpire/README.md deleted file mode 100644 index 4af1fccb..00000000 --- a/rio/ottomanEmpire/README.md +++ /dev/null @@ -1,7 +0,0 @@ -robotpy-commands-v2 -=================== - -Python wrappers around a modified version of the new WPILib commands library. - -* Documentation @ https://robotpy.readthedocs.io/projects/commands-v2 -* Examples @ https://github.com/robotpy/examples/tree/main/commands-v2 diff --git a/rio/ottomanEmpire/__init__.py b/rio/ottomanEmpire/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/rio/ottomanEmpire/bursa/__init__.py b/rio/ottomanEmpire/bursa/__init__.py deleted file mode 100644 index 52efd58b..00000000 --- a/rio/ottomanEmpire/bursa/__init__.py +++ /dev/null @@ -1,117 +0,0 @@ -from .button import Trigger -from .command import Command, InterruptionBehavior - -from . import cmd - -# from .cmd import ( -# deadline, -# either, -# none, -# parallel, -# print_, -# race, -# repeatingSequence, -# run, -# runEnd, -# runOnce, -# select, -# sequence, -# startEnd, -# waitSeconds, -# waitUntil, -# ) -from .commandgroup import CommandGroup, IllegalCommandUse -from .commandscheduler import CommandScheduler -from .conditionalcommand import ConditionalCommand -from .functionalcommand import FunctionalCommand -from .instantcommand import InstantCommand -from .notifiercommand import NotifierCommand -from .parallelcommandgroup import ParallelCommandGroup -from .paralleldeadlinegroup import ParallelDeadlineGroup -from .parallelracegroup import ParallelRaceGroup -from .perpetualcommand import PerpetualCommand -from .pidcommand import PIDCommand -from .pidsubsystem import PIDSubsystem -from .printcommand import PrintCommand -from .proxycommand import ProxyCommand -from .proxyschedulecommand import ProxyScheduleCommand -from .repeatcommand import RepeatCommand -from .runcommand import RunCommand -from .schedulecommand import ScheduleCommand -from .selectcommand import SelectCommand -from .sequentialcommandgroup import SequentialCommandGroup -from .startendcommand import StartEndCommand -from .subsystem import Subsystem -from .swervecontrollercommand import SwerveControllerCommand -from .timedcommandrobot import TimedCommandRobot -from .trapezoidprofilesubsystem import TrapezoidProfileSubsystem -from .waitcommand import WaitCommand -from .waituntilcommand import WaitUntilCommand -from .wrappercommand import WrapperCommand - -__all__ = [ - "cmd", - "Command", - "CommandGroup", - "CommandScheduler", - "ConditionalCommand", - "FunctionalCommand", - "IllegalCommandUse", - "InstantCommand", - "InterruptionBehavior", - "NotifierCommand", - "ParallelCommandGroup", - "ParallelDeadlineGroup", - "ParallelRaceGroup", - "PerpetualCommand", - "PIDCommand", - "PIDSubsystem", - "PrintCommand", - "ProxyCommand", - "ProxyScheduleCommand", - "RepeatCommand", - "RunCommand", - "ScheduleCommand", - "SelectCommand", - "SequentialCommandGroup", - "StartEndCommand", - "Subsystem", - "SwerveControllerCommand", - "TimedCommandRobot", - "TrapezoidProfileSubsystem", - "WaitCommand", - "WaitUntilCommand", - "WrapperCommand", - # "none", - # "runOnce", - # "run", - # "startEnd", - # "runEnd", - # "print_", - # "waitSeconds", - # "waitUntil", - # "either", - # "select", - # "sequence", - # "repeatingSequence", - # "parallel", - # "race", - # "deadline", - "Trigger", # was here in 2023 -] - - -def __getattr__(attr): - if attr == "SubsystemBase": - import warnings - - warnings.warn("SubsystemBase is deprecated", DeprecationWarning, stacklevel=2) - return Subsystem - - if attr == "CommandBase": - import warnings - - warnings.warn("CommandBase is deprecated", DeprecationWarning, stacklevel=2) - return Command - - raise AttributeError("module {!r} has no attribute " "{!r}".format(__name__, attr)) diff --git a/rio/ottomanEmpire/bursa/button/__init__.py b/rio/ottomanEmpire/bursa/button/__init__.py deleted file mode 100644 index f6947397..00000000 --- a/rio/ottomanEmpire/bursa/button/__init__.py +++ /dev/null @@ -1,19 +0,0 @@ -from .commandgenerichid import CommandGenericHID -from .commandjoystick import CommandJoystick -from .commandps4controller import CommandPS4Controller -from .commandxboxcontroller import CommandXboxController -from .joystickbutton import JoystickButton -from .networkbutton import NetworkButton -from .povbutton import POVButton -from .trigger import Trigger - -__all__ = [ - "Trigger", - "CommandGenericHID", - "CommandJoystick", - "CommandPS4Controller", - "CommandXboxController", - "JoystickButton", - "NetworkButton", - "POVButton", -] diff --git a/rio/ottomanEmpire/bursa/button/commandgenerichid.py b/rio/ottomanEmpire/bursa/button/commandgenerichid.py deleted file mode 100644 index 1512fd15..00000000 --- a/rio/ottomanEmpire/bursa/button/commandgenerichid.py +++ /dev/null @@ -1,190 +0,0 @@ -from typing import Optional - -from wpilib.event import EventLoop -from wpilib.interfaces import GenericHID - -from ..commandscheduler import CommandScheduler -from .trigger import Trigger - - -class CommandGenericHID: - """ - A version of GenericHID with Trigger factories for command-based. - """ - - def __init__(self, port: int): - """ - Construct an instance of a device. - - :param port: The port on the Driver Station that the device is plugged into. - """ - self._hid = GenericHID(port) - - def getHID(self) -> GenericHID: - """ - Get the underlying GenericHID object. - """ - return self._hid - - def button(self, button: int, loop: Optional[EventLoop] = None) -> Trigger: - """ - Constructs an event instance around this button's digital signal. - - :param button: The button index - :param loop: the event loop instance to attache the event to. - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getRawButtonPressed(button)) - - def pov( - self, angle: int, *, pov: int = 0, loop: Optional[EventLoop] = None - ) -> Trigger: - """ - Constructs a Trigger instance based around this angle of a POV on the HID. - - The POV angles start at 0 in the up direction, and increase clockwise (e.g. right is 90, - upper-left is 315). - - :param angle: POV angle in degrees, or -1 for the center / not pressed. - :param pov: index of the POV to read (starting at 0). Defaults to 0. - :param loop: the event loop instance to attach the event to. Defaults to {@link - CommandScheduler#getDefaultButtonLoop() the default command scheduler button loop}. - :returns: a Trigger instance based around this angle of a POV on the HID. - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getPOV(pov) == angle) - - def povUp(self) -> Trigger: - """ - Constructs a Trigger instance based around the 0 degree angle (up) of the default (index 0) POV - on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command - scheduler button loop}. - - :returns: a Trigger instance based around the 0 degree angle of a POV on the HID. - """ - return self.pov(0) - - def povUpRight(self) -> Trigger: - """ - Constructs a Trigger instance based around the 45 degree angle (right up) of the default (index - 0) POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default - command scheduler button loop}. - - :returns: a Trigger instance based around the 45 degree angle of a POV on the HID. - """ - return self.pov(45) - - def povRight(self) -> Trigger: - """ - Constructs a Trigger instance based around the 90 degree angle (right) of the default (index 0) - POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command - scheduler button loop}. - - :returns: a Trigger instance based around the 90 degree angle of a POV on the HID. - """ - return self.pov(90) - - def povDownRight(self) -> Trigger: - """ - Constructs a Trigger instance based around the 135 degree angle (right down) of the default - (index 0) POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the - default command scheduler button loop}. - - :returns: a Trigger instance based around the 135 degree angle of a POV on the HID. - """ - return self.pov(135) - - def povDown(self) -> Trigger: - """ - Constructs a Trigger instance based around the 180 degree angle (down) of the default (index 0) - POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command - scheduler button loop}. - - :returns: a Trigger instance based around the 180 degree angle of a POV on the HID. - """ - return self.pov(180) - - def povDownLeft(self) -> Trigger: - """ - Constructs a Trigger instance based around the 225 degree angle (down left) of the default - (index 0) POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the - default command scheduler button loop}. - - :returns: a Trigger instance based around the 225 degree angle of a POV on the HID. - """ - return self.pov(225) - - def povLeft(self) -> Trigger: - """ - Constructs a Trigger instance based around the 270 degree angle (left) of the default (index 0) - POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default command - scheduler button loop}. - - :returns: a Trigger instance based around the 270 degree angle of a POV on the HID. - """ - return self.pov(270) - - def povUpLeft(self) -> Trigger: - """ - Constructs a Trigger instance based around the 315 degree angle (left up) of the default (index - 0) POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the default - command scheduler button loop}. - - :returns: a Trigger instance based around the 315 degree angle of a POV on the HID. - """ - return self.pov(315) - - def povCenter(self) -> Trigger: - """ - Constructs a Trigger instance based around the center (not pressed) position of the default - (index 0) POV on the HID, attached to {@link CommandScheduler#getDefaultButtonLoop() the - default command scheduler button loop}. - - :returns: a Trigger instance based around the center position of a POV on the HID. - """ - return self.pov(-1) - - def axisLessThan( - self, axis: int, threshold: float, loop: Optional[EventLoop] = None - ) -> Trigger: - """ - Constructs a Trigger instance that is true when the axis value is less than {@code threshold}, - attached to the given loop. - - :param axis: The axis to read, starting at 0 - :param threshold: The value below which this trigger should return true. - :param loop: the event loop instance to attach the trigger to - :returns: a Trigger instance that is true when the axis value is less than the provided - threshold. - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getRawAxis(axis) < threshold) - - def axisGreaterThan( - self, axis: int, threshold: float, loop: Optional[EventLoop] = None - ) -> Trigger: - """ - Constructs a Trigger instance that is true when the axis value is greater than {@code - threshold}, attached to the given loop. - - :param axis: The axis to read, starting at 0 - :param threshold: The value above which this trigger should return true. - :param loop: the event loop instance to attach the trigger to. - :returns: a Trigger instance that is true when the axis value is greater than the provided - threshold. - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getRawAxis(axis) > threshold) - - def getRawAxis(self, axis: int) -> float: - """ - Get the value of the axis. - - :param axis: The axis to read, starting at 0. - :returns: The value of the axis. - """ - return self._hid.getRawAxis(axis) diff --git a/rio/ottomanEmpire/bursa/button/commandjoystick.py b/rio/ottomanEmpire/bursa/button/commandjoystick.py deleted file mode 100644 index f5f28c51..00000000 --- a/rio/ottomanEmpire/bursa/button/commandjoystick.py +++ /dev/null @@ -1,205 +0,0 @@ -from typing import Optional - -from wpilib import Joystick -from wpilib.event import EventLoop - -from ..commandscheduler import CommandScheduler -from .commandgenerichid import CommandGenericHID -from .trigger import Trigger - - -class CommandJoystick(CommandGenericHID): - """ - A version of Joystick with Trigger factories for command-based. - """ - - _hid: Joystick - - def __init__(self, port: int): - """ - Construct an instance of a controller. - - :param port: The port index on the Driver Station that the controller is plugged into. - """ - - super().__init__(port) - self._hid = Joystick(port) - - def getHID(self): - """ - Get the underlying GenericHID object. - - :returns: the wrapped GenericHID object - """ - return self._hid - - def trigger(self, loop: Optional[EventLoop] = None) -> Trigger: - """ - Constructs an event instance around the trigger button's digital signal. - - :param loop: the event loop instance to attach the event to. - :returns: an event instance representing the trigger button's digital signal attached to the - given loop. - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getTrigger()) - - def top(self, loop: Optional[EventLoop] = None) -> Trigger: - """ - Constructs an event instance around the top button's digital signal. - - :param loop: the event loop instance to attach the event to. - :returns: an event instance representing the top button's digital signal attached to the given - loop. - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getTop()) - - def setXChannel(self, channel: int): - """ - Set the channel associated with the X axis. - - :param channel: The channel to set the axis to. - """ - self._hid.setXChannel(channel) - - def setYChannel(self, channel: int): - """ - Set the channel associated with the Y axis. - - :param channel: The channel to set the axis to. - """ - self._hid.setYChannel(channel) - - def setZChannel(self, channel: int): - """ - Set the channel associated with the Z axis. - - :param channel: The channel to set the axis to. - """ - self._hid.setZChannel(channel) - - def setThrottleChannel(self, channel: int): - """ - Set the channel associated with the throttle axis. - - :param channel: The channel to set the axis to. - """ - self._hid.setThrottleChannel(channel) - - def setTwistChannel(self, channel: int): - """ - Set the channel associated with the twist axis. - - :param channel: The channel to set the axis to. - """ - self._hid.setTwistChannel(channel) - - def getXChannel(self) -> int: - """ - Get the channel currently associated with the X axis. - - :returns: The channel for the axis. - """ - return self._hid.getXChannel() - - def getYChannel(self) -> int: - """ - Get the channel currently associated with the Y axis. - - :returns: The channel for the axis. - """ - return self._hid.getYChannel() - - def getZChannel(self) -> int: - """ - Get the channel currently associated with the Z axis. - - :returns: The channel for the axis. - """ - return self._hid.getZChannel() - - def getThrottleChannel(self) -> int: - """ - Get the channel currently associated with the throttle axis. - - :returns: The channel for the axis. - """ - return self._hid.getThrottleChannel() - - def getTwistChannel(self) -> int: - """ - Get the channel currently associated with the twist axis. - - :returns: The channel for the axis. - """ - return self._hid.getTwistChannel() - - def getX(self) -> float: - """ - Get the x position of the HID. - - :returns: the x position - """ - return self._hid.getX() - - def getY(self) -> float: - """ - Get the y position of the HID. - - :returns: the y position - """ - return self._hid.getY() - - def getZ(self) -> float: - """ - Get the z position of the HID. - - :returns: the z position - """ - return self._hid.getZ() - - def getTwist(self) -> float: - """ - Get the twist value of the current joystick. This depends on the mapping of the joystick - connected to the current port. - - :returns: The Twist value of the joystick. - """ - return self._hid.getTwist() - - def getThrottle(self) -> float: - """ - Get the throttle value of the current joystick. This depends on the mapping of the joystick - connected to the current port. - - :returns: The Throttle value of the joystick. - """ - return self._hid.getThrottle() - - def getMagnitude(self) -> float: - """ - Get the magnitude of the direction vector formed by the joystick's current position relative to - its origin. - - :returns: The magnitude of the direction vector - """ - return self._hid.getMagnitude() - - def getDirectionRadians(self) -> float: - """ - Get the direction of the vector formed by the joystick and its origin in radians. - - :returns: The direction of the vector in radians - """ - return self._hid.getDirectionRadians() - - def getDirectionDegrees(self) -> float: - """ - Get the direction of the vector formed by the joystick and its origin in degrees. - - :returns: The direction of the vector in degrees - """ - return self._hid.getDirectionDegrees() diff --git a/rio/ottomanEmpire/bursa/button/commandps4controller.py b/rio/ottomanEmpire/bursa/button/commandps4controller.py deleted file mode 100644 index 9b2da01d..00000000 --- a/rio/ottomanEmpire/bursa/button/commandps4controller.py +++ /dev/null @@ -1,251 +0,0 @@ -from typing import Optional - -from wpilib import PS4Controller -from wpilib.event import EventLoop - -from ..commandscheduler import CommandScheduler -from .commandgenerichid import CommandGenericHID -from .trigger import Trigger - - -class CommandPS4Controller(CommandGenericHID): - """ - A version of PS4Controller with Trigger factories for command-based. - """ - - _hid: PS4Controller - - def __init__(self, port: int): - """ - Construct an instance of a device. - - :param port: The port index on the Driver Station that the device is plugged into. - """ - super().__init__(port) - self._hid = PS4Controller(port) - - def getHID(self): - """ - Get the underlying GenericHID object. - - :returns: the wrapped GenericHID object - """ - return self._hid - - def L2(self, loop: Optional[EventLoop] = None) -> Trigger: - """ - Constructs an event instance around the L2 button's digital signal. - - :param loop: the event loop instance to attach the event to. - :returns: an event instance representing the L2 button's digital signal attached to the given - loop. - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getL2Button()) - - def R2(self, loop: Optional[EventLoop] = None) -> Trigger: - """ - Constructs an event instance around the R2 button's digital signal. - - :param loop: the event loop instance to attach the event to. - :returns: an event instance representing the R2 button's digital signal attached to the given - loop. - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getR2Button()) - - def L1(self, loop: Optional[EventLoop] = None) -> Trigger: - """ - Constructs an event instance around the L1 button's digital signal. - - :param loop: the event loop instance to attach the event to. - :returns: an event instance representing the L1 button's digital signal attached to the given - loop. - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getL1Button()) - - def R1(self, loop: Optional[EventLoop] = None) -> Trigger: - """ - Constructs an event instance around the R1 button's digital signal. - - :param loop: the event loop instance to attach the event to. - :returns: an event instance representing the R1 button's digital signal attached to the given - loop. - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getR1Button()) - - def L3(self, loop: Optional[EventLoop] = None) -> Trigger: - """ - Constructs an event instance around the L3 button's digital signal. - - :param loop: the event loop instance to attach the event to. - :returns: an event instance representing the L3 button's digital signal attached to the given - loop. - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getL3Button()) - - def R3(self, loop: Optional[EventLoop] = None) -> Trigger: - """ - Constructs an event instance around the R3 button's digital signal. - - :param loop: the event loop instance to attach the event to. - :returns: an event instance representing the R3 button's digital signal attached to the given - loop. - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getR3Button()) - - def square(self, loop: Optional[EventLoop] = None) -> Trigger: - """ - Constructs an event instance around the square button's digital signal. - - :param loop: the event loop instance to attach the event to. - :returns: an event instance representing the square button's digital signal attached to the given - loop. - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getSquareButton()) - - def cross(self, loop: Optional[EventLoop] = None) -> Trigger: - """ - Constructs an event instance around the cross button's digital signal. - - :param loop: the event loop instance to attach the event to. - :returns: an event instance representing the cross button's digital signal attached to the given - loop. - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getCrossButton()) - - def triangle(self, loop: Optional[EventLoop] = None) -> Trigger: - """ - Constructs an event instance around the triangle button's digital signal. - - :param loop: the event loop instance to attach the event to. - :returns: an event instance representing the triangle button's digital signal attached to the - given loop. - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getTriangleButton()) - - def circle(self, loop: Optional[EventLoop] = None) -> Trigger: - """ - Constructs an event instance around the circle button's digital signal. - - :param loop: the event loop instance to attach the event to. - :returns: an event instance representing the circle button's digital signal attached to the given - loop. - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getCircleButton()) - - def share(self, loop: Optional[EventLoop] = None) -> Trigger: - """ - Constructs an event instance around the share button's digital signal. - - :param loop: the event loop instance to attach the event to. - :returns: an event instance representing the share button's digital signal attached to the given - loop. - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getShareButton()) - - def PS(self, loop: Optional[EventLoop] = None) -> Trigger: - """ - Constructs an event instance around the PS button's digital signal. - - :param loop: the event loop instance to attach the event to. - :returns: an event instance representing the PS button's digital signal attached to the given - loop. - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getPSButton()) - - def options(self, loop: Optional[EventLoop] = None) -> Trigger: - """ - Constructs an event instance around the options button's digital signal. - - :param loop: the event loop instance to attach the event to. - :returns: an event instance representing the options button's digital signal attached to the - given loop. - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getOptionsButton()) - - def touchpad(self, loop: Optional[EventLoop] = None) -> Trigger: - """ - Constructs an event instance around the touchpad's digital signal. - - :param loop: the event loop instance to attach the event to. - :returns: an event instance representing the touchpad's digital signal attached to the given - loop. - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getTouchpad()) - - def getLeftX(self) -> float: - """ - Get the X axis value of left side of the controller. - - :returns: the axis value. - """ - return self._hid.getLeftX() - - def getRightX(self) -> float: - """ - Get the X axis value of right side of the controller. - - :returns: the axis value. - """ - return self._hid.getRightX() - - def getLeftY(self) -> float: - """ - Get the Y axis value of left side of the controller. - - :returns: the axis value. - """ - return self._hid.getLeftY() - - def getRightY(self) -> float: - """ - Get the Y axis value of right side of the controller. - - :returns: the axis value. - """ - return self._hid.getRightY() - - def getL2Axis(self) -> float: - """ - Get the L2 axis value of the controller. Note that this axis is bound to the range of [0, 1] as - opposed to the usual [-1, 1]. - - :returns: the axis value. - """ - return self._hid.getL2Axis() - - def getR2Axis(self) -> float: - """ - Get the R2 axis value of the controller. Note that this axis is bound to the range of [0, 1] as - opposed to the usual [-1, 1]. - - :returns: the axis value. - """ - return self._hid.getR2Axis() diff --git a/rio/ottomanEmpire/bursa/button/commandxboxcontroller.py b/rio/ottomanEmpire/bursa/button/commandxboxcontroller.py deleted file mode 100644 index 2c613f4b..00000000 --- a/rio/ottomanEmpire/bursa/button/commandxboxcontroller.py +++ /dev/null @@ -1,237 +0,0 @@ -from typing import Optional - -from wpilib import XboxController -from wpilib.event import EventLoop - -from ..commandscheduler import CommandScheduler -from .commandgenerichid import CommandGenericHID -from .trigger import Trigger - - -class CommandXboxController(CommandGenericHID): - """ - A version of XboxController with Trigger factories for command-based. - """ - - _hid: XboxController - - def __init__(self, port: int): - """ - Construct an instance of a controller. - - :param port: The port index on the Driver Station that the controller is plugged into. - """ - super().__init__(port) - self._hid = XboxController(port) - - def getHID(self): - """ - Get the underlying GenericHID object. - - :returns: the wrapped GenericHID object - """ - return self._hid - - def leftBumper(self, loop: Optional[EventLoop] = None) -> Trigger: - """ - Constructs an event instance around the left bumper's digital signal. - - :param loop: the event loop instance to attach the event to. - :returns: an event instance representing the right bumper's digital signal attached to the given - loop. - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getLeftBumper()) - - def rightBumper(self, loop: Optional[EventLoop] = None) -> Trigger: - """ - Constructs an event instance around the right bumper's digital signal. - - :param loop: the event loop instance to attach the event to. - :returns: an event instance representing the left bumper's digital signal attached to the given - loop. - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getRightBumper()) - - def leftStick(self, loop: Optional[EventLoop] = None) -> Trigger: - """ - Constructs an event instance around the left stick button's digital signal. - - :param loop: the event loop instance to attach the event to. - :returns: an event instance representing the left stick button's digital signal attached to the - given loop. - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getLeftStickButton()) - - def rightStick(self, loop: Optional[EventLoop] = None) -> Trigger: - """ - Constructs an event instance around the right stick button's digital signal. - - :param loop: the event loop instance to attach the event to. - :returns: an event instance representing the right stick button's digital signal attached to the - given loop. - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getRightStickButton()) - - def a(self, loop: Optional[EventLoop] = None) -> Trigger: - """ - Constructs an event instance around the A button's digital signal. - - :param loop: the event loop instance to attach the event to. - :returns: an event instance representing the A button's digital signal attached to the given - loop. - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getAButton()) - - def b(self, loop: Optional[EventLoop] = None) -> Trigger: - """ - Constructs an event instance around the B button's digital signal. - - :param loop: the event loop instance to attach the event to. - :returns: an event instance representing the B button's digital signal attached to the given - loop. - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getBButton()) - - def x(self, loop: Optional[EventLoop] = None) -> Trigger: - """ - Constructs an event instance around the X button's digital signal. - - :param loop: the event loop instance to attach the event to. - :returns: an event instance representing the X button's digital signal attached to the given - loop. - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getXButton()) - - def y(self, loop: Optional[EventLoop] = None) -> Trigger: - """ - Constructs an event instance around the Y button's digital signal. - - :param loop: the event loop instance to attach the event to. - :returns: an event instance representing the Y button's digital signal attached to the given - loop. - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getYButton()) - - def start(self, loop: Optional[EventLoop] = None) -> Trigger: - """ - Constructs an event instance around the start button's digital signal. - - :param loop: the event loop instance to attach the event to. - :returns: an event instance representing the start button's digital signal attached to the given - loop. - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getStartButton()) - - def back(self, loop: Optional[EventLoop] = None) -> Trigger: - """ - Constructs an event instance around the back button's digital signal. - - :param loop: the event loop instance to attach the event to. - :returns: an event instance representing the back button's digital signal attached to the given - loop. - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getBackButton()) - - def leftTrigger( - self, threshold: float = 0.5, loop: Optional[EventLoop] = None - ) -> Trigger: - """ - Constructs a Trigger instance around the axis value of the left trigger. The returned trigger - will be true when the axis value is greater than {@code threshold}. - - :param threshold: the minimum axis value for the returned Trigger to be true. This value - should be in the range [0, 1] where 0 is the unpressed state of the axis. - :param loop: the event loop instance to attach the Trigger to. - :returns: a Trigger instance that is true when the left trigger's axis exceeds the provided - threshold, attached to the given event loop - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getLeftTriggerAxis() > threshold) - - def rightTrigger( - self, threshold: float = 0.5, loop: Optional[EventLoop] = None - ) -> Trigger: - """ - Constructs a Trigger instance around the axis value of the right trigger. The returned trigger - will be true when the axis value is greater than {@code threshold}. - - :param threshold: the minimum axis value for the returned Trigger to be true. This value - should be in the range [0, 1] where 0 is the unpressed state of the axis. - :param loop: the event loop instance to attach the Trigger to. - :returns: a Trigger instance that is true when the right trigger's axis exceeds the provided - threshold, attached to the given event loop - """ - if loop is None: - loop = CommandScheduler.getInstance().getDefaultButtonLoop() - return Trigger(loop, lambda: self._hid.getRightTriggerAxis() > threshold) - - def getLeftX(self) -> float: - """ - Get the X axis value of left side of the controller. - - :returns: The axis value. - """ - return self._hid.getLeftX() - - def getRightX(self) -> float: - """ - Get the X axis value of right side of the controller. - - :returns: The axis value. - """ - return self._hid.getRightX() - - def getLeftY(self) -> float: - """ - Get the Y axis value of left side of the controller. - - :returns: The axis value. - """ - return self._hid.getLeftY() - - def getRightY(self) -> float: - """ - Get the Y axis value of right side of the controller. - - :returns: The axis value. - """ - return self._hid.getRightY() - - def getLeftTriggerAxis(self) -> float: - """ - Get the left trigger (LT) axis value of the controller. Note that this axis is bound to the - range of [0, 1] as opposed to the usual [-1, 1]. - - :returns: The axis value. - """ - return self._hid.getLeftTriggerAxis() - - def getRightTriggerAxis(self) -> float: - """ - Get the right trigger (RT) axis value of the controller. Note that this axis is bound to the - range of [0, 1] as opposed to the usual [-1, 1]. - - :returns: The axis value. - """ - return self._hid.getRightTriggerAxis() diff --git a/rio/ottomanEmpire/bursa/button/joystickbutton.py b/rio/ottomanEmpire/bursa/button/joystickbutton.py deleted file mode 100644 index 2e0e107c..00000000 --- a/rio/ottomanEmpire/bursa/button/joystickbutton.py +++ /dev/null @@ -1,20 +0,0 @@ -from wpilib.interfaces import GenericHID - -from .trigger import Trigger - - -class JoystickButton(Trigger): - """ - A Button that gets its state from a GenericHID. - - This class is provided by the NewCommands VendorDep - """ - - def __init__(self, joystick: GenericHID, buttonNumber: int): - """ - Creates a joystick button for triggering commands. - - :param joystick: The GenericHID object that has the button (e.g. Joystick, KinectStick, etc) - :param buttonNumber: The button number (see GenericHID#getRawButton(int) - """ - super().__init__(lambda: joystick.getRawButton(buttonNumber)) diff --git a/rio/ottomanEmpire/bursa/button/networkbutton.py b/rio/ottomanEmpire/bursa/button/networkbutton.py deleted file mode 100644 index c6c6602c..00000000 --- a/rio/ottomanEmpire/bursa/button/networkbutton.py +++ /dev/null @@ -1,127 +0,0 @@ -from typing import overload - -from ntcore import BooleanSubscriber, BooleanTopic, NetworkTable, NetworkTableInstance - -from ..util import format_args_kwargs -from .trigger import Trigger - - -class NetworkButton(Trigger): - """ - A Button that uses a NetworkTable boolean field. - - This class is provided by the NewCommands VendorDep - """ - - @overload - def __init__(self, topic: BooleanTopic) -> None: - """ - Creates a NetworkButton that commands can be bound to. - - :param topic: The boolean topic that contains the value. - """ - pass - - @overload - def __init__(self, sub: BooleanSubscriber) -> None: - """ - Creates a NetworkButton that commands can be bound to. - - :param sub: The boolean subscriber that provides the value. - """ - pass - - @overload - def __init__(self, table: NetworkTable, field: str) -> None: - """ - Creates a NetworkButton that commands can be bound to. - - :param table: The table where the networktable value is located. - :param field: The field that is the value. - """ - pass - - @overload - def __init__(self, table: str, field: str) -> None: - """ - Creates a NetworkButton that commands can be bound to. - - :param table: The table where the networktable value is located. - :param field: The field that is the value. - """ - pass - - @overload - def __init__(self, inst: NetworkTableInstance, table: str, field: str) -> None: - """ - Creates a NetworkButton that commands can be bound to. - - :param inst: The NetworkTable instance to use - :param table: The table where the networktable value is located. - :param field: the field that is the value. - """ - pass - - def __init__(self, *args, **kwargs) -> None: - def init_sub(sub: BooleanSubscriber): - return super(NetworkButton, self).__init__( - lambda: sub.getTopic().getInstance().isConnected() and sub.get() - ) - - def init_topic(topic: BooleanTopic): - init_sub(topic.subscribe(False)) - - def init_table_field(table: NetworkTable, field: str): - init_topic(table.getBooleanTopic(field)) - - def init_inst_table_field(inst: NetworkTableInstance, table: str, field: str): - init_table_field(inst.getTable(table), field) - - def init_str_table_field(table: str, field: str): - init_inst_table_field(NetworkTableInstance.getDefault(), table, field) - - num_args = len(args) + len(kwargs) - - if num_args == 1: - if "topic" in kwargs: - return init_topic(kwargs["topic"]) - if "sub" in kwargs: - return init_sub(kwargs["sub"]) - if isinstance(args[0], BooleanTopic): - return init_topic(args[0]) - if isinstance(args[0], BooleanSubscriber): - return init_sub(args[0]) - elif num_args == 2: - table, field, *_ = args + (None, None) - if "table" in kwargs: - table = kwargs["table"] - if "field" in kwargs: - field = kwargs["field"] - if table is not None and field is not None: - if isinstance(table, NetworkTable): - return init_table_field(table, field) - if isinstance(table, str): - return init_str_table_field(table, field) - elif num_args == 3: - inst, table, field, *_ = args + (None, None, None) - if "inst" in kwargs: - inst = kwargs["inst"] - if "table" in kwargs: - table = kwargs["table"] - if "field" in kwargs: - field = kwargs["field"] - if inst is not None and table is not None and field is not None: - return init_inst_table_field(inst, table, field) - - raise TypeError( - f""" -TypeError: NetworkButton(): incompatible function arguments. The following argument types are supported: - 1. (self: NetworkButton, topic: BooleanTopic) - 2. (self: NetworkButton, sub: BooleanSubscriber) - 3. (self: NetworkButton, table: NetworkTable, field: str) - 4. (self: NetworkButton, table: str, field: str) - 5. (self: NetworkButton, inst: NetworkTableInstance, table: str, field: str) - -Invoked with: {format_args_kwargs(self, *args, **kwargs)} -""" - ) diff --git a/rio/ottomanEmpire/bursa/button/povbutton.py b/rio/ottomanEmpire/bursa/button/povbutton.py deleted file mode 100644 index 8e0c06cd..00000000 --- a/rio/ottomanEmpire/bursa/button/povbutton.py +++ /dev/null @@ -1,21 +0,0 @@ -from wpilib.interfaces import GenericHID - -from .trigger import Trigger - - -class POVButton(Trigger): - """ - A Button that gets its state from a POV on a GenericHID. - - This class is provided by the NewCommands VendorDep - """ - - def __init__(self, joystick: GenericHID, angle: int, povNumber: int = 0): - """ - Creates a POV button for triggering commands. - - :param joystick: The GenericHID object that has the POV - :param angle: The desired angle in degrees (e.g. 90, 270) - :param povNumber: The POV number (see GenericHID#getPOV(int)) - """ - super().__init__(lambda: joystick.getPOV(povNumber) == angle) diff --git a/rio/ottomanEmpire/bursa/button/trigger.py b/rio/ottomanEmpire/bursa/button/trigger.py deleted file mode 100644 index d9f88329..00000000 --- a/rio/ottomanEmpire/bursa/button/trigger.py +++ /dev/null @@ -1,267 +0,0 @@ -from types import SimpleNamespace -from typing import Callable, overload - -from typing_extensions import Self -from wpilib.event import EventLoop -from wpimath.filter import Debouncer - -from ..command import Command -from ..commandscheduler import CommandScheduler -from ..util import format_args_kwargs - - -class Trigger: - """ - This class provides an easy way to link commands to conditions. - - It is very easy to link a button to a command. For instance, you could link the trigger button - of a joystick to a "score" command. - """ - - _loop: EventLoop - _condition: Callable[[], bool] - - @overload - def __init__(self, condition: Callable[[], bool] = lambda: False): - """ - Creates a new trigger based on the given condition. - - Polled by the default scheduler button loop. - - :param condition: the condition represented by this trigger - """ - ... - - @overload - def __init__(self, loop: EventLoop, condition: Callable[[], bool]): - """ - Creates a new trigger based on the given condition. - - :param loop: The loop instance that polls this trigger. - :param condition: the condition represented by this trigger - """ - ... - - def __init__(self, *args, **kwargs): - def init_loop_condition(loop: EventLoop, condition: Callable[[], bool]): - self._loop = loop - self._condition = condition - - def init_condition(condition: Callable[[], bool]): - init_loop_condition( - CommandScheduler.getInstance().getDefaultButtonLoop(), condition - ) - - num_args = len(args) + len(kwargs) - - if num_args == 0: - return init_condition(lambda: False) - elif num_args == 1 and len(kwargs) == 1: - if "condition" in kwargs: - return init_condition(kwargs["condition"]) - elif num_args == 1 and len(args) == 1: - if callable(args[0]): - return init_condition(args[0]) - elif num_args == 2: - loop, condition, *_ = args + (None, None) - if "loop" in kwargs: - loop = kwargs["loop"] - if "condition" in kwargs: - condition = kwargs["condition"] - if loop is not None and condition is not None: - return init_loop_condition(loop, condition) - - raise TypeError( - f""" -TypeError: Trigger(): incompatible function arguments. The following argument types are supported: - 1. (self: Trigger) - 2. (self: Trigger, condition: () -> bool) - 3. (self: Trigger, loop: EventLoop, condition: () -> bool) - -Invoked with: {format_args_kwargs(self, *args, **kwargs)} -""" - ) - - def onTrue(self, command: Command) -> Self: - """ - Starts the given command whenever the condition changes from `False` to `True`. - - :param command: the command to start - :returns: this trigger, so calls can be chained - """ - - @self._loop.bind - def _(state=SimpleNamespace(pressed_last=self._condition())): - pressed = self._condition() - if not state.pressed_last and pressed: - command.schedule() - state.pressed_last = pressed - - return self - - def onFalse(self, command: Command) -> Self: - """ - Starts the given command whenever the condition changes from `True` to `False`. - - :param command: the command to start - :returns: this trigger, so calls can be chained - """ - - @self._loop.bind - def _(state=SimpleNamespace(pressed_last=self._condition())): - pressed = self._condition() - if state.pressed_last and not pressed: - command.schedule() - state.pressed_last = pressed - - return self - - def whileTrue(self, command: Command) -> Self: - """ - Starts the given command when the condition changes to `True` and cancels it when the condition - changes to `False`. - - Doesn't re-start the command if it ends while the condition is still `True`. If the command - should restart, see RepeatCommand. - - :param command: the command to start - :returns: this trigger, so calls can be chained - """ - - @self._loop.bind - def _(state=SimpleNamespace(pressed_last=self._condition())): - pressed = self._condition() - if not state.pressed_last and pressed: - command.schedule() - elif state.pressed_last and not pressed: - command.cancel() - state.pressed_last = pressed - - return self - - def whileFalse(self, command: Command) -> Self: - """ - Starts the given command when the condition changes to `False` and cancels it when the - condition changes to `True`. - - Doesn't re-start the command if it ends while the condition is still `False`. If the command - should restart, see RepeatCommand. - - :param command: the command to start - :returns: this trigger, so calls can be chained - """ - - @self._loop.bind - def _(state=SimpleNamespace(pressed_last=self._condition())): - pressed = self._condition() - if state.pressed_last and not pressed: - command.schedule() - elif not state.pressed_last and pressed: - command.cancel() - state.pressed_last = pressed - - return self - - def toggleOnTrue(self, command: Command) -> Self: - """ - Toggles a command when the condition changes from `False` to `True`. - - :param command: the command to toggle - :returns: this trigger, so calls can be chained - """ - - @self._loop.bind - def _(state=SimpleNamespace(pressed_last=self._condition())): - pressed = self._condition() - if not state.pressed_last and pressed: - if command.isScheduled(): - command.cancel() - else: - command.schedule() - state.pressed_last = pressed - - return self - - def toggleOnFalse(self, command: Command) -> Self: - """ - Toggles a command when the condition changes from `True` to `False`. - - :param command: the command to toggle - :returns: this trigger, so calls can be chained - """ - - @self._loop.bind - def _(state=SimpleNamespace(pressed_last=self._condition())): - pressed = self._condition() - if state.pressed_last and not pressed: - if command.isScheduled(): - command.cancel() - else: - command.schedule() - state.pressed_last = pressed - - return self - - def __call__(self) -> bool: - return self._condition() - - def getAsBoolean(self) -> bool: - return self._condition() - - def __bool__(self) -> bool: - return self._condition() - - def __and__(self, other: Callable[[], bool]) -> "Trigger": - return Trigger(lambda: self() and other()) - - def and_(self, other: Callable[[], bool]) -> "Trigger": - """ - Composes two triggers with logical AND. - - :param trigger: the condition to compose with - :returns: A trigger which is active when both component triggers are active. - """ - return self & other - - def __or__(self, other: Callable[[], bool]) -> "Trigger": - return Trigger(lambda: self() or other()) - - def or_(self, other: Callable[[], bool]) -> "Trigger": - """ - Composes two triggers with logical OR. - - :param trigger: the condition to compose with - :returns: A trigger which is active when either component trigger is active. - """ - return self | other - - def __invert__(self) -> "Trigger": - return Trigger(lambda: not self()) - - def negate(self) -> "Trigger": - """ - Creates a new trigger that is active when this trigger is inactive, i.e. that acts as the - negation of this trigger. - - :returns: the negated trigger - """ - return ~self - - def not_(self) -> "Trigger": - return ~self - - def debounce( - self, - seconds: float, - debounce_type: Debouncer.DebounceType = Debouncer.DebounceType.kRising, - ) -> "Trigger": - """ - Creates a new debounced trigger from this trigger - it will become active when this trigger has - been active for longer than the specified period. - - :param seconds: The debounce period. - :param type: The debounce type. - :returns: The debounced trigger. - """ - debouncer = Debouncer(seconds, debounce_type) - return Trigger(lambda: debouncer.calculate(self())) diff --git a/rio/ottomanEmpire/bursa/cmd.py b/rio/ottomanEmpire/bursa/cmd.py deleted file mode 100644 index 62a2e24c..00000000 --- a/rio/ottomanEmpire/bursa/cmd.py +++ /dev/null @@ -1,191 +0,0 @@ -from typing import Any, Callable, Dict, Hashable - -from .command import Command -from .conditionalcommand import ConditionalCommand -from .functionalcommand import FunctionalCommand -from .instantcommand import InstantCommand -from .parallelcommandgroup import ParallelCommandGroup -from .paralleldeadlinegroup import ParallelDeadlineGroup -from .parallelracegroup import ParallelRaceGroup -from .printcommand import PrintCommand -from .runcommand import RunCommand -from .selectcommand import SelectCommand -from .sequentialcommandgroup import SequentialCommandGroup -from .startendcommand import StartEndCommand -from .subsystem import Subsystem -from .waitcommand import WaitCommand -from .waituntilcommand import WaitUntilCommand - -# Is this whole file just to avoid the `new` keyword in Java? - - -def none() -> Command: - """ - Constructs a command that does nothing, finishing immediately. - - :returns: the command - """ - return InstantCommand() - - -def runOnce(action: Callable[[], Any], *requirements: Subsystem) -> Command: - """ - Constructs a command that runs an action once and finishes. - - :param action: the action to run - :param requirements: subsystems the action requires - :returns: the command - """ - return InstantCommand(action, *requirements) - - -def run(action: Callable[[], Any], *requirements: Subsystem) -> Command: - """ - Constructs a command that runs an action every iteration until interrupted. - - :param action: the action to run - :param requirements: subsystems the action requires - :returns: the command - """ - return RunCommand(action, *requirements) - - -def startEnd( - run: Callable[[], Any], end: Callable[[], Any], *requirements: Subsystem -) -> Command: - """ - Constructs a command that runs an action once and another action when the command is - interrupted. - - :param start: the action to run on start - :param end: the action to run on interrupt - :param requirements: subsystems the action requires - :returns: the command - """ - return StartEndCommand(run, lambda: end(), *requirements) - - -def runEnd( - run: Callable[[], Any], end: Callable[[], Any], *requirements: Subsystem -) -> Command: - """ - Constructs a command that runs an action every iteration until interrupted, and then runs a - second action. - - :param run: the action to run every iteration - :param end: the action to run on interrupt - :param requirements: subsystems the action requires - :returns: the command - """ - return FunctionalCommand( - lambda: None, run, lambda interrupted: end(), lambda: False, *requirements - ) - - -def print_(message: str) -> Command: - """ - Constructs a command that prints a message and finishes. - - :param message: the message to print - :returns: the command - """ - return PrintCommand(message) - - -def waitSeconds(seconds: float) -> Command: - """ - Constructs a command that does nothing, finishing after a specified duration. - - :param seconds: after how long the command finishes - :returns: the command - """ - return WaitCommand(seconds) - - -def waitUntil(condition: Callable[[], bool]) -> Command: - """ - Constructs a command that does nothing, finishing once a condition becomes true. - - :param condition: the condition - :returns: the command - """ - return WaitUntilCommand(condition) - - -def either(onTrue: Command, onFalse: Command, selector: Callable[[], bool]) -> Command: - """ - Runs one of two commands, based on the boolean selector function. - - :param onTrue: the command to run if the selector function returns true - :param onFalse: the command to run if the selector function returns false - :param selector: the selector function - :returns: the command - """ - return ConditionalCommand(onTrue, onFalse, selector) - - -def select( - commands: Dict[Hashable, Command], selector: Callable[[], Hashable] -) -> Command: - """ - Runs one of several commands, based on the selector function. - - :param selector: the selector function - :param commands: map of commands to select from - :returns: the command - """ - return SelectCommand(commands, selector) - - -def sequence(*commands: Command) -> Command: - """ - Runs a group of commands in series, one after the other. - - :param commands: the commands to include - :returns: the command group - """ - return SequentialCommandGroup(*commands) - - -def repeatingSequence(*commands: Command) -> Command: - """ - Runs a group of commands in series, one after the other. Once the last command ends, the group - is restarted. - - :param commands: the commands to include - :returns: the command group - """ - return sequence(*commands).repeatedly() - - -def parallel(*commands: Command) -> Command: - """ - Runs a group of commands at the same time. Ends once all commands in the group finish. - - :param commands: the commands to include - :returns: the command - """ - return ParallelCommandGroup(*commands) - - -def race(*commands: Command) -> Command: - """ - Runs a group of commands at the same time. Ends once any command in the group finishes, and - cancels the others. - - :param commands: the commands to include - :returns: the command group - """ - return ParallelRaceGroup(*commands) - - -def deadline(deadline: Command, *commands: Command) -> Command: - """ - Runs a group of commands at the same time. Ends once a specific command finishes, and cancels - the others. - - :param deadline: the deadline command - :param commands: the commands to include - :returns: the command group - """ - return ParallelDeadlineGroup(deadline, *commands) diff --git a/rio/ottomanEmpire/bursa/command.py b/rio/ottomanEmpire/bursa/command.py deleted file mode 100644 index e8b0546f..00000000 --- a/rio/ottomanEmpire/bursa/command.py +++ /dev/null @@ -1,557 +0,0 @@ -# Don't import stuff from the package here, it'll cause a circular import - - -from __future__ import annotations - -from enum import Enum -from typing import TYPE_CHECKING, Any, Callable, Set - -from typing_extensions import Self, TypeAlias - -if TYPE_CHECKING: - from .instantcommand import InstantCommand - from .subsystem import Subsystem - from .parallelracegroup import ParallelRaceGroup - from .sequentialcommandgroup import SequentialCommandGroup - from .paralleldeadlinegroup import ParallelDeadlineGroup - from .parallelcommandgroup import ParallelCommandGroup - from .perpetualcommand import PerpetualCommand - from .repeatcommand import RepeatCommand - from .proxycommand import ProxyCommand - from .conditionalcommand import ConditionalCommand - from .wrappercommand import WrapperCommand - -from wpiutil import Sendable, SendableRegistry, SendableBuilder - - -class InterruptionBehavior(Enum): - """ - An enum describing the command's behavior when another command with a shared requirement is - scheduled. - """ - - kCancelIncoming = 0 - """ - This command ends, #end(boolean) end(true) is called, and the incoming command is - scheduled normally. - - This is the default behavior. - """ - - kCancelSelf = 1 - """ This command continues, and the incoming command is not scheduled.""" - - -class Command(Sendable): - """ - A state machine representing a complete action to be performed by the robot. Commands are run by - the CommandScheduler, and can be composed into CommandGroups to allow users to build - complicated multistep actions without the need to roll the state machine logic themselves. - - Commands are run synchronously from the main robot loop; no multithreading is used, unless - specified explicitly from the command implementation. - - This class is provided by the NewCommands VendorDep - """ - - InterruptionBehavior: TypeAlias = ( - InterruptionBehavior # type alias for 2023 location - ) - - requirements: Set[Subsystem] - - def __new__(cls, *args, **kwargs) -> Self: - instance = super().__new__( - cls, - ) - super().__init__(instance) - SendableRegistry.add(instance, cls.__name__) - instance.requirements = set() - return instance - - def __init__(self): - pass - - def initialize(self): - """The initial subroutine of a command. Called once when the command is initially scheduled.""" - pass - - def execute(self): - """The main body of a command. Called repeatedly while the command is scheduled.""" - pass - - def isFinished(self) -> bool: - """ - Whether the command has finished. Once a command finishes, the scheduler will call its end() - method and un-schedule it. - - :returns: whether the command has finished. - """ - return False - - def end(self, interrupted: bool): - """ - The action to take when the command ends. Called when either the command finishes normally, or - when it interrupted/canceled. - - Do not schedule commands here that share requirements with this command. Use {@link - #andThen(Command...)} instead. - - :param interrupted: whether the command was interrupted/canceled - """ - pass - - def getRequirements(self) -> Set[Subsystem]: - """ - Specifies the set of subsystems used by this command. Two commands cannot use the same - subsystem at the same time. If another command is scheduled that shares a requirement, {@link - #getInterruptionBehavior()} will be checked and followed. If no subsystems are required, return - an empty set. - - Note: it is recommended that user implementations contain the requirements as a field, and - return that field here, rather than allocating a new set every time this is called. - - :returns: the set of subsystems that are required - """ - return self.requirements - - def addRequirements(self, *requirements: Subsystem): - """ - Adds the specified subsystems to the requirements of the command. The scheduler will prevent - two commands that require the same subsystem from being scheduled simultaneously. - - Note that the scheduler determines the requirements of a command when it is scheduled, so - this method should normally be called from the command's constructor. - - :param requirements: the requirements to add - """ - self.requirements.update(requirements) - - def runsWhenDisabled(self) -> bool: - """ - Whether the given command should run when the robot is disabled. Override to return true if the - command should run when disabled. - - :returns: whether the command should run when the robot is disabled - """ - return False - - def schedule(self, interruptible: bool = True) -> None: - """Schedules this command.""" - from .commandscheduler import CommandScheduler - - CommandScheduler.getInstance().schedule(self) - - def cancel(self) -> None: - """ - Cancels this command. Will call #end(boolean) end(true). Commands will be canceled - regardless of InterruptionBehavior interruption behavior. - """ - from .commandscheduler import CommandScheduler - - CommandScheduler.getInstance().cancel(self) - - def isScheduled(self) -> bool: - """ - Whether the command is currently scheduled. Note that this does not detect whether the command - is in a composition, only whether it is directly being run by the scheduler. - - :returns: Whether the command is scheduled. - """ - from .commandscheduler import CommandScheduler - - return CommandScheduler.getInstance().isScheduled(self) - - def hasRequirement(self, requirement: Subsystem) -> bool: - """ - Whether the command requires a given subsystem. - - :param requirement: the subsystem to inquire about - :returns: whether the subsystem is required - """ - return requirement in self.requirements - - def getInterruptionBehavior(self) -> InterruptionBehavior: - """ - How the command behaves when another command with a shared requirement is scheduled. - - :returns: a variant of InterruptionBehavior, defaulting to {@link InterruptionBehavior#kCancelSelf kCancelSelf}. - """ - return InterruptionBehavior.kCancelSelf - - def withTimeout(self, seconds: float) -> ParallelRaceGroup: - """ - Decorates this command with a timeout. If the specified timeout is exceeded before the command - finishes normally, the command will be interrupted and un-scheduled. Note that the timeout only - applies to the command returned by this method; the calling command is not itself changed. - - Note: This decorator works by adding this command to a composition. The command the - decorator was called on cannot be scheduled independently or be added to a different - composition (namely, decorators), unless it is manually cleared from the list of composed - commands with CommandScheduler#removeComposedCommand(Command). The command composition - returned from this method can be further decorated without issue. - - :param seconds: the timeout duration - :returns: the command with the timeout added - """ - from .waitcommand import WaitCommand - - return self.raceWith(WaitCommand(seconds)) - - def until(self, condition: Callable[[], bool]) -> ParallelRaceGroup: - """ - Decorates this command with an interrupt condition. If the specified condition becomes true - before the command finishes normally, the command will be interrupted and un-scheduled. - - Note: This decorator works by adding this command to a composition. The command the - decorator was called on cannot be scheduled independently or be added to a different - composition (namely, decorators), unless it is manually cleared from the list of composed - commands with CommandScheduler#removeComposedCommand(Command). The command composition - returned from this method can be further decorated without issue. - - :param condition: the interrupt condition - :returns: the command with the interrupt condition added - """ - from .waituntilcommand import WaitUntilCommand - - return self.raceWith(WaitUntilCommand(condition)) - - def onlyWhile(self, condition: Callable[[], bool]) -> ParallelRaceGroup: - """ - Decorates this command with a run condition. If the specified condition becomes false before - the command finishes normally, the command will be interrupted and un-scheduled. - - Note: This decorator works by adding this command to a composition. The command the - decorator was called on cannot be scheduled independently or be added to a different - composition (namely, decorators), unless it is manually cleared from the list of composed - commands with CommandScheduler#removeComposedCommand(Command). The command composition - returned from this method can be further decorated without issue. - - :param condition: the interrupt condition - :returns: the command with the interrupt condition added - """ - return self.until(lambda: not condition()) - - def withInterrupt(self, condition: Callable[[], bool]) -> ParallelRaceGroup: - """ - Decorates this command with an interrupt condition. If the specified condition becomes true - before the command finishes normally, the command will be interrupted and un-scheduled. Note - that this only applies to the command returned by this method; the calling command is not - itself changed. - - Note: This decorator works by adding this command to a composition. The command the - decorator was called on cannot be scheduled independently or be added to a different - composition (namely, decorators), unless it is manually cleared from the list of composed - commands with CommandScheduler#removeComposedCommand(Command). The command composition - returned from this method can be further decorated without issue. - - :param condition: the interrupt condition - :returns: the command with the interrupt condition added - @deprecated Replace with #until(BooleanSupplier) - """ - return self.until(condition) - - def beforeStarting(self, before: Command) -> SequentialCommandGroup: - """ - Decorates this command with another command to run before this command starts. - - Note: This decorator works by adding this command to a composition. The command the - decorator was called on cannot be scheduled independently or be added to a different - composition (namely, decorators), unless it is manually cleared from the list of composed - commands with CommandScheduler#removeComposedCommand(Command). The command composition - returned from this method can be further decorated without issue. - - :param before: the command to run before this one - :returns: the decorated command - """ - from .sequentialcommandgroup import SequentialCommandGroup - - return SequentialCommandGroup(before, self) - - def andThen(self, *next: Command) -> SequentialCommandGroup: - """ - Decorates this command with a set of commands to run after it in sequence. Often more - convenient/less-verbose than constructing a new SequentialCommandGroup explicitly. - - Note: This decorator works by adding this command to a composition. The command the - decorator was called on cannot be scheduled independently or be added to a different - composition (namely, decorators), unless it is manually cleared from the list of composed - commands with CommandScheduler#removeComposedCommand(Command). The command composition - returned from this method can be further decorated without issue. - - :param next: the commands to run next - :returns: the decorated command - """ - from .sequentialcommandgroup import SequentialCommandGroup - - return SequentialCommandGroup(self, *next) - - def deadlineWith(self, *parallel: Command) -> ParallelDeadlineGroup: - """ - Decorates this command with a set of commands to run parallel to it, ending when the calling - command ends and interrupting all the others. Often more convenient/less-verbose than - constructing a new ParallelDeadlineGroup explicitly. - - Note: This decorator works by adding this command to a composition. The command the - decorator was called on cannot be scheduled independently or be added to a different - composition (namely, decorators), unless it is manually cleared from the list of composed - commands with CommandScheduler#removeComposedCommand(Command). The command composition - returned from this method can be further decorated without issue. - - :param parallel: the commands to run in parallel - :returns: the decorated command - """ - from .paralleldeadlinegroup import ParallelDeadlineGroup - - return ParallelDeadlineGroup(self, *parallel) - - def alongWith(self, *parallel: Command) -> ParallelCommandGroup: - """ - Decorates this command with a set of commands to run parallel to it, ending when the last - command ends. Often more convenient/less-verbose than constructing a new {@link - ParallelCommandGroup} explicitly. - - Note: This decorator works by adding this command to a composition. The command the - decorator was called on cannot be scheduled independently or be added to a different - composition (namely, decorators), unless it is manually cleared from the list of composed - commands with CommandScheduler#removeComposedCommand(Command). The command composition - returned from this method can be further decorated without issue. - - :param parallel: the commands to run in parallel - :returns: the decorated command - """ - from .parallelcommandgroup import ParallelCommandGroup - - return ParallelCommandGroup(self, *parallel) - - def raceWith(self, *parallel: Command) -> ParallelRaceGroup: - """ - Decorates this command with a set of commands to run parallel to it, ending when the first - command ends. Often more convenient/less-verbose than constructing a new {@link - ParallelRaceGroup} explicitly. - - Note: This decorator works by adding this command to a composition. The command the - decorator was called on cannot be scheduled independently or be added to a different - composition (namely, decorators), unless it is manually cleared from the list of composed - commands with CommandScheduler#removeComposedCommand(Command). The command composition - returned from this method can be further decorated without issue. - - :param parallel: the commands to run in parallel - :returns: the decorated command - """ - from .parallelracegroup import ParallelRaceGroup - - return ParallelRaceGroup(self, *parallel) - - def perpetually(self) -> PerpetualCommand: - """ - Decorates this command to run perpetually, ignoring its ordinary end conditions. The decorated - command can still be interrupted or canceled. - - Note: This decorator works by adding this command to a composition. The command the - decorator was called on cannot be scheduled independently or be added to a different - composition (namely, decorators), unless it is manually cleared from the list of composed - commands with CommandScheduler#removeComposedCommand(Command). The command composition - returned from this method can be further decorated without issue. - - :returns: the decorated command - @deprecated PerpetualCommand violates the assumption that execute() doesn't get called after - isFinished() returns true -- an assumption that should be valid. This was unsafe/undefined - behavior from the start, and RepeatCommand provides an easy way to achieve similar end - results with slightly different (and safe) semantics. - """ - from .perpetualcommand import PerpetualCommand - - return PerpetualCommand(self) - - def repeatedly(self) -> RepeatCommand: - """ - Decorates this command to run repeatedly, restarting it when it ends, until this command is - interrupted. The decorated command can still be canceled. - - Note: This decorator works by adding this command to a composition. The command the - decorator was called on cannot be scheduled independently or be added to a different - composition (namely, decorators), unless it is manually cleared from the list of composed - commands with CommandScheduler#removeComposedCommand(Command). The command composition - returned from this method can be further decorated without issue. - - :returns: the decorated command - """ - from .repeatcommand import RepeatCommand - - return RepeatCommand(self) - - def asProxy(self) -> ProxyCommand: - """ - Decorates this command to run "by proxy" by wrapping it in a ProxyCommand. This is - useful for "forking off" from command compositions when the user does not wish to extend the - command's requirements to the entire command composition. - - :returns: the decorated command - """ - from .proxycommand import ProxyCommand - - return ProxyCommand(self) - - def unless(self, condition: Callable[[], bool]) -> ConditionalCommand: - """ - Decorates this command to only run if this condition is not met. If the command is already - running and the condition changes to true, the command will not stop running. The requirements - of this command will be kept for the new conditional command. - - Note: This decorator works by adding this command to a composition. The command the - decorator was called on cannot be scheduled independently or be added to a different - composition (namely, decorators), unless it is manually cleared from the list of composed - commands with CommandScheduler#removeComposedCommand(Command). The command composition - returned from this method can be further decorated without issue. - - :param condition: the condition that will prevent the command from running - :returns: the decorated command - """ - from .conditionalcommand import ConditionalCommand - from .instantcommand import InstantCommand - - return ConditionalCommand(InstantCommand(), self, condition) - - def onlyIf(self, condition: Callable[[], bool]) -> ConditionalCommand: - """ - Decorates this command to only run if this condition is met. If the command is already running - and the condition changes to false, the command will not stop running. The requirements of this - command will be kept for the new conditional command. - - Note: This decorator works by adding this command to a composition. The command the - decorator was called on cannot be scheduled independently or be added to a different - composition (namely, decorators), unless it is manually cleared from the list of composed - commands with CommandScheduler#removeComposedCommand(Command). The command composition - returned from this method can be further decorated without issue. - - :param condition: the condition that will allow the command to run - :returns: the decorated command - """ - return self.unless(lambda: not condition()) - - def ignoringDisable(self, doesRunWhenDisabled: bool) -> WrapperCommand: - """ - Decorates this command to run or stop when disabled. - - :param doesRunWhenDisabled: true to run when disabled. - :returns: the decorated command - """ - from .wrappercommand import WrapperCommand - - class W(WrapperCommand): - def runsWhenDisabled(self) -> bool: - return doesRunWhenDisabled - - return W(self) - - def withInterruptBehavior(self, behavior: InterruptionBehavior) -> WrapperCommand: - """ - Decorates this command to have a different InterruptionBehavior interruption behavior. - - :param interruptBehavior: the desired interrupt behavior - :returns: the decorated command - """ - from .wrappercommand import WrapperCommand - - class W(WrapperCommand): - def getInterruptionBehavior(self) -> InterruptionBehavior: - return behavior - - return W(self) - - def finallyDo(self, end: Callable[[bool], Any]) -> WrapperCommand: - """ - Decorates this command with a lambda to call on interrupt or end, following the command's - inherent #end(boolean) method. - - :param end: a lambda accepting a boolean parameter specifying whether the command was - interrupted. - :return: the decorated command - """ - from .wrappercommand import WrapperCommand - - class W(WrapperCommand): - def end(self, interrupted: bool) -> None: - self._command.end(interrupted) - end(interrupted) - - return W(self) - - def handleInterrupt(self, handler: Callable[[], Any]) -> WrapperCommand: - """ - Decorates this command with a lambda to call on interrupt, following the command's inherent - #end(boolean) method. - - :param handler: a lambda to run when the command is interrupted - :returns: the decorated command - """ - return self.finallyDo(lambda interrupted: handler() if interrupted else None) - - def getName(self) -> str: - """ - Gets the name of this Command. - - :returns: Name - """ - return SendableRegistry.getName(self) - - def setName(self, name: str): - """ - Sets the name of this Command. - - :param name: Name - """ - SendableRegistry.setName(self, name) - - def withName(self, name: str) -> WrapperCommand: - """ - Decorates this command with a name. - - :param name: the name of the command - :returns: the decorated command - """ - from .wrappercommand import WrapperCommand - - wrapper = WrapperCommand(self) - wrapper.setName(name) - return wrapper - - def initSendable(self, builder: SendableBuilder) -> None: - from .commandscheduler import CommandScheduler - - builder.setSmartDashboardType("Command") - builder.addStringProperty( - ".name", - self.getName, - lambda _: None, - ) - - def on_set(value: bool) -> None: - if value: - if not self.isScheduled(): - self.schedule() - else: - if self.isScheduled(): - self.cancel() - - builder.addBooleanProperty( - "running", - self.isScheduled, - on_set, - ) - builder.addBooleanProperty( - ".isParented", - lambda: CommandScheduler.getInstance().isComposed(self), - lambda _: None, - ) - builder.addStringProperty( - "interruptBehavior", - lambda: self.getInterruptionBehavior().name, - lambda _: None, - ) - builder.addBooleanProperty( - "runsWhenDisabled", - self.runsWhenDisabled, - lambda _: None, - ) diff --git a/rio/ottomanEmpire/bursa/commandgroup.py b/rio/ottomanEmpire/bursa/commandgroup.py deleted file mode 100644 index 8a00f242..00000000 --- a/rio/ottomanEmpire/bursa/commandgroup.py +++ /dev/null @@ -1,21 +0,0 @@ -from __future__ import annotations - -from .command import Command - - -class IllegalCommandUse(Exception): - pass - - -class CommandGroup(Command): - """ - A base for CommandGroups. - """ - - def addCommands(self, *commands: Command): - """ - Adds the given commands to the command group. - - :param commands: The commands to add. - """ - raise NotImplementedError diff --git a/rio/ottomanEmpire/bursa/commandscheduler.py b/rio/ottomanEmpire/bursa/commandscheduler.py deleted file mode 100644 index ef51150c..00000000 --- a/rio/ottomanEmpire/bursa/commandscheduler.py +++ /dev/null @@ -1,475 +0,0 @@ -from __future__ import annotations - -from typing import Any, Callable, Dict, Iterable, List, Optional, Set, Union - -import hal -from typing_extensions import Self -from wpilib import RobotBase, RobotState, TimedRobot, Watchdog -from wpilib.event import EventLoop - -from .command import Command, InterruptionBehavior -from .commandgroup import * -from .subsystem import Subsystem - - -class CommandScheduler: - """ - The scheduler responsible for running Commands. A Command-based robot should call {@link - CommandScheduler#run()} on the singleton instance in its periodic block in order to run commands - synchronously from the main loop. Subsystems should be registered with the scheduler using {@link - CommandScheduler#registerSubsystem(Subsystem...)} in order for their Subsystem#periodic() - methods to be called and for their default commands to be scheduled. - """ - - _instance: Optional[CommandScheduler] = None - - def __new__(cls) -> CommandScheduler: - if cls._instance is None: - return super().__new__(cls) - return cls._instance - - @staticmethod - def getInstance() -> "CommandScheduler": - """ - Returns the Scheduler instance. - - :returns: the instance - """ - return CommandScheduler() - - @staticmethod - def resetInstance() -> None: - """ - Resets the scheduler instance, which is useful for testing purposes. This should not be - called by user code. - """ - inst = CommandScheduler._instance - if inst: - inst._defaultButtonLoop.clear() - CommandScheduler._instance = None - - def __init__(self) -> None: - """ - Gets the scheduler instance. - """ - if CommandScheduler._instance is not None: - return - CommandScheduler._instance = self - self._composedCommands: Set[Command] = set() - self._scheduledCommands: Dict[Command, None] = {} - self._requirements: Dict[Subsystem, Command] = {} - self._subsystems: Dict[Subsystem, Optional[Command]] = {} - - self._defaultButtonLoop = EventLoop() - self.setActiveButtonLoop(self._defaultButtonLoop) - - self._disabled = False - - self._initActions: List[Callable[[Command], None]] = [] - self._executeActions: List[Callable[[Command], None]] = [] - self._interruptActions: List[Callable[[Command], None]] = [] - self._finishActions: List[Callable[[Command], None]] = [] - - self._inRunLoop = False - self._toSchedule: Dict[Command, None] = {} - self._toCancel: Dict[Command, None] = {} - - self._watchdog = Watchdog(TimedRobot.kDefaultPeriod, lambda: None) - - hal.report( - hal.tResourceType.kResourceType_Command.value, - hal.tInstances.kCommand2_Scheduler.value, - ) - - def setPeriod(self, period: float) -> None: - """ - Changes the period of the loop overrun watchdog. This should be kept in sync with the - TimedRobot period. - - :param period: Period in seconds. - """ - self._watchdog.setTimeout(period) - - def getDefaultButtonLoop(self) -> EventLoop: - """ - Get the default button poll. - - :returns: a reference to the default EventLoop object polling buttons. - """ - return self._defaultButtonLoop - - def getActiveButtonLoop(self) -> EventLoop: - """ - Get the active button poll. - - :returns: a reference to the current EventLoop object polling buttons. - """ - return self._activeButtonLoop - - def setActiveButtonLoop(self, loop: EventLoop) -> None: - """ - Replace the button poll with another one. - - :param loop: the new button polling loop object. - """ - self._activeButtonLoop = loop - - def initCommand(self, command: Command, *requirements: Subsystem) -> None: - """ - Initializes a given command, adds its requirements to the list, and performs the init actions. - - :param command: The command to initialize - :param requirements: The command requirements - """ - self._scheduledCommands[command] = None - for requirement in requirements: - self._requirements[requirement] = command - command.initialize() - for action in self._initActions: - action(command) - # self._watchdog.addEpoch() - - def schedule(self, *commands) -> None: - """ - Schedules a command for execution. Does nothing if the command is already scheduled. If a - command's requirements are not available, it will only be started if all the commands currently - using those requirements have been scheduled as interruptible. If this is the case, they will - be interrupted and the command will be scheduled. - - :param commands: the commands to schedule. - """ - if len(commands) > 1: - for command in commands: - self.schedule(command) - return - - command = commands[0] - - if command is None: - # DriverStation.reportWarning("CommandScheduler tried to schedule a null command!", True) - return - - if self._inRunLoop: - self._toSchedule[command] = None - return - - if command in self.getComposedCommands(): - raise IllegalCommandUse( - "A command that is part of a CommandGroup cannot be independently scheduled" - ) - - if self._disabled: - return - - if RobotState.isDisabled() and not command.runsWhenDisabled(): - return - - if self.isScheduled(command): - return - - requirements = command.getRequirements() - - if self._requirements.keys().isdisjoint(requirements): - self.initCommand(command, *requirements) - else: - for requirement in requirements: - requiringCommand = self.requiring(requirement) - if ( - requiringCommand is not None - and requiringCommand.getInterruptionBehavior() - == InterruptionBehavior.kCancelIncoming - ): - return - - for requirement in requirements: - requiringCommand = self.requiring(requirement) - if requiringCommand is not None: - self.cancel(requiringCommand) - - self.initCommand(command, *requirements) - - def run(self): - """ - Runs a single iteration of the scheduler. The execution occurs in the following order: - - Subsystem periodic methods are called. - - Button bindings are polled, and new commands are scheduled from them. - - Currently-scheduled commands are executed. - - End conditions are checked on currently-scheduled commands, and commands that are finished - have their end methods called and are removed. - - Any subsystems not being used as requirements have their default methods started. - """ - if self._disabled: - return - self._watchdog.reset() - - for subsystem in self._subsystems: - subsystem.periodic() - if RobotBase.isSimulation(): - subsystem.simulationPeriodic() - # self._watchdog.addEpoch() - - loopCache = self._activeButtonLoop - loopCache.poll() - self._watchdog.addEpoch("buttons.run()") - - self._inRunLoop = True - - for command in self._scheduledCommands.copy(): - if not command.runsWhenDisabled() and RobotState.isDisabled(): - command.end(True) - for action in self._interruptActions: - action(command) - for requirement in command.getRequirements(): - self._requirements.pop(requirement) - self._scheduledCommands.pop(command) - continue - - command.execute() - for action in self._executeActions: - action(command) - # self._watchdog.addEpoch() - if command.isFinished(): - command.end(False) - for action in self._finishActions: - action(command) - self._scheduledCommands.pop(command) - for requirement in command.getRequirements(): - self._requirements.pop(requirement) - - self._inRunLoop = False - - for command in self._toSchedule: - self.schedule(command) - - for command in self._toCancel: - self.cancel(command) - - self._toSchedule.clear() - self._toCancel.clear() - - for subsystem, command in self._subsystems.items(): - if subsystem not in self._requirements and command is not None: - self.schedule(command) - - self._watchdog.disable() - if self._watchdog.isExpired(): - self._watchdog.printEpochs() - - def registerSubsystem(self, *subsystems: Subsystem) -> None: - """ - Registers subsystems with the scheduler. This must be called for the subsystem's periodic block - to run when the scheduler is run, and for the subsystem's default command to be scheduled. It - is recommended to call this from the constructor of your subsystem implementations. - - :param subsystems: the subsystem to register - """ - for subsystem in subsystems: - if subsystem in self._subsystems: - # DriverStation.reportWarning("Tried to register an already-registered subsystem", True) - continue - self._subsystems[subsystem] = None - - def unregisterSubsystem(self, *subsystems: Subsystem) -> None: - """ - Un-registers subsystems with the scheduler. The subsystem will no longer have its periodic - block called, and will not have its default command scheduled. - - :param subsystems: the subsystem to un-register - """ - for subsystem in subsystems: - self._subsystems.pop(subsystem) - - def setDefaultCommand(self, subsystem: Subsystem, defaultCommand: Command) -> None: - """ - Sets the default command for a subsystem. Registers that subsystem if it is not already - registered. Default commands will run whenever there is no other command currently scheduled - that requires the subsystem. Default commands should be written to never end (i.e. their {@link - Command#isFinished()} method should return false), as they would simply be re-scheduled if they - do. Default commands must also require their subsystem. - - :param subsystem: the subsystem whose default command will be set - :param defaultCommand: the default command to associate with the subsystem - """ - self.requireNotComposed([defaultCommand]) - if subsystem not in defaultCommand.getRequirements(): - raise IllegalCommandUse("Default commands must require their subsystem!") - if ( - defaultCommand.getInterruptionBehavior() - != InterruptionBehavior.kCancelIncoming - ): - # DriverStation.reportWarning("Registering a non-interruptible default command\nThis will likely prevent any other commands from requiring this subsystem.", True) - pass - self._subsystems[subsystem] = defaultCommand - - def removeDefaultCommand(self, subsystem: Subsystem) -> None: - """ - Removes the default command for a subsystem. The current default command will run until another - command is scheduled that requires the subsystem, at which point the current default command - will not be re-scheduled. - - :param subsystem: the subsystem whose default command will be removed - """ - self._subsystems[subsystem] = None - - def getDefaultCommand(self, subsystem: Subsystem) -> Optional[Command]: - """ - Gets the default command associated with this subsystem. Null if this subsystem has no default - command associated with it. - - :param subsystem: the subsystem to inquire about - :returns: the default command associated with the subsystem - """ - return self._subsystems[subsystem] - - def cancel(self, *commands: Command) -> None: - """ - Cancels commands. The scheduler will only call Command#end(boolean) method of the - canceled command with {@code true}, indicating they were canceled (as opposed to finishing - normally). - - Commands will be canceled regardless of InterruptionBehavior interruption behavior. - - :param commands: the commands to cancel - """ - if self._inRunLoop: - for command in commands: - self._toCancel[command] = None - return - - for command in commands: - if not self.isScheduled(command): - continue - - self._scheduledCommands.pop(command) - for requirement in command.getRequirements(): - del self._requirements[requirement] - command.end(True) - for action in self._interruptActions: - action(command) - - def cancelAll(self) -> None: - """Cancels all commands that are currently scheduled.""" - self.cancel(*self._scheduledCommands) - - def isScheduled(self, *commands: Command) -> bool: - """ - Whether the given commands are running. Note that this only works on commands that are directly - scheduled by the scheduler; it will not work on commands inside compositions, as the scheduler - does not see them. - - :param commands: the command to query - :returns: whether the command is currently scheduled - """ - return all(command in self._scheduledCommands for command in commands) - - def requiring(self, subsystem: Subsystem) -> Optional[Command]: - """ - Returns the command currently requiring a given subsystem. None if no command is currently - requiring the subsystem - - :param subsystem: the subsystem to be inquired about - :returns: the command currently requiring the subsystem, or None if no command is currently - scheduled - """ - return self._requirements.get(subsystem) - - def disable(self) -> None: - """Disables the command scheduler.""" - self._disabled = True - - def enable(self) -> None: - """Enables the command scheduler.""" - self._disabled = False - - def onCommandInitialize(self, action: Callable[[Command], Any]) -> None: - """ - Adds an action to perform on the initialization of any command by the scheduler. - - :param action: the action to perform - """ - self._initActions.append(action) - - def onCommandExecute(self, action: Callable[[Command], Any]) -> None: - """ - Adds an action to perform on the execution of any command by the scheduler. - - :param action: the action to perform - """ - self._executeActions.append(action) - - def onCommandInterrupt(self, action: Callable[[Command], Any]) -> None: - """ - Adds an action to perform on the interruption of any command by the scheduler. - - :param action: the action to perform - """ - self._interruptActions.append(action) - - def onCommandFinish(self, action: Callable[[Command], Any]) -> None: - """ - Adds an action to perform on the finishing of any command by the scheduler. - - :param action: the action to perform - """ - self._finishActions.append(action) - - def registerComposedCommands(self, commands: Iterable[Command]) -> None: - """ - Register commands as composed. An exception will be thrown if these commands are scheduled - directly or added to a composition. - - :param commands: the commands to register - @throws IllegalArgumentException if the given commands have already been composed. - """ - self.requireNotComposed(commands) - self._composedCommands.update(commands) - - def clearComposedCommands(self) -> None: - """ - Clears the list of composed commands, allowing all commands to be freely used again. - - WARNING: Using this haphazardly can result in unexpected/undesirable behavior. Do not use - this unless you fully understand what you are doing. - """ - self._composedCommands.clear() - - def removeComposedCommands(self, commands: Iterable[Command]) -> None: - """ - Removes a single command from the list of composed commands, allowing it to be freely used - again. - - WARNING: Using this haphazardly can result in unexpected/undesirable behavior. Do not use - this unless you fully understand what you are doing. - - :param command: the command to remove from the list of grouped commands - """ - self._composedCommands.difference_update(commands) - - def requireNotComposed(self, commands: Iterable[Command]) -> None: - """ - Requires that the specified command hasn't been already added to a composition. - - :param commands: The commands to check - @throws IllegalArgumentException if the given commands have already been composed. - """ - if not self._composedCommands.isdisjoint(commands): - raise IllegalCommandUse( - "Commands that have been composed may not be added to another composition or scheduled individually" - ) - - def isComposed(self, command: Command) -> bool: - """ - Check if the given command has been composed. - - :param command: The command to check - :returns: true if composed - """ - return command in self.getComposedCommands() - - def getComposedCommands(self) -> Set[Command]: - return self._composedCommands diff --git a/rio/ottomanEmpire/bursa/conditionalcommand.py b/rio/ottomanEmpire/bursa/conditionalcommand.py deleted file mode 100644 index b26760de..00000000 --- a/rio/ottomanEmpire/bursa/conditionalcommand.py +++ /dev/null @@ -1,61 +0,0 @@ -from __future__ import annotations - -from typing import Callable - -from .command import Command -from .commandgroup import * -from .commandscheduler import CommandScheduler - - -class ConditionalCommand(Command): - """ - A command composition that runs one of two commands, depending on the value of the given - condition when this command is initialized. - - The rules for command compositions apply: command instances that are passed to it cannot be - added to any other composition or scheduled individually, and the composition requires all - subsystems its components require. - - This class is provided by the NewCommands VendorDep""" - - selectedCommand: Command - - def __init__( - self, onTrue: Command, onFalse: Command, condition: Callable[[], bool] - ): - """ - Creates a new ConditionalCommand. - - :param onTrue: the command to run if the condition is true - :param onFalse: the command to run if the condition is false - :param condition: the condition to determine which command to run""" - super().__init__() - - CommandScheduler.getInstance().registerComposedCommands([onTrue, onFalse]) - - self.onTrue = onTrue - self.onFalse = onFalse - self.condition = condition - - self.addRequirements(*onTrue.getRequirements()) - self.addRequirements(*onFalse.getRequirements()) - - def initialize(self): - if self.condition(): - self.selectedCommand = self.onTrue - else: - self.selectedCommand = self.onFalse - - self.selectedCommand.initialize() - - def execute(self): - self.selectedCommand.execute() - - def isFinished(self) -> bool: - return self.selectedCommand.isFinished() - - def end(self, interrupted: bool): - self.selectedCommand.end(interrupted) - - def runsWhenDisabled(self) -> bool: - return self.onTrue.runsWhenDisabled() and self.onFalse.runsWhenDisabled() diff --git a/rio/ottomanEmpire/bursa/functionalcommand.py b/rio/ottomanEmpire/bursa/functionalcommand.py deleted file mode 100644 index 9498568e..00000000 --- a/rio/ottomanEmpire/bursa/functionalcommand.py +++ /dev/null @@ -1,52 +0,0 @@ -from __future__ import annotations - -from typing import Any, Callable - -from .command import Command -from .commandgroup import * -from .subsystem import Subsystem - - -class FunctionalCommand(Command): - """ - A command that allows the user to pass in functions for each of the basic command methods through - the constructor. Useful for inline definitions of complex commands - note, however, that if a - command is beyond a certain complexity it is usually better practice to write a proper class for - it than to inline it.""" - - def __init__( - self, - onInit: Callable[[], Any], - onExecute: Callable[[], Any], - onEnd: Callable[[bool], Any], - isFinished: Callable[[], bool], - *requirements: Subsystem, - ): - """ - Creates a new FunctionalCommand. - - :param onInit: the function to run on command initialization - :param onExecute: the function to run on command execution - :param onEnd: the function to run on command end - :param isFinished: the function that determines whether the command has finished - :param requirements: the subsystems required by this command""" - super().__init__() - - self._onInit = onInit - self._onExecute = onExecute - self._onEnd = onEnd - self._isFinished = isFinished - - self.addRequirements(*requirements) - - def initialize(self): - self._onInit() - - def execute(self): - self._onExecute() - - def end(self, interrupted: bool): - self._onEnd(interrupted) - - def isFinished(self) -> bool: - return self._isFinished() diff --git a/rio/ottomanEmpire/bursa/instantcommand.py b/rio/ottomanEmpire/bursa/instantcommand.py deleted file mode 100644 index 6d20a2e6..00000000 --- a/rio/ottomanEmpire/bursa/instantcommand.py +++ /dev/null @@ -1,29 +0,0 @@ -from __future__ import annotations - -from typing import Callable, Optional - -from .functionalcommand import FunctionalCommand -from .subsystem import Subsystem - - -class InstantCommand(FunctionalCommand): - """ - A Command that runs instantly; it will initialize, execute once, and end on the same iteration of - the scheduler. Users can either pass in a Runnable and a set of requirements, or else subclass - this command if desired.""" - - def __init__( - self, toRun: Optional[Callable[[], None]] = None, *requirements: Subsystem - ): - """ - Creates a new InstantCommand that runs the given Runnable with the given requirements. - - :param toRun: the Runnable to run - :param requirements: the subsystems required by this command""" - super().__init__( - toRun or (lambda: None), - lambda: None, - lambda _: None, - lambda: True, - *requirements, - ) diff --git a/rio/ottomanEmpire/bursa/notifiercommand.py b/rio/ottomanEmpire/bursa/notifiercommand.py deleted file mode 100644 index 0a6e57e8..00000000 --- a/rio/ottomanEmpire/bursa/notifiercommand.py +++ /dev/null @@ -1,41 +0,0 @@ -from __future__ import annotations - -from typing import Any, Callable - -from wpilib import Notifier - -from .command import Command -from .commandgroup import * -from .subsystem import Subsystem - - -class NotifierCommand(Command): - """ - A command that starts a notifier to run the given runnable periodically in a separate thread. Has - no end condition as-is; either subclass it or use Command#withTimeout(double) or {@link - Command#until(java.util.function.BooleanSupplier)} to give it one. - - WARNING: Do not use this class unless you are confident in your ability to make the executed - code thread-safe. If you do not know what "thread-safe" means, that is a good sign that you - should not use this class.""" - - def __init__( - self, toRun: Callable[[], Any], period: float, *requirements: Subsystem - ): - """ - Creates a new NotifierCommand. - - :param toRun: the runnable for the notifier to run - :param period: the period at which the notifier should run, in seconds - :param requirements: the subsystems required by this command""" - super().__init__() - - self.notifier = Notifier(toRun) - self.period = period - self.addRequirements(*requirements) - - def initialize(self): - self.notifier.startPeriodic(self.period) - - def end(self, interrupted: bool): - self.notifier.stop() diff --git a/rio/ottomanEmpire/bursa/parallelcommandgroup.py b/rio/ottomanEmpire/bursa/parallelcommandgroup.py deleted file mode 100644 index e3ad01dd..00000000 --- a/rio/ottomanEmpire/bursa/parallelcommandgroup.py +++ /dev/null @@ -1,87 +0,0 @@ -from __future__ import annotations - -from typing import Dict - -from commands2.command import Command, InterruptionBehavior - -from .command import Command, InterruptionBehavior -from .commandgroup import * -from .commandscheduler import CommandScheduler -from .util import flatten_args_commands - - -class ParallelCommandGroup(CommandGroup): - """ - A command composition that runs a set of commands in parallel, ending when the last command ends. - - The rules for command compositions apply: command instances that are passed to it cannot be - added to any other composition or scheduled individually, and the composition requires all - subsystems its components require.""" - - def __init__(self, *commands: Command): - """ - Creates a new ParallelCommandGroup. The given commands will be executed simultaneously. The - command composition will finish when the last command finishes. If the composition is - interrupted, only the commands that are still running will be interrupted. - - :param commands: the commands to include in this composition.""" - super().__init__() - self._commands: Dict[Command, bool] = {} - self._runsWhenDisabled = True - self._interruptBehavior = InterruptionBehavior.kCancelIncoming - self.addCommands(*commands) - - def addCommands(self, *commands: Command): - commands = flatten_args_commands(commands) - if True in self._commands.values(): - raise IllegalCommandUse( - "Commands cannot be added to a composition while it is running" - ) - - CommandScheduler.getInstance().registerComposedCommands(commands) - - for command in commands: - if not command.getRequirements().isdisjoint(self.requirements): - raise IllegalCommandUse( - "Multiple comands in a parallel composition cannot require the same subsystems." - ) - - self._commands[command] = False - self.requirements.update(command.getRequirements()) - self._runsWhenDisabled = ( - self._runsWhenDisabled and command.runsWhenDisabled() - ) - - if command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf: - self._interruptBehavior = InterruptionBehavior.kCancelSelf - - def initialize(self): - for command in self._commands: - command.initialize() - self._commands[command] = True - - def execute(self): - for command, isRunning in self._commands.items(): - if not isRunning: - continue - command.execute() - if command.isFinished(): - command.end(False) - self._commands[command] = False - - def end(self, interrupted: bool): - if interrupted: - for command, isRunning in self._commands.items(): - if not isRunning: - continue - command.end(True) - self._commands[command] = False - - def isFinished(self) -> bool: - return True not in self._commands.values() - - def runsWhenDisabled(self) -> bool: - return self._runsWhenDisabled - - def getInterruptionBehavior(self) -> InterruptionBehavior: - return self._interruptBehavior diff --git a/rio/ottomanEmpire/bursa/paralleldeadlinegroup.py b/rio/ottomanEmpire/bursa/paralleldeadlinegroup.py deleted file mode 100644 index eb064f49..00000000 --- a/rio/ottomanEmpire/bursa/paralleldeadlinegroup.py +++ /dev/null @@ -1,98 +0,0 @@ -from __future__ import annotations - -from typing import Dict - -from commands2.command import Command, InterruptionBehavior - -from .command import Command, InterruptionBehavior -from .commandgroup import * -from .commandscheduler import CommandScheduler -from .util import flatten_args_commands - - -class ParallelDeadlineGroup(CommandGroup): - """ - A command composition that runs one of a selection of commands, either using a selector and a key - to command mapping, or a supplier that returns the command directly at runtime. - - The rules for command compositions apply: command instances that are passed to it cannot be - added to any other composition or scheduled individually, and the composition requires all - subsystems its components require.""" - - def __init__(self, deadline: Command, *commands: Command): - """ - Creates a new SelectCommand. - - :param commands: the map of commands to choose from - :param selector: the selector to determine which command to run""" - super().__init__() - self._commands: Dict[Command, bool] = {} - self._runsWhenDisabled = True - self._finished = True - self._deadline = deadline - self._interruptBehavior = InterruptionBehavior.kCancelIncoming - self.addCommands(*commands) - if deadline not in self._commands: - self.addCommands(deadline) - - def setDeadline(self, deadline: Command): - if deadline not in self._commands: - self.addCommands(deadline) - self._deadline = deadline - - def addCommands(self, *commands: Command): - commands = flatten_args_commands(commands) - if not self._finished: - raise IllegalCommandUse( - "Commands cannot be added to a composition while it is running" - ) - - CommandScheduler.getInstance().registerComposedCommands(commands) - - for command in commands: - if not command.getRequirements().isdisjoint(self.requirements): - raise IllegalCommandUse( - "Multiple comands in a parallel composition cannot require the same subsystems." - ) - - self._commands[command] = False - self.requirements.update(command.getRequirements()) - self._runsWhenDisabled = ( - self._runsWhenDisabled and command.runsWhenDisabled() - ) - - if command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf: - self._interruptBehavior = InterruptionBehavior.kCancelSelf - - def initialize(self): - for command in self._commands: - command.initialize() - self._commands[command] = True - self._finished = False - - def execute(self): - for command, isRunning in self._commands.items(): - if not isRunning: - continue - command.execute() - if command.isFinished(): - command.end(False) - self._commands[command] = False - if command == self._deadline: - self._finished = True - - def end(self, interrupted: bool): - for command, isRunning in self._commands.items(): - if not isRunning: - continue - command.end(True) - self._commands[command] = False - - def isFinished(self) -> bool: - return self._finished - - def runsWhenDisabled(self) -> bool: - return self._runsWhenDisabled - - def getInterruptionBehavior(self) -> InterruptionBehavior: - return self._interruptBehavior diff --git a/rio/ottomanEmpire/bursa/parallelracegroup.py b/rio/ottomanEmpire/bursa/parallelracegroup.py deleted file mode 100644 index 3b66c14a..00000000 --- a/rio/ottomanEmpire/bursa/parallelracegroup.py +++ /dev/null @@ -1,82 +0,0 @@ -from __future__ import annotations - -from typing import Set - -from commands2.command import Command, InterruptionBehavior - -from .command import Command, InterruptionBehavior -from .commandgroup import * -from .commandscheduler import CommandScheduler -from .util import flatten_args_commands - - -class ParallelRaceGroup(CommandGroup): - """ - A composition that runs a set of commands in parallel, ending when any one of the commands ends - and interrupting all the others. - - The rules for command compositions apply: command instances that are passed to it cannot be - added to any other composition or scheduled individually, and the composition requires all - subsystems its components require.""" - - def __init__(self, *commands: Command): - """ - Creates a new ParallelCommandRace. The given commands will be executed simultaneously, and will - "race to the finish" - the first command to finish ends the entire command, with all other - commands being interrupted. - - :param commands: the commands to include in this composition.""" - super().__init__() - self._commands: Set[Command] = set() - self._runsWhenDisabled = True - self._interruptBehavior = InterruptionBehavior.kCancelIncoming - self._finished = True - self.addCommands(*commands) - - def addCommands(self, *commands: Command): - commands = flatten_args_commands(commands) - if not self._finished: - raise IllegalCommandUse( - "Commands cannot be added to a composition while it is running" - ) - - CommandScheduler.getInstance().registerComposedCommands(commands) - - for command in commands: - if not command.getRequirements().isdisjoint(self.requirements): - raise IllegalCommandUse( - "Multiple comands in a parallel composition cannot require the same subsystems." - ) - - self._commands.add(command) - self.requirements.update(command.getRequirements()) - self._runsWhenDisabled = ( - self._runsWhenDisabled and command.runsWhenDisabled() - ) - - if command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf: - self._interruptBehavior = InterruptionBehavior.kCancelSelf - - def initialize(self): - self._finished = False - for command in self._commands: - command.initialize() - - def execute(self): - for command in self._commands: - command.execute() - if command.isFinished(): - self._finished = True - - def end(self, interrupted: bool): - for command in self._commands: - command.end(not command.isFinished()) - - def isFinished(self) -> bool: - return self._finished - - def runsWhenDisabled(self) -> bool: - return self._runsWhenDisabled - - def getInterruptionBehavior(self) -> InterruptionBehavior: - return self._interruptBehavior diff --git a/rio/ottomanEmpire/bursa/perpetualcommand.py b/rio/ottomanEmpire/bursa/perpetualcommand.py deleted file mode 100644 index 1e535e34..00000000 --- a/rio/ottomanEmpire/bursa/perpetualcommand.py +++ /dev/null @@ -1,46 +0,0 @@ -from __future__ import annotations - -from .command import Command -from .commandgroup import * -from .commandscheduler import CommandScheduler - - -class PerpetualCommand(Command): - """ - A command that runs another command in perpetuity, ignoring that command's end conditions. While - this class does not extend CommandGroupBase, it is still considered a composition, as it - allows one to compose another command within it; the command instances that are passed to it - cannot be added to any other groups, or scheduled individually. - - As a rule, CommandGroups require the union of the requirements of their component commands. - - This class is provided by the NewCommands VendorDep - - @deprecated PerpetualCommand violates the assumption that execute() doesn't get called after - isFinished() returns true -- an assumption that should be valid. This was unsafe/undefined - behavior from the start, and RepeatCommand provides an easy way to achieve similar end - results with slightly different (and safe) semantics.""" - - def __init__(self, command: Command): - """ - Creates a new PerpetualCommand. Will run another command in perpetuity, ignoring that command's - end conditions, unless this command itself is interrupted. - - :param command: the command to run perpetually""" - super().__init__() - - CommandScheduler.getInstance().registerComposedCommands([command]) - self._command = command - self.addRequirements(*command.getRequirements()) - - def initialize(self): - self._command.initialize() - - def execute(self): - self._command.execute - - def end(self, interrupted: bool): - self._command.end(interrupted) - - def runsWhenDisabled(self) -> bool: - return self._command.runsWhenDisabled() diff --git a/rio/ottomanEmpire/bursa/pidcommand.py b/rio/ottomanEmpire/bursa/pidcommand.py deleted file mode 100644 index 339764c0..00000000 --- a/rio/ottomanEmpire/bursa/pidcommand.py +++ /dev/null @@ -1,74 +0,0 @@ -# Copyright (c) FIRST and other WPILib contributors. -# Open Source Software; you can modify and/or share it under the terms of -# the WPILib BSD license file in the root directory of this project. -from __future__ import annotations - -from typing import Any, Callable, Union - -from .command import Command -from .subsystem import Subsystem - -from wpimath.controller import PIDController - - -class PIDCommand(Command): - """ - A command that controls an output with a PIDController. Runs forever by default - to add - exit conditions and/or other behavior, subclass this class. The controller calculation and output - are performed synchronously in the command's execute() method. - """ - - def __init__( - self, - controller: PIDController, - measurementSource: Callable[[], float], - setpoint: Union[Callable[[], float], float, int], - useOutput: Callable[[float], Any], - *requirements: Subsystem, - ): - """ - Creates a new PIDCommand, which controls the given output with a PIDController. - - :param controller: the controller that controls the output. - :param measurementSource: the measurement of the process variable - :param setpoint: the controller's setpoint (either a function that returns a) - number or a number - :param useOutput: the controller's output - :param requirements: the subsystems required by this command - """ - super().__init__() - - self._controller = controller - self._useOutput = useOutput - self._measurement = measurementSource - - if isinstance(setpoint, (float, int)): - setpoint = float(setpoint) - self._setpoint = lambda: setpoint - elif callable(setpoint): - self._setpoint = setpoint - else: - raise ValueError( - f"invalid setpoint (must be callable or number; got {type(setpoint)})" - ) - - self.addRequirements(*requirements) - - def initialize(self): - self._controller.reset() - - def execute(self): - self._useOutput( - self._controller.calculate(self._measurement(), self._setpoint()) - ) - - def end(self, interrupted): - self._useOutput(0) - - def getController(self): - """ - Returns the PIDController used by the command. - - :return: The PIDController - """ - return self._controller diff --git a/rio/ottomanEmpire/bursa/pidsubsystem.py b/rio/ottomanEmpire/bursa/pidsubsystem.py deleted file mode 100644 index c63469a3..00000000 --- a/rio/ottomanEmpire/bursa/pidsubsystem.py +++ /dev/null @@ -1,99 +0,0 @@ -# Copyright (c) FIRST and other WPILib contributors. -# Open Source Software; you can modify and/or share it under the terms of -# the WPILib BSD license file in the root directory of this project. -from __future__ import annotations - -from wpimath.controller import PIDController - -from .subsystem import Subsystem - - -class PIDSubsystem(Subsystem): - """ - A subsystem that uses a {@link PIDController} to control an output. The - controller is run synchronously from the subsystem's periodic() method. - """ - - def __init__(self, controller: PIDController, initial_position: float = 0.0): - """ - Creates a new PIDSubsystem. - - :param controller: The PIDController to use. - :param initial_position: The initial setpoint of the subsystem. - """ - super().__init__() - - self._controller = controller - self.setSetpoint(initial_position) - self.addChild("PID Controller", self._controller) - self._enabled = False - - def periodic(self): - """ - Executes the PID control logic during each periodic update. - - This method is called synchronously from the subsystem's periodic() method. - """ - if self._enabled: - self.useOutput( - self._controller.calculate(self.getMeasurement()), self.getSetpoint() - ) - - def getController(self) -> PIDController: - """ - Returns the PIDController used by the subsystem. - - :return: The PIDController. - """ - return self._controller - - def setSetpoint(self, setpoint: float): - """ - Sets the setpoint for the subsystem. - - :param setpoint: The setpoint for the subsystem. - """ - self._controller.setSetpoint(setpoint) - - def getSetpoint(self) -> float: - """ - Returns the current setpoint of the subsystem. - - :return: The current setpoint. - """ - return self._controller.getSetpoint() - - def useOutput(self, output: float, setpoint: float): - """ - Uses the output from the PIDController. - - :param output: The output of the PIDController. - :param setpoint: The setpoint of the PIDController (for feedforward). - """ - raise NotImplementedError("Subclasses must implement this method") - - def getMeasurement(self) -> float: - """ - Returns the measurement of the process variable used by the PIDController. - - :return: The measurement of the process variable. - """ - raise NotImplementedError("Subclasses must implement this method") - - def enable(self): - """Enables the PID control. Resets the controller.""" - self._enabled = True - self._controller.reset() - - def disable(self): - """Disables the PID control. Sets output to zero.""" - self._enabled = False - self.useOutput(0, 0) - - def isEnabled(self) -> bool: - """ - Returns whether the controller is enabled. - - :return: Whether the controller is enabled. - """ - return self._enabled diff --git a/rio/ottomanEmpire/bursa/printcommand.py b/rio/ottomanEmpire/bursa/printcommand.py deleted file mode 100644 index 082a8c7f..00000000 --- a/rio/ottomanEmpire/bursa/printcommand.py +++ /dev/null @@ -1,18 +0,0 @@ -from __future__ import annotations - -from .instantcommand import InstantCommand - - -class PrintCommand(InstantCommand): - """ - A command that prints a string when initialized.""" - - def __init__(self, message: str): - """ - Creates a new a PrintCommand. - - :param message: the message to print""" - super().__init__(lambda: print(message)) - - def runsWhenDisabled(self) -> bool: - return True diff --git a/rio/ottomanEmpire/bursa/proxycommand.py b/rio/ottomanEmpire/bursa/proxycommand.py deleted file mode 100644 index 630e7b52..00000000 --- a/rio/ottomanEmpire/bursa/proxycommand.py +++ /dev/null @@ -1,90 +0,0 @@ -from __future__ import annotations - -from typing import Callable, overload - -from .command import Command -from .commandgroup import * -from .util import format_args_kwargs - - -class ProxyCommand(Command): - """ - Schedules the given command when this command is initialized, and ends when it ends. Useful for - forking off from CommandGroups. If this command is interrupted, it will cancel the command. - """ - - _supplier: Callable[[], Command] - - @overload - def __init__(self, supplier: Callable[[], Command]): - """ - Creates a new ProxyCommand that schedules the supplied command when initialized, and ends when - it is no longer scheduled. Useful for lazily creating commands at runtime. - - :param supplier: the command supplier""" - ... - - @overload - def __init__(self, command: Command): - """ - Creates a new ProxyCommand that schedules the given command when initialized, and ends when it - is no longer scheduled. - - :param command: the command to run by proxy""" - ... - - def __init__(self, *args, **kwargs): - super().__init__() - - def init_supplier(supplier: Callable[[], Command]): - self._supplier = supplier - - def init_command(command: Command): - self._supplier = lambda: command - - num_args = len(args) + len(kwargs) - - if num_args == 1 and len(kwargs) == 1: - if "supplier" in kwargs: - return init_supplier(kwargs["supplier"]) - elif "command" in kwargs: - return init_command(kwargs["command"]) - elif num_args == 1 and len(args) == 1: - if isinstance(args[0], Command): - return init_command(args[0]) - elif callable(args[0]): - return init_supplier(args[0]) - - raise TypeError( - f""" -TypeError: ProxyCommand(): incompatible function arguments. The following argument types are supported: - 1. (self: ProxyCommand, supplier: () -> Command) - 2. (self: ProxyCommand, command: Command) - -Invoked with: {format_args_kwargs(self, *args, **kwargs)} -""" - ) - - def initialize(self): - self._command = self._supplier() - self._command.schedule() - - def end(self, interrupted: bool): - if interrupted and self._command is not None: - self._command.cancel() - self._command = None - - def execute(self): - pass - - def isFinished(self) -> bool: - return self._command is None or not self._command.isScheduled() - - def runsWhenDisabled(self) -> bool: - """ - Whether the given command should run when the robot is disabled. Override to return true if the - command should run when disabled. - - :returns: true. Otherwise, this proxy would cancel commands that do run when disabled. - """ - return True diff --git a/rio/ottomanEmpire/bursa/proxyschedulecommand.py b/rio/ottomanEmpire/bursa/proxyschedulecommand.py deleted file mode 100644 index 23b9d8a4..00000000 --- a/rio/ottomanEmpire/bursa/proxyschedulecommand.py +++ /dev/null @@ -1,41 +0,0 @@ -from __future__ import annotations - -from .command import Command -from .commandgroup import * - - -class ProxyScheduleCommand(Command): - """ - Schedules the given commands when this command is initialized, and ends when all the commands are - no longer scheduled. Useful for forking off from CommandGroups. If this command is interrupted, - it will cancel all the commands. - """ - - def __init__(self, *toSchedule: Command): - """ - Creates a new ProxyScheduleCommand that schedules the given commands when initialized, and ends - when they are all no longer scheduled. - - :param toSchedule: the commands to schedule - @deprecated Replace with ProxyCommand, composing multiple of them in a {@link - ParallelRaceGroup} if needed.""" - super().__init__() - self.toSchedule = set(toSchedule) - self._finished = False - - def initialize(self): - for command in self.toSchedule: - command.schedule() - - def end(self, interrupted: bool): - if interrupted: - for command in self.toSchedule: - command.cancel() - - def execute(self): - self._finished = True - for command in self.toSchedule: - self._finished = self._finished and not command.isScheduled() - - def isFinished(self) -> bool: - return self._finished diff --git a/rio/ottomanEmpire/bursa/repeatcommand.py b/rio/ottomanEmpire/bursa/repeatcommand.py deleted file mode 100644 index dc7dc991..00000000 --- a/rio/ottomanEmpire/bursa/repeatcommand.py +++ /dev/null @@ -1,55 +0,0 @@ -from __future__ import annotations - -from commands2.command import InterruptionBehavior - -from .command import Command -from .commandgroup import * - - -class RepeatCommand(Command): - """ - A command that runs another command repeatedly, restarting it when it ends, until this command is - interrupted. Command instances that are passed to it cannot be added to any other groups, or - scheduled individually. - - The rules for command compositions apply: command instances that are passed to it cannot be - added to any other composition or scheduled individually,and the composition requires all - subsystems its components require.""" - - def __init__(self, command: Command): - """ - Creates a new RepeatCommand. Will run another command repeatedly, restarting it whenever it - ends, until this command is interrupted. - - :param command: the command to run repeatedly""" - super().__init__() - self._command = command - - def initialize(self): - self._ended = False - self._command.initialize() - - def execute(self): - if self._ended: - self._ended = False - self._command.initialize() - - self._command.execute() - - if self._command.isFinished(): - self._command.end(False) - self._ended = True - - def isFinished(self) -> bool: - return False - - def end(self, interrupted: bool): - if not self._ended: - self._command.end(interrupted) - self._ended = True - - def runsWhenDisabled(self) -> bool: - return self._command.runsWhenDisabled() - - def getInterruptionBehavior(self) -> InterruptionBehavior: - return self._command.getInterruptionBehavior() diff --git a/rio/ottomanEmpire/bursa/runcommand.py b/rio/ottomanEmpire/bursa/runcommand.py deleted file mode 100644 index 231567f5..00000000 --- a/rio/ottomanEmpire/bursa/runcommand.py +++ /dev/null @@ -1,25 +0,0 @@ -from __future__ import annotations - -from typing import Any, Callable - -from .commandgroup import * -from .functionalcommand import FunctionalCommand -from .subsystem import Subsystem - - -class RunCommand(FunctionalCommand): - """ - A command that runs a Runnable continuously. Has no end condition as-is; either subclass it or - use Command#withTimeout(double) or Command#until(BooleanSupplier) to give it one. - If you only wish to execute a Runnable once, use InstantCommand.""" - - def __init__(self, toRun: Callable[[], Any], *requirements: Subsystem): - """ - Creates a new RunCommand. The Runnable will be run continuously until the command ends. Does - not run when disabled. - - :param toRun: the Runnable to run - :param requirements: the subsystems to require""" - super().__init__( - lambda: None, toRun, lambda interrupted: None, lambda: False, *requirements - ) diff --git a/rio/ottomanEmpire/bursa/schedulecommand.py b/rio/ottomanEmpire/bursa/schedulecommand.py deleted file mode 100644 index 1d987e6d..00000000 --- a/rio/ottomanEmpire/bursa/schedulecommand.py +++ /dev/null @@ -1,30 +0,0 @@ -from __future__ import annotations - -from .command import Command -from .commandgroup import * - - -class ScheduleCommand(Command): - """ - Schedules the given commands when this command is initialized. Useful for forking off from - CommandGroups. Note that if run from a composition, the composition will not know about the - status of the scheduled commands, and will treat this command as finishing instantly. - """ - - def __init__(self, *commands: Command): - """ - Creates a new ScheduleCommand that schedules the given commands when initialized. - - :param toSchedule: the commands to schedule""" - super().__init__() - self.toSchedule = set(commands) - - def initialize(self): - for command in self.toSchedule: - command.schedule() - - def isFinished(self) -> bool: - return True - - def runsWhenDisabled(self) -> bool: - return True diff --git a/rio/ottomanEmpire/bursa/selectcommand.py b/rio/ottomanEmpire/bursa/selectcommand.py deleted file mode 100644 index 56710dcd..00000000 --- a/rio/ottomanEmpire/bursa/selectcommand.py +++ /dev/null @@ -1,71 +0,0 @@ -from __future__ import annotations - -from typing import Callable, Dict, Hashable - -from commands2.command import InterruptionBehavior - -from .command import Command, InterruptionBehavior -from .commandgroup import * -from .commandscheduler import CommandScheduler -from .printcommand import PrintCommand - - -class SelectCommand(Command): - """ - A command composition that runs one of a selection of commands, either using a selector and a key - to command mapping, or a supplier that returns the command directly at runtime. - - The rules for command compositions apply: command instances that are passed to it cannot be - added to any other composition or scheduled individually, and the composition requires all - subsystems its components require.""" - - def __init__( - self, - commands: Dict[Hashable, Command], - selector: Callable[[], Hashable], - ): - """ - Creates a new SelectCommand. - - :param commands: the map of commands to choose from - :param selector: the selector to determine which command to run""" - super().__init__() - - self._commands = commands - self._selector = selector - - CommandScheduler.getInstance().registerComposedCommands(commands.values()) - - self._runsWhenDisabled = True - self._interruptBehavior = InterruptionBehavior.kCancelIncoming - for command in commands.values(): - self.addRequirements(*command.getRequirements()) - self._runsWhenDisabled = ( - self._runsWhenDisabled and command.runsWhenDisabled() - ) - if command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf: - self._interruptBehavior = InterruptionBehavior.kCancelSelf - - def initialize(self): - if self._selector() not in self._commands: - self._selectedCommand = PrintCommand( - "SelectCommand selector value does not correspond to any command!" - ) - return - self._selectedCommand = self._commands[self._selector()] - self._selectedCommand.initialize() - - def execute(self): - self._selectedCommand.execute() - - def end(self, interrupted: bool): - self._selectedCommand.end(interrupted) - - def isFinished(self) -> bool: - return self._selectedCommand.isFinished() - - def runsWhenDisabled(self) -> bool: - return self._runsWhenDisabled - - def getInterruptionBehavior(self) -> InterruptionBehavior: - return self._interruptBehavior diff --git a/rio/ottomanEmpire/bursa/sequentialcommandgroup.py b/rio/ottomanEmpire/bursa/sequentialcommandgroup.py deleted file mode 100644 index ef6984d4..00000000 --- a/rio/ottomanEmpire/bursa/sequentialcommandgroup.py +++ /dev/null @@ -1,90 +0,0 @@ -from __future__ import annotations - -from typing import List - -from commands2.command import Command, InterruptionBehavior - -from .command import Command, InterruptionBehavior -from .commandgroup import * -from .commandscheduler import CommandScheduler -from .util import flatten_args_commands - - -class SequentialCommandGroup(CommandGroup): - """ - A command composition that runs a list of commands in sequence. - - The rules for command compositions apply: command instances that are passed to it cannot be - added to any other composition or scheduled individually, and the composition requires all - subsystems its components require.""" - - def __init__(self, *commands: Command): - """ - Creates a new SequentialCommandGroup. The given commands will be run sequentially, with the - composition finishing when the last command finishes. - - :param commands: the commands to include in this composition.""" - super().__init__() - self._commands: List[Command] = [] - self._runsWhenDisabled = True - self._interruptBehavior = InterruptionBehavior.kCancelIncoming - self._currentCommandIndex = -1 - self.addCommands(*commands) - - def addCommands(self, *commands: Command): - commands = flatten_args_commands(commands) - if self._currentCommandIndex != -1: - raise IllegalCommandUse( - "Commands cannot be added to a composition while it is running" - ) - - CommandScheduler.getInstance().registerComposedCommands(commands) - - for command in commands: - self._commands.append(command) - self.requirements.update(command.getRequirements()) - self._runsWhenDisabled = ( - self._runsWhenDisabled and command.runsWhenDisabled() - ) - if command.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf: - self._interruptBehavior = InterruptionBehavior.kCancelSelf - - def initialize(self): - self._currentCommandIndex = 0 - if self._commands: - self._commands[0].initialize() - - def execute(self): - if not self._commands: - return - - currentCommand = self._commands[self._currentCommandIndex] - - currentCommand.execute() - if currentCommand.isFinished(): - currentCommand.end(False) - self._currentCommandIndex += 1 - if self._currentCommandIndex < len(self._commands): - self._commands[self._currentCommandIndex].initialize() - - def end(self, interrupted: bool): - if not interrupted: - return - if not self._commands: - return - if not self._currentCommandIndex > -1: - return - if not self._currentCommandIndex < len(self._commands): - return - - self._commands[self._currentCommandIndex].end(True) - self._currentCommandIndex = -1 - - def isFinished(self) -> bool: - return self._currentCommandIndex == len(self._commands) - - def runsWhenDisabled(self) -> bool: - return self._runsWhenDisabled - - def getInterruptionBehavior(self) -> InterruptionBehavior: - return self._interruptBehavior diff --git a/rio/ottomanEmpire/bursa/startendcommand.py b/rio/ottomanEmpire/bursa/startendcommand.py deleted file mode 100644 index 1ea896af..00000000 --- a/rio/ottomanEmpire/bursa/startendcommand.py +++ /dev/null @@ -1,33 +0,0 @@ -from __future__ import annotations - -from typing import Any, Callable - -from .commandgroup import * -from .functionalcommand import FunctionalCommand -from .subsystem import Subsystem - - -class StartEndCommand(FunctionalCommand): - """ - A command that runs a given runnable when it is initialized, and another runnable when it ends. - Useful for running and then stopping a motor, or extending and then retracting a solenoid. Has no - end condition as-is; either subclass it or use Command#withTimeout(double) or {@link - Command#until(java.util.function.BooleanSupplier)} to give it one. - """ - - def __init__( - self, - onInit: Callable[[], Any], - onEnd: Callable[[], Any], - *requirements: Subsystem, - ): - """ - Creates a new StartEndCommand. Will run the given runnables when the command starts and when it - ends. - - :param onInit: the Runnable to run on command init - :param onEnd: the Runnable to run on command end - :param requirements: the subsystems required by this command""" - super().__init__( - onInit, lambda: None, lambda _: onEnd(), lambda: False, *requirements - ) diff --git a/rio/ottomanEmpire/bursa/subsystem.py b/rio/ottomanEmpire/bursa/subsystem.py deleted file mode 100644 index a3ba81ca..00000000 --- a/rio/ottomanEmpire/bursa/subsystem.py +++ /dev/null @@ -1,180 +0,0 @@ -# Don't import stuff from the package here, it'll cause a circular import - -from __future__ import annotations - -from typing import TYPE_CHECKING, Callable, Optional - -if TYPE_CHECKING: - from .command import Command - from .commandscheduler import CommandScheduler - -from wpiutil import Sendable, SendableBuilder, SendableRegistry - - -class Subsystem(Sendable): - """ - A robot subsystem. Subsystems are the basic unit of robot organization in the Command-based - framework; they encapsulate low-level hardware objects (motor controllers, sensors, etc.) and - provide methods through which they can be used by Commands. Subsystems are used by the - CommandScheduler's resource management system to ensure multiple robot actions are not - "fighting" over the same hardware; Commands that use a subsystem should include that subsystem in - their Command#getRequirements() method, and resources used within a subsystem should - generally remain encapsulated and not be shared by other parts of the robot. - - Subsystems must be registered with the scheduler with the {@link - CommandScheduler#registerSubsystem(Subsystem...)} method in order for the {@link - Subsystem#periodic()} method to be called. It is recommended that this method be called from the - constructor of users' Subsystem implementations. The SubsystemBase class offers a simple - base for user implementations that handles this. - """ - - def __new__(cls, *arg, **kwargs) -> "Subsystem": - instance = super().__new__(cls) - super().__init__(instance) - SendableRegistry.addLW(instance, cls.__name__, cls.__name__) - # add to the scheduler - from .commandscheduler import CommandScheduler - - CommandScheduler.getInstance().registerSubsystem(instance) - return instance - - def __init__(self) -> None: - pass - - def periodic(self) -> None: - """ - This method is called periodically by the CommandScheduler. Useful for updating - subsystem-specific state that you don't want to offload to a Command. Teams should try - to be consistent within their own codebases about which responsibilities will be handled by - Commands, and which will be handled here.""" - pass - - def simulationPeriodic(self) -> None: - """ - This method is called periodically by the CommandScheduler. Useful for updating - subsystem-specific state that needs to be maintained for simulations, such as for updating - edu.wpi.first.wpilibj.simulation classes and setting simulated sensor readings. - """ - pass - - def setDefaultCommand(self, command: Command) -> None: - """ - Sets the default Command of the subsystem. The default command will be automatically - scheduled when no other commands are scheduled that require the subsystem. Default commands - should generally not end on their own, i.e. their Command#isFinished() method should - always return false. Will automatically register this subsystem with the {@link - CommandScheduler}. - - :param defaultCommand: the default command to associate with this subsystem""" - from .commandscheduler import CommandScheduler - - CommandScheduler.getInstance().setDefaultCommand(self, command) - - def removeDefaultCommand(self) -> None: - """ - Removes the default command for the subsystem. This will not cancel the default command if it - is currently running.""" - CommandScheduler.getInstance().removeDefaultCommand(self) - - def getDefaultCommand(self) -> Optional[Command]: - """ - Gets the default command for this subsystem. Returns null if no default command is currently - associated with the subsystem. - - :returns: the default command associated with this subsystem""" - from .commandscheduler import CommandScheduler - - return CommandScheduler.getInstance().getDefaultCommand(self) - - def getCurrentCommand(self) -> Optional[Command]: - """ - Returns the command currently running on this subsystem. Returns null if no command is - currently scheduled that requires this subsystem. - - :returns: the scheduled command currently requiring this subsystem""" - from .commandscheduler import CommandScheduler - - return CommandScheduler.getInstance().requiring(self) - - def runOnce(self, action: Callable[[], None]) -> Command: - """ - Constructs a command that runs an action once and finishes. Requires this subsystem. - - :param action: the action to run - :return: the command""" - from .cmd import runOnce - - return runOnce(action, self) - - def run(self, action: Callable[[], None]) -> Command: - """ - Constructs a command that runs an action every iteration until interrupted. Requires this - subsystem. - - :param action: the action to run - :returns: the command""" - from .cmd import run - - return run(action, self) - - def startEnd(self, start: Callable[[], None], end: Callable[[], None]) -> Command: - """ - Constructs a command that runs an action once and another action when the command is - interrupted. Requires this subsystem. - - :param start: the action to run on start - :param end: the action to run on interrupt - :returns: the command""" - from .cmd import startEnd - - return startEnd(start, end, self) - - def runEnd(self, run: Callable[[], None], end: Callable[[], None]) -> Command: - """ - Constructs a command that runs an action every iteration until interrupted, and then runs a - second action. Requires this subsystem. - - :param run: the action to run every iteration - :param end: the action to run on interrupt - :returns: the command""" - from .cmd import runEnd - - return runEnd(run, end, self) - - def getName(self) -> str: - return SendableRegistry.getName(self) - - def setName(self, name: str) -> None: - SendableRegistry.setName(self, name) - - def getSubsystem(self) -> str: - return SendableRegistry.getSubsystem(self) - - def addChild(self, name: str, child: Sendable) -> None: - SendableRegistry.addLW(child, self.getSubsystem(), name) - - def initSendable(self, builder: SendableBuilder) -> None: - builder.setSmartDashboardType("Subsystem") - - builder.addBooleanProperty( - ".hasDefault", lambda: self.getDefaultCommand() is not None, lambda _: None - ) - - def get_default(): - command = self.getDefaultCommand() - if command is not None: - return command.getName() - return "none" - - builder.addStringProperty(".default", get_default, lambda _: None) - builder.addBooleanProperty( - ".hasCommand", lambda: self.getCurrentCommand() is not None, lambda _: None - ) - - def get_current(): - command = self.getCurrentCommand() - if command is not None: - return command.getName() - return "none" - - builder.addStringProperty(".command", get_current, lambda _: None) diff --git a/rio/ottomanEmpire/bursa/swervecontrollercommand.py b/rio/ottomanEmpire/bursa/swervecontrollercommand.py deleted file mode 100644 index 10cf15d0..00000000 --- a/rio/ottomanEmpire/bursa/swervecontrollercommand.py +++ /dev/null @@ -1,293 +0,0 @@ -# Copyright (c) FIRST and other WPILib contributors. -# Open Source Software; you can modify and/or share it under the terms of -# the WPILib BSD license file in the root directory of this project. -from __future__ import annotations -from typing import Callable, Optional, Union, Tuple -from typing_extensions import overload - -# from overtake import overtake -from wpimath.controller import ( - HolonomicDriveController, - PIDController, - ProfiledPIDControllerRadians, -) -from wpimath.geometry import Pose2d, Rotation2d -from wpimath.kinematics import ( - SwerveDrive2Kinematics, - SwerveDrive3Kinematics, - SwerveDrive4Kinematics, - SwerveDrive6Kinematics, - SwerveModuleState, -) -from wpimath.trajectory import Trajectory -from wpilib import Timer - -from .command import Command -from .subsystem import Subsystem - - -class SwerveControllerCommand(Command): - """ - A command that uses two PID controllers (PIDController) and a ProfiledPIDController - (ProfiledPIDController) to follow a trajectory (Trajectory) with a swerve drive. - - This command outputs the raw desired Swerve Module States (SwerveModuleState) in an - array. The desired wheel and module rotation velocities should be taken from those and used in - velocity PIDs. - - The robot angle controller does not follow the angle given by the trajectory but rather goes - to the angle given in the final state of the trajectory. - """ - - @overload - def __init__( - self, - trajectory: Trajectory, - pose: Callable[[], Pose2d], - kinematics: Union[ - SwerveDrive2Kinematics, - SwerveDrive3Kinematics, - SwerveDrive4Kinematics, - SwerveDrive6Kinematics, - ], - xController: PIDController, - yController: PIDController, - thetaController: ProfiledPIDControllerRadians, - desiredRotation: Callable[[], Rotation2d], - outputModuleStates: Callable[[SwerveModuleState], None], - requirements: Tuple[Subsystem], - ) -> None: - """ - Constructs a new SwerveControllerCommand that when executed will follow the - provided trajectory. This command will not return output voltages but - rather raw module states from the position controllers which need to be put - into a velocity PID. - - Note: The controllers will *not* set the outputVolts to zero upon - completion of the path- this is left to the user, since it is not - appropriate for paths with nonstationary endstates. - :param trajectory: The trajectory to follow. - :param pose: A function that supplies the robot pose - use one of the odometry classes to - provide this. - :param kinematics: The kinematics for the robot drivetrain. Can be kinematics for 2/3/4/6 - SwerveKinematics. - :param outputModuleStates: The raw output module states from the position controllers. - :param requirements: The subsystems to require. - :param xController: The Trajectory Tracker PID controller - for the robot's x position. - :param yController: The Trajectory Tracker PID controller - for the robot's y position. - :param thetaController: The Trajectory Tracker PID controller - for angle for the robot. - :param desiredRotation: The angle that the drivetrain should be - facing. This is sampled at each time step. - """ - super().__init__() - self._trajectory = trajectory - self._pose = pose - self._kinematics = kinematics - self._outputModuleStates = outputModuleStates - self._controller = HolonomicDriveController( - xController, yController, thetaController - ) - self._desiredRotation = desiredRotation - self._timer = Timer() - self.addRequirements(requirements) # type: ignore - - @overload - def __init__( - self, - trajectory: Trajectory, - pose: Callable[[], Pose2d], - kinematics: Union[ - SwerveDrive2Kinematics, - SwerveDrive3Kinematics, - SwerveDrive4Kinematics, - SwerveDrive6Kinematics, - ], - xController: PIDController, - yController: PIDController, - thetaController: ProfiledPIDControllerRadians, - outputModuleStates: Callable[[SwerveModuleState], None], - requirements: Tuple[Subsystem], - ) -> None: - """ - Constructs a new SwerveControllerCommand that when executed will follow the - provided trajectory. This command will not return output voltages but - rather raw module states from the position controllers which need to be put - into a velocity PID. - - Note: The controllers will *not* set the outputVolts to zero upon - completion of the path- this is left to the user, since it is not - appropriate for paths with nonstationary endstates. - - Note 2: The final rotation of the robot will be set to the rotation of - the final pose in the trajectory. The robot will not follow the rotations - from the poses at each timestep. If alternate rotation behavior is desired, - the other constructor with a supplier for rotation should be used. - - :param trajectory: The trajectory to follow. - :param pose: A function that supplies the robot pose - use one of the odometry classes to - provide this. - :param kinematics: The kinematics for the robot drivetrain. Can be kinematics for 2/3/4/6 - SwerveKinematics. - :param xController: The Trajectory Tracker PID controller - for the robot's x position. - :param yController: The Trajectory Tracker PID controller - for the robot's y position. - :param thetaController: The Trajectory Tracker PID controller - for angle for the robot. - :param outputModuleStates: The raw output module states from the position controllers. - :param requirements: The subsystems to require. - """ - super().__init__() - self._trajectory = trajectory - self._pose = pose - self._kinematics = kinematics - self._outputModuleStates = outputModuleStates - self._controller = HolonomicDriveController( - xController, yController, thetaController - ) - self._desiredRotation = self._trajectory.states()[-1].pose.rotation - self._timer = Timer() - self.addRequirements(requirements) # type:ignore - - @overload - def __init__( - self, - trajectory: Trajectory, - pose: Callable[[], Pose2d], - kinematics: Union[ - SwerveDrive2Kinematics, - SwerveDrive3Kinematics, - SwerveDrive4Kinematics, - SwerveDrive6Kinematics, - ], - controller: HolonomicDriveController, - desiredRotation: Callable[[], Rotation2d], - outputModuleStates: Callable[[SwerveModuleState], None], - requirements: Tuple[Subsystem], - ) -> None: - """ - Constructs a new SwerveControllerCommand that when executed will follow the - provided trajectory. This command will not return output voltages but - rather raw module states from the position controllers which need to be put - into a velocity PID. - - Note: The controllers will *not* set the outputVolts to zero upon - completion of the path- this is left to the user, since it is not - appropriate for paths with nonstationary endstates. - - :param trajectory: The trajectory to follow. - :param pose: A function that supplies the robot pose - use one of the odometry classes to - provide this. - :param kinematics: The kinematics for the robot drivetrain. Can be kinematics for 2/3/4/6 - SwerveKinematics. - :param controller: The HolonomicDriveController for the drivetrain. - :param desiredRotation: The angle that the drivetrain should be - facing. This is sampled at each time step. - :param outputModuleStates: The raw output module states from the position controllers. - :param requirements: The subsystems to require. - - """ - super().__init__() - self._trajectory = trajectory - self._pose = pose - self._kinematics = kinematics - self._outputModuleStates = outputModuleStates - self._controller = controller - self._desiredRotation = desiredRotation - self._timer = Timer() - self.addRequirements(requirements) # type:ignore - - @overload - def __init__( - self, - trajectory: Trajectory, - pose: Callable[[], Pose2d], - kinematics: Union[ - SwerveDrive2Kinematics, - SwerveDrive3Kinematics, - SwerveDrive4Kinematics, - SwerveDrive6Kinematics, - ], - controller: HolonomicDriveController, - outputModuleStates: Callable[[SwerveModuleState], None], - requirements: Tuple[Subsystem], - ) -> None: - """ - Constructs a new SwerveControllerCommand that when executed will follow the - provided trajectory. This command will not return output voltages but - rather raw module states from the position controllers which need to be put - into a velocity PID. - - Note: The controllers will *not* set the outputVolts to zero upon - completion of the path- this is left to the user, since it is not - appropriate for paths with nonstationary endstates. - - Note 2: The final rotation of the robot will be set to the rotation of - the final pose in the trajectory. The robot will not follow the rotations - from the poses at each timestep. If alternate rotation behavior is desired, - the other constructor with a supplier for rotation should be used. - - :param trajectory: The trajectory to follow. - :param pose: A function that supplies the robot pose - use one of the odometry classes to - provide this. - :param kinematics: The kinematics for the robot drivetrain. Can be kinematics for 2/3/4/6 - SwerveKinematics. - :param controller: The HolonomicDriveController for the drivetrain. - :param outputModuleStates: The raw output module states from the position controllers. - :param requirements: The subsystems to require. - - """ - super().__init__() - self._trajectory = trajectory - self._pose = pose - self._kinematics = kinematics - self._outputModuleStates = outputModuleStates - self._controller = controller - self._desiredRotation = self._trajectory.states()[-1].pose.rotation - self._timer = Timer() - self.addRequirements(requirements) # type: ignore - - # @overtake(runtime_type_checker="beartype") - def __init__( - self, - trajectory: Trajectory, - pose: Callable[[], Pose2d], - kinematics: Union[ - SwerveDrive2Kinematics, - SwerveDrive3Kinematics, - SwerveDrive4Kinematics, - SwerveDrive6Kinematics, - ], - outputModuleStates: Callable[[SwerveModuleState], None], - requirements: Tuple[Subsystem], - *, - controller: Optional[HolonomicDriveController] = None, - xController: Optional[PIDController] = None, - yController: Optional[PIDController] = None, - thetaController: Optional[ProfiledPIDControllerRadians] = None, - desiredRotation: Optional[Callable[[], Rotation2d]] = None, - ): - ... - - def initialize(self): - self._timer.restart() - - def execute(self): - curTime = self._timer.get() - desiredState = self._trajectory.sample(curTime) - - targetChassisSpeeds = self._controller.calculate( - self._pose(), desiredState, self._desiredRotation() - ) - targetModuleStates = self._kinematics.toSwerveModuleStates(targetChassisSpeeds) - - self._outputModuleStates(targetModuleStates) - - def end(self, interrupted): - self._timer.stop() - - def isFinished(self): - return self._timer.hasElapsed(self._trajectory.totalTime()) diff --git a/rio/ottomanEmpire/bursa/timedcommandrobot.py b/rio/ottomanEmpire/bursa/timedcommandrobot.py deleted file mode 100644 index 4a4b3bc5..00000000 --- a/rio/ottomanEmpire/bursa/timedcommandrobot.py +++ /dev/null @@ -1,15 +0,0 @@ -from wpilib import TimedRobot - -from .commandscheduler import CommandScheduler - -seconds = float - - -class TimedCommandRobot(TimedRobot): - kSchedulerOffset = 0.005 - - def __init__(self, period: seconds = TimedRobot.kDefaultPeriod / 1000) -> None: - super().__init__(period) - self.addPeriodic( - CommandScheduler.getInstance().run, period, self.kSchedulerOffset - ) diff --git a/rio/ottomanEmpire/bursa/trapezoidprofilesubsystem.py b/rio/ottomanEmpire/bursa/trapezoidprofilesubsystem.py deleted file mode 100644 index 2b40fd53..00000000 --- a/rio/ottomanEmpire/bursa/trapezoidprofilesubsystem.py +++ /dev/null @@ -1,75 +0,0 @@ -# Copyright (c) FIRST and other WPILib contributors. -# Open Source Software; you can modify and/or share it under the terms of -# the WPILib BSD license file in the root directory of this project. -from __future__ import annotations - -from typing import Union - -from .subsystem import Subsystem -from wpimath.trajectory import TrapezoidProfile - - -class TrapezoidProfileSubsystem(Subsystem): - """ - A subsystem that generates and runs trapezoidal motion profiles automatically. The user specifies - how to use the current state of the motion profile by overriding the `useState` method. - """ - - def __init__( - self, - constraints: TrapezoidProfile.Constraints, - initial_position: float = 0.0, - period: float = 0.02, - ): - """ - Creates a new TrapezoidProfileSubsystem. - - :param constraints: The constraints (maximum velocity and acceleration) for the profiles. - :param initial_position: The initial position of the controlled mechanism when the subsystem is constructed. - :param period: The period of the main robot loop, in seconds. - """ - self._profile = TrapezoidProfile(constraints) - self._state = TrapezoidProfile.State(initial_position, 0) - self.setGoal(initial_position) - self._period = period - self._enabled = True - - def periodic(self): - """ - Executes the TrapezoidProfileSubsystem logic during each periodic update. - - This method is called synchronously from the subsystem's periodic() method. - """ - self._state = self._profile.calculate(self._period, self._goal, self._state) - if self._enabled: - self.useState(self._state) - - def setGoal(self, goal: Union[TrapezoidProfile.State, float]): - """ - Sets the goal state for the subsystem. Goal velocity assumed to be zero. - - :param goal: The goal position for the subsystem's motion profile. The goal - can either be a `TrapezoidProfile.State` or `float`. If float is provided, - the assumed velocity for the goal will be 0. - """ - # If we got a float, instantiate the state - if isinstance(goal, (float, int)): - goal = TrapezoidProfile.State(goal, 0) - - self._goal = goal - - def enable(self): - """Enable the TrapezoidProfileSubsystem's output.""" - self._enabled = True - - def disable(self): - """Disable the TrapezoidProfileSubsystem's output.""" - self._enabled = False - - def useState(self, state: TrapezoidProfile.State): - """ - Users should override this to consume the current state of the motion profile. - - :param state: The current state of the motion profile. - """ - raise NotImplementedError("Subclasses must implement this method") diff --git a/rio/ottomanEmpire/bursa/util.py b/rio/ottomanEmpire/bursa/util.py deleted file mode 100644 index 617a5762..00000000 --- a/rio/ottomanEmpire/bursa/util.py +++ /dev/null @@ -1,24 +0,0 @@ -from __future__ import annotations - -from typing import Iterable, List, Tuple, Union - -from .command import Command - - -def flatten_args_commands( - *commands: Union[Command, Iterable[Command]] -) -> Tuple[Command, ...]: - flattened_commands: List[Command] = [] - for command in commands: - if isinstance(command, Command): - flattened_commands.append(command) - elif isinstance(command, Iterable): - flattened_commands.extend(flatten_args_commands(*command)) - return tuple(flattened_commands) - - -def format_args_kwargs(*args, **kwargs) -> str: - return ", ".join( - [repr(arg) for arg in args] - + [f"{key}={repr(value)}" for key, value in kwargs.items()] - ) diff --git a/rio/ottomanEmpire/bursa/waitcommand.py b/rio/ottomanEmpire/bursa/waitcommand.py deleted file mode 100644 index 4f7ed04d..00000000 --- a/rio/ottomanEmpire/bursa/waitcommand.py +++ /dev/null @@ -1,33 +0,0 @@ -from __future__ import annotations - -from wpilib import Timer - -from .command import Command -from .commandgroup import * - - -class WaitCommand(Command): - """ - A command that does nothing but takes a specified amount of time to finish.""" - - def __init__(self, seconds: float): - """ - Creates a new WaitCommand. This command will do nothing, and end after the specified duration. - - :param seconds: the time to wait, in seconds""" - super().__init__() - self._duration = seconds - self._timer = Timer() - - def initialize(self): - self._timer.reset() - self._timer.start() - - def end(self, interrupted: bool): - self._timer.stop() - - def isFinished(self) -> bool: - return self._timer.hasElapsed(self._duration) - - def runsWhenDisabled(self) -> bool: - return True diff --git a/rio/ottomanEmpire/bursa/waituntilcommand.py b/rio/ottomanEmpire/bursa/waituntilcommand.py deleted file mode 100644 index 34839c9f..00000000 --- a/rio/ottomanEmpire/bursa/waituntilcommand.py +++ /dev/null @@ -1,74 +0,0 @@ -from __future__ import annotations - -from typing import Callable, overload - -from wpilib import Timer - -from .command import Command -from .util import format_args_kwargs - - -class WaitUntilCommand(Command): - """ - A command that does nothing but ends after a specified match time or condition. Useful for - CommandGroups.""" - - _condition: Callable[[], bool] - - @overload - def __init__(self, condition: Callable[[], bool]): - """ - Creates a new WaitUntilCommand that ends after a given condition becomes true. - - :param condition: the condition to determine when to end""" - ... - - @overload - def __init__(self, time: float): - """ - Creates a new WaitUntilCommand that ends after a given match time. - - NOTE: The match timer used for this command is UNOFFICIAL. Using this command does NOT - guarantee that the time at which the action is performed will be judged to be legal by the - referees. When in doubt, add a safety factor or time the action manually. - - :param time: the match time at which to end, in seconds""" - ... - - def __init__(self, *args, **kwargs): - super().__init__() - - def init_condition(condition: Callable[[], bool]) -> None: - self._condition = condition - - def init_time(time: float) -> None: - self._condition = lambda: Timer.getMatchTime() - time > 0 - - num_args = len(args) + len(kwargs) - - if num_args == 1 and len(kwargs) == 1: - if "condition" in kwargs: - return init_condition(kwargs["condition"]) - elif "time" in kwargs: - return init_time(kwargs["time"]) - elif num_args == 1 and len(args) == 1: - if isinstance(args[0], float): - return init_time(args[0]) - elif callable(args[0]): - return init_condition(args[0]) - - raise TypeError( - f""" -TypeError: WaitUntilCommand(): incompatible function arguments. The following argument types are supported: - 1. (self: WaitUntilCommand, condition: () -> bool) - 2. (self: WaitUntilCommand, time: float) - -Invoked with: {format_args_kwargs(self, *args, **kwargs)} -""" - ) - - def isFinished(self) -> bool: - return self._condition() - - def runsWhenDisabled(self) -> bool: - return True diff --git a/rio/ottomanEmpire/bursa/wrappercommand.py b/rio/ottomanEmpire/bursa/wrappercommand.py deleted file mode 100644 index 43c50372..00000000 --- a/rio/ottomanEmpire/bursa/wrappercommand.py +++ /dev/null @@ -1,80 +0,0 @@ -from __future__ import annotations - -from typing import Set - -from .command import Command, InterruptionBehavior -from .commandgroup import * -from .commandscheduler import CommandScheduler - - -class WrapperCommand(Command): - """ - A class used internally to wrap commands while overriding a specific method; all other methods - will call through to the wrapped command. - - The rules for command compositions apply: command instances that are passed to it cannot be - added to any other composition or scheduled individually, and the composition requires all - subsystems its components require.""" - - def __init__(self, command: Command): - """ - Wrap a command. - - :param command: the command being wrapped. Trying to directly schedule this command or add it to - a composition will throw an exception.""" - super().__init__() - - CommandScheduler.getInstance().registerComposedCommands([command]) - self._command = command - self.setName(self._command.getName()) - - def initialize(self): - """The initial subroutine of a command. Called once when the command is initially scheduled.""" - self._command.initialize() - - def execute(self): - """The main body of a command. Called repeatedly while the command is scheduled.""" - self._command.execute() - - def end(self, interrupted: bool): - """ - The action to take when the command ends. Called when either the command finishes normally, or - when it interrupted/canceled. - - Do not schedule commands here that share requirements with this command. Use {@link - #andThen(Command...)} instead. - - :param interrupted: whether the command was interrupted/canceled""" - self._command.end(interrupted) - - def isFinished(self) -> bool: - """ - Whether the command has finished. Once a command finishes, the scheduler will call its end() - method and un-schedule it. - - :returns: whether the command has finished.""" - return self._command.isFinished() - - def getRequirements(self) -> Set: - """ - Specifies the set of subsystems used by this command. Two commands cannot use the same - subsystem at the same time. If the command is scheduled as interruptible and another command is - scheduled that shares a requirement, the command will be interrupted. Else, the command will - not be scheduled. If no subsystems are required, return an empty set. - - Note: it is recommended that user implementations contain the requirements as a field, and - return that field here, rather than allocating a new set every time this is called. - - :returns: the set of subsystems that are required""" - return self._command.getRequirements() - - def runsWhenDisabled(self) -> bool: - """ - Whether the given command should run when the robot is disabled. Override to return true if the - command should run when disabled. - - :returns: whether the command should run when the robot is disabled""" - return self._command.runsWhenDisabled() - - def getInterruptionBehavior(self) -> InterruptionBehavior: - return self._command.getInterruptionBehavior() diff --git a/rio/ottomanEmpire/docs/.gitignore b/rio/ottomanEmpire/docs/.gitignore deleted file mode 100644 index 4080f00f..00000000 --- a/rio/ottomanEmpire/docs/.gitignore +++ /dev/null @@ -1,11 +0,0 @@ - - -# Doc-related stuffs -/_build -/_sidebar.rst.inc -/_sidebar.toml - -/commands2.rst -/commands2 -/commands2.button.rst -/commands2.button \ No newline at end of file diff --git a/rio/ottomanEmpire/docs/Makefile b/rio/ottomanEmpire/docs/Makefile deleted file mode 100644 index b17322b3..00000000 --- a/rio/ottomanEmpire/docs/Makefile +++ /dev/null @@ -1,183 +0,0 @@ -# Makefile for Sphinx documentation -# - -# You can set these variables from the command line. -SPHINXOPTS = -SPHINXBUILD = sphinx-build -PAPER = -BUILDDIR = _build - -# User-friendly check for sphinx-build -ifeq ($(shell which $(SPHINXBUILD) >/dev/null 2>&1; echo $$?), 1) -$(error The '$(SPHINXBUILD)' command was not found. Make sure you have Sphinx installed, then set the SPHINXBUILD environment variable to point to the full path of the '$(SPHINXBUILD)' executable. Alternatively you can add the directory with the executable to your PATH. If you don't have Sphinx installed, grab it from http://sphinx-doc.org/) -endif - -# Internal variables. -PAPEROPT_a4 = -D latex_paper_size=a4 -PAPEROPT_letter = -D latex_paper_size=letter -ALLSPHINXOPTS = -d $(BUILDDIR)/doctrees $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) . -# the i18n builder cannot share the environment and doctrees with the others -I18NSPHINXOPTS = $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) . - -.PHONY: help clean html dirhtml singlehtml pickle json htmlhelp qthelp devhelp epub latex latexpdf text man changes linkcheck doctest gettext - -help: - @echo "Please use \`make ' where is one of" - @echo " html to make standalone HTML files" - @echo " dirhtml to make HTML files named index.html in directories" - @echo " singlehtml to make a single large HTML file" - @echo " pickle to make pickle files" - @echo " json to make JSON files" - @echo " htmlhelp to make HTML files and a HTML help project" - @echo " qthelp to make HTML files and a qthelp project" - @echo " devhelp to make HTML files and a Devhelp project" - @echo " epub to make an epub" - @echo " latex to make LaTeX files, you can set PAPER=a4 or PAPER=letter" - @echo " latexpdf to make LaTeX files and run them through pdflatex" - @echo " latexpdfja to make LaTeX files and run them through platex/dvipdfmx" - @echo " text to make text files" - @echo " man to make manual pages" - @echo " texinfo to make Texinfo files" - @echo " info to make Texinfo files and run them through makeinfo" - @echo " gettext to make PO message catalogs" - @echo " changes to make an overview of all changed/added/deprecated items" - @echo " xml to make Docutils-native XML files" - @echo " pseudoxml to make pseudoxml-XML files for display purposes" - @echo " linkcheck to check all external links for integrity" - @echo " doctest to run all doctests embedded in the documentation (if enabled)" - -clean: - rm -rf $(BUILDDIR)/* - rm -rf __pycache__ - for i in commands2.buttons \ - commands2; do \ - rm -f $$i.rst; \ - rm -rf $$i; \ - done - -html: - $(SPHINXBUILD) -b html $(ALLSPHINXOPTS) $(BUILDDIR)/html - @echo - @echo "Build finished. The HTML pages are in $(BUILDDIR)/html." - -dirhtml: - $(SPHINXBUILD) -b dirhtml $(ALLSPHINXOPTS) $(BUILDDIR)/dirhtml - @echo - @echo "Build finished. The HTML pages are in $(BUILDDIR)/dirhtml." - -singlehtml: - $(SPHINXBUILD) -b singlehtml $(ALLSPHINXOPTS) $(BUILDDIR)/singlehtml - @echo - @echo "Build finished. The HTML page is in $(BUILDDIR)/singlehtml." - -pickle: - $(SPHINXBUILD) -b pickle $(ALLSPHINXOPTS) $(BUILDDIR)/pickle - @echo - @echo "Build finished; now you can process the pickle files." - -json: - $(SPHINXBUILD) -b json $(ALLSPHINXOPTS) $(BUILDDIR)/json - @echo - @echo "Build finished; now you can process the JSON files." - -htmlhelp: - $(SPHINXBUILD) -b htmlhelp $(ALLSPHINXOPTS) $(BUILDDIR)/htmlhelp - @echo - @echo "Build finished; now you can run HTML Help Workshop with the" \ - ".hhp project file in $(BUILDDIR)/htmlhelp." - -qthelp: - $(SPHINXBUILD) -b qthelp $(ALLSPHINXOPTS) $(BUILDDIR)/qthelp - @echo - @echo "Build finished; now you can run "qcollectiongenerator" with the" \ - ".qhcp project file in $(BUILDDIR)/qthelp, like this:" - @echo "# qcollectiongenerator $(BUILDDIR)/qthelp/RobotPyWPILib.qhcp" - @echo "To view the help file:" - @echo "# assistant -collectionFile $(BUILDDIR)/qthelp/RobotPyWPILib.qhc" - -devhelp: - $(SPHINXBUILD) -b devhelp $(ALLSPHINXOPTS) $(BUILDDIR)/devhelp - @echo - @echo "Build finished." - @echo "To view the help file:" - @echo "# mkdir -p $$HOME/.local/share/devhelp/RobotPyWPILib" - @echo "# ln -s $(BUILDDIR)/devhelp $$HOME/.local/share/devhelp/RobotPyWPILib" - @echo "# devhelp" - -epub: - $(SPHINXBUILD) -b epub $(ALLSPHINXOPTS) $(BUILDDIR)/epub - @echo - @echo "Build finished. The epub file is in $(BUILDDIR)/epub." - -latex: - $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex - @echo - @echo "Build finished; the LaTeX files are in $(BUILDDIR)/latex." - @echo "Run \`make' in that directory to run these through (pdf)latex" \ - "(use \`make latexpdf' here to do that automatically)." - -latexpdf: - $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex - @echo "Running LaTeX files through pdflatex..." - $(MAKE) -C $(BUILDDIR)/latex all-pdf - @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." - -latexpdfja: - $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex - @echo "Running LaTeX files through platex and dvipdfmx..." - $(MAKE) -C $(BUILDDIR)/latex all-pdf-ja - @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." - -text: - $(SPHINXBUILD) -b text $(ALLSPHINXOPTS) $(BUILDDIR)/text - @echo - @echo "Build finished. The text files are in $(BUILDDIR)/text." - -man: - $(SPHINXBUILD) -b man $(ALLSPHINXOPTS) $(BUILDDIR)/man - @echo - @echo "Build finished. The manual pages are in $(BUILDDIR)/man." - -texinfo: - $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo - @echo - @echo "Build finished. The Texinfo files are in $(BUILDDIR)/texinfo." - @echo "Run \`make' in that directory to run these through makeinfo" \ - "(use \`make info' here to do that automatically)." - -info: - $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo - @echo "Running Texinfo files through makeinfo..." - make -C $(BUILDDIR)/texinfo info - @echo "makeinfo finished; the Info files are in $(BUILDDIR)/texinfo." - -gettext: - $(SPHINXBUILD) -b gettext $(I18NSPHINXOPTS) $(BUILDDIR)/locale - @echo - @echo "Build finished. The message catalogs are in $(BUILDDIR)/locale." - -changes: - $(SPHINXBUILD) -b changes $(ALLSPHINXOPTS) $(BUILDDIR)/changes - @echo - @echo "The overview file is in $(BUILDDIR)/changes." - -linkcheck: - $(SPHINXBUILD) -b linkcheck $(ALLSPHINXOPTS) $(BUILDDIR)/linkcheck - @echo - @echo "Link check complete; look for any errors in the above output " \ - "or in $(BUILDDIR)/linkcheck/output.txt." - -doctest: - $(SPHINXBUILD) -b doctest $(ALLSPHINXOPTS) $(BUILDDIR)/doctest - @echo "Testing of doctests in the sources finished, look at the " \ - "results in $(BUILDDIR)/doctest/output.txt." - -xml: - $(SPHINXBUILD) -b xml $(ALLSPHINXOPTS) $(BUILDDIR)/xml - @echo - @echo "Build finished. The XML files are in $(BUILDDIR)/xml." - -pseudoxml: - $(SPHINXBUILD) -b pseudoxml $(ALLSPHINXOPTS) $(BUILDDIR)/pseudoxml - @echo - @echo "Build finished. The pseudo-XML files are in $(BUILDDIR)/pseudoxml." diff --git a/rio/ottomanEmpire/docs/api.rst b/rio/ottomanEmpire/docs/api.rst deleted file mode 100644 index faf88f70..00000000 --- a/rio/ottomanEmpire/docs/api.rst +++ /dev/null @@ -1,24 +0,0 @@ - -.. _command_v2_api: - -Commands V2 API ---------------- - -Objects in this package allow you to implement a robot using the -latest version of WPILib's Command-based programming. Command -based programming is a design pattern to help you organize your -robot programs, by organizing your robot program into components -based on :class:`.Command` and :class:`.Subsystem` - -Each one of the objects in the Command framework has detailed -documentation available. If you need more information, for examples, -tutorials, and other detailed information on programming your robot -using this pattern, we recommend that you consult the Java version of the -`FRC Control System documentation `_ - - -.. toctree:: - - commands2 - commands2.button - commands2.cmd diff --git a/rio/ottomanEmpire/docs/conf.py b/rio/ottomanEmpire/docs/conf.py deleted file mode 100644 index 0f2e83a8..00000000 --- a/rio/ottomanEmpire/docs/conf.py +++ /dev/null @@ -1,158 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- - -# -# Imports -# - -import os -from os.path import abspath, dirname - -# Project must be built+installed to generate docs -import commands2 - -# -- RTD configuration ------------------------------------------------ - -# on_rtd is whether we are on readthedocs.org, this line of code grabbed from docs.readthedocs.org -on_rtd = os.environ.get("READTHEDOCS", None) == "True" - -# This is used for linking and such so we link to the thing we're building -rtd_version = os.environ.get("READTHEDOCS_VERSION", "latest") -if rtd_version not in ["stable", "latest"]: - rtd_version = "stable" - -# -- General configuration ------------------------------------------------ - - -# Add any Sphinx extension module names here, as strings. They can be -# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom -# ones. -extensions = [ - "sphinx.ext.autodoc", - "sphinx.ext.autosummary", - "sphinx.ext.intersphinx", - "sphinx.ext.viewcode", - "robotpy_sphinx.all", -] - -# Add any paths that contain templates here, relative to this directory. -templates_path = ["_templates"] - -# The suffix of source filenames. -source_suffix = ".rst" - -# The master toctree document. -master_doc = "index" - -# General information about the project. -project = "RobotPy Commands v2" -copyright = "2021, RobotPy development team" - -# The version info for the project you're documenting, acts as replacement for -# |version| and |release|, also used in various other places throughout the -# built documents. -# -# The short X.Y version. -version = ".".join(commands2.__version__.split(".")[:2]) -# The full version, including alpha/beta/rc tags. -release = commands2.__version__ - -autoclass_content = "both" - -intersphinx_mapping = { - "networktables": ( - f"https://robotpy.readthedocs.io/projects/pynetworktables/en/{rtd_version}/", - None, - ), - "wpilib": ( - f"https://robotpy.readthedocs.io/projects/wpilib/en/{rtd_version}/", - None, - ), - "wpimath": ( - f"https://robotpy.readthedocs.io/projects/wpimath/en/{rtd_version}/", - None, - ), -} - -# List of patterns, relative to source directory, that match files and -# directories to ignore when looking for source files. -exclude_patterns = ["_build"] - -# The name of the Pygments (syntax highlighting) style to use. -pygments_style = "sphinx" - -# -- Options for HTML output ---------------------------------------------- - -if not on_rtd: # only import and set the theme if we're building docs locally - import sphinx_rtd_theme - - html_theme = "sphinx_rtd_theme" - html_theme_path = [sphinx_rtd_theme.get_html_theme_path()] -else: - html_theme = "default" - -# Output file base name for HTML help builder. -htmlhelp_basename = "RobotPyCommandDoc" - - -# -- Options for LaTeX output --------------------------------------------- - -latex_elements = {} - -# Grouping the document tree into LaTeX files. List of tuples -# (source start file, target name, title, -# author, documentclass [howto, manual, or own class]). -latex_documents = [ - ( - "index", - "RobotPyCommandV2Doc.tex", - "RobotPy CommandV2 Documentation", - "RobotPy development team", - "manual", - ) -] - -# -- Options for Texinfo output ------------------------------------------- - -# Grouping the document tree into Texinfo files. List of tuples -# (source start file, target name, title, author, -# dir menu entry, description, category) -texinfo_documents = [ - ( - "index", - "RobotPyCommandV2Doc", - "RobotPy CommandV2 Documentation", - "RobotPy development team", - "RobotPyCommandV2Doc", - "One line description of project.", - "Miscellaneous", - ) -] - -# -- Options for Epub output ---------------------------------------------- - -# Bibliographic Dublin Core info. -epub_title = "RobotPy CommandV2" -epub_author = "RobotPy development team" -epub_publisher = "RobotPy development team" -epub_copyright = "2021, RobotPy development team" - -# A list of files that should not be packed into the epub file. -epub_exclude_files = ["search.html"] - -# -- Custom Document processing ---------------------------------------------- - -from robotpy_sphinx.regen import gen_package -from robotpy_sphinx.sidebar import generate_sidebar - -generate_sidebar( - globals(), - "commandv2", - "https://raw.githubusercontent.com/robotpy/docs-sidebar/master/sidebar.toml", -) - -root = abspath(dirname(__file__)) - -gen_package(root, "commands2") -gen_package(root, "commands2.button") -gen_package(root, "commands2.cmd") diff --git a/rio/ottomanEmpire/docs/index.rst b/rio/ottomanEmpire/docs/index.rst deleted file mode 100644 index cd7b4269..00000000 --- a/rio/ottomanEmpire/docs/index.rst +++ /dev/null @@ -1,11 +0,0 @@ -Commands V2 -=========== - -.. include:: _sidebar.rst.inc - -Indices and tables -================== - -* :ref:`genindex` -* :ref:`modindex` -* :ref:`search` \ No newline at end of file diff --git a/rio/ottomanEmpire/docs/requirements.txt b/rio/ottomanEmpire/docs/requirements.txt deleted file mode 100644 index 10a9f290..00000000 --- a/rio/ottomanEmpire/docs/requirements.txt +++ /dev/null @@ -1,5 +0,0 @@ -# This file is intended for use on readthedocs -sphinx -sphinx-rtd-theme -robotpy-sphinx-plugin --e docs \ No newline at end of file diff --git a/rio/ottomanEmpire/docs/setup.py b/rio/ottomanEmpire/docs/setup.py deleted file mode 100644 index 1786ba89..00000000 --- a/rio/ottomanEmpire/docs/setup.py +++ /dev/null @@ -1,23 +0,0 @@ -# this is a dirty hack to convince readthedocs to install a specific -# version of our software. We assume that this will always be triggered -# on a specific version - -from setuptools import setup - -try: - from setuptools_scm import get_version -except ImportError: - import subprocess - - def get_version(*args, **kwargs): - return subprocess.check_output(["git", "describe", "--tags"], encoding="utf-8") - - -package = "robotpy-commands-v2" -version = get_version(root="..", relative_to=__file__) - -setup( - name="dummy-package", - version="1.0", - install_requires=["%s==%s" % (package, version)], -) diff --git a/rio/ottomanEmpire/setup.py b/rio/ottomanEmpire/setup.py deleted file mode 100644 index 99b6e032..00000000 --- a/rio/ottomanEmpire/setup.py +++ /dev/null @@ -1,23 +0,0 @@ -#!/usr/bin/env python3 - -import setuptools - -setuptools.setup( - name="robotpy-commands-v2", - use_scm_version=True, - setup_requires=["setuptools_scm"], - author="RobotPy Development Team", - author_email="robotpy@googlegroups.com", - description="WPILib command framework v2", - url="https://github.com/robotpy/robotpy-commands-v2", - packages=["commands2"], - install_requires=[ - "wpilib<2025,>=2024.0.0b2", - "typing_extensions>=4.1.0,<5", - "overtake~=0.4.0", - "beartype~=0.16.4", - ], - license="BSD-3-Clause", - python_requires=">=3.8", - include_package_data=True, -) diff --git a/rio/ottomanEmpire/tests/compositiontestbase.py b/rio/ottomanEmpire/tests/compositiontestbase.py deleted file mode 100644 index ca1bfad4..00000000 --- a/rio/ottomanEmpire/tests/compositiontestbase.py +++ /dev/null @@ -1,154 +0,0 @@ -from typing import Generic, TypeVar - -import commands2 -import pytest - -# T = TypeVar("T", bound=commands2.Command) -# T = commands2.Command - -from util import * - -if not IS_OLD_COMMANDS: - - class SingleCompositionTestBase: - def composeSingle(self, member: commands2.Command): - raise NotImplementedError - - @pytest.mark.parametrize( - "interruptionBehavior", - [ - commands2.InterruptionBehavior.kCancelSelf, - commands2.InterruptionBehavior.kCancelIncoming, - ], - ) - def test_interruptible( - self, interruptionBehavior: commands2.InterruptionBehavior - ): - command = self.composeSingle( - commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( - interruptionBehavior - ) - ) - assert command.getInterruptionBehavior() == interruptionBehavior - - @pytest.mark.parametrize("runsWhenDisabled", [True, False]) - def test_runWhenDisabled(self, runsWhenDisabled: bool): - command = self.composeSingle( - commands2.WaitUntilCommand(lambda: False).ignoringDisable( - runsWhenDisabled - ) - ) - assert command.runsWhenDisabled() == runsWhenDisabled - - class MultiCompositionTestBase(SingleCompositionTestBase): - def compose(self, *members: commands2.Command): - raise NotImplementedError - - def composeSingle(self, member: commands2.Command): - return self.compose(member) - - @pytest.mark.parametrize( - "expected,command1,command2,command3", - [ - pytest.param( - commands2.InterruptionBehavior.kCancelSelf, - commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( - commands2.InterruptionBehavior.kCancelSelf - ), - commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( - commands2.InterruptionBehavior.kCancelSelf - ), - commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( - commands2.InterruptionBehavior.kCancelSelf - ), - id="AllCancelSelf", - ), - pytest.param( - commands2.InterruptionBehavior.kCancelIncoming, - commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( - commands2.InterruptionBehavior.kCancelIncoming - ), - commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( - commands2.InterruptionBehavior.kCancelIncoming - ), - commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( - commands2.InterruptionBehavior.kCancelIncoming - ), - id="AllCancelIncoming", - ), - pytest.param( - commands2.InterruptionBehavior.kCancelSelf, - commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( - commands2.InterruptionBehavior.kCancelSelf - ), - commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( - commands2.InterruptionBehavior.kCancelSelf - ), - commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( - commands2.InterruptionBehavior.kCancelIncoming - ), - id="TwoCancelSelfOneIncoming", - ), - pytest.param( - commands2.InterruptionBehavior.kCancelSelf, - commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( - commands2.InterruptionBehavior.kCancelIncoming - ), - commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( - commands2.InterruptionBehavior.kCancelIncoming - ), - commands2.WaitUntilCommand(lambda: False).withInterruptBehavior( - commands2.InterruptionBehavior.kCancelSelf - ), - id="TwoCancelIncomingOneSelf", - ), - ], - ) - def test_interruptible(self, expected, command1, command2, command3): - command = self.compose(command1, command2, command3) - assert command.getInterruptionBehavior() == expected - - @pytest.mark.parametrize( - "expected,command1,command2,command3", - [ - pytest.param( - False, - commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), - commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), - commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), - id="AllFalse", - ), - pytest.param( - True, - commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), - commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), - commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), - id="AllTrue", - ), - pytest.param( - False, - commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), - commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), - commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), - id="TwoTrueOneFalse", - ), - pytest.param( - False, - commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), - commands2.WaitUntilCommand(lambda: False).ignoringDisable(False), - commands2.WaitUntilCommand(lambda: False).ignoringDisable(True), - id="TwoFalseOneTrue", - ), - ], - ) - def test_runWhenDisabled(self, expected, command1, command2, command3): - command = self.compose(command1, command2, command3) - assert command.runsWhenDisabled() == expected - -else: - - class SingleCompositionTestBase: - ... - - class MultiCompositionTestBase: - ... diff --git a/rio/ottomanEmpire/tests/conftest.py b/rio/ottomanEmpire/tests/conftest.py deleted file mode 100644 index 96f18263..00000000 --- a/rio/ottomanEmpire/tests/conftest.py +++ /dev/null @@ -1,20 +0,0 @@ -import commands2 -import pytest -from ntcore import NetworkTableInstance -from wpilib.simulation import DriverStationSim - - -@pytest.fixture(autouse=True) -def scheduler(): - commands2.CommandScheduler.resetInstance() - DriverStationSim.setEnabled(True) - DriverStationSim.notifyNewData() - return commands2.CommandScheduler.getInstance() - - -@pytest.fixture() -def nt_instance(): - inst = NetworkTableInstance.create() - inst.startLocal() - yield inst - inst.stopLocal() diff --git a/rio/ottomanEmpire/tests/requirements.txt b/rio/ottomanEmpire/tests/requirements.txt deleted file mode 100644 index e079f8a6..00000000 --- a/rio/ottomanEmpire/tests/requirements.txt +++ /dev/null @@ -1 +0,0 @@ -pytest diff --git a/rio/ottomanEmpire/tests/run_tests.py b/rio/ottomanEmpire/tests/run_tests.py deleted file mode 100755 index 62696f8c..00000000 --- a/rio/ottomanEmpire/tests/run_tests.py +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python3 - -import os -import subprocess -import sys -from os.path import abspath, dirname - -if __name__ == "__main__": - root = abspath(dirname(__file__)) - os.chdir(root) - - subprocess.check_call([sys.executable, "-m", "pytest"]) diff --git a/rio/ottomanEmpire/tests/test_command_decorators.py b/rio/ottomanEmpire/tests/test_command_decorators.py deleted file mode 100644 index 9c89ff9c..00000000 --- a/rio/ottomanEmpire/tests/test_command_decorators.py +++ /dev/null @@ -1,222 +0,0 @@ -from typing import TYPE_CHECKING - -from util import * # type: ignore - -if TYPE_CHECKING: - from .util import * - -import commands2 -import pytest - - -def test_timeout(scheduler: commands2.CommandScheduler): - with ManualSimTime() as sim: - command1 = commands2.WaitCommand(1) - timeout = command1.withTimeout(2) - scheduler.schedule(timeout) - scheduler.run() - assert not command1.isScheduled() - assert timeout.isScheduled() - sim.step(3) - scheduler.run() - assert not timeout.isScheduled() - - -def test_until(scheduler: commands2.CommandScheduler): - condition = OOBoolean(False) - command = commands2.WaitCommand(10).until(condition) - scheduler.schedule(command) - scheduler.run() - assert command.isScheduled() - condition.set(True) - scheduler.run() - assert not command.isScheduled() - - -@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") -def test_only_while(scheduler: commands2.CommandScheduler): - condition = OOBoolean(True) - command = commands2.WaitCommand(10).onlyWhile(condition) - scheduler.schedule(command) - scheduler.run() - assert command.isScheduled() - condition.set(False) - scheduler.run() - assert not command.isScheduled() - - -@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") -def test_ignoringDisable(scheduler: commands2.CommandScheduler): - command = commands2.RunCommand(lambda: None).ignoringDisable(True) - DriverStationSim.setEnabled(False) - DriverStationSim.notifyNewData() - scheduler.schedule(command) - scheduler.run() - assert command.isScheduled() - - -def test_beforeStarting(scheduler: commands2.CommandScheduler): - condition = OOBoolean(False) - condition.set(False) - command = commands2.InstantCommand() - scheduler.schedule( - command.beforeStarting(commands2.InstantCommand(lambda: condition.set(True))) - ) - assert condition == True - - -@pytest.mark.skip -def test_andThenLambda(scheduler: commands2.CommandScheduler): - ... - - -def test_andThen(scheduler: commands2.CommandScheduler): - condition = OOBoolean(False) - condition.set(False) - command1 = commands2.InstantCommand() - command2 = commands2.InstantCommand(lambda: condition.set(True)) - scheduler.schedule(command1.andThen(command2)) - assert condition == False - scheduler.run() - assert condition == True - - -def test_deadlineWith(scheduler: commands2.CommandScheduler): - condition = OOBoolean(False) - condition.set(False) - - dictator = commands2.WaitUntilCommand(condition) - endsBefore = commands2.InstantCommand() - endsAfter = commands2.WaitUntilCommand(lambda: False) - - group = dictator.deadlineWith(endsBefore, endsAfter) - - scheduler.schedule(group) - scheduler.run() - assert group.isScheduled() - condition.set(True) - scheduler.run() - assert not group.isScheduled() - - -def test_alongWith(scheduler: commands2.CommandScheduler): - condition = OOBoolean() - condition.set(False) - - command1 = commands2.WaitUntilCommand(condition) - command2 = commands2.InstantCommand() - - group = command1.alongWith(command2) - - scheduler.schedule(group) - scheduler.run() - assert group.isScheduled() - condition.set(True) - scheduler.run() - assert not group.isScheduled() - - -def test_raceWith(scheduler: commands2.CommandScheduler): - command1 = commands2.WaitUntilCommand(lambda: False) - command2 = commands2.InstantCommand() - - group = command1.raceWith(command2) - - scheduler.schedule(group) - scheduler.run() - assert not group.isScheduled() - - -def test_perpetually(scheduler: commands2.CommandScheduler): - command = commands2.InstantCommand() - perpetual = command.perpetually() - scheduler.schedule(perpetual) - scheduler.run() - scheduler.run() - scheduler.run() - assert perpetual.isScheduled() - - -@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") -def test_unless(scheduler: commands2.CommandScheduler): - unlessCondition = OOBoolean(True) - hasRunCondition = OOBoolean(False) - - command = commands2.InstantCommand(lambda: hasRunCondition.set(True)).unless( - unlessCondition - ) - - scheduler.schedule(command) - scheduler.run() - assert hasRunCondition == False - unlessCondition.set(False) - scheduler.schedule(command) - scheduler.run() - assert hasRunCondition == True - - -@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") -def test_onlyIf(scheduler: commands2.CommandScheduler): - onlyIfCondition = OOBoolean(False) - hasRunCondition = OOBoolean(False) - - command = commands2.InstantCommand(lambda: hasRunCondition.set(True)).onlyIf( - onlyIfCondition - ) - - scheduler.schedule(command) - scheduler.run() - assert hasRunCondition == False - onlyIfCondition.set(True) - scheduler.schedule(command) - scheduler.run() - assert hasRunCondition == True - - -@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") -def test_finallyDo(scheduler: commands2.CommandScheduler): - first = OOInteger(0) - second = OOInteger(0) - - command = commands2.FunctionalCommand( - lambda: None, - lambda: None, - lambda interrupted: first.incrementAndGet() if not interrupted else None, - lambda: True, - ).finallyDo(lambda interrupted: second.addAndGet(1 + first())) - - scheduler.schedule(command) - assert first == 0 - assert second == 0 - scheduler.run() - assert first == 1 - assert second == 2 - - -@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") -def test_handleInterrupt(scheduler: commands2.CommandScheduler): - first = OOInteger(0) - second = OOInteger(0) - - command = commands2.FunctionalCommand( - lambda: None, - lambda: None, - lambda interrupted: first.incrementAndGet() if interrupted else None, - lambda: False, - ).handleInterrupt(lambda: second.addAndGet(1 + first())) - - scheduler.schedule(command) - scheduler.run() - assert first == 0 - assert second == 0 - scheduler.cancel(command) - assert first == 1 - assert second == 2 - - -@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") -def test_withName(scheduler: commands2.CommandScheduler): - command = commands2.InstantCommand() - name = "Named" - named = command.withName(name) - assert named.getName() == name diff --git a/rio/ottomanEmpire/tests/test_command_requirements.py b/rio/ottomanEmpire/tests/test_command_requirements.py deleted file mode 100644 index 0215e0bc..00000000 --- a/rio/ottomanEmpire/tests/test_command_requirements.py +++ /dev/null @@ -1,59 +0,0 @@ -from typing import TYPE_CHECKING - -import commands2 -from util import * # type: ignore - -if TYPE_CHECKING: - from .util import * - -import pytest - - -def test_requirementInterrupt(scheduler: commands2.CommandScheduler): - requirement = commands2.Subsystem() - interrupted = commands2.Command() - interrupted.addRequirements(requirement) - interrupter = commands2.Command() - interrupter.addRequirements(requirement) - start_spying_on(interrupted) - start_spying_on(interrupter) - - scheduler.schedule(interrupted) - scheduler.run() - scheduler.schedule(interrupter) - scheduler.run() - - verify(interrupted).initialize() - verify(interrupted).execute() - verify(interrupted).end(True) - - verify(interrupter).initialize() - verify(interrupter).execute() - - assert not interrupted.isScheduled() - assert interrupter.isScheduled() - - -@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") -def test_requirementUninterruptible(scheduler: commands2.CommandScheduler): - requirement = commands2.Subsystem() - notInterrupted = commands2.RunCommand( - lambda: None, requirement - ).withInterruptBehavior(commands2.InterruptionBehavior.kCancelIncoming) - interrupter = commands2.Command() - interrupter.addRequirements(requirement) - start_spying_on(notInterrupted) - - scheduler.schedule(notInterrupted) - scheduler.schedule(interrupter) - - assert scheduler.isScheduled(notInterrupted) - assert not scheduler.isScheduled(interrupter) - - -def test_defaultCommandRequirementError(scheduler: commands2.CommandScheduler): - system = commands2.Subsystem() - missingRequirement = commands2.WaitUntilCommand(lambda: False) - - with pytest.raises(commands2.IllegalCommandUse): - scheduler.setDefaultCommand(system, missingRequirement) diff --git a/rio/ottomanEmpire/tests/test_command_schedule.py b/rio/ottomanEmpire/tests/test_command_schedule.py deleted file mode 100644 index 62d21823..00000000 --- a/rio/ottomanEmpire/tests/test_command_schedule.py +++ /dev/null @@ -1,90 +0,0 @@ -from typing import TYPE_CHECKING - -import commands2 -from util import * # type: ignore - -if TYPE_CHECKING: - from .util import * - -import pytest - - -def test_instantSchedule(scheduler: commands2.CommandScheduler): - command = commands2.Command() - command.isFinished = lambda: True - start_spying_on(command) - - scheduler.schedule(command) - assert scheduler.isScheduled(command) - verify(command).initialize() - - scheduler.run() - - verify(command).execute() - verify(command).end(False) - assert not scheduler.isScheduled(command) - - -def test_singleIterationSchedule(scheduler: commands2.CommandScheduler): - command = commands2.Command() - start_spying_on(command) - - scheduler.schedule(command) - assert scheduler.isScheduled(command) - - scheduler.run() - command.isFinished = lambda: True - scheduler.run() - - verify(command).initialize() - verify(command, times(2)).execute() - verify(command).end(False) - assert not scheduler.isScheduled(command) - - -def test_multiSchedule(scheduler: commands2.CommandScheduler): - command1 = commands2.Command() - command2 = commands2.Command() - command3 = commands2.Command() - - scheduler.schedule(command1, command2, command3) - assert scheduler.isScheduled(command1, command2, command3) - scheduler.run() - assert scheduler.isScheduled(command1, command2, command3) - - command1.isFinished = lambda: True - scheduler.run() - assert scheduler.isScheduled(command2, command3) - assert not scheduler.isScheduled(command1) - - command2.isFinished = lambda: True - scheduler.run() - assert scheduler.isScheduled(command3) - assert not scheduler.isScheduled(command1, command2) - - command3.isFinished = lambda: True - scheduler.run() - assert not scheduler.isScheduled(command1, command2, command3) - - -def test_schedulerCancel(scheduler: commands2.CommandScheduler): - command = commands2.Command() - start_spying_on(command) - - scheduler.schedule(command) - - scheduler.run() - scheduler.cancel(command) - scheduler.run() - - verify(command).execute() - verify(command).end(True) - verify(command, never()).end(False) - - assert not scheduler.isScheduled(command) - - -def test_notScheduledCancel(scheduler: commands2.CommandScheduler): - command = commands2.Command() - - scheduler.cancel(command) diff --git a/rio/ottomanEmpire/tests/test_commandgroup_error.py b/rio/ottomanEmpire/tests/test_commandgroup_error.py deleted file mode 100644 index 36008f1a..00000000 --- a/rio/ottomanEmpire/tests/test_commandgroup_error.py +++ /dev/null @@ -1,38 +0,0 @@ -from typing import TYPE_CHECKING - -import commands2 -from util import * # type: ignore - -if TYPE_CHECKING: - from .util import * - -import pytest - - -def test_commandInMultipleGroups(): - command1 = commands2.Command() - command2 = commands2.Command() - - commands2.ParallelCommandGroup(command1, command2) - with pytest.raises(commands2.IllegalCommandUse): - commands2.ParallelCommandGroup(command1, command2) - - -def test_commandInGroupExternallyScheduled(scheduler: commands2.CommandScheduler): - command1 = commands2.Command() - command2 = commands2.Command() - - commands2.ParallelCommandGroup(command1, command2) - - with pytest.raises(commands2.IllegalCommandUse): - scheduler.schedule(command1) - - -@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") -def test_redecoratedCommandError(scheduler: commands2.CommandScheduler): - command = commands2.InstantCommand() - command.withTimeout(10).until(lambda: False) - with pytest.raises(commands2.IllegalCommandUse): - command.withTimeout(10) - scheduler.removeComposedCommands([command]) - command.withTimeout(10) diff --git a/rio/ottomanEmpire/tests/test_conditional_command.py b/rio/ottomanEmpire/tests/test_conditional_command.py deleted file mode 100644 index caeff2da..00000000 --- a/rio/ottomanEmpire/tests/test_conditional_command.py +++ /dev/null @@ -1,55 +0,0 @@ -from typing import TYPE_CHECKING - -import commands2 -from util import * # type: ignore - -if TYPE_CHECKING: - from .util import * - -import pytest - - -def test_conditionalCommand(scheduler: commands2.CommandScheduler): - command1 = commands2.Command() - command1.isFinished = lambda: True - command2 = commands2.Command() - - start_spying_on(command1) - start_spying_on(command2) - - conditionalCommand = commands2.ConditionalCommand(command1, command2, lambda: True) - - scheduler.schedule(conditionalCommand) - scheduler.run() - - verify(command1).initialize() - verify(command1).execute() - verify(command1).end(False) - - verify(command2, never()).initialize() - verify(command2, never()).execute() - verify(command2, never()).end(False) - - -def test_conditionalCommandRequirement(scheduler: commands2.CommandScheduler): - system1 = commands2.Subsystem() - system2 = commands2.Subsystem() - system3 = commands2.Subsystem() - - command1 = commands2.Command() - command1.addRequirements(system1, system2) - command2 = commands2.Command() - command2.addRequirements(system3) - - start_spying_on(command1) - start_spying_on(command2) - - conditionalCommand = commands2.ConditionalCommand(command1, command2, lambda: True) - - scheduler.schedule(conditionalCommand) - scheduler.schedule(commands2.InstantCommand(lambda: None, system3)) - - assert not scheduler.isScheduled(conditionalCommand) - - assert command1.end.called_with(True) - assert not command2.end.called_with(True) diff --git a/rio/ottomanEmpire/tests/test_default_command.py b/rio/ottomanEmpire/tests/test_default_command.py deleted file mode 100644 index 211ecae4..00000000 --- a/rio/ottomanEmpire/tests/test_default_command.py +++ /dev/null @@ -1,73 +0,0 @@ -from typing import TYPE_CHECKING - -import commands2 -from util import * # type: ignore - -if TYPE_CHECKING: - from .util import * - -import pytest - - -def test_defaultCommandSchedule(scheduler: commands2.CommandScheduler): - hasDefaultCommand = commands2.Subsystem() - - defaultCommand = commands2.Command() - defaultCommand.addRequirements(hasDefaultCommand) - - scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand) - scheduler.run() - - assert scheduler.isScheduled(defaultCommand) - - -def test_defaultCommandInterruptResume(scheduler: commands2.CommandScheduler): - hasDefaultCommand = commands2.Subsystem() - - defaultCommand = commands2.Command() - defaultCommand.addRequirements(hasDefaultCommand) - - interrupter = commands2.Command() - interrupter.addRequirements(hasDefaultCommand) - - scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand) - scheduler.run() - scheduler.schedule(interrupter) - - assert not scheduler.isScheduled(defaultCommand) - assert scheduler.isScheduled(interrupter) - - scheduler.cancel(interrupter) - scheduler.run() - - assert scheduler.isScheduled(defaultCommand) - assert not scheduler.isScheduled(interrupter) - - -def test_defaultCommandDisableResume(scheduler: commands2.CommandScheduler): - hasDefaultCommand = commands2.Subsystem() - - defaultCommand = commands2.Command() - defaultCommand.addRequirements(hasDefaultCommand) - defaultCommand.runsWhenDisabled = lambda: False - - start_spying_on(defaultCommand) - - scheduler.setDefaultCommand(hasDefaultCommand, defaultCommand) - scheduler.run() - - assert scheduler.isScheduled(defaultCommand) - - DriverStationSim.setEnabled(False) - DriverStationSim.notifyNewData() - scheduler.run() - - assert not scheduler.isScheduled(defaultCommand) - - DriverStationSim.setEnabled(True) - DriverStationSim.notifyNewData() - scheduler.run() - - assert scheduler.isScheduled(defaultCommand) - - assert defaultCommand.end.called_with(True) diff --git a/rio/ottomanEmpire/tests/test_functional_command.py b/rio/ottomanEmpire/tests/test_functional_command.py deleted file mode 100644 index c6481cab..00000000 --- a/rio/ottomanEmpire/tests/test_functional_command.py +++ /dev/null @@ -1,36 +0,0 @@ -from typing import TYPE_CHECKING - -import commands2 -from util import * # type: ignore - -if TYPE_CHECKING: - from .util import * - -import pytest - - -def test_functionalCommandSchedule(scheduler: commands2.CommandScheduler): - cond1 = OOBoolean() - cond2 = OOBoolean() - cond3 = OOBoolean() - cond4 = OOBoolean() - - command = commands2.FunctionalCommand( - lambda: cond1.set(True), - lambda: cond2.set(True), - lambda _: cond3.set(True), - lambda: cond4.get(), - ) - - scheduler.schedule(command) - scheduler.run() - - assert scheduler.isScheduled(command) - - cond4.set(True) - scheduler.run() - - assert not scheduler.isScheduled(command) - assert cond1 - assert cond2 - assert cond3 diff --git a/rio/ottomanEmpire/tests/test_instant_command.py b/rio/ottomanEmpire/tests/test_instant_command.py deleted file mode 100644 index 96d3089e..00000000 --- a/rio/ottomanEmpire/tests/test_instant_command.py +++ /dev/null @@ -1,21 +0,0 @@ -from typing import TYPE_CHECKING - -import commands2 -from util import * # type: ignore - -if TYPE_CHECKING: - from .util import * - -import pytest - - -def test_instantCommandSchedule(scheduler: commands2.CommandScheduler): - cond = OOBoolean() - - command = commands2.InstantCommand(lambda: cond.set(True)) - - scheduler.schedule(command) - scheduler.run() - - assert cond - assert not scheduler.isScheduled(command) diff --git a/rio/ottomanEmpire/tests/test_networkbutton.py b/rio/ottomanEmpire/tests/test_networkbutton.py deleted file mode 100644 index d49f687d..00000000 --- a/rio/ottomanEmpire/tests/test_networkbutton.py +++ /dev/null @@ -1,29 +0,0 @@ -from typing import TYPE_CHECKING - -import commands2 -from ntcore import NetworkTableInstance -from util import * # type: ignore - -if TYPE_CHECKING: - from .util import * - - -def test_networkbutton( - scheduler: commands2.CommandScheduler, nt_instance: NetworkTableInstance -): - # command = commands2.Command() - command = commands2.Command() - start_spying_on(command) - - pub = nt_instance.getTable("TestTable").getBooleanTopic("Test").publish() - - button = commands2.button.NetworkButton(nt_instance, "TestTable", "Test") - - pub.set(False) - button.onTrue(command) - scheduler.run() - assert command.schedule.times_called == 0 - pub.set(True) - scheduler.run() - scheduler.run() - verify(command).schedule() diff --git a/rio/ottomanEmpire/tests/test_notifier_command.py b/rio/ottomanEmpire/tests/test_notifier_command.py deleted file mode 100644 index 3022c20c..00000000 --- a/rio/ottomanEmpire/tests/test_notifier_command.py +++ /dev/null @@ -1,23 +0,0 @@ -from typing import TYPE_CHECKING - -import commands2 -from util import * # type: ignore - -if TYPE_CHECKING: - from .util import * - -import pytest - - -@pytest.mark.skip(reason="NotifierCommand is broken") -def test_notifierCommandScheduler(scheduler: commands2.CommandScheduler): - with ManualSimTime() as sim: - counter = OOInteger(0) - command = commands2.NotifierCommand(counter.incrementAndGet, 0.01) - - scheduler.schedule(command) - for i in range(5): - sim.step(0.005) - scheduler.cancel(command) - - assert counter == 2 diff --git a/rio/ottomanEmpire/tests/test_parallelcommandgroup.py b/rio/ottomanEmpire/tests/test_parallelcommandgroup.py deleted file mode 100644 index 04355af7..00000000 --- a/rio/ottomanEmpire/tests/test_parallelcommandgroup.py +++ /dev/null @@ -1,117 +0,0 @@ -from typing import TYPE_CHECKING - -import commands2 -from compositiontestbase import MultiCompositionTestBase # type: ignore -from util import * # type: ignore - -# from tests.compositiontestbase import T - -if TYPE_CHECKING: - from .util import * - from .compositiontestbase import MultiCompositionTestBase - -import pytest - - -class TestParallelCommandGroupComposition(MultiCompositionTestBase): - def compose(self, *members: commands2.Command): - return commands2.ParallelCommandGroup(*members) - - -def test_parallelGroupSchedule(scheduler: commands2.CommandScheduler): - command1 = commands2.Command() - command2 = commands2.Command() - - start_spying_on(command1) - start_spying_on(command2) - - group = commands2.ParallelCommandGroup(command1, command2) - - scheduler.schedule(group) - - verify(command1).initialize() - verify(command2).initialize() - - command1.isFinished = lambda: True - scheduler.run() - command2.isFinished = lambda: True - scheduler.run() - - verify(command1).execute() - verify(command1).end(False) - verify(command2, times(2)).execute() - verify(command2).end(False) - - assert not scheduler.isScheduled(group) - - -def test_parallelGroupInterrupt(scheduler: commands2.CommandScheduler): - command1 = commands2.Command() - command2 = commands2.Command() - - start_spying_on(command1) - start_spying_on(command2) - - group = commands2.ParallelCommandGroup(command1, command2) - - scheduler.schedule(group) - - command1.isFinished = lambda: True - scheduler.run() - scheduler.run() - scheduler.cancel(group) - - verify(command1).execute() - verify(command1).end(False) - verify(command1, never()).end(True) - - verify(command2, times(2)).execute() - verify(command2, never()).end(False) - verify(command2).end(True) - - assert not scheduler.isScheduled(group) - - -def test_notScheduledCancel(scheduler: commands2.CommandScheduler): - command1 = commands2.Command() - command2 = commands2.Command() - - group = commands2.ParallelCommandGroup(command1, command2) - - scheduler.cancel(group) - - -def test_parallelGroupRequirement(scheduler: commands2.CommandScheduler): - system1 = commands2.Subsystem() - system2 = commands2.Subsystem() - system3 = commands2.Subsystem() - system4 = commands2.Subsystem() - - command1 = commands2.Command() - command1.addRequirements(system1, system2) - command2 = commands2.Command() - command2.addRequirements(system3) - command3 = commands2.Command() - command3.addRequirements(system3, system4) - - group = commands2.ParallelCommandGroup(command1, command2) - - scheduler.schedule(group) - scheduler.schedule(command3) - - assert not scheduler.isScheduled(group) - assert scheduler.isScheduled(command3) - - -def test_parallelGroupRequirementError(): - system1 = commands2.Subsystem() - system2 = commands2.Subsystem() - system3 = commands2.Subsystem() - - command1 = commands2.Command() - command1.addRequirements(system1, system2) - command2 = commands2.Command() - command2.addRequirements(system2, system3) - - with pytest.raises(commands2.IllegalCommandUse): - commands2.ParallelCommandGroup(command1, command2) diff --git a/rio/ottomanEmpire/tests/test_paralleldeadlinegroup.py b/rio/ottomanEmpire/tests/test_paralleldeadlinegroup.py deleted file mode 100644 index 9136509b..00000000 --- a/rio/ottomanEmpire/tests/test_paralleldeadlinegroup.py +++ /dev/null @@ -1,119 +0,0 @@ -from typing import TYPE_CHECKING - -import commands2 -from compositiontestbase import MultiCompositionTestBase # type: ignore -from util import * # type: ignore - -# from tests.compositiontestbase import T - -if TYPE_CHECKING: - from .util import * - from .compositiontestbase import MultiCompositionTestBase - -import pytest - - -class TestParallelDeadlineGroupComposition(MultiCompositionTestBase): - def compose(self, *members: commands2.Command): - return commands2.ParallelDeadlineGroup(members[0], *members[1:]) - - -def test_parallelDeadlineSchedule(scheduler: commands2.CommandScheduler): - command1 = commands2.Command() - command2 = commands2.Command() - command2.isFinished = lambda: True - command3 = commands2.Command() - - start_spying_on(command1) - start_spying_on(command2) - start_spying_on(command3) - - group = commands2.ParallelDeadlineGroup(command1, command2, command3) - - scheduler.schedule(group) - scheduler.run() - - assert scheduler.isScheduled(group) - - command1.isFinished = lambda: True - scheduler.run() - - assert not scheduler.isScheduled(group) - - verify(command2).initialize() - verify(command2).execute() - verify(command2).end(False) - verify(command2, never()).end(True) - - verify(command1).initialize() - verify(command1, times(2)).execute() - verify(command1).end(False) - verify(command1, never()).end(True) - - verify(command3).initialize() - verify(command3, times(2)).execute() - verify(command3, never()).end(False) - verify(command3).end(True) - - -def test_parallelDeadlineInterrupt(scheduler: commands2.CommandScheduler): - command1 = commands2.Command() - command2 = commands2.Command() - command2.isFinished = lambda: True - - start_spying_on(command1) - start_spying_on(command2) - - group = commands2.ParallelDeadlineGroup(command1, command2) - - scheduler.schedule(group) - - scheduler.run() - scheduler.run() - scheduler.cancel(group) - - verify(command1, times(2)).execute() - verify(command1, never()).end(False) - verify(command1).end(True) - - verify(command2).execute() - verify(command2).end(False) - verify(command2, never()).end(True) - - assert not scheduler.isScheduled(group) - - -def test_parallelDeadlineRequirement(scheduler: commands2.CommandScheduler): - system1 = commands2.Subsystem() - system2 = commands2.Subsystem() - system3 = commands2.Subsystem() - system4 = commands2.Subsystem() - - command1 = commands2.Command() - command1.addRequirements(system1, system2) - command2 = commands2.Command() - command2.addRequirements(system3) - command3 = commands2.Command() - command3.addRequirements(system3, system4) - - group = commands2.ParallelDeadlineGroup(command1, command2) - - scheduler.schedule(group) - scheduler.schedule(command3) - - assert not scheduler.isScheduled(group) - assert scheduler.isScheduled(command3) - - -def test_parallelDeadlineRequirementError(scheduler: commands2.CommandScheduler): - system1 = commands2.Subsystem() - system2 = commands2.Subsystem() - system3 = commands2.Subsystem() - - command1 = commands2.Command() - command1.addRequirements(system1, system2) - command2 = commands2.Command() - command2.addRequirements(system2, system3) - - with pytest.raises(commands2.IllegalCommandUse): - commands2.ParallelDeadlineGroup(command1, command2) diff --git a/rio/ottomanEmpire/tests/test_parallelracegroup.py b/rio/ottomanEmpire/tests/test_parallelracegroup.py deleted file mode 100644 index 58e2ace4..00000000 --- a/rio/ottomanEmpire/tests/test_parallelracegroup.py +++ /dev/null @@ -1,183 +0,0 @@ -from typing import TYPE_CHECKING - -import commands2 -from compositiontestbase import MultiCompositionTestBase # type: ignore -from util import * # type: ignore - -# from tests.compositiontestbase import T - -if TYPE_CHECKING: - from .util import * - from .compositiontestbase import MultiCompositionTestBase - -import pytest - - -class TestParallelRaceGroupComposition(MultiCompositionTestBase): - def compose(self, *members: commands2.Command): - return commands2.ParallelRaceGroup(*members) - - -def test_parallelRaceSchedule(scheduler: commands2.CommandScheduler): - command1 = commands2.Command() - command2 = commands2.Command() - - start_spying_on(command1) - start_spying_on(command2) - - group = commands2.ParallelRaceGroup(command1, command2) - - scheduler.schedule(group) - - verify(command1).initialize() - verify(command2).initialize() - - command1.isFinished = lambda: True - scheduler.run() - command2.isFinished = lambda: True - scheduler.run() - - verify(command1).execute() - verify(command1).end(False) - verify(command2).execute() - verify(command2).end(True) - verify(command2, never()).end(False) - - assert not scheduler.isScheduled(group) - - -def test_parallelRaceInterrupt(scheduler: commands2.CommandScheduler): - command1 = commands2.Command() - command2 = commands2.Command() - - start_spying_on(command1) - start_spying_on(command2) - - group = commands2.ParallelRaceGroup(command1, command2) - - scheduler.schedule(group) - - scheduler.run() - scheduler.run() - scheduler.cancel(group) - - verify(command1, times(2)).execute() - verify(command1, never()).end(False) - verify(command1).end(True) - - verify(command2, times(2)).execute() - verify(command2, never()).end(False) - verify(command2).end(True) - - assert not scheduler.isScheduled(group) - - -def test_notScheduledCancel(scheduler: commands2.CommandScheduler): - command1 = commands2.Command() - command2 = commands2.Command() - - group = commands2.ParallelRaceGroup(command1, command2) - - scheduler.cancel(group) - - -def test_parallelRaceRequirement(scheduler: commands2.CommandScheduler): - system1 = commands2.Subsystem() - system2 = commands2.Subsystem() - system3 = commands2.Subsystem() - system4 = commands2.Subsystem() - - command1 = commands2.Command() - command1.addRequirements(system1, system2) - command2 = commands2.Command() - command2.addRequirements(system3) - command3 = commands2.Command() - command3.addRequirements(system3, system4) - - group = commands2.ParallelRaceGroup(command1, command2) - - scheduler.schedule(group) - scheduler.schedule(command3) - - assert not scheduler.isScheduled(group) - assert scheduler.isScheduled(command3) - - -def test_parallelRaceRequirementError(): - system1 = commands2.Subsystem() - system2 = commands2.Subsystem() - system3 = commands2.Subsystem() - - command1 = commands2.Command() - command1.addRequirements(system1, system2) - command2 = commands2.Command() - command2.addRequirements(system2, system3) - - with pytest.raises(commands2.IllegalCommandUse): - commands2.ParallelRaceGroup(command1, command2) - - -def test_parallelRaceOnlyCallsEndOnce(scheduler: commands2.CommandScheduler): - system1 = commands2.Subsystem() - system2 = commands2.Subsystem() - - command1 = commands2.Command() - command1.addRequirements(system1) - command2 = commands2.Command() - command2.addRequirements(system2) - command3 = commands2.Command() - - group1 = commands2.SequentialCommandGroup(command1, command2) - group2 = commands2.ParallelRaceGroup(group1, command3) - - scheduler.schedule(group2) - scheduler.run() - command1.isFinished = lambda: True - scheduler.run() - command2.isFinished = lambda: True - scheduler.run() - assert not scheduler.isScheduled(group2) - - -def test_parallelRaceScheduleTwiceTest(scheduler: commands2.CommandScheduler): - command1 = commands2.Command() - command2 = commands2.Command() - - start_spying_on(command1) - start_spying_on(command2) - - group = commands2.ParallelRaceGroup(command1, command2) - - scheduler.schedule(group) - - verify(command1).initialize() - verify(command2).initialize() - - command1.isFinished = lambda: True - scheduler.run() - command2.isFinished = lambda: True - scheduler.run() - - verify(command1).execute() - verify(command1).end(False) - verify(command2).execute() - verify(command2).end(True) - verify(command2, never()).end(False) - - assert not scheduler.isScheduled(group) - - reset(command1) - reset(command2) - - scheduler.schedule(group) - - verify(command1).initialize() - verify(command2).initialize() - - scheduler.run() - scheduler.run() - assert scheduler.isScheduled(group) - command2.isFinished = lambda: True - scheduler.run() - - assert not scheduler.isScheduled(group) diff --git a/rio/ottomanEmpire/tests/test_perpetualcommand.py b/rio/ottomanEmpire/tests/test_perpetualcommand.py deleted file mode 100644 index 55284ff1..00000000 --- a/rio/ottomanEmpire/tests/test_perpetualcommand.py +++ /dev/null @@ -1,18 +0,0 @@ -from typing import TYPE_CHECKING - -import commands2 -from util import * # type: ignore - -if TYPE_CHECKING: - from .util import * - -import pytest - - -def test_perpetualCommandSchedule(scheduler: commands2.CommandScheduler): - command = commands2.PerpetualCommand(commands2.InstantCommand()) - - scheduler.schedule(command) - scheduler.run() - - assert scheduler.isScheduled(command) diff --git a/rio/ottomanEmpire/tests/test_pidcommand.py b/rio/ottomanEmpire/tests/test_pidcommand.py deleted file mode 100644 index 5ba51650..00000000 --- a/rio/ottomanEmpire/tests/test_pidcommand.py +++ /dev/null @@ -1,114 +0,0 @@ -from typing import TYPE_CHECKING - -from util import * # type: ignore -import wpimath.controller as controller -import commands2 - -if TYPE_CHECKING: - from .util import * - -import pytest - - -def test_pidCommandSupplier(scheduler: commands2.CommandScheduler): - with ManualSimTime() as sim: - output_float = OOFloat(0.0) - measurement_source = OOFloat(5.0) - setpoint_source = OOFloat(2.0) - pid_controller = controller.PIDController(0.1, 0.01, 0.001) - system = commands2.Subsystem() - pidCommand = commands2.PIDCommand( - pid_controller, - measurement_source, - setpoint_source, - output_float.set, - system, - ) - start_spying_on(pidCommand) - scheduler.schedule(pidCommand) - scheduler.run() - sim.step(1) - scheduler.run() - - assert scheduler.isScheduled(pidCommand) - - assert not pidCommand._controller.atSetpoint() - - # Tell the pid command we're at our setpoint through the controller - measurement_source.set(setpoint_source()) - - sim.step(2) - - scheduler.run() - - # Should be measuring error of 0 now - assert pidCommand._controller.atSetpoint() - - -def test_pidCommandScalar(scheduler: commands2.CommandScheduler): - with ManualSimTime() as sim: - output_float = OOFloat(0.0) - measurement_source = OOFloat(5.0) - setpoint_source = 2.0 - pid_controller = controller.PIDController(0.1, 0.01, 0.001) - system = commands2.Subsystem() - pidCommand = commands2.PIDCommand( - pid_controller, - measurement_source, - setpoint_source, - output_float.set, - system, - ) - start_spying_on(pidCommand) - scheduler.schedule(pidCommand) - scheduler.run() - sim.step(1) - scheduler.run() - - assert scheduler.isScheduled(pidCommand) - - assert not pidCommand._controller.atSetpoint() - - # Tell the pid command we're at our setpoint through the controller - measurement_source.set(setpoint_source) - - sim.step(2) - - scheduler.run() - - # Should be measuring error of 0 now - assert pidCommand._controller.atSetpoint() - - -def test_withTimeout(scheduler: commands2.CommandScheduler): - with ManualSimTime() as sim: - output_float = OOFloat(0.0) - measurement_source = OOFloat(5.0) - setpoint_source = OOFloat(2.0) - pid_controller = controller.PIDController(0.1, 0.01, 0.001) - system = commands2.Subsystem() - command1 = commands2.PIDCommand( - pid_controller, - measurement_source, - setpoint_source, - output_float.set, - system, - ) - start_spying_on(command1) - - timeout = command1.withTimeout(2) - - scheduler.schedule(timeout) - scheduler.run() - - verify(command1).initialize() - verify(command1).execute() - assert not scheduler.isScheduled(command1) - assert scheduler.isScheduled(timeout) - - sim.step(3) - scheduler.run() - - verify(command1).end(True) - verify(command1, never()).end(False) - assert not scheduler.isScheduled(timeout) diff --git a/rio/ottomanEmpire/tests/test_printcommand.py b/rio/ottomanEmpire/tests/test_printcommand.py deleted file mode 100644 index 6d28cad6..00000000 --- a/rio/ottomanEmpire/tests/test_printcommand.py +++ /dev/null @@ -1,17 +0,0 @@ -from typing import TYPE_CHECKING - -import commands2 -from util import * # type: ignore - -if TYPE_CHECKING: - from .util import * - -import pytest - - -def test_printCommandSchedule(capsys, scheduler: commands2.CommandScheduler): - command = commands2.PrintCommand("Test!") - scheduler.schedule(command) - scheduler.run() - assert not scheduler.isScheduled(command) - assert capsys.readouterr().out == "Test!\n" diff --git a/rio/ottomanEmpire/tests/test_proxycommand.py b/rio/ottomanEmpire/tests/test_proxycommand.py deleted file mode 100644 index 5f1add10..00000000 --- a/rio/ottomanEmpire/tests/test_proxycommand.py +++ /dev/null @@ -1,40 +0,0 @@ -from typing import TYPE_CHECKING - -import commands2 -from util import * # type: ignore - -if TYPE_CHECKING: - from .util import * - -import pytest - - -@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") -def test_proxyCommandSchedule(scheduler: commands2.CommandScheduler): - command1 = commands2.Command() - start_spying_on(command1) - - scheduleCommand = commands2.ProxyCommand(command1) - - scheduler.schedule(scheduleCommand) - - verify(command1).schedule() - - -@pytest.mark.skipif(IS_OLD_COMMANDS, reason="not in old commands") -def test_proxyCommandEnd(scheduler: commands2.CommandScheduler): - cond = OOBoolean() - - command = commands2.WaitUntilCommand(cond) - - scheduleCommand = commands2.ProxyCommand(command) - - scheduler.schedule(scheduleCommand) - scheduler.run() - - assert scheduler.isScheduled(scheduleCommand) - - cond.set(True) - scheduler.run() - scheduler.run() - assert not scheduler.isScheduled(scheduleCommand) diff --git a/rio/ottomanEmpire/tests/test_repeatcommand.py b/rio/ottomanEmpire/tests/test_repeatcommand.py deleted file mode 100644 index e2ff3be9..00000000 --- a/rio/ottomanEmpire/tests/test_repeatcommand.py +++ /dev/null @@ -1,69 +0,0 @@ -from typing import TYPE_CHECKING - -import commands2 -from compositiontestbase import SingleCompositionTestBase # type: ignore -from util import * # type: ignore - -if TYPE_CHECKING: - from .util import * - from .compositiontestbase import SingleCompositionTestBase - -import pytest - - -class RepeatCommandCompositionTest(SingleCompositionTestBase): - def composeSingle(self, member): - return member.repeatedly() - - -def test_callsMethodsCorrectly(scheduler: commands2.CommandScheduler): - command = commands2.Command() - repeated = command.repeatedly() - - start_spying_on(command) - - assert command.initialize.times_called == 0 - assert command.execute.times_called == 0 - assert command.isFinished.times_called == 0 - assert command.end.times_called == 0 - - scheduler.schedule(repeated) - assert command.initialize.times_called == 1 - assert command.execute.times_called == 0 - assert command.isFinished.times_called == 0 - assert command.end.times_called == 0 - - command.isFinished = lambda: False - - scheduler.run() - assert command.initialize.times_called == 1 - assert command.execute.times_called == 1 - assert command.isFinished.times_called == 1 - assert command.end.times_called == 0 - - command.isFinished = lambda: True - scheduler.run() - assert command.initialize.times_called == 1 - assert command.execute.times_called == 2 - assert command.isFinished.times_called == 2 - assert command.end.times_called == 1 - - command.isFinished = lambda: False - scheduler.run() - assert command.initialize.times_called == 2 - assert command.execute.times_called == 3 - assert command.isFinished.times_called == 3 - assert command.end.times_called == 1 - - command.isFinished = lambda: True - scheduler.run() - assert command.initialize.times_called == 2 - assert command.execute.times_called == 4 - assert command.isFinished.times_called == 4 - assert command.end.times_called == 2 - - scheduler.cancel(repeated) - assert command.initialize.times_called == 2 - assert command.execute.times_called == 4 - assert command.isFinished.times_called == 4 - assert command.end.times_called == 2 diff --git a/rio/ottomanEmpire/tests/test_robotdisabledcommand.py b/rio/ottomanEmpire/tests/test_robotdisabledcommand.py deleted file mode 100644 index d34915f3..00000000 --- a/rio/ottomanEmpire/tests/test_robotdisabledcommand.py +++ /dev/null @@ -1,157 +0,0 @@ -from typing import TYPE_CHECKING - -import commands2 -from util import * # type: ignore - -if TYPE_CHECKING: - from .util import * - -if IS_OLD_COMMANDS: - import commands2.cmd - -import pytest -from wpilib import RobotState - - -def test_robotDisabledCommandCancel(scheduler: commands2.CommandScheduler): - command = commands2.Command() - scheduler.schedule(command) - assert scheduler.isScheduled(command) - DriverStationSim.setEnabled(False) - DriverStationSim.notifyNewData() - scheduler.run() - assert not scheduler.isScheduled(command) - DriverStationSim.setEnabled(True) - DriverStationSim.notifyNewData() - - -def test_runWhenDisabled(scheduler: commands2.CommandScheduler): - command = commands2.Command() - command.runsWhenDisabled = lambda: True - - scheduler.schedule(command) - - assert scheduler.isScheduled(command) - - DriverStationSim.setEnabled(False) - DriverStationSim.notifyNewData() - - scheduler.run() - - assert scheduler.isScheduled(command) - - -def test_sequentialGroupRunWhenDisabled(scheduler: commands2.CommandScheduler): - command1 = commands2.Command() - command1.runsWhenDisabled = lambda: True - command2 = commands2.Command() - command2.runsWhenDisabled = lambda: True - command3 = commands2.Command() - command3.runsWhenDisabled = lambda: True - command4 = commands2.Command() - command4.runsWhenDisabled = lambda: False - - runWhenDisabled = commands2.SequentialCommandGroup(command1, command2) - dontRunWhenDisabled = commands2.SequentialCommandGroup(command3, command4) - - scheduler.schedule(runWhenDisabled) - scheduler.schedule(dontRunWhenDisabled) - - DriverStationSim.setEnabled(False) - DriverStationSim.notifyNewData() - - scheduler.run() - - assert scheduler.isScheduled(runWhenDisabled) - assert not scheduler.isScheduled(dontRunWhenDisabled) - - -def test_parallelGroupRunWhenDisabled(scheduler: commands2.CommandScheduler): - command1 = commands2.Command() - command1.runsWhenDisabled = lambda: True - command2 = commands2.Command() - command2.runsWhenDisabled = lambda: True - command3 = commands2.Command() - command3.runsWhenDisabled = lambda: True - command4 = commands2.Command() - command4.runsWhenDisabled = lambda: False - - runWhenDisabled = commands2.ParallelCommandGroup(command1, command2) - dontRunWhenDisabled = commands2.ParallelCommandGroup(command3, command4) - - scheduler.schedule(runWhenDisabled) - scheduler.schedule(dontRunWhenDisabled) - - DriverStationSim.setEnabled(False) - DriverStationSim.notifyNewData() - - scheduler.run() - - assert scheduler.isScheduled(runWhenDisabled) - assert not scheduler.isScheduled(dontRunWhenDisabled) - - -def test_conditionalRunWhenDisabled(scheduler: commands2.CommandScheduler): - DriverStationSim.setEnabled(False) - DriverStationSim.notifyNewData() - - command1 = commands2.Command() - command1.runsWhenDisabled = lambda: True - command2 = commands2.Command() - command2.runsWhenDisabled = lambda: True - command3 = commands2.Command() - command3.runsWhenDisabled = lambda: True - command4 = commands2.Command() - command4.runsWhenDisabled = lambda: False - - runWhenDisabled = commands2.ConditionalCommand(command1, command2, lambda: True) - dontRunWhenDisabled = commands2.ConditionalCommand(command3, command4, lambda: True) - - scheduler.schedule(runWhenDisabled, dontRunWhenDisabled) - - assert scheduler.isScheduled(runWhenDisabled) - assert not scheduler.isScheduled(dontRunWhenDisabled) - - -def test_selectRunWhenDisabled(scheduler: commands2.CommandScheduler): - DriverStationSim.setEnabled(False) - DriverStationSim.notifyNewData() - - command1 = commands2.Command() - command1.runsWhenDisabled = lambda: True - command2 = commands2.Command() - command2.runsWhenDisabled = lambda: True - command3 = commands2.Command() - command3.runsWhenDisabled = lambda: True - command4 = commands2.Command() - command4.runsWhenDisabled = lambda: False - - runWhenDisabled = commands2.SelectCommand({1: command1, 2: command2}, lambda: 1) - dontRunWhenDisabled = commands2.SelectCommand({1: command3, 2: command4}, lambda: 1) - - scheduler.schedule(runWhenDisabled, dontRunWhenDisabled) - assert scheduler.isScheduled(runWhenDisabled) - assert not scheduler.isScheduled(dontRunWhenDisabled) - - -def test_parallelConditionalRunWhenDisabledTest(scheduler: commands2.CommandScheduler): - DriverStationSim.setEnabled(False) - DriverStationSim.notifyNewData() - - command1 = commands2.Command() - command1.runsWhenDisabled = lambda: True - command2 = commands2.Command() - command2.runsWhenDisabled = lambda: True - command3 = commands2.Command() - command3.runsWhenDisabled = lambda: True - command4 = commands2.Command() - command4.runsWhenDisabled = lambda: False - - runWhenDisabled = commands2.ConditionalCommand(command1, command2, lambda: True) - dontRunWhenDisabled = commands2.ConditionalCommand(command3, command4, lambda: True) - - parallel = commands2.cmd.parallel(runWhenDisabled, dontRunWhenDisabled) - - scheduler.schedule(parallel) - - assert not scheduler.isScheduled(runWhenDisabled) diff --git a/rio/ottomanEmpire/tests/test_runcommand.py b/rio/ottomanEmpire/tests/test_runcommand.py deleted file mode 100644 index 6bd4fcff..00000000 --- a/rio/ottomanEmpire/tests/test_runcommand.py +++ /dev/null @@ -1,22 +0,0 @@ -from typing import TYPE_CHECKING - -import commands2 -from util import * # type: ignore - -if TYPE_CHECKING: - from .util import * - -import pytest - - -def test_runCommandSchedule(scheduler: commands2.CommandScheduler): - counter = OOInteger(0) - - command = commands2.RunCommand(counter.incrementAndGet) - - scheduler.schedule(command) - scheduler.run() - scheduler.run() - scheduler.run() - - assert counter == 3 diff --git a/rio/ottomanEmpire/tests/test_schedulecommand.py b/rio/ottomanEmpire/tests/test_schedulecommand.py deleted file mode 100644 index 401f4e0d..00000000 --- a/rio/ottomanEmpire/tests/test_schedulecommand.py +++ /dev/null @@ -1,36 +0,0 @@ -from typing import TYPE_CHECKING - -import commands2 -from util import * # type: ignore - -if TYPE_CHECKING: - from .util import * - -import pytest - - -def test_scheduleCommandSchedule(scheduler: commands2.CommandScheduler): - command1 = commands2.Command() - command2 = commands2.Command() - - start_spying_on(command1) - start_spying_on(command2) - - scheduleCommand = commands2.ScheduleCommand(command1, command2) - - scheduler.schedule(scheduleCommand) - - verify(command1).schedule() - verify(command2).schedule() - - -def test_scheduleCommandDruingRun(scheduler: commands2.CommandScheduler): - toSchedule = commands2.InstantCommand() - scheduleCommand = commands2.ScheduleCommand(toSchedule) - group = commands2.SequentialCommandGroup( - commands2.InstantCommand(), scheduleCommand - ) - - scheduler.schedule(group) - scheduler.schedule(commands2.RunCommand(lambda: None)) - scheduler.run() diff --git a/rio/ottomanEmpire/tests/test_scheduler.py b/rio/ottomanEmpire/tests/test_scheduler.py deleted file mode 100644 index 4ee7f32c..00000000 --- a/rio/ottomanEmpire/tests/test_scheduler.py +++ /dev/null @@ -1,67 +0,0 @@ -from typing import TYPE_CHECKING - -import commands2 -from util import * # type: ignore - -if TYPE_CHECKING: - from .util import * - -import pytest - - -def test_schedulerLambdaTestNoInterrupt(scheduler: commands2.CommandScheduler): - counter = OOInteger() - - scheduler.onCommandInitialize(lambda _: counter.incrementAndGet()) - scheduler.onCommandExecute(lambda _: counter.incrementAndGet()) - scheduler.onCommandFinish(lambda _: counter.incrementAndGet()) - - scheduler.schedule(commands2.InstantCommand()) - scheduler.run() - - assert counter == 3 - - -def test_schedulerInterruptLambda(scheduler: commands2.CommandScheduler): - counter = OOInteger() - - scheduler.onCommandInterrupt(lambda _: counter.incrementAndGet()) - - command = commands2.WaitCommand(10) - - scheduler.schedule(command) - scheduler.cancel(command) - - assert counter == 1 - - -def test_unregisterSubsystem(scheduler: commands2.CommandScheduler): - system = commands2.Subsystem() - scheduler.registerSubsystem(system) - scheduler.unregisterSubsystem(system) - - -def test_schedulerCancelAll(scheduler: commands2.CommandScheduler): - counter = OOInteger() - - scheduler.onCommandInterrupt(lambda _: counter.incrementAndGet()) - - command = commands2.WaitCommand(10) - command2 = commands2.WaitCommand(10) - - scheduler.schedule(command) - scheduler.schedule(command2) - scheduler.cancelAll() - - assert counter == 2 - - -def test_scheduleScheduledNoOp(scheduler: commands2.CommandScheduler): - counter = OOInteger() - - command = commands2.cmd.startEnd(counter.incrementAndGet, lambda: None) - - scheduler.schedule(command) - scheduler.schedule(command) - - assert counter == 1 diff --git a/rio/ottomanEmpire/tests/test_schedulingrecursion.py b/rio/ottomanEmpire/tests/test_schedulingrecursion.py deleted file mode 100644 index ff6230dc..00000000 --- a/rio/ottomanEmpire/tests/test_schedulingrecursion.py +++ /dev/null @@ -1,165 +0,0 @@ -from typing import TYPE_CHECKING - -import commands2 -from util import * # type: ignore - -if TYPE_CHECKING: - from .util import * - -import pytest - - -@pytest.mark.parametrize( - "interruptionBehavior", - [ - commands2.InterruptionBehavior.kCancelIncoming, - commands2.InterruptionBehavior.kCancelSelf, - ], -) -def test_cancelFromInitialize( - interruptionBehavior: commands2.InterruptionBehavior, - scheduler: commands2.CommandScheduler, -): - hasOtherRun = OOBoolean() - requirement = commands2.Subsystem() - - selfCancels = commands2.Command() - selfCancels.addRequirements(requirement) - selfCancels.getInterruptionBehavior = lambda: interruptionBehavior - selfCancels.initialize = lambda: scheduler.cancel(selfCancels) - - other = commands2.RunCommand(lambda: hasOtherRun.set(True), requirement) - - scheduler.schedule(selfCancels) - scheduler.run() - scheduler.schedule(other) - - assert not scheduler.isScheduled(selfCancels) - assert scheduler.isScheduled(other) - scheduler.run() - assert hasOtherRun == True - - -@pytest.mark.parametrize( - "interruptionBehavior", - [ - commands2.InterruptionBehavior.kCancelIncoming, - commands2.InterruptionBehavior.kCancelSelf, - ], -) -def test_defaultCommandGetsRescheduledAfterSelfCanceling( - interruptionBehavior: commands2.InterruptionBehavior, - scheduler: commands2.CommandScheduler, -): - hasOtherRun = OOBoolean() - requirement = commands2.Subsystem() - - selfCancels = commands2.Command() - selfCancels.addRequirements(requirement) - selfCancels.getInterruptionBehavior = lambda: interruptionBehavior - selfCancels.initialize = lambda: scheduler.cancel(selfCancels) - - other = commands2.RunCommand(lambda: hasOtherRun.set(True), requirement) - scheduler.setDefaultCommand(requirement, other) - - scheduler.schedule(selfCancels) - scheduler.run() - - scheduler.run() - assert not scheduler.isScheduled(selfCancels) - assert scheduler.isScheduled(other) - scheduler.run() - assert hasOtherRun == True - - -def test_cancelFromEnd(scheduler: commands2.CommandScheduler): - counter = OOInteger() - selfCancels = commands2.Command() - - @patch_via_decorator(selfCancels) - def end(self, interrupted): - counter.incrementAndGet() - scheduler.cancel(self) - - scheduler.schedule(selfCancels) - - scheduler.cancel(selfCancels) - assert counter == 1 - assert not scheduler.isScheduled(selfCancels) - - -def test_scheduleFromEnd(scheduler: commands2.CommandScheduler): - counter = OOInteger() - requirement = commands2.Subsystem() - other = commands2.InstantCommand(lambda: None, requirement) - - selfCancels = commands2.Command() - selfCancels.addRequirements(requirement) - - @patch_via_decorator(selfCancels) - def end(self, interrupted): - counter.incrementAndGet() - scheduler.schedule(other) - - scheduler.schedule(selfCancels) - - scheduler.cancel(selfCancels) - assert counter == 1 - assert not scheduler.isScheduled(selfCancels) - - -def test_scheduleFromEndInterrupt(scheduler: commands2.CommandScheduler): - counter = OOInteger() - requirement = commands2.Subsystem() - other = commands2.InstantCommand(lambda: None, requirement) - - selfCancels = commands2.Command() - selfCancels.addRequirements(requirement) - - @patch_via_decorator(selfCancels) - def end(self, interrupted): - counter.incrementAndGet() - scheduler.schedule(other) - - scheduler.schedule(selfCancels) - - scheduler.schedule(other) - assert counter == 1 - assert not scheduler.isScheduled(selfCancels) - assert scheduler.isScheduled(other) - - -@pytest.mark.parametrize( - "interruptionBehavior", - [ - commands2.InterruptionBehavior.kCancelIncoming, - commands2.InterruptionBehavior.kCancelSelf, - ], -) -def test_scheduleInitializeFromDefaultCommand( - interruptionBehavior: commands2.InterruptionBehavior, - scheduler: commands2.CommandScheduler, -): - counter = OOInteger() - requirement = commands2.Subsystem() - other = commands2.InstantCommand(lambda: None, requirement).withInterruptBehavior( - interruptionBehavior - ) - - defaultCommand = commands2.Command() - defaultCommand.addRequirements(requirement) - - @patch_via_decorator(defaultCommand) - def initialize(self): - counter.incrementAndGet() - scheduler.schedule(other) - - scheduler.setDefaultCommand(requirement, defaultCommand) - - scheduler.run() - scheduler.run() - scheduler.run() - - assert counter == 3 - assert not scheduler.isScheduled(defaultCommand) - assert scheduler.isScheduled(other) diff --git a/rio/ottomanEmpire/tests/test_selectcommand.py b/rio/ottomanEmpire/tests/test_selectcommand.py deleted file mode 100644 index 3ceb2a72..00000000 --- a/rio/ottomanEmpire/tests/test_selectcommand.py +++ /dev/null @@ -1,94 +0,0 @@ -from typing import TYPE_CHECKING - -import commands2 -from compositiontestbase import MultiCompositionTestBase # type: ignore -from util import * # type: ignore - -if TYPE_CHECKING: - from .util import * - from .compositiontestbase import MultiCompositionTestBase - -import pytest - - -class TestSelectCommandComposition(MultiCompositionTestBase): - def compose(self, *members: commands2.Command): - return commands2.SelectCommand(dict(enumerate(members)), lambda: 0) - - -def test_selectCommand(scheduler: commands2.CommandScheduler): - command1 = commands2.Command() - command2 = commands2.Command() - command3 = commands2.Command() - - start_spying_on(command1) - start_spying_on(command2) - start_spying_on(command3) - - command1.isFinished = lambda: True - - selectCommand = commands2.SelectCommand( - {"one": command1, "two": command2, "three": command3}, lambda: "one" - ) - - scheduler.schedule(selectCommand) - scheduler.run() - - verify(command1).initialize() - verify(command1).execute() - verify(command1).end(False) - - verify(command2, never()).initialize() - verify(command2, never()).execute() - verify(command2, never()).end(False) - - verify(command3, never()).initialize() - verify(command3, never()).execute() - verify(command3, never()).end(False) - - -def test_selectCommandInvalidKey(scheduler: commands2.CommandScheduler): - command1 = commands2.Command() - command2 = commands2.Command() - command3 = commands2.Command() - - start_spying_on(command1) - start_spying_on(command2) - start_spying_on(command3) - - command1.isFinished = lambda: True - - selectCommand = commands2.SelectCommand( - {"one": command1, "two": command2, "three": command3}, lambda: "four" - ) - - scheduler.schedule(selectCommand) - - -def test_selectCommandRequirement(scheduler: commands2.CommandScheduler): - system1 = commands2.Subsystem() - system2 = commands2.Subsystem() - system3 = commands2.Subsystem() - system4 = commands2.Subsystem() - - command1 = commands2.Command() - command1.addRequirements(system1, system2) - command2 = commands2.Command() - command2.addRequirements(system3) - command3 = commands2.Command() - command3.addRequirements(system3, system4) - - start_spying_on(command1) - start_spying_on(command2) - start_spying_on(command3) - - selectCommand = commands2.SelectCommand( - {"one": command1, "two": command2, "three": command3}, lambda: "one" - ) - - scheduler.schedule(selectCommand) - scheduler.schedule(commands2.InstantCommand(lambda: None, system3)) - - verify(command1).end(interrupted=True) - verify(command2, never()).end(interrupted=True) - verify(command3, never()).end(interrupted=True) diff --git a/rio/ottomanEmpire/tests/test_sequentialcommandgroup.py b/rio/ottomanEmpire/tests/test_sequentialcommandgroup.py deleted file mode 100644 index a6ecfb96..00000000 --- a/rio/ottomanEmpire/tests/test_sequentialcommandgroup.py +++ /dev/null @@ -1,114 +0,0 @@ -from typing import TYPE_CHECKING - -import commands2 -from compositiontestbase import MultiCompositionTestBase # type: ignore -from util import * # type: ignore - -if TYPE_CHECKING: - from .util import * - from .compositiontestbase import MultiCompositionTestBase - -import pytest - - -class TestSequentialCommandGroupComposition(MultiCompositionTestBase): - def compose(self, *members: commands2.Command): - return commands2.SequentialCommandGroup(*members) - - -def test_sequntialGroupSchedule(scheduler: commands2.CommandScheduler): - command1 = commands2.Command() - command2 = commands2.Command() - - start_spying_on(command1) - start_spying_on(command2) - - group = commands2.SequentialCommandGroup(command1, command2) - - scheduler.schedule(group) - - verify(command1).initialize() - verify(command2, never()).initialize() - - command1.isFinished = lambda: True - scheduler.run() - - verify(command1).execute() - verify(command1).end(False) - verify(command2).initialize() - verify(command2, never()).execute() - verify(command2, never()).end(False) - - command2.isFinished = lambda: True - scheduler.run() - - verify(command1).execute() - verify(command1).end(False) - verify(command2).execute() - verify(command2).end(False) - - assert not scheduler.isScheduled(group) - - -def test_sequentialGroupInterrupt(scheduler: commands2.CommandScheduler): - command1 = commands2.Command() - command2 = commands2.Command() - command3 = commands2.Command() - - start_spying_on(command1) - start_spying_on(command2) - start_spying_on(command3) - - group = commands2.SequentialCommandGroup(command1, command2, command3) - - scheduler.schedule(group) - - command1.isFinished = lambda: True - scheduler.run() - scheduler.cancel(group) - scheduler.run() - - verify(command1).execute() - verify(command1, never()).end(True) - verify(command1).end(False) - verify(command2, never()).execute() - verify(command2).end(True) - verify(command3, never()).initialize() - verify(command3, never()).execute() - - # assert command3.end.times_called == 0 - verify(command3, never()).end(True) - verify(command3, never()).end(False) - - assert not scheduler.isScheduled(group) - - -def test_notScheduledCancel(scheduler: commands2.CommandScheduler): - command1 = commands2.Command() - command2 = commands2.Command() - - group = commands2.SequentialCommandGroup(command1, command2) - - scheduler.cancel(group) - - -def test_sequentialGroupRequirement(scheduler: commands2.CommandScheduler): - system1 = commands2.Subsystem() - system2 = commands2.Subsystem() - system3 = commands2.Subsystem() - system4 = commands2.Subsystem() - - command1 = commands2.InstantCommand() - command1.addRequirements(system1, system2) - command2 = commands2.InstantCommand() - command2.addRequirements(system3) - command3 = commands2.InstantCommand() - command3.addRequirements(system3, system4) - - group = commands2.SequentialCommandGroup(command1, command2) - - scheduler.schedule(group) - scheduler.schedule(command3) - - assert not scheduler.isScheduled(group) - assert scheduler.isScheduled(command3) diff --git a/rio/ottomanEmpire/tests/test_startendcommand.py b/rio/ottomanEmpire/tests/test_startendcommand.py deleted file mode 100644 index 700412f7..00000000 --- a/rio/ottomanEmpire/tests/test_startendcommand.py +++ /dev/null @@ -1,30 +0,0 @@ -from typing import TYPE_CHECKING - -import commands2 -from util import * # type: ignore - -if TYPE_CHECKING: - from .util import * - -import pytest - - -def test_startEndCommandSchedule(scheduler: commands2.CommandScheduler): - cond1 = OOBoolean(False) - cond2 = OOBoolean(False) - - command = commands2.StartEndCommand( - lambda: cond1.set(True), - lambda: cond2.set(True), - ) - - scheduler.schedule(command) - scheduler.run() - - assert scheduler.isScheduled(command) - - scheduler.cancel(command) - - assert not scheduler.isScheduled(command) - assert cond1 == True - assert cond2 == True diff --git a/rio/ottomanEmpire/tests/test_swervecontrollercommand.py b/rio/ottomanEmpire/tests/test_swervecontrollercommand.py deleted file mode 100644 index b94a47f7..00000000 --- a/rio/ottomanEmpire/tests/test_swervecontrollercommand.py +++ /dev/null @@ -1,1304 +0,0 @@ -# Copyright (c) FIRST and other WPILib contributors. -# Open Source Software; you can modify and/or share it under the terms of -# the WPILib BSD license file in the root directory of this project. - -from typing import TYPE_CHECKING, List, Tuple -import math - -import wpimath.controller as controller -import wpimath.trajectory as trajectory -import wpimath.geometry as geometry -import wpimath.kinematics as kinematics -from wpilib import Timer - -from util import * # type: ignore - -if TYPE_CHECKING: - from .util import * - -import pytest - -import commands2 - -TWO = int(2) -THREE = int(3) -FOUR = int(4) -SIX = int(6) -MISMATCHED_KINEMATICS_AND_ODOMETRY = int(101) -INCOMPLETE_PID_CLASSES = int(102) - - -class SwerveControllerCommandTestDataFixtures: - def __init__(self, selector: int): - self._timer = Timer() - self._angle: geometry.Rotation2d = geometry.Rotation2d(0) - - self._kxTolerance = 1 / 12.0 - self._kyTolerance = 1 / 12.0 - self._kAngularTolerance = 1 / 12.0 - self._kWheelBase = 0.5 - self._kTrackWidth = 0.5 - - # The module positions and states start empty and will be populated below in the selector - # self._modulePositions: Tuple[kinematics.SwerveModulePosition] = [] - self._modulePositions: Tuple[kinematics.SwerveModulePosition] = [] - self._moduleStates: Tuple[kinematics.SwerveModuleState] = [] - - # Setup PID controllers, but if an error test case is requested, make sure it provides - # data that should break the command instantiation - if selector != INCOMPLETE_PID_CLASSES: - self._xController = controller.PIDController(0.6, 0, 0) - self._yController = controller.PIDController(0.6, 0, 0) - constraints = trajectory.TrapezoidProfileRadians.Constraints( - 3 * math.pi, math.pi - ) - self._rotationController = controller.ProfiledPIDControllerRadians( - 1, 0, 0, constraints - ) - - self._holonomic = controller.HolonomicDriveController( - self._xController, self._yController, self._rotationController - ) - else: - self._xController = None - self._yController = controller.PIDController(0.6, 0, 0) - constraints = trajectory.TrapezoidProfileRadians.Constraints( - 3 * math.pi, math.pi - ) - self._rotationController = controller.ProfiledPIDControllerRadians( - 1, 0, 0, constraints - ) - self._holonomic = None - - if (selector == TWO) or (selector == INCOMPLETE_PID_CLASSES): - self._kinematics = kinematics.SwerveDrive2Kinematics( - geometry.Translation2d(self._kWheelBase / 2, self._kTrackWidth / 2), - geometry.Translation2d(self._kWheelBase / 2, -self._kTrackWidth / 2), - ) - - self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) - self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) - - self._modulePositions.append( - kinematics.SwerveModulePosition(0, self._angle) - ) - self._modulePositions.append( - kinematics.SwerveModulePosition(0, self._angle) - ) - - self._odometry = kinematics.SwerveDrive2Odometry( - self._kinematics, - self._angle, - self._modulePositions, - geometry.Pose2d(0, 0, geometry.Rotation2d(0)), - ) - elif selector == THREE: - self._kinematics = kinematics.SwerveDrive3Kinematics( - geometry.Translation2d(self._kWheelBase / 2, self._kTrackWidth / 2), - geometry.Translation2d(self._kWheelBase / 2, -self._kTrackWidth / 2), - geometry.Translation2d(self._kWheelBase / 2, self._kTrackWidth / 2), - ) - - self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) - self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) - self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) - - self._modulePositions.append( - kinematics.SwerveModulePosition(0, self._angle) - ) - self._modulePositions.append( - kinematics.SwerveModulePosition(0, self._angle) - ) - self._modulePositions.append( - kinematics.SwerveModulePosition(0, self._angle) - ) - - self._odometry = kinematics.SwerveDrive3Odometry( - self._kinematics, - self._angle, - self._modulePositions, - geometry.Pose2d(0, 0, geometry.Rotation2d(0)), - ) - elif selector == FOUR: - self._kinematics = kinematics.SwerveDrive4Kinematics( - geometry.Translation2d(self._kWheelBase / 2, self._kTrackWidth / 2), - geometry.Translation2d(self._kWheelBase / 2, -self._kTrackWidth / 2), - geometry.Translation2d(self._kWheelBase / 2, self._kTrackWidth / 2), - geometry.Translation2d(self._kWheelBase / 2, -self._kTrackWidth / 2), - ) - - self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) - self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) - self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) - self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) - - self._modulePositions.append( - kinematics.SwerveModulePosition(0, self._angle) - ) - self._modulePositions.append( - kinematics.SwerveModulePosition(0, self._angle) - ) - self._modulePositions.append( - kinematics.SwerveModulePosition(0, self._angle) - ) - self._modulePositions.append( - kinematics.SwerveModulePosition(0, self._angle) - ) - - self._odometry = kinematics.SwerveDrive4Odometry( - self._kinematics, - self._angle, - self._modulePositions, - geometry.Pose2d(0, 0, geometry.Rotation2d(0)), - ) - elif selector == SIX: - self._kinematics = kinematics.SwerveDrive6Kinematics( - geometry.Translation2d(self._kWheelBase / 2, self._kTrackWidth / 2), - geometry.Translation2d(self._kWheelBase / 2, -self._kTrackWidth / 2), - geometry.Translation2d(self._kWheelBase / 2, self._kTrackWidth / 2), - geometry.Translation2d(self._kWheelBase / 2, -self._kTrackWidth / 2), - geometry.Translation2d(self._kWheelBase / 2, self._kTrackWidth / 2), - geometry.Translation2d(self._kWheelBase / 2, -self._kTrackWidth / 2), - ) - - self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) - self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) - self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) - self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) - self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) - self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) - - self._modulePositions.append( - kinematics.SwerveModulePosition(0, self._angle) - ) - self._modulePositions.append( - kinematics.SwerveModulePosition(0, self._angle) - ) - self._modulePositions.append( - kinematics.SwerveModulePosition(0, self._angle) - ) - self._modulePositions.append( - kinematics.SwerveModulePosition(0, self._angle) - ) - self._modulePositions.append( - kinematics.SwerveModulePosition(0, self._angle) - ) - self._modulePositions.append( - kinematics.SwerveModulePosition(0, self._angle) - ) - - self._odometry = kinematics.SwerveDrive6Odometry( - self._kinematics, - self._angle, - self._modulePositions, - geometry.Pose2d(0, 0, geometry.Rotation2d(0)), - ) - elif selector == MISMATCHED_KINEMATICS_AND_ODOMETRY: - self._kinematics = kinematics.SwerveDrive2Kinematics( - geometry.Translation2d(self._kWheelBase / 2, self._kTrackWidth / 2), - geometry.Translation2d(self._kWheelBase / 2, -self._kTrackWidth / 2), - ) - - self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) - self._moduleStates.append(kinematics.SwerveModuleState(0, self._angle)) - - self._modulePositions.append( - kinematics.SwerveModulePosition(0, self._angle) - ) - self._modulePositions.append( - kinematics.SwerveModulePosition(0, self._angle) - ) - - self._odometry = kinematics.SwerveDrive6Odometry( - self._kinematics, - self._angle, - self._modulePositions, - geometry.Pose2d(0, 0, geometry.Rotation2d(0)), - ) - - def setModuleStates(self, states: List[kinematics.SwerveModuleState]) -> None: - self._moduleStates = states - - def getRobotPose(self) -> geometry.Pose2d: - self._odometry.update(self._angle, self._modulePositions) - - return self._odometry.getPose() - - def getRotationHeadingZero(self) -> geometry.Rotation2d: - return geometry.Rotation2d() - - -@pytest.fixture() -def get_swerve_controller_data() -> SwerveControllerCommandTestDataFixtures: - def _method(selector): - return SwerveControllerCommandTestDataFixtures(selector) - - return _method - - -def test_SwerveControllerMismatchedKinematicsAndOdometry( - scheduler: commands2.CommandScheduler, get_swerve_controller_data -): - with ManualSimTime() as sim: - subsystem = commands2.Subsystem() - waypoints: List[geometry.Pose2d] = [] - waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) - waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) - traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) - new_trajectory: trajectory.Trajectory = ( - trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) - ) - end_state = new_trajectory.sample(new_trajectory.totalTime()) - - with pytest.raises(TypeError): - fixture_data: SwerveControllerCommandTestDataFixtures = ( - get_swerve_controller_data(MISMATCHED_KINEMATICS_AND_ODOMETRY) - ) - - -def test_SwerveControllerIncompletePID( - scheduler: commands2.CommandScheduler, get_swerve_controller_data -): - with ManualSimTime() as sim: - subsystem = commands2.Subsystem() - waypoints: List[geometry.Pose2d] = [] - waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) - waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) - traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) - new_trajectory: trajectory.Trajectory = ( - trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) - ) - end_state = new_trajectory.sample(new_trajectory.totalTime()) - - with pytest.raises(Exception): - fixture_data: SwerveControllerCommandTestDataFixtures = ( - get_swerve_controller_data(INCOMPLETE_PID_CLASSES) - ) - - command = commands2.SwerveControllerCommand( - new_trajectory, - fixture_data.getRobotPose, - fixture_data._kinematics, - fixture_data.setModuleStates, - fixture_data._xController, - fixture_data._yController, - fixture_data._rotationController, - fixture_data.getRotationHeadingZero, - subsystem, - ) - - -def test_SwerveControllerCommand2Holonomic( - scheduler: commands2.CommandScheduler, get_swerve_controller_data -): - with ManualSimTime() as sim: - subsystem = commands2.Subsystem() - waypoints: List[geometry.Pose2d] = [] - waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) - waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) - traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) - new_trajectory: trajectory.Trajectory = ( - trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) - ) - end_state = new_trajectory.sample(new_trajectory.totalTime()) - - fixture_data: SwerveControllerCommandTestDataFixtures = ( - get_swerve_controller_data(TWO) - ) - - command = commands2.SwerveControllerCommand( - new_trajectory, - fixture_data.getRobotPose, - fixture_data._kinematics, - fixture_data._holonomic, - fixture_data.setModuleStates, - subsystem, - ) - - fixture_data._timer.restart() - - command.initialize() - - while not command.isFinished(): - command.execute() - fixture_data._angle = new_trajectory.sample( - fixture_data._timer.get() - ).pose.rotation() - - for i in range(0, len(fixture_data._modulePositions)): - fixture_data._modulePositions[i].distance += ( - fixture_data._moduleStates[i].speed * 0.005 - ) - fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ - i - ].angle - - sim.step(0.005) - - fixture_data._timer.stop() - command.end(True) - - assert end_state.pose.X() == pytest.approx( - fixture_data.getRobotPose().X(), fixture_data._kxTolerance - ) - assert end_state.pose.Y() == pytest.approx( - fixture_data.getRobotPose().Y(), fixture_data._kyTolerance - ) - assert end_state.pose.rotation().radians() == pytest.approx( - fixture_data.getRobotPose().rotation().radians(), - fixture_data._kAngularTolerance, - ) - - -def test_SwerveControllerCommand2HolonomicWithRotation( - scheduler: commands2.CommandScheduler, get_swerve_controller_data -): - with ManualSimTime() as sim: - subsystem = commands2.Subsystem() - waypoints: List[geometry.Pose2d] = [] - waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) - waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) - traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) - new_trajectory: trajectory.Trajectory = ( - trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) - ) - end_state = new_trajectory.sample(new_trajectory.totalTime()) - - fixture_data: SwerveControllerCommandTestDataFixtures = ( - get_swerve_controller_data(TWO) - ) - - command = commands2.SwerveControllerCommand( - new_trajectory, - fixture_data.getRobotPose, - fixture_data._kinematics, - fixture_data._holonomic, - fixture_data.getRotationHeadingZero, - fixture_data.setModuleStates, - subsystem, - ) - - fixture_data._timer.restart() - - command.initialize() - - while not command.isFinished(): - command.execute() - fixture_data._angle = new_trajectory.sample( - fixture_data._timer.get() - ).pose.rotation() - - for i in range(0, len(fixture_data._modulePositions)): - fixture_data._modulePositions[i].distance += ( - fixture_data._moduleStates[i].speed * 0.005 - ) - fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ - i - ].angle - - sim.step(0.005) - - fixture_data._timer.stop() - command.end(True) - - assert end_state.pose.X() == pytest.approx( - fixture_data.getRobotPose().X(), fixture_data._kxTolerance - ) - assert end_state.pose.Y() == pytest.approx( - fixture_data.getRobotPose().Y(), fixture_data._kyTolerance - ) - assert end_state.pose.rotation().radians() == pytest.approx( - fixture_data.getRobotPose().rotation().radians(), - fixture_data._kAngularTolerance, - ) - - -def test_SwerveControllerCommand2PID( - scheduler: commands2.CommandScheduler, get_swerve_controller_data -): - with ManualSimTime() as sim: - subsystem = commands2.Subsystem() - waypoints: List[geometry.Pose2d] = [] - waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) - waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) - traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) - new_trajectory: trajectory.Trajectory = ( - trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) - ) - end_state = new_trajectory.sample(new_trajectory.totalTime()) - - fixture_data: SwerveControllerCommandTestDataFixtures = ( - get_swerve_controller_data(TWO) - ) - - command = commands2.SwerveControllerCommand( - new_trajectory, - fixture_data.getRobotPose, - fixture_data._kinematics, - fixture_data._xController, - fixture_data._yController, - fixture_data._rotationController, - fixture_data.setModuleStates, - subsystem, - ) - - fixture_data._timer.restart() - - command.initialize() - - while not command.isFinished(): - command.execute() - fixture_data._angle = new_trajectory.sample( - fixture_data._timer.get() - ).pose.rotation() - - for i in range(0, len(fixture_data._modulePositions)): - fixture_data._modulePositions[i].distance += ( - fixture_data._moduleStates[i].speed * 0.005 - ) - fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ - i - ].angle - - sim.step(0.005) - - fixture_data._timer.stop() - command.end(True) - - assert end_state.pose.X() == pytest.approx( - fixture_data.getRobotPose().X(), fixture_data._kxTolerance - ) - assert end_state.pose.Y() == pytest.approx( - fixture_data.getRobotPose().Y(), fixture_data._kyTolerance - ) - assert end_state.pose.rotation().radians() == pytest.approx( - fixture_data.getRobotPose().rotation().radians(), - fixture_data._kAngularTolerance, - ) - - -def test_SwerveControllerCommand2PIDWithRotation( - scheduler: commands2.CommandScheduler, get_swerve_controller_data -): - with ManualSimTime() as sim: - subsystem = commands2.Subsystem() - waypoints: List[geometry.Pose2d] = [] - waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) - waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) - traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) - new_trajectory: trajectory.Trajectory = ( - trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) - ) - end_state = new_trajectory.sample(new_trajectory.totalTime()) - - fixture_data: SwerveControllerCommandTestDataFixtures = ( - get_swerve_controller_data(TWO) - ) - - command = commands2.SwerveControllerCommand( - new_trajectory, - fixture_data.getRobotPose, - fixture_data._kinematics, - fixture_data._xController, - fixture_data._yController, - fixture_data._rotationController, - fixture_data.getRotationHeadingZero, - fixture_data.setModuleStates, - subsystem, - ) - - fixture_data._timer.restart() - - command.initialize() - - while not command.isFinished(): - command.execute() - fixture_data._angle = new_trajectory.sample( - fixture_data._timer.get() - ).pose.rotation() - - for i in range(0, len(fixture_data._modulePositions)): - fixture_data._modulePositions[i].distance += ( - fixture_data._moduleStates[i].speed * 0.005 - ) - fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ - i - ].angle - - sim.step(0.005) - - fixture_data._timer.stop() - command.end(True) - - assert end_state.pose.X() == pytest.approx( - fixture_data.getRobotPose().X(), fixture_data._kxTolerance - ) - assert end_state.pose.Y() == pytest.approx( - fixture_data.getRobotPose().Y(), fixture_data._kyTolerance - ) - assert end_state.pose.rotation().radians() == pytest.approx( - fixture_data.getRobotPose().rotation().radians(), - fixture_data._kAngularTolerance, - ) - - -def test_SwerveControllerCommand3Holonomic( - scheduler: commands2.CommandScheduler, get_swerve_controller_data -): - with ManualSimTime() as sim: - subsystem = commands2.Subsystem() - waypoints: List[geometry.Pose2d] = [] - waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) - waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) - traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) - new_trajectory: trajectory.Trajectory = ( - trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) - ) - end_state = new_trajectory.sample(new_trajectory.totalTime()) - - fixture_data: SwerveControllerCommandTestDataFixtures = ( - get_swerve_controller_data(THREE) - ) - - command = commands2.SwerveControllerCommand( - new_trajectory, - fixture_data.getRobotPose, - fixture_data._kinematics, - fixture_data._holonomic, - fixture_data.setModuleStates, - subsystem, - ) - - fixture_data._timer.restart() - - command.initialize() - - while not command.isFinished(): - command.execute() - fixture_data._angle = new_trajectory.sample( - fixture_data._timer.get() - ).pose.rotation() - - for i in range(0, len(fixture_data._modulePositions)): - fixture_data._modulePositions[i].distance += ( - fixture_data._moduleStates[i].speed * 0.005 - ) - fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ - i - ].angle - - sim.step(0.005) - - fixture_data._timer.stop() - command.end(True) - - assert end_state.pose.X() == pytest.approx( - fixture_data.getRobotPose().X(), fixture_data._kxTolerance - ) - assert end_state.pose.Y() == pytest.approx( - fixture_data.getRobotPose().Y(), fixture_data._kyTolerance - ) - assert end_state.pose.rotation().radians() == pytest.approx( - fixture_data.getRobotPose().rotation().radians(), - fixture_data._kAngularTolerance, - ) - - -def test_SwerveControllerCommand3HolonomicWithRotation( - scheduler: commands2.CommandScheduler, get_swerve_controller_data -): - with ManualSimTime() as sim: - subsystem = commands2.Subsystem() - waypoints: List[geometry.Pose2d] = [] - waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) - waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) - traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) - new_trajectory: trajectory.Trajectory = ( - trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) - ) - end_state = new_trajectory.sample(new_trajectory.totalTime()) - - fixture_data: SwerveControllerCommandTestDataFixtures = ( - get_swerve_controller_data(THREE) - ) - - command = commands2.SwerveControllerCommand( - new_trajectory, - fixture_data.getRobotPose, - fixture_data._kinematics, - fixture_data._holonomic, - fixture_data.getRotationHeadingZero, - fixture_data.setModuleStates, - subsystem, - ) - - fixture_data._timer.restart() - - command.initialize() - - while not command.isFinished(): - command.execute() - fixture_data._angle = new_trajectory.sample( - fixture_data._timer.get() - ).pose.rotation() - - for i in range(0, len(fixture_data._modulePositions)): - fixture_data._modulePositions[i].distance += ( - fixture_data._moduleStates[i].speed * 0.005 - ) - fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ - i - ].angle - - sim.step(0.005) - - fixture_data._timer.stop() - command.end(True) - - assert end_state.pose.X() == pytest.approx( - fixture_data.getRobotPose().X(), fixture_data._kxTolerance - ) - assert end_state.pose.Y() == pytest.approx( - fixture_data.getRobotPose().Y(), fixture_data._kyTolerance - ) - assert end_state.pose.rotation().radians() == pytest.approx( - fixture_data.getRobotPose().rotation().radians(), - fixture_data._kAngularTolerance, - ) - - -def test_SwerveControllerCommand3PID( - scheduler: commands2.CommandScheduler, get_swerve_controller_data -): - with ManualSimTime() as sim: - subsystem = commands2.Subsystem() - waypoints: List[geometry.Pose2d] = [] - waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) - waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) - traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) - new_trajectory: trajectory.Trajectory = ( - trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) - ) - end_state = new_trajectory.sample(new_trajectory.totalTime()) - - fixture_data: SwerveControllerCommandTestDataFixtures = ( - get_swerve_controller_data(THREE) - ) - - command = commands2.SwerveControllerCommand( - new_trajectory, - fixture_data.getRobotPose, - fixture_data._kinematics, - fixture_data._xController, - fixture_data._yController, - fixture_data._rotationController, - fixture_data.setModuleStates, - subsystem, - ) - - fixture_data._timer.restart() - - command.initialize() - - while not command.isFinished(): - command.execute() - fixture_data._angle = new_trajectory.sample( - fixture_data._timer.get() - ).pose.rotation() - - for i in range(0, len(fixture_data._modulePositions)): - fixture_data._modulePositions[i].distance += ( - fixture_data._moduleStates[i].speed * 0.005 - ) - fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ - i - ].angle - - sim.step(0.005) - - fixture_data._timer.stop() - command.end(True) - - assert end_state.pose.X() == pytest.approx( - fixture_data.getRobotPose().X(), fixture_data._kxTolerance - ) - assert end_state.pose.Y() == pytest.approx( - fixture_data.getRobotPose().Y(), fixture_data._kyTolerance - ) - assert end_state.pose.rotation().radians() == pytest.approx( - fixture_data.getRobotPose().rotation().radians(), - fixture_data._kAngularTolerance, - ) - - -def test_SwerveControllerCommand3PIDWithRotation( - scheduler: commands2.CommandScheduler, get_swerve_controller_data -): - with ManualSimTime() as sim: - subsystem = commands2.Subsystem() - waypoints: List[geometry.Pose2d] = [] - waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) - waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) - traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) - new_trajectory: trajectory.Trajectory = ( - trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) - ) - end_state = new_trajectory.sample(new_trajectory.totalTime()) - - fixture_data: SwerveControllerCommandTestDataFixtures = ( - get_swerve_controller_data(THREE) - ) - - command = commands2.SwerveControllerCommand( - new_trajectory, - fixture_data.getRobotPose, - fixture_data._kinematics, - fixture_data._xController, - fixture_data._yController, - fixture_data._rotationController, - fixture_data.getRotationHeadingZero, - fixture_data.setModuleStates, - subsystem, - ) - - fixture_data._timer.restart() - - command.initialize() - - while not command.isFinished(): - command.execute() - fixture_data._angle = new_trajectory.sample( - fixture_data._timer.get() - ).pose.rotation() - - for i in range(0, len(fixture_data._modulePositions)): - fixture_data._modulePositions[i].distance += ( - fixture_data._moduleStates[i].speed * 0.005 - ) - fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ - i - ].angle - - sim.step(0.005) - - fixture_data._timer.stop() - command.end(True) - - assert end_state.pose.X() == pytest.approx( - fixture_data.getRobotPose().X(), fixture_data._kxTolerance - ) - assert end_state.pose.Y() == pytest.approx( - fixture_data.getRobotPose().Y(), fixture_data._kyTolerance - ) - assert end_state.pose.rotation().radians() == pytest.approx( - fixture_data.getRobotPose().rotation().radians(), - fixture_data._kAngularTolerance, - ) - - -def test_SwerveControllerCommand4Holonomic( - scheduler: commands2.CommandScheduler, get_swerve_controller_data -): - with ManualSimTime() as sim: - subsystem = commands2.Subsystem() - waypoints: List[geometry.Pose2d] = [] - waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) - waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) - traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) - new_trajectory: trajectory.Trajectory = ( - trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) - ) - end_state = new_trajectory.sample(new_trajectory.totalTime()) - - fixture_data: SwerveControllerCommandTestDataFixtures = ( - get_swerve_controller_data(FOUR) - ) - - command = commands2.SwerveControllerCommand( - new_trajectory, - fixture_data.getRobotPose, - fixture_data._kinematics, - fixture_data._holonomic, - fixture_data.setModuleStates, - subsystem, - ) - - fixture_data._timer.restart() - - command.initialize() - - while not command.isFinished(): - command.execute() - fixture_data._angle = new_trajectory.sample( - fixture_data._timer.get() - ).pose.rotation() - - for i in range(0, len(fixture_data._modulePositions)): - fixture_data._modulePositions[i].distance += ( - fixture_data._moduleStates[i].speed * 0.005 - ) - fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ - i - ].angle - - sim.step(0.005) - - fixture_data._timer.stop() - command.end(True) - - assert end_state.pose.X() == pytest.approx( - fixture_data.getRobotPose().X(), fixture_data._kxTolerance - ) - assert end_state.pose.Y() == pytest.approx( - fixture_data.getRobotPose().Y(), fixture_data._kyTolerance - ) - assert end_state.pose.rotation().radians() == pytest.approx( - fixture_data.getRobotPose().rotation().radians(), - fixture_data._kAngularTolerance, - ) - - -def test_SwerveControllerCommand4HolonomicWithRotation( - scheduler: commands2.CommandScheduler, get_swerve_controller_data -): - with ManualSimTime() as sim: - subsystem = commands2.Subsystem() - waypoints: List[geometry.Pose2d] = [] - waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) - waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) - traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) - new_trajectory: trajectory.Trajectory = ( - trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) - ) - end_state = new_trajectory.sample(new_trajectory.totalTime()) - - fixture_data: SwerveControllerCommandTestDataFixtures = ( - get_swerve_controller_data(FOUR) - ) - - command = commands2.SwerveControllerCommand( - new_trajectory, - fixture_data.getRobotPose, - fixture_data._kinematics, - fixture_data._holonomic, - fixture_data.getRotationHeadingZero, - fixture_data.setModuleStates, - subsystem, - ) - - fixture_data._timer.restart() - - command.initialize() - - while not command.isFinished(): - command.execute() - fixture_data._angle = new_trajectory.sample( - fixture_data._timer.get() - ).pose.rotation() - - for i in range(0, len(fixture_data._modulePositions)): - fixture_data._modulePositions[i].distance += ( - fixture_data._moduleStates[i].speed * 0.005 - ) - fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ - i - ].angle - - sim.step(0.005) - - fixture_data._timer.stop() - command.end(True) - - assert end_state.pose.X() == pytest.approx( - fixture_data.getRobotPose().X(), fixture_data._kxTolerance - ) - assert end_state.pose.Y() == pytest.approx( - fixture_data.getRobotPose().Y(), fixture_data._kyTolerance - ) - assert end_state.pose.rotation().radians() == pytest.approx( - fixture_data.getRobotPose().rotation().radians(), - fixture_data._kAngularTolerance, - ) - - -def test_SwerveControllerCommand4PID( - scheduler: commands2.CommandScheduler, get_swerve_controller_data -): - with ManualSimTime() as sim: - subsystem = commands2.Subsystem() - waypoints: List[geometry.Pose2d] = [] - waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) - waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) - traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) - new_trajectory: trajectory.Trajectory = ( - trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) - ) - end_state = new_trajectory.sample(new_trajectory.totalTime()) - - fixture_data: SwerveControllerCommandTestDataFixtures = ( - get_swerve_controller_data(FOUR) - ) - - command = commands2.SwerveControllerCommand( - new_trajectory, - fixture_data.getRobotPose, - fixture_data._kinematics, - fixture_data._xController, - fixture_data._yController, - fixture_data._rotationController, - fixture_data.setModuleStates, - subsystem, - ) - - fixture_data._timer.restart() - - command.initialize() - - while not command.isFinished(): - command.execute() - fixture_data._angle = new_trajectory.sample( - fixture_data._timer.get() - ).pose.rotation() - - for i in range(0, len(fixture_data._modulePositions)): - fixture_data._modulePositions[i].distance += ( - fixture_data._moduleStates[i].speed * 0.005 - ) - fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ - i - ].angle - - sim.step(0.005) - - fixture_data._timer.stop() - command.end(True) - - assert end_state.pose.X() == pytest.approx( - fixture_data.getRobotPose().X(), fixture_data._kxTolerance - ) - assert end_state.pose.Y() == pytest.approx( - fixture_data.getRobotPose().Y(), fixture_data._kyTolerance - ) - assert end_state.pose.rotation().radians() == pytest.approx( - fixture_data.getRobotPose().rotation().radians(), - fixture_data._kAngularTolerance, - ) - - -def test_SwerveControllerCommand4PIDWithRotation( - scheduler: commands2.CommandScheduler, get_swerve_controller_data -): - with ManualSimTime() as sim: - subsystem = commands2.Subsystem() - waypoints: List[geometry.Pose2d] = [] - waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) - waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) - traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) - new_trajectory: trajectory.Trajectory = ( - trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) - ) - end_state = new_trajectory.sample(new_trajectory.totalTime()) - - fixture_data: SwerveControllerCommandTestDataFixtures = ( - get_swerve_controller_data(FOUR) - ) - - command = commands2.SwerveControllerCommand( - new_trajectory, - fixture_data.getRobotPose, - fixture_data._kinematics, - fixture_data._xController, - fixture_data._yController, - fixture_data._rotationController, - fixture_data.getRotationHeadingZero, - fixture_data.setModuleStates, - subsystem, - ) - - fixture_data._timer.restart() - - command.initialize() - - while not command.isFinished(): - command.execute() - fixture_data._angle = new_trajectory.sample( - fixture_data._timer.get() - ).pose.rotation() - - for i in range(0, len(fixture_data._modulePositions)): - fixture_data._modulePositions[i].distance += ( - fixture_data._moduleStates[i].speed * 0.005 - ) - fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ - i - ].angle - - sim.step(0.005) - - fixture_data._timer.stop() - command.end(True) - - assert end_state.pose.X() == pytest.approx( - fixture_data.getRobotPose().X(), fixture_data._kxTolerance - ) - assert end_state.pose.Y() == pytest.approx( - fixture_data.getRobotPose().Y(), fixture_data._kyTolerance - ) - assert end_state.pose.rotation().radians() == pytest.approx( - fixture_data.getRobotPose().rotation().radians(), - fixture_data._kAngularTolerance, - ) - - -def test_SwerveControllerCommand6Holonomic( - scheduler: commands2.CommandScheduler, get_swerve_controller_data -): - with ManualSimTime() as sim: - subsystem = commands2.Subsystem() - waypoints: List[geometry.Pose2d] = [] - waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) - waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) - traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) - new_trajectory: trajectory.Trajectory = ( - trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) - ) - end_state = new_trajectory.sample(new_trajectory.totalTime()) - - fixture_data: SwerveControllerCommandTestDataFixtures = ( - get_swerve_controller_data(SIX) - ) - - command = commands2.SwerveControllerCommand( - new_trajectory, - fixture_data.getRobotPose, - fixture_data._kinematics, - fixture_data._holonomic, - fixture_data.setModuleStates, - subsystem, - ) - - fixture_data._timer.restart() - - command.initialize() - - while not command.isFinished(): - command.execute() - fixture_data._angle = new_trajectory.sample( - fixture_data._timer.get() - ).pose.rotation() - - for i in range(0, len(fixture_data._modulePositions)): - fixture_data._modulePositions[i].distance += ( - fixture_data._moduleStates[i].speed * 0.005 - ) - fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ - i - ].angle - - sim.step(0.005) - - fixture_data._timer.stop() - command.end(True) - - assert end_state.pose.X() == pytest.approx( - fixture_data.getRobotPose().X(), fixture_data._kxTolerance - ) - assert end_state.pose.Y() == pytest.approx( - fixture_data.getRobotPose().Y(), fixture_data._kyTolerance - ) - assert end_state.pose.rotation().radians() == pytest.approx( - fixture_data.getRobotPose().rotation().radians(), - fixture_data._kAngularTolerance, - ) - - -def test_SwerveControllerCommand6HolonomicWithRotation( - scheduler: commands2.CommandScheduler, get_swerve_controller_data -): - with ManualSimTime() as sim: - subsystem = commands2.Subsystem() - waypoints: List[geometry.Pose2d] = [] - waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) - waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) - traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) - new_trajectory: trajectory.Trajectory = ( - trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) - ) - end_state = new_trajectory.sample(new_trajectory.totalTime()) - - fixture_data: SwerveControllerCommandTestDataFixtures = ( - get_swerve_controller_data(SIX) - ) - - command = commands2.SwerveControllerCommand( - new_trajectory, - fixture_data.getRobotPose, - fixture_data._kinematics, - fixture_data._holonomic, - fixture_data.getRotationHeadingZero, - fixture_data.setModuleStates, - subsystem, - ) - - fixture_data._timer.restart() - - command.initialize() - - while not command.isFinished(): - command.execute() - fixture_data._angle = new_trajectory.sample( - fixture_data._timer.get() - ).pose.rotation() - - for i in range(0, len(fixture_data._modulePositions)): - fixture_data._modulePositions[i].distance += ( - fixture_data._moduleStates[i].speed * 0.005 - ) - fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ - i - ].angle - - sim.step(0.005) - - fixture_data._timer.stop() - command.end(True) - - assert end_state.pose.X() == pytest.approx( - fixture_data.getRobotPose().X(), fixture_data._kxTolerance - ) - assert end_state.pose.Y() == pytest.approx( - fixture_data.getRobotPose().Y(), fixture_data._kyTolerance - ) - assert end_state.pose.rotation().radians() == pytest.approx( - fixture_data.getRobotPose().rotation().radians(), - fixture_data._kAngularTolerance, - ) - - -def test_SwerveControllerCommand6PID( - scheduler: commands2.CommandScheduler, get_swerve_controller_data -): - with ManualSimTime() as sim: - subsystem = commands2.Subsystem() - waypoints: List[geometry.Pose2d] = [] - waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) - waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) - traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) - new_trajectory: trajectory.Trajectory = ( - trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) - ) - end_state = new_trajectory.sample(new_trajectory.totalTime()) - - fixture_data: SwerveControllerCommandTestDataFixtures = ( - get_swerve_controller_data(SIX) - ) - - command = commands2.SwerveControllerCommand( - new_trajectory, - fixture_data.getRobotPose, - fixture_data._kinematics, - fixture_data._xController, - fixture_data._yController, - fixture_data._rotationController, - fixture_data.setModuleStates, - subsystem, - ) - - fixture_data._timer.restart() - - command.initialize() - - while not command.isFinished(): - command.execute() - fixture_data._angle = new_trajectory.sample( - fixture_data._timer.get() - ).pose.rotation() - - for i in range(0, len(fixture_data._modulePositions)): - fixture_data._modulePositions[i].distance += ( - fixture_data._moduleStates[i].speed * 0.005 - ) - fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ - i - ].angle - - sim.step(0.005) - - fixture_data._timer.stop() - command.end(True) - - assert end_state.pose.X() == pytest.approx( - fixture_data.getRobotPose().X(), fixture_data._kxTolerance - ) - assert end_state.pose.Y() == pytest.approx( - fixture_data.getRobotPose().Y(), fixture_data._kyTolerance - ) - assert end_state.pose.rotation().radians() == pytest.approx( - fixture_data.getRobotPose().rotation().radians(), - fixture_data._kAngularTolerance, - ) - - -def test_SwerveControllerCommand6PIDWithRotation( - scheduler: commands2.CommandScheduler, get_swerve_controller_data -): - with ManualSimTime() as sim: - subsystem = commands2.Subsystem() - waypoints: List[geometry.Pose2d] = [] - waypoints.append(geometry.Pose2d(0, 0, geometry.Rotation2d(0))) - waypoints.append(geometry.Pose2d(1, 5, geometry.Rotation2d(3))) - traj_config: trajectory.TrajectoryConfig = trajectory.TrajectoryConfig(8.8, 0.1) - new_trajectory: trajectory.Trajectory = ( - trajectory.TrajectoryGenerator.generateTrajectory(waypoints, traj_config) - ) - end_state = new_trajectory.sample(new_trajectory.totalTime()) - - fixture_data: SwerveControllerCommandTestDataFixtures = ( - get_swerve_controller_data(SIX) - ) - - command = commands2.SwerveControllerCommand( - new_trajectory, - fixture_data.getRobotPose, - fixture_data._kinematics, - fixture_data._xController, - fixture_data._yController, - fixture_data._rotationController, - fixture_data.getRotationHeadingZero, - fixture_data.setModuleStates, - subsystem, - ) - - fixture_data._timer.restart() - - command.initialize() - - while not command.isFinished(): - command.execute() - fixture_data._angle = new_trajectory.sample( - fixture_data._timer.get() - ).pose.rotation() - - for i in range(0, len(fixture_data._modulePositions)): - fixture_data._modulePositions[i].distance += ( - fixture_data._moduleStates[i].speed * 0.005 - ) - fixture_data._modulePositions[i].angle = fixture_data._moduleStates[ - i - ].angle - - sim.step(0.005) - - fixture_data._timer.stop() - command.end(True) - - assert end_state.pose.X() == pytest.approx( - fixture_data.getRobotPose().X(), fixture_data._kxTolerance - ) - assert end_state.pose.Y() == pytest.approx( - fixture_data.getRobotPose().Y(), fixture_data._kyTolerance - ) - assert end_state.pose.rotation().radians() == pytest.approx( - fixture_data.getRobotPose().rotation().radians(), - fixture_data._kAngularTolerance, - ) diff --git a/rio/ottomanEmpire/tests/test_trigger.py b/rio/ottomanEmpire/tests/test_trigger.py deleted file mode 100644 index 876ae9da..00000000 --- a/rio/ottomanEmpire/tests/test_trigger.py +++ /dev/null @@ -1,219 +0,0 @@ -from typing import TYPE_CHECKING - -import commands2 -from util import * # type: ignore -from wpilib.simulation import stepTiming - -if TYPE_CHECKING: - from .util import * - - -def test_onTrue(scheduler: commands2.CommandScheduler): - finished = OOBoolean(False) - command1 = commands2.WaitUntilCommand(finished) - - button = InternalButton() - button.setPressed(False) - button.onTrue(command1) - scheduler.run() - assert not command1.isScheduled() - button.setPressed(True) - scheduler.run() - assert command1.isScheduled() - finished.set(True) - scheduler.run() - assert not command1.isScheduled() - - -def test_onFalse(scheduler: commands2.CommandScheduler): - finished = OOBoolean(False) - command1 = commands2.WaitUntilCommand(finished) - - button = InternalButton() - button.setPressed(True) - button.onFalse(command1) - scheduler.run() - assert not command1.isScheduled() - button.setPressed(False) - scheduler.run() - assert command1.isScheduled() - finished.set(True) - scheduler.run() - assert not command1.isScheduled() - - -def test_whileTrueRepeatedly(scheduler: commands2.CommandScheduler): - inits = OOInteger(0) - counter = OOInteger(0) - - command1 = commands2.FunctionalCommand( - inits.incrementAndGet, - lambda: None, - lambda _: None, - lambda: counter.incrementAndGet() % 2 == 0, - ).repeatedly() - - button = InternalButton() - button.setPressed(False) - button.whileTrue(command1) - scheduler.run() - assert inits == 0 - button.setPressed(True) - scheduler.run() - assert inits == 1 - scheduler.run() - assert inits == 1 - scheduler.run() - assert inits == 2 - button.setPressed(False) - scheduler.run() - assert inits == 2 - - -def test_whileTrueLambdaRunCommand(scheduler: commands2.CommandScheduler): - counter = OOInteger(0) - - command1 = commands2.RunCommand(counter.incrementAndGet) - - button = InternalButton() - button.setPressed(False) - button.whileTrue(command1) - scheduler.run() - assert counter == 0 - button.setPressed(True) - scheduler.run() - assert counter == 1 - scheduler.run() - assert counter == 2 - button.setPressed(False) - scheduler.run() - assert counter == 2 - - -def test_whileTrueOnce(scheduler: commands2.CommandScheduler): - startCounter = OOInteger(0) - endCounter = OOInteger(0) - - command1 = commands2.StartEndCommand( - startCounter.incrementAndGet, endCounter.incrementAndGet - ) - - button = InternalButton() - button.setPressed(False) - button.whileTrue(command1) - scheduler.run() - assert startCounter == 0 - assert endCounter == 0 - button.setPressed(True) - scheduler.run() - scheduler.run() - assert startCounter == 1 - assert endCounter == 0 - button.setPressed(False) - scheduler.run() - assert startCounter == 1 - assert endCounter == 1 - - -def test_toggleOnTrue(scheduler: commands2.CommandScheduler): - startCounter = OOInteger(0) - endCounter = OOInteger(0) - - command1 = commands2.StartEndCommand( - startCounter.incrementAndGet, endCounter.incrementAndGet - ) - - button = InternalButton() - button.setPressed(False) - button.toggleOnTrue(command1) - scheduler.run() - assert startCounter == 0 - assert endCounter == 0 - button.setPressed(True) - scheduler.run() - scheduler.run() - assert startCounter == 1 - assert endCounter == 0 - button.setPressed(False) - scheduler.run() - assert startCounter == 1 - assert endCounter == 0 - button.setPressed(True) - scheduler.run() - assert startCounter == 1 - assert endCounter == 1 - - -def test_cancelWhenActive(scheduler: commands2.CommandScheduler): - startCounter = OOInteger(0) - endCounter = OOInteger(0) - - button = InternalButton() - command1 = commands2.StartEndCommand( - startCounter.incrementAndGet, endCounter.incrementAndGet - ).until(button) - - button.setPressed(False) - command1.schedule() - scheduler.run() - assert startCounter == 1 - assert endCounter == 0 - button.setPressed(True) - scheduler.run() - assert startCounter == 1 - assert endCounter == 1 - scheduler.run() - assert startCounter == 1 - assert endCounter == 1 - - -def test_triggerComposition(): - button1 = InternalButton() - button2 = InternalButton() - - button1.setPressed(True) - button2.setPressed(False) - - assert button1.and_(button2).getAsBoolean() == False - assert button1.or_(button2)() == True - assert bool(button1.negate()) == False - assert (button1 & ~button2)() == True - - -def test_triggerCompositionSupplier(): - button1 = InternalButton() - supplier = lambda: False - - button1.setPressed(True) - - assert button1.and_(supplier)() == False - assert button1.or_(supplier)() == True - - -def test_debounce(scheduler: commands2.CommandScheduler): - command = commands2.Command() - start_spying_on(command) - - button = InternalButton() - debounced = button.debounce(0.1) - - debounced.onTrue(command) - - button.setPressed(True) - scheduler.run() - - verify(command, never()).schedule() - - stepTiming(0.3) - - button.setPressed(True) - scheduler.run() - verify(command).schedule() - - -def test_booleanSupplier(): - button = InternalButton() - - assert button() == False - button.setPressed(True) - assert button() == True diff --git a/rio/ottomanEmpire/tests/test_waitcommand.py b/rio/ottomanEmpire/tests/test_waitcommand.py deleted file mode 100644 index 125a2e31..00000000 --- a/rio/ottomanEmpire/tests/test_waitcommand.py +++ /dev/null @@ -1,50 +0,0 @@ -from typing import TYPE_CHECKING - -import commands2 -from util import * # type: ignore - -if TYPE_CHECKING: - from .util import * - -import pytest - - -def test_waitCommand(scheduler: commands2.CommandScheduler): - with ManualSimTime() as sim: - waitCommand = commands2.WaitCommand(2) - - scheduler.schedule(waitCommand) - scheduler.run() - sim.step(1) - scheduler.run() - - assert scheduler.isScheduled(waitCommand) - - sim.step(2) - - scheduler.run() - - assert not scheduler.isScheduled(waitCommand) - - -def test_withTimeout(scheduler: commands2.CommandScheduler): - with ManualSimTime() as sim: - command1 = commands2.Command() - start_spying_on(command1) - - timeout = command1.withTimeout(2) - - scheduler.schedule(timeout) - scheduler.run() - - verify(command1).initialize() - verify(command1).execute() - assert not scheduler.isScheduled(command1) - assert scheduler.isScheduled(timeout) - - sim.step(3) - scheduler.run() - - verify(command1).end(True) - verify(command1, never()).end(False) - assert not scheduler.isScheduled(timeout) diff --git a/rio/ottomanEmpire/tests/test_waituntilcommand.py b/rio/ottomanEmpire/tests/test_waituntilcommand.py deleted file mode 100644 index 2762a1f6..00000000 --- a/rio/ottomanEmpire/tests/test_waituntilcommand.py +++ /dev/null @@ -1,22 +0,0 @@ -from typing import TYPE_CHECKING - -import commands2 -from util import * # type: ignore - -if TYPE_CHECKING: - from .util import * - -import pytest - - -def test_waitUntil(scheduler: commands2.CommandScheduler): - condition = OOBoolean() - - command = commands2.WaitUntilCommand(condition) - - scheduler.schedule(command) - scheduler.run() - assert scheduler.isScheduled(command) - condition.set(True) - scheduler.run() - assert not scheduler.isScheduled(command) diff --git a/rio/ottomanEmpire/tests/util.py b/rio/ottomanEmpire/tests/util.py deleted file mode 100644 index 0c1a835f..00000000 --- a/rio/ottomanEmpire/tests/util.py +++ /dev/null @@ -1,331 +0,0 @@ -from typing import Any, Dict, TypeVar, Type - -import inspect - -import commands2 -from wpilib.simulation import DriverStationSim, pauseTiming, resumeTiming, stepTiming - - -Y = TypeVar("Y") - - -def full_subclass_of(cls: Type[Y]) -> Type[Y]: - # Pybind classes can't be monkeypatched. - # This generates a subclass with every method filled out - # so that it can be monkeypatched. - retlist = [] - clsname = cls.__name__ # + "_Subclass" - classdef = f"class {clsname}(cls):\n" - for name in dir(cls): - # for name in set(dir(cls)): - value = getattr(cls, name) - if callable(value) and not inspect.isclass(value) and not name.startswith("_"): - classdef += f" def {name}(self, *args, **kwargs):\n" - classdef += f" return super().{name}(*args, **kwargs)\n" - classdef += " ...\n" - classdef += f"retlist.append({clsname})\n" - print(classdef) - exec(classdef, globals(), locals()) - return retlist[0] - - -################### -# Compat for wrapped commands -IS_OLD_COMMANDS = False -try: - if commands2.__version__ == "2023.4.3.0": # type: ignore - IS_OLD_COMMANDS = True -except AttributeError: - pass - -if IS_OLD_COMMANDS: - # not needed for pure but works in pure - import commands2.button - - # In Java, Trigger is in the same package as Button - # I did rexport it in commands so using - # the incorrect `commands2.Trigger` instead of `commands2.button.Trigger` works - commands2.button.Trigger = commands2.Trigger - - # I moved this so its not a nested Enum. - # The old one is still there for compat - commands2.InterruptionBehavior = commands2.Command.InterruptionBehavior - - commands2.Command = commands2.CommandBase - - for name in dir(commands2): - if name == "CommandScheduler": - continue - value = getattr(commands2, name) - if inspect.isclass(value): - setattr(commands2, name, full_subclass_of(value)) - - commands2.IllegalCommandUse = RuntimeError - - -################### - - -class ManualSimTime: - def __enter__(self) -> "ManualSimTime": - pauseTiming() - return self - - def __exit__(self, *args): - resumeTiming() - - def step(self, delta: float): - stepTiming(delta) - - -class OOInteger: - def __init__(self, value: int = 0) -> None: - self.value = value - - def get(self) -> int: - return self.value - - def set(self, value: int): - self.value = value - - def incrementAndGet(self) -> int: - self.value += 1 - return self.value - - def addAndGet(self, value: int) -> int: - self.value += value - return self.value - - def __eq__(self, value: float) -> bool: - return self.value == value - - def __lt__(self, value: float) -> bool: - return self.value < value - - def __call__(self) -> int: - return self.value - - -class OOBoolean: - def __init__(self, value: bool = False) -> None: - self.value = value - - def get(self) -> bool: - return self.value - - def set(self, value: bool): - self.value = value - - def __eq__(self, value: object) -> bool: - return self.value == value - - def __bool__(self) -> bool: - return self.value - - def __call__(self) -> bool: - return self.value - - -class InternalButton(commands2.button.Trigger): - def __init__(self): - super().__init__(self.isPressed) - self.pressed = False - - def isPressed(self) -> bool: - return self.pressed - - def setPressed(self, value: bool): - self.pressed = value - - def __call__(self) -> bool: - return self.pressed - - -class OOFloat: - def __init__(self, value: float = 0.0) -> None: - self.value = value - - def get(self) -> float: - return self.value - - def set(self, value: float): - self.value = value - - def incrementAndGet(self) -> float: - self.value += 1 - return self.value - - def addAndGet(self, value: float) -> float: - self.value += value - return self.value - - def __eq__(self, value: float) -> bool: - return self.value == value - - def __lt__(self, value: float) -> bool: - return self.value < value - - def __call__(self) -> float: - return self.value - - def __name__(self) -> str: - return "OOFloat" - - -########################################## -# Fakito Framework - - -def _get_all_args_as_kwargs(method, *args, **kwargs) -> Dict[str, Any]: - try: - import inspect - - method_args = inspect.getcallargs(method, *args, **kwargs) - - method_arg_names = list(inspect.signature(method).parameters.keys()) - - for idx, arg in enumerate(args): - method_args[method_arg_names[idx]] = arg - - try: - del method_args["self"] - except KeyError: - pass - return method_args - except TypeError: - # Pybind methods can't be inspected - # The exact args/kwargs that are passed in are checked instead - r = {} - for idx, arg in enumerate(args): - r[idx] = arg - r.update(kwargs) - return r - - -class MethodWrapper: - def __init__(self, method): - self.method = method - self.og_method = method - self.times_called = 0 - self.call_log = [] - - def __call__(self, *args, **kwargs): - self.times_called += 1 - method_args = _get_all_args_as_kwargs(self.method, *args, **kwargs) - self.call_log.append(method_args) - return self.method(*args, **kwargs) - - def called_with(self, *args, **kwargs): - return _get_all_args_as_kwargs(self.method, *args, **kwargs) in self.call_log - - def times_called_with(self, *args, **kwargs): - return self.call_log.count( - _get_all_args_as_kwargs(self.method, *args, **kwargs) - ) - - -def start_spying_on(obj: Any) -> None: - """ - Mocks all methods on an object, so that that call info can be used in asserts. - - Example: - ``` - obj = SomeClass() - start_spying_on(obj) - obj.method() - obj.method = lambda: None # supports monkeypatching - assert obj.method.times_called == 2 - assert obj.method.called_with(arg1=1, arg2=2) - assert obj.method.times_called_with(arg1=1, arg2=2) == 2 - ``` - """ - - for name in dir(obj): - value = getattr(obj, name) - if callable(value) and not inspect.isclass(value) and not name.startswith("_"): - setattr(obj, name, MethodWrapper(value)) - - if not hasattr(obj.__class__, "_is_being_spied_on"): - try: - old_setattr = obj.__class__.__setattr__ - except AttributeError: - old_setattr = object.__setattr__ - - def _setattr(self, name, value): - if name in dir(self): - existing_value = getattr(self, name) - if isinstance(existing_value, MethodWrapper): - existing_value.method = value - return - old_setattr(self, name, value) - - obj.__class__.__setattr__ = _setattr - obj.__class__._is_being_spied_on = True - - -# fakito verify -def reset(obj: Any) -> None: - """ - Resets the call log of all mocked methods on an object. - Also restores all monkeypatched methods. - """ - for name in dir(obj): - value = getattr(obj, name) - if isinstance(value, MethodWrapper): - value.method = value.og_method - value.times_called = 0 - value.call_log = [] - - -class times: - def __init__(self, times: int) -> None: - self.times = times - - -def never() -> times: - return times(0) - - -class _verify: - def __init__(self, obj: Any, times: times = times(1)): - self.obj = obj - self.times = times.times - - def __getattribute__(self, name: str) -> Any: - def self_dot(name: str): - return super(_verify, self).__getattribute__(name) - - def times_string(times: int) -> str: - if times == 1: - return "1 time" - else: - return f"{times} times" - - def check(*args, **kwargs): - __tracebackhide__ = True - # import code - # code.interact(local={**globals(), **locals()}) - method = getattr(self_dot("obj"), name) - # method = getattr(self1.obj, name) - assert method.times_called_with(*args, **kwargs) == self_dot( - "times" - ), f"Expected {name} to be called {times_string(self_dot('times'))} with {args} {kwargs}, but was called {times_string(method.times_called_with(*args, **kwargs))}" - - return check - - -T = TypeVar("T") - - -def verify(obj: T, times: times = times(1)) -> T: - # import code - # code.interact(local={**globals(), **locals()}) - return _verify(obj, times) # type: ignore - - -def patch_via_decorator(obj: Any): - def decorator(method): - setattr(obj, method.__name__, method.__get__(obj, obj.__class__)) - return method - - return decorator diff --git a/rio/ottomanEmpire/workflows/dist.yml b/rio/ottomanEmpire/workflows/dist.yml deleted file mode 100644 index abd40c7c..00000000 --- a/rio/ottomanEmpire/workflows/dist.yml +++ /dev/null @@ -1,36 +0,0 @@ ---- -name: dist - -on: - pull_request: - push: - branches: - - main - tags: - - '*' - -jobs: - ci: - uses: robotpy/build-actions/.github/workflows/package-pure.yml@v2024 - with: - enable_sphinx_check: false - secrets: - META_REPO_ACCESS_TOKEN: ${{ secrets.REPO_ACCESS_TOKEN }} - PYPI_API_TOKEN: ${{ secrets.PYPI_PASSWORD }} - - check-mypy: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v3 - - uses: actions/setup-python@v4 - with: - python-version: "3.12" - - name: Install requirements - run: | - pip --disable-pip-version-check install mypy setuptools wheel setuptools_scm - pip --disable-pip-version-check install --no-build-isolation -e . - - name: Run mypy - uses: liskin/gh-problem-matcher-wrap@v2 - with: - linters: mypy - run: mypy --show-column-numbers commands2 diff --git a/rio/ottomanempire b/rio/ottomanempire new file mode 160000 index 00000000..bb2778c2 --- /dev/null +++ b/rio/ottomanempire @@ -0,0 +1 @@ +Subproject commit bb2778c242e08d3cf9dc4f6d894c46cd29103f30 diff --git a/rio/robotcontainer.py b/rio/robotcontainer.py index 9fcd6b66..55a2f40f 100644 --- a/rio/robotcontainer.py +++ b/rio/robotcontainer.py @@ -19,7 +19,7 @@ from constants.constants import getConstants # yes its the enitire ottoman empire -from ottomanEmpire.bursa.swervecontrollercommand import SwerveControllerCommand +from ottomanempire.commands2.swervecontrollercommand import SwerveControllerCommand # misc import math From 0c38f4fee25f8eab4547e2d424545074d3aefc3d Mon Sep 17 00:00:00 2001 From: kredcool Date: Sat, 13 Jan 2024 14:47:41 -0500 Subject: [PATCH 18/61] this now works --- rio/.wpilib/wpilib_preferences.json | 1 - rio/Makefile | 2 +- rio/{constants => autonomous}/__init__.py | 0 rio/commands/FlyByWire.py | 55 ----- rio/components/__init__.py | 0 rio/components/drivetrain.py | 104 ++++++++ rio/components/swervemodule.py | 141 +++++++++++ rio/constants/constants.py | 44 ---- rio/constants/mathConstant.py | 93 ------- rio/constants/robot_controls.yaml | 7 - rio/constants/robot_hardware.yaml | 72 ------ rio/constants/robot_pid.yaml | 19 -- rio/extras/deployData.py | 28 --- rio/mathConstant.py | 89 ------- rio/ottomanempire | 1 - rio/robot.py | 77 ++++-- rio/robot_requirements.txt | 14 -- rio/robotcontainer.py | 83 ------- rio/simgui-window.json | 4 +- rio/subsystems/drivesubsystem.py | 282 ---------------------- rio/subsystems/swervemodule.py | 152 ------------ rio/swerveutils.py | 91 ------- rio/tests/pyfrc_test.py | 23 +- 23 files changed, 315 insertions(+), 1067 deletions(-) delete mode 100644 rio/.wpilib/wpilib_preferences.json rename rio/{constants => autonomous}/__init__.py (100%) delete mode 100644 rio/commands/FlyByWire.py create mode 100644 rio/components/__init__.py create mode 100644 rio/components/drivetrain.py create mode 100644 rio/components/swervemodule.py delete mode 100644 rio/constants/constants.py delete mode 100644 rio/constants/mathConstant.py delete mode 100644 rio/constants/robot_controls.yaml delete mode 100644 rio/constants/robot_hardware.yaml delete mode 100644 rio/constants/robot_pid.yaml delete mode 100644 rio/extras/deployData.py delete mode 100644 rio/mathConstant.py delete mode 160000 rio/ottomanempire mode change 100755 => 100644 rio/robot.py delete mode 100644 rio/robot_requirements.txt delete mode 100644 rio/robotcontainer.py delete mode 100644 rio/subsystems/drivesubsystem.py delete mode 100644 rio/subsystems/swervemodule.py delete mode 100644 rio/swerveutils.py diff --git a/rio/.wpilib/wpilib_preferences.json b/rio/.wpilib/wpilib_preferences.json deleted file mode 100644 index 5c004bc4..00000000 --- a/rio/.wpilib/wpilib_preferences.json +++ /dev/null @@ -1 +0,0 @@ -{"teamNumber": 1721} \ No newline at end of file diff --git a/rio/Makefile b/rio/Makefile index 711762c1..7119747c 100644 --- a/rio/Makefile +++ b/rio/Makefile @@ -43,4 +43,4 @@ install-python: ## Install python (for robot) robotpy-installer install-python clean: ## Clean the repo - git clean -fdX + git clean -fdX \ No newline at end of file diff --git a/rio/constants/__init__.py b/rio/autonomous/__init__.py similarity index 100% rename from rio/constants/__init__.py rename to rio/autonomous/__init__.py diff --git a/rio/commands/FlyByWire.py b/rio/commands/FlyByWire.py deleted file mode 100644 index 7bfe84c3..00000000 --- a/rio/commands/FlyByWire.py +++ /dev/null @@ -1,55 +0,0 @@ -import typing -import commands2 -from wpilib import RobotBase -from subsystems.drivesubsystem import DriveSubsystem - - -class FlyByWire(commands2.CommandBase): - """ - FlyByWire uses pure joystick inputs - to direct the robot. Tradionally, this - is the most direct way to command the robot. - """ - - def __init__( - self, - drivetrain: DriveSubsystem, - forward: typing.Callable[[], float], - rotation: typing.Callable[[], float], - ) -> None: - super().__init__() - - self.drivetrain = drivetrain # This is a 'local' instance of drivetrain - self.forward = forward # Forward command - self.rotation = rotation # Rotation command - - # Adding drivetrain as a requirement ensures no other command will interrupt us - self.addRequirements(self.drivetrain) - - def execute(self) -> None: - if RobotBase.isReal(): - self.drivetrain.arcadeDrive( - self.exponential_dampen(self.forward()) * -1, - self.piecewise_dampen(self.rotation()), - ) - else: - self.drivetrain.tankDriveVolts( - self.forward() + self.rotation(), - -self.forward() + self.rotation(), - ) - - def exponential_dampen(self, x): - """ - Uses a simple math function - to dampen the user input. - """ - - return x / 1.3 * -1 - - def piecewise_dampen(self, x): - """ - Uses multiple different equations to define - a dampened user input - """ - - return x / 2.25 diff --git a/rio/components/__init__.py b/rio/components/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/rio/components/drivetrain.py b/rio/components/drivetrain.py new file mode 100644 index 00000000..bbbc0a34 --- /dev/null +++ b/rio/components/drivetrain.py @@ -0,0 +1,104 @@ +# +# 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. +# + +# math +import math + +# wpilib +import wpilib +import wpimath.geometry +import wpimath.kinematics + +# swerves +from .swervemodule import SwerveModule + +kMaxSpeed = 3.0 # 3 meters per second +kMaxAngularSpeed = math.pi # 1/2 rotation per second + + +class Drivetrain: + """ + Represents a swerve drive style drivetrain. + """ + + def __init__(self) -> None: + self.frontLeftLocation = wpimath.geometry.Translation2d(0.381, 0.381) + self.frontRightLocation = wpimath.geometry.Translation2d(0.381, -0.381) + self.backLeftLocation = wpimath.geometry.Translation2d(-0.381, 0.381) + self.backRightLocation = wpimath.geometry.Translation2d(-0.381, -0.381) + + self.frontLeft = SwerveModule(1, 2, 0, 1, 2, 3) + self.frontRight = SwerveModule(3, 4, 4, 5, 6, 7) + self.backLeft = SwerveModule(5, 6, 8, 9, 10, 11) + self.backRight = SwerveModule(7, 8, 12, 13, 14, 15) + + self.gyro = wpilib.AnalogGyro(0) + + self.kinematics = wpimath.kinematics.SwerveDrive4Kinematics( + self.frontLeftLocation, + self.frontRightLocation, + self.backLeftLocation, + self.backRightLocation, + ) + + self.odometry = wpimath.kinematics.SwerveDrive4Odometry( + self.kinematics, + self.gyro.getRotation2d(), + ( + self.frontLeft.getPosition(), + self.frontRight.getPosition(), + self.backLeft.getPosition(), + self.backRight.getPosition(), + ), + ) + + self.gyro.reset() + + def drive( + self, + xSpeed: float, + ySpeed: float, + rot: float, + fieldRelative: bool, + periodSeconds: float, + ) -> None: + """ + 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 rot: Angular rate of the robot. + :param fieldRelative: Whether the provided x and y speeds are relative to the field. + :param periodSeconds: Time + """ + swerveModuleStates = self.kinematics.toSwerveModuleStates( + wpimath.kinematics.ChassisSpeeds.discretize( + wpimath.kinematics.ChassisSpeeds.fromFieldRelativeSpeeds( + xSpeed, ySpeed, rot, self.gyro.getRotation2d() + ) + if fieldRelative + else wpimath.kinematics.ChassisSpeeds(xSpeed, ySpeed, rot), + periodSeconds, + ) + ) + wpimath.kinematics.SwerveDrive4Kinematics.desaturateWheelSpeeds( + swerveModuleStates, kMaxSpeed + ) + self.frontLeft.setDesiredState(swerveModuleStates[0]) + self.frontRight.setDesiredState(swerveModuleStates[1]) + self.backLeft.setDesiredState(swerveModuleStates[2]) + self.backRight.setDesiredState(swerveModuleStates[3]) + + def updateOdometry(self) -> None: + """Updates the field relative position of the robot.""" + self.odometry.update( + self.gyro.getRotation2d(), + ( + self.frontLeft.getPosition(), + self.frontRight.getPosition(), + self.backLeft.getPosition(), + self.backRight.getPosition(), + ), + ) diff --git a/rio/components/swervemodule.py b/rio/components/swervemodule.py new file mode 100644 index 00000000..e06367c6 --- /dev/null +++ b/rio/components/swervemodule.py @@ -0,0 +1,141 @@ +# +# 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. +# + +# math +import math + +# wpilib +import wpilib +import wpimath.kinematics +import wpimath.geometry +import wpimath.controller +import wpimath.trajectory + +kWheelRadius = 0.0508 +kEncoderResolution = 4096 +kModuleMaxAngularVelocity = math.pi +kModuleMaxAngularAcceleration = math.tau + + +class SwerveModule: + def __init__( + self, + driveMotorChannel: int, + turningMotorChannel: int, + driveEncoderChannelA: int, + driveEncoderChannelB: int, + turningEncoderChannelA: int, + turningEncoderChannelB: int, + ) -> None: + """Constructs a SwerveModule with a drive motor, turning motor, drive encoder and turning encoder. + + :param driveMotorChannel: PWM output for the drive motor. + :param turningMotorChannel: PWM output for the turning motor. + :param driveEncoderChannelA: DIO input for the drive encoder channel A + :param driveEncoderChannelB: DIO input for the drive encoder channel B + :param turningEncoderChannelA: DIO input for the turning encoder channel A + :param turningEncoderChannelB: DIO input for the turning encoder channel B + """ + self.driveMotor = wpilib.PWMSparkMax(driveMotorChannel) + self.turningMotor = wpilib.PWMSparkMax(turningMotorChannel) + + self.driveEncoder = wpilib.Encoder(driveEncoderChannelA, driveEncoderChannelB) + self.turningEncoder = wpilib.Encoder( + turningEncoderChannelA, turningEncoderChannelB + ) + + # Gains are for example purposes only - must be determined for your own robot! + self.drivePIDController = wpimath.controller.PIDController(1, 0, 0) + + # Gains are for example purposes only - must be determined for your own robot! + self.turningPIDController = wpimath.controller.ProfiledPIDController( + 1, + 0, + 0, + wpimath.trajectory.TrapezoidProfile.Constraints( + kModuleMaxAngularVelocity, + kModuleMaxAngularAcceleration, + ), + ) + + # Gains are for example purposes only - must be determined for your own robot! + self.driveFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 3) + self.turnFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 0.5) + + # Set the distance per pulse for the drive encoder. We can simply use the + # distance traveled for one rotation of the wheel divided by the encoder + # resolution. + self.driveEncoder.setDistancePerPulse( + math.tau * kWheelRadius / kEncoderResolution + ) + + # Set the distance (in this case, angle) in radians per pulse for the turning encoder. + # This is the the angle through an entire rotation (2 * pi) divided by the + # encoder resolution. + self.turningEncoder.setDistancePerPulse(math.tau / kEncoderResolution) + + # Limit the PID Controller's input range between -pi and pi and set the input + # to be continuous. + self.turningPIDController.enableContinuousInput(-math.pi, math.pi) + + def getState(self) -> wpimath.kinematics.SwerveModuleState: + """Returns the current state of the module. + + :returns: The current state of the module. + """ + return wpimath.kinematics.SwerveModuleState( + self.driveEncoder.getRate(), + wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()), + ) + + def getPosition(self) -> wpimath.kinematics.SwerveModulePosition: + """Returns the current position of the module. + + :returns: The current position of the module. + """ + return wpimath.kinematics.SwerveModulePosition( + self.driveEncoder.getRate(), + wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()), + ) + + def setDesiredState( + self, desiredState: wpimath.kinematics.SwerveModuleState + ) -> None: + """Sets the desired state for the module. + + :param desiredState: Desired state with speed and angle. + """ + + encoderRotation = wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()) + + # Optimize the reference state to avoid spinning further than 90 degrees + state = wpimath.kinematics.SwerveModuleState.optimize( + desiredState, encoderRotation + ) + + # Scale speed by cosine of angle error. This scales down movement perpendicular to the desired + # direction of travel that can occur when modules change directions. This results in smoother + # driving. + state.speed *= (state.angle - encoderRotation).cos() + + # Calculate the drive output from the drive PID controller. + driveOutput = self.drivePIDController.calculate( + self.driveEncoder.getRate(), state.speed + ) + + driveFeedforward = self.driveFeedforward.calculate(state.speed) + + # Calculate the turning motor output from the turning PID controller. + turnOutput = self.turningPIDController.calculate( + self.turningEncoder.getDistance(), state.angle.radians() + ) + + turnFeedforward = self.turnFeedforward.calculate( + self.turningPIDController.getSetpoint().velocity + ) + + self.driveMotor.setVoltage(driveOutput + driveFeedforward) + self.turningMotor.setVoltage(turnOutput + turnFeedforward) diff --git a/rio/constants/constants.py b/rio/constants/constants.py deleted file mode 100644 index ffe95648..00000000 --- a/rio/constants/constants.py +++ /dev/null @@ -1,44 +0,0 @@ -# Tidal Force Robotics -# 2022 - -import os -from sre_constants import CATEGORY_WORD -from wpilib import RobotBase -import yaml -import logging - -""" -Use this file only for storing non-changing constants. -""" - - -def load(fullPath: str): - # Try opening requested .yaml - with open(f"{fullPath}.yaml", "r") as yamlFile: - # Use yaml.safe_load to load the yaml into a dict - return yaml.safe_load(yamlFile) - - -def getConstants(identifier: str): - constants = {} - - # Clunky but it works - if RobotBase.isReal(): - path = "/home/lvuser/py/constants/" - else: - path = "constants/" - - try: - # Try opening requested .yaml - constants = load(f"{path}{identifier}") - except FileNotFoundError: - try: - # Try again but from one directory in (useful for unit testing) - constants = load(f"../{path}{identifier}") - except FileNotFoundError as e: - # If the file is not found, report it! - logging.error(f"{identifier} config not found!") - raise e - - # When all is done, return the important bits! - return constants diff --git a/rio/constants/mathConstant.py b/rio/constants/mathConstant.py deleted file mode 100644 index 89ee4a8d..00000000 --- a/rio/constants/mathConstant.py +++ /dev/null @@ -1,93 +0,0 @@ -""" -this is hear to make the files cleaner -and to do alot of needed math in its own file -""" - -# wpilib -from wpimath.geometry import Translation2d -from wpimath.kinematics import SwerveDrive4Kinematics -from wpimath.trajectory import TrapezoidProfileRadians - -# vendor libs -from rev import CANSparkMax - -# constants -from constants.constants import getConstants - -# misc -import math - - -class DriveConstants: - # constants - constants = getConstants("robot_hardware") - drivetrain = constants["driveTrain"] - - # allowed max speed - maxAngularSpeed = math.tau - - # Chassis config - trackWidth = drivetrain["TrackWidth"] - # Distance between centers of right and left wheels - wheelBase = drivetrain["WheelBase"] - - # Distance of front and back wheels - modulePositions = [ - Translation2d(wheelBase / 2, trackWidth / 2), - Translation2d(wheelBase / 2, -trackWidth / 2), - Translation2d(-wheelBase / 2, trackWidth / 2), - Translation2d(-wheelBase / 2, -trackWidth / 2), - ] - driveKinematics = SwerveDrive4Kinematics(*modulePositions) - - # Angular offsets of modules relative to chassis in radians - fPChassisAngularOffset = -math.pi / 2 - fSChassisAngularOffset = 0 - aPChassisAngularOffset = math.pi - aSChassisAngularOffset = math.pi / 2 - - -class ModuleConstants: - # constants - # hardware - hardwareConstants = getConstants("robot_hardware") - drivetrain = hardwareConstants["driveTrain"] - NeoMotor = hardwareConstants["NeoMotor"] - - # pids - pid = getConstants("robot_pid")["PID"] - - driveMotorPinionTeeth = drivetrain["driveMotorPinionTeeth"] - - # Calculations for drive motor conversion factors and feed forward - driveMotorFreeSpeedRps = NeoMotor["FreeSpeedRpm"] / 60 - WheelDiameterMeters = drivetrain["WheelDiameterMeters"] - WheelCircumferenceMeters = WheelDiameterMeters * math.pi - - driveMotorReduction = ( - drivetrain["BevelGearTeeth"] * drivetrain["firstStageSpurTeeth"] - ) / (driveMotorPinionTeeth * drivetrain["bevelPinion"]) - - driveEncoderPositionFactor = (WheelDiameterMeters * math.pi) / driveMotorReduction - driveEncoderVelocityFactor = ( - (WheelDiameterMeters * math.pi) / driveMotorReduction - ) / 60.0 - - turnEncoderPositionFactor = math.tau - turnEncoderVelocityFactor = math.tau / 60.0 - - turnEncoderPositionPIDMinInput = pid["turnEncoderPositionPIDMinInput"] # radian - turnEncoderPositionPIDMaxInput = turnEncoderPositionFactor - - driveMotorIdleMode = CANSparkMax.IdleMode.kBrake - turnMotorIdleMode = CANSparkMax.IdleMode.kBrake - - -class AutoConstants: - maxAngularSpeedRadiansPerSecond = math.pi - maxAngularSpeedRadiansPerSecondSquared = math.pi - - # Constraint for motion robot angle controller - thetaControllerConstraints = TrapezoidProfileRadians.Constraints( - maxAngularSpeedRadiansPerSecond, maxAngularSpeedRadiansPerSecondSquared - ) diff --git a/rio/constants/robot_controls.yaml b/rio/constants/robot_controls.yaml deleted file mode 100644 index dda89737..00000000 --- a/rio/constants/robot_controls.yaml +++ /dev/null @@ -1,7 +0,0 @@ -# This file defines the input -# and control scheme of the robot, -# Arranged into operating "modes" - -driver: - ControllerPort: 0 - DeadZone: 0.05 \ No newline at end of file diff --git a/rio/constants/robot_hardware.yaml b/rio/constants/robot_hardware.yaml deleted file mode 100644 index b7979c06..00000000 --- a/rio/constants/robot_hardware.yaml +++ /dev/null @@ -1,72 +0,0 @@ -# This file defines the robot's -# physical dimensions. Things like -# Motor placement, max and min extensions -# of arms, and similar should go here. - -# rev's code will help with replacing values -# https://github.com/robotpy/robotpy-rev/tree/main/examples/maxswerve - -driveTrain: - MaxSpeedMeterPerSecond: 4.8 - - DirectionSlewRate: 1.2 - MagnitudeSlewRate: 1.8 - RotationalSlewRate: 2.0 - - TrackWidth: 0.6731 - WheelBase: 0.6731 - - # may need to be changed depending on if it spins the same way - turnEncoderInverted: True - GyroReversed: False - - ModuleConstants: - driveMotorPinionTeeth: 14 - BevelGearTeeth: 45 - firstStageSpurTeeth: 22 - bevelPinion: 15 - - WheelDiameterMeters: 0.0762 - - # in amps - driveMotorCurrentLimit: 50 - turnMotorCurrentLimit: 20 - - autonomous: - maxSpeedMetersPerSecond: 3 - maxAccelerationMetersPerSecondSquared: 3 - - pXController: 1 - pYController: 1 - pThetaController: 1 - - FPMotors: - driveID: 1 # Updated Never by Nobody - turnID: 2 # Updated Never by Nobody - EncoderIDs: [1, 2] # Updated Never by Nobody - EncoderReversed: False # Updated Never by Nobody - Inverted: False # Updated Never by Nobody - - APMotors: - driveID: 3 # Updated Never by Nobody - turnID: 4 # Updated Never by Nobody - EncoderIDs: [3, 4] # Updated Never by Nobody - EncoderReversed: False # Updated Never by Nobody - Inverted: False # Updated Never by Nobody - - FSMotors: - driveID: 5 # Updated Never by Nobody - turnID: 6 # Updated Never by Nobody - EncoderIDs: [5, 6] # Updated Never by Nobody - EncoderReversed: False # Updated Never by Nobody - Inverted: False # Updated Never by Nobody - - ASMotors: - driveID: 7 # Updated Never by Nobody - turnID: 8 # Updated Never by Nobody - EncoderIDs: [7, 8] # Updated Never by Nobody - EncoderReversed: False # Updated Never by Nobody - Inverted: False # Updated Never by Nobody - -NeoMotor: - FreeSpeedRpm: 5676 diff --git a/rio/constants/robot_pid.yaml b/rio/constants/robot_pid.yaml deleted file mode 100644 index e6f9db2b..00000000 --- a/rio/constants/robot_pid.yaml +++ /dev/null @@ -1,19 +0,0 @@ -# rev's code will help with replacing values -# https://github.com/robotpy/robotpy-rev/tree/main/examples/maxswerve - -PID: - turnEncoderPositionPIDMinInput: 0 - - driveP: 0.04 - driveI: 0 - driveD: 0 - driveFF: 1 - driveMinOutput: -1 - driveMaxOutput: 1 - - turnP: 1 - turnI: 0 - turnD: 0 - turnFF: 0 - turnMinOutput: -1 - turnMaxOutput: 1 \ No newline at end of file diff --git a/rio/extras/deployData.py b/rio/extras/deployData.py deleted file mode 100644 index ec7a3f40..00000000 --- a/rio/extras/deployData.py +++ /dev/null @@ -1,28 +0,0 @@ -# Helper function for getting deploy data -import json - -from wpilib import RobotBase - - -def getDeployData(): - """ - Returns appropriate deploy data. - """ - - fakedata = { - "git-desc": "week0-69-g42096-dirty", - "git-branch": "sim/sim", - "deploy-host": "SimulatedLaptop", - "deploy-user": "SimUser", - "code-path": "/sim/simulatedrobot", - "deploy-date": "never", - } - - if RobotBase.isReal(): - try: - with open("/home/lvuser/py/deploy.json") as fp: - return json.load(fp) - except FileNotFoundError: - return fakedata - else: - return fakedata diff --git a/rio/mathConstant.py b/rio/mathConstant.py deleted file mode 100644 index bc0e1f8a..00000000 --- a/rio/mathConstant.py +++ /dev/null @@ -1,89 +0,0 @@ -""" -this is hear to make the files cleaner -and to do alot of needed math in its own file -""" - -# wpilib -from wpimath.geometry import Translation2d -from wpimath.kinematics import SwerveDrive4Kinematics -from wpimath.trajectory import TrapezoidProfileRadians - -# vendor libs -from rev import CANSparkMax - -# constants -from constants.constants import getConstants - -# misc -import math - - -class DriveConstants: - # constants - constants = getConstants("robot_hardware") - drivetrain = constants["driveTrain"] - - # allowed max speed - maxAngularSpeed = math.tau - - # Chassis config - trackWidth = drivetrain["TrackWidth"] - # Distance between centers of right and left wheels - wheelBase = drivetrain["WheelBase"] - - # Distance of front and back wheels - modulePositions = [ - Translation2d(wheelBase / 2, trackWidth / 2), - Translation2d(wheelBase / 2, -trackWidth / 2), - Translation2d(-wheelBase / 2, trackWidth / 2), - Translation2d(-wheelBase / 2, -trackWidth / 2), - ] - driveKinematics = SwerveDrive4Kinematics(*modulePositions) - - # Angular offsets of modules relative to chassis in radians - fPChassisAngularOffset = -math.pi / 2 - fSChassisAngularOffset = 0 - aPChassisAngularOffset = math.pi - aSChassisAngularOffset = math.pi / 2 - - -class ModuleConstants: - constants = getConstants("robot_hardware") - drivetrain = constants["driveTrain"] - NeoMotor = constants["NeoMotor"] - pid = constants["PID"] - - driveMotorPinionTeeth = drivetrain["driveMotorPinionTeeth"] - - # Calculations for drive motor conversion factors and feed forward - driveMotorFreeSpeedRps = NeoMotor["FreeSpeedRpm"] / 60 - WheelDiameterMeters = drivetrain["WheelDiameterMeters"] - WheelCircumferenceMeters = WheelDiameterMeters * math.pi - - driveMotorReduction = ( - drivetrain["BevelGearTeeth"] * drivetrain["firstStageSpurTeeth"] - ) / (driveMotorPinionTeeth * drivetrain["bevelPinion"]) - - driveEncoderPositionFactor = (WheelDiameterMeters * math.pi) / driveMotorReduction - driveEncoderVelocityFactor = ( - (WheelDiameterMeters * math.pi) / driveMotorReduction - ) / 60.0 - - turnEncoderPositionFactor = math.tau - turnEncoderVelocityFactor = math.tau / 60.0 - - turnEncoderPositionPIDMinInput = pid["turnEncoderPositionPIDMinInput"] # radian - turnEncoderPositionPIDMaxInput = turnEncoderPositionFactor - - driveMotorIdleMode = CANSparkMax.IdleMode.kBrake - turnMotorIdleMode = CANSparkMax.IdleMode.kBrake - - -class AutoConstants: - maxAngularSpeedRadiansPerSecond = math.pi - maxAngularSpeedRadiansPerSecondSquared = math.pi - - # Constraint for motion robot angle controller - thetaControllerConstraints = TrapezoidProfileRadians.Constraints( - maxAngularSpeedRadiansPerSecond, maxAngularSpeedRadiansPerSecondSquared - ) diff --git a/rio/ottomanempire b/rio/ottomanempire deleted file mode 160000 index bb2778c2..00000000 --- a/rio/ottomanempire +++ /dev/null @@ -1 +0,0 @@ -Subproject commit bb2778c242e08d3cf9dc4f6d894c46cd29103f30 diff --git a/rio/robot.py b/rio/robot.py old mode 100755 new mode 100644 index 1bf9b908..8282bee8 --- a/rio/robot.py +++ b/rio/robot.py @@ -1,29 +1,68 @@ -# vendor libs -import commands2 +#!/usr/bin/env python3 +# +# 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. +# + +# wpilib import wpilib +import wpimath +import wpilib.drive +import wpimath.filter +import wpimath.controller + +# drivetrain +from components.drivetrain import Drivetrain, kMaxSpeed -# robot container -from robotcontainer import RobotContainer +class MyRobot(wpilib.TimedRobot): + def robotInit(self) -> None: + """Robot initialization function""" + self.controller = wpilib.XboxController(0) + self.swerve = Drivetrain() -class UnnamedToaster(commands2.TimedCommandRobot): - def robotInit(self): - self.container = RobotContainer() - self.autonomousCommand = None + # Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1. + self.xspeedLimiter = wpimath.filter.SlewRateLimiter(3) + self.yspeedLimiter = wpimath.filter.SlewRateLimiter(3) + self.rotLimiter = wpimath.filter.SlewRateLimiter(3) - def autonomousInit(self) -> None: - self.autonomousCommand = self.container.getAutonomousCommand() + def autonomousPeriodic(self) -> None: + self.driveWithJoystick(False) + self.swerve.updateOdometry() - if self.autonomousCommand: - self.autonomousCommand.schedule() + def teleopPeriodic(self) -> None: + self.driveWithJoystick(True) - def teleopInit(self) -> None: - if self.autonomousCommand: - self.autonomousCommand.cancel() + def driveWithJoystick(self, fieldRelative: bool) -> None: + # Get the x speed. We are inverting this because Xbox controllers return + # negative values when we push forward. + xSpeed = ( + -self.xspeedLimiter.calculate( + wpimath.applyDeadband(self.controller.getLeftY(), 0.02) + ) + * kMaxSpeed + ) - def testInit(self) -> None: - commands2.CommandScheduler.getInstance().cancelAll() + # Get the y speed or sideways/strafe speed. We are inverting this because + # we want a positive value when we pull to the left. Xbox controllers + # return positive values when you pull to the right by default. + ySpeed = ( + -self.yspeedLimiter.calculate( + wpimath.applyDeadband(self.controller.getLeftX(), 0.02) + ) + * kMaxSpeed + ) + # Get the rate of angular rotation. We are inverting this because we want a + # positive value when we pull to the left (remember, CCW is positive in + # mathematics). Xbox controllers return positive values when you pull to + # the right by default. + rot = ( + -self.rotLimiter.calculate( + wpimath.applyDeadband(self.controller.getRightX(), 0.02) + ) + * kMaxSpeed + ) -if __name__ == "__main__": - wpilib.run(UnnamedToaster) + self.swerve.drive(xSpeed, ySpeed, rot, fieldRelative, self.getPeriod()) diff --git a/rio/robot_requirements.txt b/rio/robot_requirements.txt deleted file mode 100644 index b41433e5..00000000 --- a/rio/robot_requirements.txt +++ /dev/null @@ -1,14 +0,0 @@ -# These are not for you! -# Use pipenv install -# for developing. These -# requirements are only for -# the ROBOT - -PyYAML -robotpy -robotpy-commands-v2 -robotpy-cscore -robotpy-ctre -robotpy-hal -robotpy-rev -wpilib \ No newline at end of file diff --git a/rio/robotcontainer.py b/rio/robotcontainer.py deleted file mode 100644 index 55a2f40f..00000000 --- a/rio/robotcontainer.py +++ /dev/null @@ -1,83 +0,0 @@ -# commands -import commands2 - -from commands2 import cmd - -# wpilib -import wpimath -import wpilib - -from wpimath.controller import PIDController, ProfiledPIDControllerRadians -from wpimath.geometry import Pose2d, Rotation2d, Translation2d -from wpimath.trajectory import TrajectoryConfig, TrajectoryGenerator - -# subsystems -from subsystems.drivesubsystem import DriveSubsystem - -# constants -from constants.mathConstant import AutoConstants, DriveConstants -from constants.constants import getConstants - -# yes its the enitire ottoman empire -from ottomanempire.commands2.swervecontrollercommand import SwerveControllerCommand - -# misc -import math - - -class RobotContainer: - def __init__(self) -> None: - # constants - # drivetrain - hardwareConstants = getConstants("robot_hardware") - self.driveConst = hardwareConstants["driveTrain"] - self.autoConst = self.driveConst["autonomous"] - - # controller - controllerConst = getConstants("robot_controls") - self.driverConst = controllerConst["driver"] - - # The robot's subsystems - self.robotDrive = DriveSubsystem() - - # The driver's controller - self.driverController = wpilib.XboxController( - self.driverConst["ControllerPort"] - ) - - # Config buttons - self.configureButtonBindings() - - # default command - self.robotDrive.setDefaultCommand( - commands2.RunCommand( - lambda: self.robotDrive.drive( - -wpimath.applyDeadband( - self.driverController.getLeftY(), self.driverConst["DeadZone"] - ), - -wpimath.applyDeadband( - self.driverController.getLeftX(), self.driverConst["DeadZone"] - ), - -wpimath.applyDeadband( - self.driverController.getRightX(), self.driverConst["DeadZone"] - ), - True, - True, - ), - self.robotDrive, - ) - ) - - def configureButtonBindings(self) -> None: - pass - - def disablePIDSubsystems(self) -> None: - """ - 'Disables all ProfiledPIDSubsystem and PIDSubsystem instances. - This should be called on robot disable to prevent integral windup.' - - rev - """ - - def getAutonomousCommand(self) -> commands2.Command: - # rev has stuff here but I don't want to debug it - pass diff --git a/rio/simgui-window.json b/rio/simgui-window.json index d76a5956..610705b1 100644 --- a/rio/simgui-window.json +++ b/rio/simgui-window.json @@ -11,7 +11,7 @@ "userScale": "2", "width": "1280", "xpos": "636", - "ypos": "329" + "ypos": "24" } }, "Window": { @@ -48,7 +48,7 @@ "###Timing": { "Collapsed": "0", "Pos": "5,150", - "Size": "135,150" + "Size": "135,127" }, "Debug##Default": { "Collapsed": "0", diff --git a/rio/subsystems/drivesubsystem.py b/rio/subsystems/drivesubsystem.py deleted file mode 100644 index 4bd5c36f..00000000 --- a/rio/subsystems/drivesubsystem.py +++ /dev/null @@ -1,282 +0,0 @@ -# wpilib -import wpilib - -from wpimath.filter import SlewRateLimiter -from wpimath.geometry import Pose2d, Rotation2d -from wpimath.kinematics import ( - ChassisSpeeds, - SwerveModuleState, - SwerveDrive4Kinematics, - SwerveDrive4Odometry, -) - -# swerve stuff -import swerveutils -from .swervemodule import SwerveModule - -# constants -from constants.constants import getConstants -from constants.mathConstant import DriveConstants - -# misc -import math - -from commands2 import Subsystem -from typing import Tuple - - -class DriveSubsystem(Subsystem): - def __init__(self) -> None: - super().__init__() - # constants - constants = getConstants("robot_hardware") - self.driveConsts = constants["driveTrain"] - self.autoConsts = self.driveConsts["autonomous"] - - # module constants - self.fPConsts = self.driveConsts["FPMotors"] - self.fSConsts = self.driveConsts["FSMotors"] - self.aPConsts = self.driveConsts["APMotors"] - self.aSConsts = self.driveConsts["ASMotors"] - - # Create SwerveModules - self.fPModule = SwerveModule( - self.fPConsts["driveID"], - self.fPConsts["turnID"], - DriveConstants.fPChassisAngularOffset, - ) - - self.fSModule = SwerveModule( - self.fSConsts["driveID"], - self.fSConsts["turnID"], - DriveConstants.fSChassisAngularOffset, - ) - - self.aPModule = SwerveModule( - self.aPConsts["driveID"], - self.aPConsts["turnID"], - DriveConstants.aPChassisAngularOffset, - ) - - self.aSModule = SwerveModule( - self.aSConsts["driveID"], - self.aSConsts["turnID"], - DriveConstants.aSChassisAngularOffset, - ) - - # The gyro sensor - self.gyro = wpilib.ADIS16448_IMU() - - # Slew rate filter variables for controlling lateral acceleration - self.currentRotation = 0.0 - self.currentTranslationDir = 0.0 - self.currentTranslationMag = 0.0 - - self.magLimiter = SlewRateLimiter(self.driveConsts["MagnitudeSlewRate"]) - self.rotLimiter = SlewRateLimiter(self.driveConsts["RotationalSlewRate"]) - self.prevTime = wpilib.Timer.getFPGATimestamp() - - # Odometry class for tracking robot pose - self.odometry = SwerveDrive4Odometry( - DriveConstants.driveKinematics, - Rotation2d.fromDegrees(self.gyro.getAngle()), - ( - self.fPModule.getPosition(), - self.fSModule.getPosition(), - self.aPModule.getPosition(), - self.aSModule.getPosition(), - ), - ) - - def periodic(self) -> None: - # Update the odometry in the periodic block - self.odometry.update( - Rotation2d.fromDegrees(self.gyro.getAngle()), - ( - self.fPModule.getPosition(), - self.fSModule.getPosition(), - self.aPModule.getPosition(), - self.aSModule.getPosition(), - ), - ) - - def getPose(self) -> Pose2d: - """Returns the currently-estimated pose of the robot. - - :returns: The pose. - """ - return self.odometry.getPose() - - def resetOdometry(self, pose: Pose2d) -> None: - """Resets the odometry to the specified pose. - - :param pose: The pose to which to set the odometry. - - """ - self.odometry.resetPosition( - Rotation2d.fromDegrees(self.gyro.getAngle()), - ( - self.fPModule.getPosition(), - self.fSModule.getPosition(), - self.aPModule.getPosition(), - self.aSModule.getPosition(), - ), - pose, - ) - - def drive( - self, - xSpeed: float, - ySpeed: float, - rot: float, - fieldRelative: bool, - rateLimit: bool, - ) -> None: - """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 rot: Angular rate of the robot. - :param fieldRelative: Whether the provided x and y speeds are relative to the - field. - :param rateLimit: Whether to enable rate limiting for smoother control. - """ - - xSpeedCommanded = xSpeed - ySpeedCommanded = ySpeed - - if rateLimit: - # Convert XY to polar for rate limiting - inputTranslationDir = math.atan2(ySpeed, xSpeed) - inputTranslationMag = math.hypot(xSpeed, ySpeed) - - # Calculate the direction slew rate based on an estimate of the lateral acceleration - if self.currentTranslationMag != 0.0: - directionSlewRate = abs( - self.driveConsts["DirectionSlewRate"] / self.currentTranslationMag - ) - else: - directionSlewRate = 500.0 - # some high number that means the slew rate is effectively instantaneous - - currentTime = wpilib.Timer.getFPGATimestamp() - elapsedTime = currentTime - self.prevTime - angleDif = swerveutils.angleDifference( - inputTranslationDir, self.currentTranslationDir - ) - if angleDif < 0.45 * math.pi: - self.currentTranslationDir = swerveutils.stepTowardsCircular( - self.currentTranslationDir, - inputTranslationDir, - directionSlewRate * elapsedTime, - ) - self.currentTranslationMag = self.magLimiter.calculate( - inputTranslationMag - ) - - elif angleDif > 0.85 * math.pi: - # some small number to avoid floating-point errors with equality checking - # keep currentTranslationDir unchanged - if self.currentTranslationMag > 1e-4: - self.currentTranslationMag = self.magLimiter.calculate(0.0) - else: - self.currentTranslationDir = swerveutils.wrapAngle( - self.currentTranslationDir + math.pi - ) - self.currentTranslationMag = self.magLimiter.calculate( - inputTranslationMag - ) - - else: - self.currentTranslationDir = swerveutils.stepTowardsCircular( - self.currentTranslationDir, - inputTranslationDir, - directionSlewRate * elapsedTime, - ) - self.currentTranslationMag = self.magLimiter.calculate(0.0) - - self.prevTime = currentTime - - xSpeedCommanded = self.currentTranslationMag * math.cos( - self.currentTranslationDir - ) - ySpeedCommanded = self.currentTranslationMag * math.sin( - self.currentTranslationDir - ) - self.currentRotation = self.rotLimiter.calculate(rot) - - else: - self.currentRotation = rot - - # Convert the commanded speeds into the correct units for the drivetrain - xSpeedDelivered = xSpeedCommanded * self.autoConsts["maxSpeedMetersPerSecond"] - ySpeedDelivered = ySpeedCommanded * self.autoConsts["maxSpeedMetersPerSecond"] - rotDelivered = self.currentRotation * DriveConstants.maxAngularSpeed - - swerveModuleStates = DriveConstants.driveKinematics.toSwerveModuleStates( - ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeedDelivered, - ySpeedDelivered, - rotDelivered, - Rotation2d.fromDegrees(self.gyro.getAngle()), - ) - if fieldRelative - else ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered) - ) - fl, fr, rl, rr = SwerveDrive4Kinematics.desaturateWheelSpeeds( - swerveModuleStates, self.autoConsts["maxSpeedMetersPerSecond"] - ) - self.fPModule.setDesiredState(fl) - self.fSModule.setDesiredState(fr) - self.aPModule.setDesiredState(rl) - self.aSModule.setDesiredState(rr) - - def setX(self) -> None: - """Sets the wheels into an X formation to prevent movement.""" - self.fPModule.setDesiredState(SwerveModuleState(0, Rotation2d.fromDegrees(45))) - self.fSModule.setDesiredState(SwerveModuleState(0, Rotation2d.fromDegrees(-45))) - self.aPModule.setDesiredState(SwerveModuleState(0, Rotation2d.fromDegrees(-45))) - self.aSModule.setDesiredState(SwerveModuleState(0, Rotation2d.fromDegrees(45))) - - def setModuleStates( - self, - desiredStates: Tuple[ - SwerveModuleState, SwerveModuleState, SwerveModuleState, SwerveModuleState - ], - ) -> None: - """Sets the swerve ModuleStates. - - :param desiredStates: The desired SwerveModule states. - """ - fl, fr, rl, rr = SwerveDrive4Kinematics.desaturateWheelSpeeds( - desiredStates, self.autoConsts["maxSpeedMetersPerSecond"] - ) - self.fPModule.setDesiredState(fl) - self.fSModule.setDesiredState(fr) - self.aPModule.setDesiredState(rl) - self.aSModule.setDesiredState(rr) - - def resetEncoders(self) -> None: - """Resets the drive encoders to currently read a position of 0.""" - self.fPModule.resetEncoders() - self.aPModule.resetEncoders() - self.fSModule.resetEncoders() - self.aSModule.resetEncoders() - - def zeroHeading(self) -> None: - """Zeroes the heading of the robot.""" - self.gyro.reset() - - def getHeading(self) -> float: - """Returns the heading of the robot. - - :returns: the robot's heading in degrees, from -180 to 180 - """ - return Rotation2d.fromDegrees(self.gyro.getAngle()).degrees() - - def getTurnRate(self) -> float: - """Returns the turn rate of the robot. - - :returns: The turn rate of the robot, in degrees per second - """ - return self.gyro.getRate() * (-1.0 if self.driveConsts["GyroReversed"] else 1.0) diff --git a/rio/subsystems/swervemodule.py b/rio/subsystems/swervemodule.py deleted file mode 100644 index 5b18a8c5..00000000 --- a/rio/subsystems/swervemodule.py +++ /dev/null @@ -1,152 +0,0 @@ -# vendor libs -from rev import CANSparkMax, SparkMaxAbsoluteEncoder -from wpimath.geometry import Rotation2d -from wpimath.kinematics import SwerveModuleState, SwerveModulePosition - -# constants -from constants.mathConstant import ModuleConstants -from constants.constants import getConstants - - -class SwerveModule: - def __init__(self, driveCANId: int, turnCANId: int, chassisAngularOffset: float): - """ - A Swerve Module - """ - # constants - # hardware - hardwareConstants = getConstants("robot_hardware") - self.driveconsts = hardwareConstants["driveTrain"] - - # PID - self.pidConsts = getConstants("robot_pid")["PID"] - - self.chassisAngularOffset = 0 - self.desiredState = SwerveModuleState(0.0, Rotation2d()) - - self.driveSparkMax = CANSparkMax(driveCANId, CANSparkMax.MotorType.kBrushless) - self.turnSparkMax = CANSparkMax(turnCANId, CANSparkMax.MotorType.kBrushless) - - # This helps get it to a known state for less tomfoolery - self.driveSparkMax.restoreFactoryDefaults() - self.turnSparkMax.restoreFactoryDefaults() - - self.driveEncoder = self.driveSparkMax.getEncoder() - self.turnEncoder = self.turnSparkMax.getAbsoluteEncoder( - SparkMaxAbsoluteEncoder.Type.kDutyCycle - ) - self.drivePIDController = self.driveSparkMax.getPIDController() - self.turnPIDController = self.turnSparkMax.getPIDController() - self.drivePIDController.setFeedbackDevice(self.driveEncoder) - self.turnPIDController.setFeedbackDevice(self.turnEncoder) - - # Appling position and velocity conversion factors for driving encoder - self.driveEncoder.setPositionConversionFactor( - ModuleConstants.driveEncoderPositionFactor - ) - self.driveEncoder.setVelocityConversionFactor( - ModuleConstants.driveEncoderVelocityFactor - ) - - # Appling position and velocity conversion factors for turning encoder - self.turnEncoder.setPositionConversionFactor( - ModuleConstants.turnEncoderPositionFactor - ) - self.turnEncoder.setVelocityConversionFactor( - ModuleConstants.turnEncoderVelocityFactor - ) - - self.turnEncoder.setInverted(self.driveconsts["turnEncoderInverted"]) - - # Enable PID wrapping - self.turnPIDController.setPositionPIDWrappingEnabled(True) - self.turnPIDController.setPositionPIDWrappingMinInput( - ModuleConstants.turnEncoderPositionPIDMinInput - ) - self.turnPIDController.setPositionPIDWrappingMaxInput( - ModuleConstants.turnEncoderPositionPIDMaxInput - ) - - # PIDS - self.drivePIDController.setP(self.pidConsts["driveP"]) - self.drivePIDController.setI(self.pidConsts["driveI"]) - self.drivePIDController.setD(self.pidConsts["driveD"]) - self.drivePIDController.setFF(self.pidConsts["driveFF"]) - self.drivePIDController.setOutputRange( - self.pidConsts["driveMinOutput"], self.pidConsts["driveMaxOutput"] - ) - - self.turnPIDController.setP(self.pidConsts["turnP"]) - self.turnPIDController.setI(self.pidConsts["turnI"]) - self.turnPIDController.setD(self.pidConsts["turnD"]) - self.turnPIDController.setFF(self.pidConsts["turnFF"]) - self.turnPIDController.setOutputRange( - self.pidConsts["turnMinOutput"], self.pidConsts["turnMaxOutput"] - ) - - self.driveSparkMax.setIdleMode(ModuleConstants.driveMotorIdleMode) - self.turnSparkMax.setIdleMode(ModuleConstants.turnMotorIdleMode) - self.driveSparkMax.setSmartCurrentLimit( - self.driveconsts["driveMotorCurrentLimit"] - ) - self.turnSparkMax.setSmartCurrentLimit( - self.driveconsts["turnMotorCurrentLimit"] - ) - - # Save the SPARK MAX configs - self.driveSparkMax.burnFlash() - self.turnSparkMax.burnFlash() - - self.chassisAngularOffset = chassisAngularOffset - self.desiredState.angle = Rotation2d(self.turnEncoder.getPosition()) - self.driveEncoder.setPosition(0) - - def getState(self) -> SwerveModuleState: - """ - This is relative to the chassis - """ - return SwerveModuleState( - self.driveEncoder.getVelocity(), - Rotation2d(self.turnEncoder.getPosition() - self.chassisAngularOffset), - ) - - def getPosition(self) -> SwerveModulePosition: - """ - This is relative to the chassis - """ - return SwerveModulePosition( - self.driveEncoder.getPosition(), - Rotation2d(self.turnEncoder.getPosition() - self.chassisAngularOffset), - ) - - def setDesiredState(self, desiredState: SwerveModuleState) -> None: - """ - sets a desired state - """ - # add angular offset - correctedDesiredState = SwerveModuleState() - correctedDesiredState.speed = desiredState.speed - correctedDesiredState.angle = desiredState.angle + Rotation2d( - self.chassisAngularOffset - ) - - # Optimizeation - optimizedDesiredState = SwerveModuleState.optimize( - correctedDesiredState, Rotation2d(self.turnEncoder.getPosition()) - ) - - # got to setpoints - self.drivePIDController.setReference( - optimizedDesiredState.speed, CANSparkMax.ControlType.kVelocity - ) - self.turnPIDController.setReference( - optimizedDesiredState.angle.radians(), CANSparkMax.ControlType.kPosition - ) - - self.desiredState = desiredState - - def resetEncoders(self) -> None: - """ - '-+es all the SwerveModule encoders.' - rev - """ - self.driveEncoder.setPosition(0) diff --git a/rio/swerveutils.py b/rio/swerveutils.py deleted file mode 100644 index 206c18c6..00000000 --- a/rio/swerveutils.py +++ /dev/null @@ -1,91 +0,0 @@ -import math - - -def stepTowards(current: float, target: float, stepsize: float) -> float: - """ - Steps a value towards a target with a specified step size. - - :param current: The current or starting value. Can be positive or negative. - :param target: The target value the algorithm will step towards. Can be positive or negative. - :param stepsize: The maximum step size that can be taken. - - :returns: The new value for {@code current} after performing the specified step towards the specified target. - """ - - if abs(current - target) <= stepsize: - return target - - elif target < current: - return current - stepsize - - else: - return current + stepsize - - -def stepTowardsCircular(current: float, target: float, stepsize: float) -> float: - """ - Steps a value (angle) towards a target (angle) taking the shortest path with a specified step size. - - :param current: The current or starting angle (in radians). Can lie outside the 0 to 2*PI range. - :param target: The target angle (in radians) the algorithm will step towards. Can lie outside the 0 to 2*PI range. - :param stepsize: The maximum step size that can be taken (in radians). - - :returns: The new angle (in radians) for {@code current} after performing the specified step towards the specified target. - This value will always lie in the range 0 to 2*PI (exclusive). - """ - - current = wrapAngle(current) - target = wrapAngle(target) - - stepDirection = math.copysign(target - current, 1) - difference = abs(current - target) - - if difference <= stepsize: - return target - elif difference > math.pi: # does the system need to wrap over eventually? - # handle the special case where you can reach the target in one step while also wrapping - if ( - current + math.tau - target < stepsize - or target + math.tau - current < stepsize - ): - return target - else: - # this will handle wrapping gracefully - return wrapAngle(current - stepDirection * stepsize) - else: - return current + stepDirection * stepsize - - -def angleDifference(angleA: float, angleB: float) -> float: - """ - Finds the (unsigned) minimum difference between two angles including calculating across 0. - - :param angleA: An angle (in radians). - :param angleB: An angle (in radians). - - :returns: The (unsigned) minimum difference between the two angles (in radians). - """ - difference = abs(angleA - angleB) - return math.tau - difference if difference > math.pi else difference - - -def wrapAngle(angle: float) -> float: - """ - Wraps an angle until it lies within the range from 0 to 2*PI (exclusive). - - :param angle: The angle (in radians) to wrap. Can be positive or negative and can lie multiple wraps outside the output range. - - :returns: An angle (in radians) from 0 and 2*PI (exclusive). - """ - - twoPi = math.tau - - # Handle this case separately to avoid floating point errors with the floor after the division in the case below - if angle == twoPi: - return 0.0 - elif angle > twoPi: - return angle - twoPi * math.floor(angle / twoPi) - elif angle < 0.0: - return angle + twoPi * (math.floor((-angle) / twoPi) + 1) - else: - return angle diff --git a/rio/tests/pyfrc_test.py b/rio/tests/pyfrc_test.py index 55ee6b49..982b0272 100644 --- a/rio/tests/pyfrc_test.py +++ b/rio/tests/pyfrc_test.py @@ -1,18 +1,13 @@ +# +# 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. +# + """ -This module provides pyfrc tests useful for almost any robot. -use `python -m robotpy test` to run + This test module imports tests that come with pyfrc, and can be used + to test basic functionality of just about any robot. """ from pyfrc.tests import * - -# Testable modules -from constants.constants import getConstants -from subsystems.drivesubsystem import DriveSubsystem - - -def test_yaml(): - assert len(getConstants("robot_controls")) > 0 - - -def test_nothing(): - assert True # Tests if true is true! +from magicbot.magicbot_tests import * From e2dea361135aa87287fed59ed621ab3ebdbc180b Mon Sep 17 00:00:00 2001 From: kredcool Date: Sun, 14 Jan 2024 15:46:26 -0500 Subject: [PATCH 19/61] this is missing a few functions will add in next git add -A; --- rio/components/swervemodule.py | 65 ++++++++++++++++++---------------- rio/simgui-window.json | 10 +++--- 2 files changed, 39 insertions(+), 36 deletions(-) diff --git a/rio/components/swervemodule.py b/rio/components/swervemodule.py index e06367c6..fd2cf937 100644 --- a/rio/components/swervemodule.py +++ b/rio/components/swervemodule.py @@ -14,6 +14,9 @@ import wpimath.controller import wpimath.trajectory +# rev +from rev import CANSparkMaxLowLevel, CANSparkMax + kWheelRadius = 0.0508 kEncoderResolution = 4096 kModuleMaxAngularVelocity = math.pi @@ -39,27 +42,31 @@ def __init__( :param turningEncoderChannelA: DIO input for the turning encoder channel A :param turningEncoderChannelB: DIO input for the turning encoder channel B """ - self.driveMotor = wpilib.PWMSparkMax(driveMotorChannel) - self.turningMotor = wpilib.PWMSparkMax(turningMotorChannel) - - self.driveEncoder = wpilib.Encoder(driveEncoderChannelA, driveEncoderChannelB) - self.turningEncoder = wpilib.Encoder( - turningEncoderChannelA, turningEncoderChannelB + self.driveMotor = CANSparkMax( + driveMotorChannel, CANSparkMaxLowLevel.MotorType.kBrushless ) + self.turningMotor = CANSparkMax( + turningMotorChannel, CANSparkMaxLowLevel.MotorType.kBrushless + ) + + self.driveEncoder = self.driveMotor.getEncoder() + self.turningEncoder = self.turningMotor.getEncoder() # Gains are for example purposes only - must be determined for your own robot! - self.drivePIDController = wpimath.controller.PIDController(1, 0, 0) + self.drivePIDController = self.driveMotor.getPIDController() + + self.drivePIDController.setP(0) + self.drivePIDController.setI(0) + self.drivePIDController.setD(0) + self.drivePIDController.setFF(0) # Gains are for example purposes only - must be determined for your own robot! - self.turningPIDController = wpimath.controller.ProfiledPIDController( - 1, - 0, - 0, - wpimath.trajectory.TrapezoidProfile.Constraints( - kModuleMaxAngularVelocity, - kModuleMaxAngularAcceleration, - ), - ) + self.turningPIDController = self.turningMotor.getPIDController() + + self.turningPIDController.setP(0) + self.turningPIDController.setI(0) + self.turningPIDController.setD(0) + self.turningPIDController.setFF(0) # Gains are for example purposes only - must be determined for your own robot! self.driveFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 3) @@ -68,18 +75,14 @@ def __init__( # Set the distance per pulse for the drive encoder. We can simply use the # distance traveled for one rotation of the wheel divided by the encoder # resolution. - self.driveEncoder.setDistancePerPulse( - math.tau * kWheelRadius / kEncoderResolution + self.driveEncoder.setPositionConversionFactor( + (math.tau * kWheelRadius / kEncoderResolution) ) # Set the distance (in this case, angle) in radians per pulse for the turning encoder. # This is the the angle through an entire rotation (2 * pi) divided by the # encoder resolution. - self.turningEncoder.setDistancePerPulse(math.tau / kEncoderResolution) - - # Limit the PID Controller's input range between -pi and pi and set the input - # to be continuous. - self.turningPIDController.enableContinuousInput(-math.pi, math.pi) + self.turningEncoder.setPositionConversionFactor((math.tau / kEncoderResolution)) def getState(self) -> wpimath.kinematics.SwerveModuleState: """Returns the current state of the module. @@ -87,18 +90,18 @@ def getState(self) -> wpimath.kinematics.SwerveModuleState: :returns: The current state of the module. """ return wpimath.kinematics.SwerveModuleState( - self.driveEncoder.getRate(), - wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()), + self.driveEncoder.getPosition(), + wpimath.geometry.Rotation2d(self.turningEncoder.getVelocity()), ) - def getPosition(self) -> wpimath.kinematics.SwerveModulePosition: + def aquirePosition(self) -> wpimath.kinematics.SwerveModulePosition: """Returns the current position of the module. :returns: The current position of the module. """ return wpimath.kinematics.SwerveModulePosition( - self.driveEncoder.getRate(), - wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()), + self.driveEncoder.getPosition(), + wpimath.geometry.Rotation2d(self.turningEncoder.getVelocity()), ) def setDesiredState( @@ -109,7 +112,7 @@ def setDesiredState( :param desiredState: Desired state with speed and angle. """ - encoderRotation = wpimath.geometry.Rotation2d(self.turningEncoder.getDistance()) + encoderRotation = wpimath.geometry.Rotation2d(self.turningEncoder.getVelocity()) # Optimize the reference state to avoid spinning further than 90 degrees state = wpimath.kinematics.SwerveModuleState.optimize( @@ -123,14 +126,14 @@ def setDesiredState( # Calculate the drive output from the drive PID controller. driveOutput = self.drivePIDController.calculate( - self.driveEncoder.getRate(), state.speed + self.driveEncoder.getPosition(), state.speed ) driveFeedforward = self.driveFeedforward.calculate(state.speed) # Calculate the turning motor output from the turning PID controller. turnOutput = self.turningPIDController.calculate( - self.turningEncoder.getDistance(), state.angle.radians() + self.turningEncoder.getVelocity(), state.angle.radians() ) turnFeedforward = self.turnFeedforward.calculate( diff --git a/rio/simgui-window.json b/rio/simgui-window.json index 610705b1..11af7805 100644 --- a/rio/simgui-window.json +++ b/rio/simgui-window.json @@ -18,12 +18,12 @@ "###FMS": { "Collapsed": "0", "Pos": "5,540", - "Size": "283,146" + "Size": "32,35" }, "###Joysticks": { "Collapsed": "0", "Pos": "250,465", - "Size": "796,73" + "Size": "32,35" }, "###NetworkTables": { "Collapsed": "0", @@ -43,12 +43,12 @@ "###System Joysticks": { "Collapsed": "0", "Pos": "5,350", - "Size": "192,218" + "Size": "32,35" }, "###Timing": { "Collapsed": "0", "Pos": "5,150", - "Size": "135,127" + "Size": "32,35" }, "Debug##Default": { "Collapsed": "0", @@ -58,7 +58,7 @@ "Robot State": { "Collapsed": "0", "Pos": "5,20", - "Size": "92,99" + "Size": "32,35" } } } From 6b7d063a1e611c58a5bab0f94b93c8f4cb9a0c35 Mon Sep 17 00:00:00 2001 From: kredcool Date: Mon, 15 Jan 2024 12:26:16 -0500 Subject: [PATCH 20/61] completed changes and fixed error while siming locally --- rio/components/drivetrain.py | 309 +++++++++++++++++++++++------- rio/components/maxswervemodule.py | 173 +++++++++++++++++ rio/components/swervemodule.py | 144 -------------- rio/constants/__init__.py | 0 rio/constants/constants.py | 141 ++++++++++++++ rio/hello | 135 +++++++++++++ rio/robot.py | 28 +-- rio/simgui-ds.json | 61 +++--- rio/simgui-window.json | 14 +- rio/swerveutils.py | 87 +++++++++ 10 files changed, 810 insertions(+), 282 deletions(-) create mode 100644 rio/components/maxswervemodule.py delete mode 100644 rio/components/swervemodule.py create mode 100644 rio/constants/__init__.py create mode 100644 rio/constants/constants.py create mode 100644 rio/hello create mode 100644 rio/swerveutils.py diff --git a/rio/components/drivetrain.py b/rio/components/drivetrain.py index bbbc0a34..dcacae9a 100644 --- a/rio/components/drivetrain.py +++ b/rio/components/drivetrain.py @@ -1,61 +1,111 @@ -# -# 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. -# - -# math import math +import typing -# wpilib import wpilib -import wpimath.geometry -import wpimath.kinematics -# swerves -from .swervemodule import SwerveModule +from commands2 import Subsystem +from wpimath.filter import SlewRateLimiter +from wpimath.geometry import Pose2d, Rotation2d +from wpimath.kinematics import ( + ChassisSpeeds, + SwerveModuleState, + SwerveDrive4Kinematics, + SwerveDrive4Odometry, +) +from navx import AHRS +from constants.constants import DriveConstants +import swerveutils +from .maxswervemodule import MAXSwerveModule -kMaxSpeed = 3.0 # 3 meters per second -kMaxAngularSpeed = math.pi # 1/2 rotation per second +class DriveSubsystem(Subsystem): + def __init__(self) -> None: + super().__init__() -class Drivetrain: - """ - Represents a swerve drive style drivetrain. - """ + # Create MAXSwerveModules + self.frontLeft = MAXSwerveModule( + DriveConstants.kFrontLeftDrivingCanId, + DriveConstants.kFrontLeftTurningCanId, + DriveConstants.kFrontLeftChassisAngularOffset, + ) - def __init__(self) -> None: - self.frontLeftLocation = wpimath.geometry.Translation2d(0.381, 0.381) - self.frontRightLocation = wpimath.geometry.Translation2d(0.381, -0.381) - self.backLeftLocation = wpimath.geometry.Translation2d(-0.381, 0.381) - self.backRightLocation = wpimath.geometry.Translation2d(-0.381, -0.381) - - self.frontLeft = SwerveModule(1, 2, 0, 1, 2, 3) - self.frontRight = SwerveModule(3, 4, 4, 5, 6, 7) - self.backLeft = SwerveModule(5, 6, 8, 9, 10, 11) - self.backRight = SwerveModule(7, 8, 12, 13, 14, 15) - - self.gyro = wpilib.AnalogGyro(0) - - self.kinematics = wpimath.kinematics.SwerveDrive4Kinematics( - self.frontLeftLocation, - self.frontRightLocation, - self.backLeftLocation, - self.backRightLocation, + self.frontRight = MAXSwerveModule( + DriveConstants.kFrontRightDrivingCanId, + DriveConstants.kFrontRightTurningCanId, + DriveConstants.kFrontRightChassisAngularOffset, ) - self.odometry = wpimath.kinematics.SwerveDrive4Odometry( - self.kinematics, - self.gyro.getRotation2d(), + self.rearLeft = MAXSwerveModule( + DriveConstants.kRearLeftDrivingCanId, + DriveConstants.kRearLeftTurningCanId, + DriveConstants.kBackLeftChassisAngularOffset, + ) + + self.rearRight = MAXSwerveModule( + DriveConstants.kRearRightDrivingCanId, + DriveConstants.kRearRightTurningCanId, + DriveConstants.kBackRightChassisAngularOffset, + ) + + # The gyro sensor + self.gyro = AHRS.create_spi() + + # Slew rate filter variables for controlling lateral acceleration + self.currentRotation = 0.0 + self.currentTranslationDir = 0.0 + self.currentTranslationMag = 0.0 + + self.magLimiter = SlewRateLimiter(DriveConstants.kMagnitudeSlewRate) + self.rotLimiter = SlewRateLimiter(DriveConstants.kRotationalSlewRate) + self.prevTime = wpilib.Timer.getFPGATimestamp() + + # Odometry class for tracking robot pose + self.odometry = SwerveDrive4Odometry( + DriveConstants.kDriveKinematics, + Rotation2d.fromDegrees(self.gyro.getAngle()), ( self.frontLeft.getPosition(), self.frontRight.getPosition(), - self.backLeft.getPosition(), - self.backRight.getPosition(), + self.rearLeft.getPosition(), + self.rearRight.getPosition(), ), ) - self.gyro.reset() + def periodic(self) -> None: + # Update the odometry in the periodic block + self.odometry.update( + Rotation2d.fromDegrees(self.gyro.getAngle()), + ( + self.frontLeft.getPosition(), + self.frontRight.getPosition(), + self.rearLeft.getPosition(), + self.rearRight.getPosition(), + ), + ) + + def getPose(self) -> Pose2d: + """Returns the currently-estimated pose of the robot. + + :returns: The pose. + """ + return self.odometry.getPose() + + def resetOdometry(self, pose: Pose2d) -> None: + """Resets the odometry to the specified pose. + + :param pose: The pose to which to set the odometry. + + """ + self.odometry.resetPosition( + Rotation2d.fromDegrees(self.gyro.getAngle()), + ( + self.frontLeft.getPosition(), + self.frontRight.getPosition(), + self.rearLeft.getPosition(), + self.rearRight.getPosition(), + ), + pose, + ) def drive( self, @@ -63,42 +113,155 @@ def drive( ySpeed: float, rot: float, fieldRelative: bool, - periodSeconds: float, + rateLimit: bool, ) -> None: + """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 rot: Angular rate of the robot. + :param fieldRelative: Whether the provided x and y speeds are relative to the + field. + :param rateLimit: Whether to enable rate limiting for smoother control. """ - 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 rot: Angular rate of the robot. - :param fieldRelative: Whether the provided x and y speeds are relative to the field. - :param periodSeconds: Time - """ - swerveModuleStates = self.kinematics.toSwerveModuleStates( - wpimath.kinematics.ChassisSpeeds.discretize( - wpimath.kinematics.ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeed, ySpeed, rot, self.gyro.getRotation2d() + + xSpeedCommanded = xSpeed + ySpeedCommanded = ySpeed + + if rateLimit: + # Convert XY to polar for rate limiting + inputTranslationDir = math.atan2(ySpeed, xSpeed) + inputTranslationMag = math.hypot(xSpeed, ySpeed) + + # Calculate the direction slew rate based on an estimate of the lateral acceleration + if self.currentTranslationMag != 0.0: + directionSlewRate = abs( + DriveConstants.kDirectionSlewRate / self.currentTranslationMag ) - if fieldRelative - else wpimath.kinematics.ChassisSpeeds(xSpeed, ySpeed, rot), - periodSeconds, + else: + directionSlewRate = 500.0 + # some high number that means the slew rate is effectively instantaneous + + currentTime = wpilib.Timer.getFPGATimestamp() + elapsedTime = currentTime - self.prevTime + angleDif = swerveutils.angleDifference( + inputTranslationDir, self.currentTranslationDir + ) + if angleDif < 0.45 * math.pi: + self.currentTranslationDir = swerveutils.stepTowardsCircular( + self.currentTranslationDir, + inputTranslationDir, + directionSlewRate * elapsedTime, + ) + self.currentTranslationMag = self.magLimiter.calculate( + inputTranslationMag + ) + + elif angleDif > 0.85 * math.pi: + # some small number to avoid floating-point errors with equality checking + # keep currentTranslationDir unchanged + if self.currentTranslationMag > 1e-4: + self.currentTranslationMag = self.magLimiter.calculate(0.0) + else: + self.currentTranslationDir = swerveutils.wrapAngle( + self.currentTranslationDir + math.pi + ) + self.currentTranslationMag = self.magLimiter.calculate( + inputTranslationMag + ) + + else: + self.currentTranslationDir = swerveutils.stepTowardsCircular( + self.currentTranslationDir, + inputTranslationDir, + directionSlewRate * elapsedTime, + ) + self.currentTranslationMag = self.magLimiter.calculate(0.0) + + self.prevTime = currentTime + + xSpeedCommanded = self.currentTranslationMag * math.cos( + self.currentTranslationDir ) + ySpeedCommanded = self.currentTranslationMag * math.sin( + self.currentTranslationDir + ) + self.currentRotation = self.rotLimiter.calculate(rot) + + else: + self.currentRotation = rot + + # Convert the commanded speeds into the correct units for the drivetrain + xSpeedDelivered = xSpeedCommanded * DriveConstants.kMaxSpeedMetersPerSecond + ySpeedDelivered = ySpeedCommanded * DriveConstants.kMaxSpeedMetersPerSecond + rotDelivered = self.currentRotation * DriveConstants.kMaxAngularSpeed + + swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( + ChassisSpeeds.fromFieldRelativeSpeeds( + xSpeedDelivered, + ySpeedDelivered, + rotDelivered, + Rotation2d.fromDegrees(self.gyro.getAngle()), + ) + if fieldRelative + else ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered) ) - wpimath.kinematics.SwerveDrive4Kinematics.desaturateWheelSpeeds( - swerveModuleStates, kMaxSpeed + fl, fr, rl, rr = SwerveDrive4Kinematics.desaturateWheelSpeeds( + swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond ) - self.frontLeft.setDesiredState(swerveModuleStates[0]) - self.frontRight.setDesiredState(swerveModuleStates[1]) - self.backLeft.setDesiredState(swerveModuleStates[2]) - self.backRight.setDesiredState(swerveModuleStates[3]) + self.frontLeft.setDesiredState(fl) + self.frontRight.setDesiredState(fr) + self.rearLeft.setDesiredState(rl) + self.rearRight.setDesiredState(rr) - def updateOdometry(self) -> None: - """Updates the field relative position of the robot.""" - self.odometry.update( - self.gyro.getRotation2d(), - ( - self.frontLeft.getPosition(), - self.frontRight.getPosition(), - self.backLeft.getPosition(), - self.backRight.getPosition(), - ), + def setX(self) -> None: + """Sets the wheels into an X formation to prevent movement.""" + self.frontLeft.setDesiredState(SwerveModuleState(0, Rotation2d.fromDegrees(45))) + self.frontRight.setDesiredState( + SwerveModuleState(0, Rotation2d.fromDegrees(-45)) ) + self.rearLeft.setDesiredState(SwerveModuleState(0, Rotation2d.fromDegrees(-45))) + self.rearRight.setDesiredState(SwerveModuleState(0, Rotation2d.fromDegrees(45))) + + def setModuleStates( + self, + desiredStates: typing.Tuple[ + SwerveModuleState, SwerveModuleState, SwerveModuleState, SwerveModuleState + ], + ) -> None: + """Sets the swerve ModuleStates. + + :param desiredStates: The desired SwerveModule states. + """ + fl, fr, rl, rr = SwerveDrive4Kinematics.desaturateWheelSpeeds( + desiredStates, DriveConstants.kMaxSpeedMetersPerSecond + ) + self.frontLeft.setDesiredState(fl) + self.frontRight.setDesiredState(fr) + self.rearLeft.setDesiredState(rl) + self.rearRight.setDesiredState(rr) + + def resetEncoders(self) -> None: + """Resets the drive encoders to currently read a position of 0.""" + self.frontLeft.resetEncoders() + self.rearLeft.resetEncoders() + self.frontRight.resetEncoders() + self.rearRight.resetEncoders() + + def zeroHeading(self) -> None: + """Zeroes the heading of the robot.""" + self.gyro.reset() + + def getHeading(self) -> float: + """Returns the heading of the robot. + + :returns: the robot's heading in degrees, from -180 to 180 + """ + return Rotation2d.fromDegrees(self.gyro.getAngle()).degrees() + + def getTurnRate(self) -> float: + """Returns the turn rate of the robot. + + :returns: The turn rate of the robot, in degrees per second + """ + return self.gyro.getRate() * (-1.0 if DriveConstants.kGyroReversed else 1.0) diff --git a/rio/components/maxswervemodule.py b/rio/components/maxswervemodule.py new file mode 100644 index 00000000..66a9c88f --- /dev/null +++ b/rio/components/maxswervemodule.py @@ -0,0 +1,173 @@ +from rev import CANSparkMax, SparkMaxAbsoluteEncoder +from wpimath.geometry import Rotation2d +from wpimath.kinematics import SwerveModuleState, SwerveModulePosition + +from constants.constants import ModuleConstants + + +class MAXSwerveModule: + def __init__( + self, drivingCANId: int, turningCANId: int, chassisAngularOffset: float + ) -> None: + """Constructs a MAXSwerveModule and configures the driving and turning motor, + encoder, and PID controller. This configuration is specific to the REV + MAXSwerve Module built with NEOs, SPARKS MAX, and a Through Bore + Encoder. + """ + + self.chassisAngularOffset = 0 + self.desiredState = SwerveModuleState(0.0, Rotation2d()) + + self.drivingSparkMax = CANSparkMax( + drivingCANId, CANSparkMax.MotorType.kBrushless + ) + self.turningSparkMax = CANSparkMax( + turningCANId, CANSparkMax.MotorType.kBrushless + ) + + # Factory reset, so we get the SPARKS MAX to a known state before configuring + # them. This is useful in case a SPARK MAX is swapped out. + self.drivingSparkMax.restoreFactoryDefaults() + self.turningSparkMax.restoreFactoryDefaults() + + # Setup encoders and PID controllers for the driving and turning SPARKS MAX. + self.drivingEncoder = self.drivingSparkMax.getEncoder() + self.turningEncoder = self.turningSparkMax.getAbsoluteEncoder( + SparkMaxAbsoluteEncoder.Type.kDutyCycle + ) + self.drivingPIDController = self.drivingSparkMax.getPIDController() + self.turningPIDController = self.turningSparkMax.getPIDController() + self.drivingPIDController.setFeedbackDevice(self.drivingEncoder) + self.turningPIDController.setFeedbackDevice(self.turningEncoder) + + # Apply position and velocity conversion factors for the driving encoder. The + # native units for position and velocity are rotations and RPM, respectively, + # but we want meters and meters per second to use with WPILib's swerve APIs. + self.drivingEncoder.setPositionConversionFactor( + ModuleConstants.kDrivingEncoderPositionFactor + ) + self.drivingEncoder.setVelocityConversionFactor( + ModuleConstants.kDrivingEncoderVelocityFactor + ) + + # Apply position and velocity conversion factors for the turning encoder. We + # want these in radians and radians per second to use with WPILib's swerve + # APIs. + self.turningEncoder.setPositionConversionFactor( + ModuleConstants.kTurningEncoderPositionFactor + ) + self.turningEncoder.setVelocityConversionFactor( + ModuleConstants.kTurningEncoderVelocityFactor + ) + + # Invert the turning encoder, since the output shaft rotates in the opposite direction of + # the steering motor in the MAXSwerve Module. + self.turningEncoder.setInverted(ModuleConstants.kTurningEncoderInverted) + + # Enable PID wrap around for the turning motor. This will allow the PID + # controller to go through 0 to get to the setpoint i.e. going from 350 degrees + # to 10 degrees will go through 0 rather than the other direction which is a + # longer route. + self.turningPIDController.setPositionPIDWrappingEnabled(True) + self.turningPIDController.setPositionPIDWrappingMinInput( + ModuleConstants.kTurningEncoderPositionPIDMinInput + ) + self.turningPIDController.setPositionPIDWrappingMaxInput( + ModuleConstants.kTurningEncoderPositionPIDMaxInput + ) + + # Set the PID gains for the driving motor. Note these are example gains, and you + # may need to tune them for your own robot! + self.drivingPIDController.setP(ModuleConstants.kDrivingP) + self.drivingPIDController.setI(ModuleConstants.kDrivingI) + self.drivingPIDController.setD(ModuleConstants.kDrivingD) + self.drivingPIDController.setFF(ModuleConstants.kDrivingFF) + self.drivingPIDController.setOutputRange( + ModuleConstants.kDrivingMinOutput, ModuleConstants.kDrivingMaxOutput + ) + + # Set the PID gains for the turning motor. Note these are example gains, and you + # may need to tune them for your own robot! + self.turningPIDController.setP(ModuleConstants.kTurningP) + self.turningPIDController.setI(ModuleConstants.kTurningI) + self.turningPIDController.setD(ModuleConstants.kTurningD) + self.turningPIDController.setFF(ModuleConstants.kTurningFF) + self.turningPIDController.setOutputRange( + ModuleConstants.kTurningMinOutput, ModuleConstants.kTurningMaxOutput + ) + + self.drivingSparkMax.setIdleMode(ModuleConstants.kDrivingMotorIdleMode) + self.turningSparkMax.setIdleMode(ModuleConstants.kTurningMotorIdleMode) + self.drivingSparkMax.setSmartCurrentLimit( + ModuleConstants.kDrivingMotorCurrentLimit + ) + self.turningSparkMax.setSmartCurrentLimit( + ModuleConstants.kTurningMotorCurrentLimit + ) + + # Save the SPARK MAX configurations. If a SPARK MAX browns out during + # operation, it will maintain the above configurations. + self.drivingSparkMax.burnFlash() + self.turningSparkMax.burnFlash() + + self.chassisAngularOffset = chassisAngularOffset + self.desiredState.angle = Rotation2d(self.turningEncoder.getPosition()) + self.drivingEncoder.setPosition(0) + + def getState(self) -> SwerveModuleState: + """Returns the current state of the module. + + :returns: The current state of the module. + """ + # Apply chassis angular offset to the encoder position to get the position + # relative to the chassis. + return SwerveModuleState( + self.drivingEncoder.getVelocity(), + Rotation2d(self.turningEncoder.getPosition() - self.chassisAngularOffset), + ) + + def getPosition(self) -> SwerveModulePosition: + """Returns the current position of the module. + + :returns: The current position of the module. + """ + # Apply chassis angular offset to the encoder position to get the position + # relative to the chassis. + return SwerveModulePosition( + self.drivingEncoder.getPosition(), + Rotation2d(self.turningEncoder.getPosition() - self.chassisAngularOffset), + ) + + def setDesiredState(self, desiredState: SwerveModuleState) -> None: + """Sets the desired state for the module. + + :param desiredState: Desired state with speed and angle. + + """ + # Apply chassis angular offset to the desired state. + correctedDesiredState = SwerveModuleState() + correctedDesiredState.speed = desiredState.speed + correctedDesiredState.angle = desiredState.angle + Rotation2d( + self.chassisAngularOffset + ) + + # Optimize the reference state to avoid spinning further than 90 degrees. + optimizedDesiredState = SwerveModuleState.optimize( + correctedDesiredState, Rotation2d(self.turningEncoder.getPosition()) + ) + + # Command driving and turning SPARKS MAX towards their respective setpoints. + self.drivingPIDController.setReference( + optimizedDesiredState.speed, CANSparkMax.ControlType.kVelocity + ) + self.turningPIDController.setReference( + optimizedDesiredState.angle.radians(), CANSparkMax.ControlType.kPosition + ) + + self.desiredState = desiredState + + def resetEncoders(self) -> None: + """ + Zeroes all the SwerveModule encoders. + """ + self.drivingEncoder.setPosition(0) diff --git a/rio/components/swervemodule.py b/rio/components/swervemodule.py deleted file mode 100644 index fd2cf937..00000000 --- a/rio/components/swervemodule.py +++ /dev/null @@ -1,144 +0,0 @@ -# -# Copyright (c) FIRST and other WPILib contributors. -# Open Source Software; you can modify and/or share it under the terms of -# the WPILib BSD license file in the root directory of this project. -# - -# math -import math - -# wpilib -import wpilib -import wpimath.kinematics -import wpimath.geometry -import wpimath.controller -import wpimath.trajectory - -# rev -from rev import CANSparkMaxLowLevel, CANSparkMax - -kWheelRadius = 0.0508 -kEncoderResolution = 4096 -kModuleMaxAngularVelocity = math.pi -kModuleMaxAngularAcceleration = math.tau - - -class SwerveModule: - def __init__( - self, - driveMotorChannel: int, - turningMotorChannel: int, - driveEncoderChannelA: int, - driveEncoderChannelB: int, - turningEncoderChannelA: int, - turningEncoderChannelB: int, - ) -> None: - """Constructs a SwerveModule with a drive motor, turning motor, drive encoder and turning encoder. - - :param driveMotorChannel: PWM output for the drive motor. - :param turningMotorChannel: PWM output for the turning motor. - :param driveEncoderChannelA: DIO input for the drive encoder channel A - :param driveEncoderChannelB: DIO input for the drive encoder channel B - :param turningEncoderChannelA: DIO input for the turning encoder channel A - :param turningEncoderChannelB: DIO input for the turning encoder channel B - """ - self.driveMotor = CANSparkMax( - driveMotorChannel, CANSparkMaxLowLevel.MotorType.kBrushless - ) - self.turningMotor = CANSparkMax( - turningMotorChannel, CANSparkMaxLowLevel.MotorType.kBrushless - ) - - self.driveEncoder = self.driveMotor.getEncoder() - self.turningEncoder = self.turningMotor.getEncoder() - - # Gains are for example purposes only - must be determined for your own robot! - self.drivePIDController = self.driveMotor.getPIDController() - - self.drivePIDController.setP(0) - self.drivePIDController.setI(0) - self.drivePIDController.setD(0) - self.drivePIDController.setFF(0) - - # Gains are for example purposes only - must be determined for your own robot! - self.turningPIDController = self.turningMotor.getPIDController() - - self.turningPIDController.setP(0) - self.turningPIDController.setI(0) - self.turningPIDController.setD(0) - self.turningPIDController.setFF(0) - - # Gains are for example purposes only - must be determined for your own robot! - self.driveFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 3) - self.turnFeedforward = wpimath.controller.SimpleMotorFeedforwardMeters(1, 0.5) - - # Set the distance per pulse for the drive encoder. We can simply use the - # distance traveled for one rotation of the wheel divided by the encoder - # resolution. - self.driveEncoder.setPositionConversionFactor( - (math.tau * kWheelRadius / kEncoderResolution) - ) - - # Set the distance (in this case, angle) in radians per pulse for the turning encoder. - # This is the the angle through an entire rotation (2 * pi) divided by the - # encoder resolution. - self.turningEncoder.setPositionConversionFactor((math.tau / kEncoderResolution)) - - def getState(self) -> wpimath.kinematics.SwerveModuleState: - """Returns the current state of the module. - - :returns: The current state of the module. - """ - return wpimath.kinematics.SwerveModuleState( - self.driveEncoder.getPosition(), - wpimath.geometry.Rotation2d(self.turningEncoder.getVelocity()), - ) - - def aquirePosition(self) -> wpimath.kinematics.SwerveModulePosition: - """Returns the current position of the module. - - :returns: The current position of the module. - """ - return wpimath.kinematics.SwerveModulePosition( - self.driveEncoder.getPosition(), - wpimath.geometry.Rotation2d(self.turningEncoder.getVelocity()), - ) - - def setDesiredState( - self, desiredState: wpimath.kinematics.SwerveModuleState - ) -> None: - """Sets the desired state for the module. - - :param desiredState: Desired state with speed and angle. - """ - - encoderRotation = wpimath.geometry.Rotation2d(self.turningEncoder.getVelocity()) - - # Optimize the reference state to avoid spinning further than 90 degrees - state = wpimath.kinematics.SwerveModuleState.optimize( - desiredState, encoderRotation - ) - - # Scale speed by cosine of angle error. This scales down movement perpendicular to the desired - # direction of travel that can occur when modules change directions. This results in smoother - # driving. - state.speed *= (state.angle - encoderRotation).cos() - - # Calculate the drive output from the drive PID controller. - driveOutput = self.drivePIDController.calculate( - self.driveEncoder.getPosition(), state.speed - ) - - driveFeedforward = self.driveFeedforward.calculate(state.speed) - - # Calculate the turning motor output from the turning PID controller. - turnOutput = self.turningPIDController.calculate( - self.turningEncoder.getVelocity(), state.angle.radians() - ) - - turnFeedforward = self.turnFeedforward.calculate( - self.turningPIDController.getSetpoint().velocity - ) - - self.driveMotor.setVoltage(driveOutput + driveFeedforward) - self.turningMotor.setVoltage(turnOutput + turnFeedforward) diff --git a/rio/constants/__init__.py b/rio/constants/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/rio/constants/constants.py b/rio/constants/constants.py new file mode 100644 index 00000000..f43472a8 --- /dev/null +++ b/rio/constants/constants.py @@ -0,0 +1,141 @@ +# 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. + +""" +The constants module is a convenience place for teams to hold robot-wide +numerical or boolean constants. Don't use this for any other purpose! +""" + + +import math + +from wpimath import units +from wpimath.geometry import Translation2d +from wpimath.kinematics import SwerveDrive4Kinematics +from wpimath.trajectory import TrapezoidProfileRadians + +from rev import CANSparkMax + + +class NeoMotorConstants: + kFreeSpeedRpm = 5676 + + +class DriveConstants: + # Driving Parameters - Note that these are not the maximum capable speeds of + # the robot, rather the allowed maximum speeds + kMaxSpeedMetersPerSecond = 4.8 + kMaxAngularSpeed = math.tau # radians per second + + kDirectionSlewRate = 1.2 # radians per second + kMagnitudeSlewRate = 1.8 # percent per second (1 = 100%) + kRotationalSlewRate = 2.0 # percent per second (1 = 100%) + + # Chassis configuration + kTrackWidth = units.inchesToMeters(26.5) + # Distance between centers of right and left wheels on robot + kWheelBase = units.inchesToMeters(26.5) + + # Distance between front and back wheels on robot + kModulePositions = [ + Translation2d(kWheelBase / 2, kTrackWidth / 2), + Translation2d(kWheelBase / 2, -kTrackWidth / 2), + Translation2d(-kWheelBase / 2, kTrackWidth / 2), + Translation2d(-kWheelBase / 2, -kTrackWidth / 2), + ] + kDriveKinematics = SwerveDrive4Kinematics(*kModulePositions) + + # Angular offsets of the modules relative to the chassis in radians + kFrontLeftChassisAngularOffset = -math.pi / 2 + kFrontRightChassisAngularOffset = 0 + kBackLeftChassisAngularOffset = math.pi + kBackRightChassisAngularOffset = math.pi / 2 + + # SPARK MAX CAN IDs + kFrontLeftDrivingCanId = 11 + kRearLeftDrivingCanId = 13 + kFrontRightDrivingCanId = 15 + kRearRightDrivingCanId = 17 + + kFrontLeftTurningCanId = 10 + kRearLeftTurningCanId = 12 + kFrontRightTurningCanId = 14 + kRearRightTurningCanId = 16 + + kGyroReversed = False + + +class ModuleConstants: + # The MAXSwerve module can be configured with one of three pinion gears: 12T, 13T, or 14T. + # This changes the drive speed of the module (a pinion gear with more teeth will result in a + # robot that drives faster). + kDrivingMotorPinionTeeth = 14 + + # Invert the turning encoder, since the output shaft rotates in the opposite direction of + # the steering motor in the MAXSwerve Module. + kTurningEncoderInverted = True + + # Calculations required for driving motor conversion factors and feed forward + kDrivingMotorFreeSpeedRps = NeoMotorConstants.kFreeSpeedRpm / 60 + kWheelDiameterMeters = 0.0762 + kWheelCircumferenceMeters = kWheelDiameterMeters * math.pi + # 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 teeth on the bevel pinion + kDrivingMotorReduction = (45.0 * 22) / (kDrivingMotorPinionTeeth * 15) + kDriveWheelFreeSpeedRps = ( + kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters + ) / kDrivingMotorReduction + + kDrivingEncoderPositionFactor = ( + kWheelDiameterMeters * math.pi + ) / kDrivingMotorReduction # meters + kDrivingEncoderVelocityFactor = ( + (kWheelDiameterMeters * math.pi) / kDrivingMotorReduction + ) / 60.0 # meters per second + + kTurningEncoderPositionFactor = math.tau # radian + kTurningEncoderVelocityFactor = math.tau / 60.0 # radians per second + + kTurningEncoderPositionPIDMinInput = 0 # radian + kTurningEncoderPositionPIDMaxInput = kTurningEncoderPositionFactor # radian + + kDrivingP = 0.04 + kDrivingI = 0 + kDrivingD = 0 + kDrivingFF = 1 / kDriveWheelFreeSpeedRps + kDrivingMinOutput = -1 + kDrivingMaxOutput = 1 + + kTurningP = 1 + kTurningI = 0 + kTurningD = 0 + kTurningFF = 0 + kTurningMinOutput = -1 + kTurningMaxOutput = 1 + + kDrivingMotorIdleMode = CANSparkMax.IdleMode.kBrake + kTurningMotorIdleMode = CANSparkMax.IdleMode.kBrake + + kDrivingMotorCurrentLimit = 50 # amp + kTurningMotorCurrentLimit = 20 # amp + + +class OIConstants: + kDriverControllerPort = 0 + kDriveDeadband = 0.05 + + +class AutoConstants: + kMaxSpeedMetersPerSecond = 3 + kMaxAccelerationMetersPerSecondSquared = 3 + kMaxAngularSpeedRadiansPerSecond = math.pi + kMaxAngularSpeedRadiansPerSecondSquared = math.pi + + kPXController = 1 + kPYController = 1 + kPThetaController = 1 + + # Constraint for the motion profiled robot angle controller + kThetaControllerConstraints = TrapezoidProfileRadians.Constraints( + kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared + ) diff --git a/rio/hello b/rio/hello new file mode 100644 index 00000000..05252006 --- /dev/null +++ b/rio/hello @@ -0,0 +1,135 @@ +{ + "System Joysticks": { + "window": { + "visible": false + } + }, + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 65, + "incKey": 68 + } + ], + "axisCount": 3, + "buttonCount": 10, + "buttonKeys": [ + 90, + 88, + 67, + 86, + 86, + 86, + 86, + 86, + 86, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + }, + { + "decKey": 265, + "incKey": 264 + }, + { + "decKey": 265, + "incKey": 264 + }, + { + "decKey": 265, + "incKey": 264 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 6, + "buttonCount": 10, + "buttonKeys": [ + 77, + 44, + 46, + 47, + 86, + 86, + 86, + 86, + 86, + 86 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0", + "name": "Driver Joystick" + }, + { + "guid": "Keyboard1", + "name": "Operator Stick" + } + ] +} \ No newline at end of file diff --git a/rio/robot.py b/rio/robot.py index 8282bee8..4858c218 100644 --- a/rio/robot.py +++ b/rio/robot.py @@ -13,14 +13,14 @@ import wpimath.controller # drivetrain -from components.drivetrain import Drivetrain, kMaxSpeed +from components.drivetrain import DriveSubsystem -class MyRobot(wpilib.TimedRobot): +class UnnamedToaster(wpilib.TimedRobot): def robotInit(self) -> None: """Robot initialization function""" self.controller = wpilib.XboxController(0) - self.swerve = Drivetrain() + self.swerve = DriveSubsystem() # Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1. self.xspeedLimiter = wpimath.filter.SlewRateLimiter(3) @@ -29,7 +29,6 @@ def robotInit(self) -> None: def autonomousPeriodic(self) -> None: self.driveWithJoystick(False) - self.swerve.updateOdometry() def teleopPeriodic(self) -> None: self.driveWithJoystick(True) @@ -37,32 +36,23 @@ def teleopPeriodic(self) -> None: def driveWithJoystick(self, fieldRelative: bool) -> None: # Get the x speed. We are inverting this because Xbox controllers return # negative values when we push forward. - xSpeed = ( - -self.xspeedLimiter.calculate( - wpimath.applyDeadband(self.controller.getLeftY(), 0.02) - ) - * kMaxSpeed + xSpeed = -self.xspeedLimiter.calculate( + wpimath.applyDeadband(self.controller.getLeftY(), 0.02) ) # Get the y speed or sideways/strafe speed. We are inverting this because # we want a positive value when we pull to the left. Xbox controllers # return positive values when you pull to the right by default. - ySpeed = ( - -self.yspeedLimiter.calculate( - wpimath.applyDeadband(self.controller.getLeftX(), 0.02) - ) - * kMaxSpeed + ySpeed = -self.yspeedLimiter.calculate( + wpimath.applyDeadband(self.controller.getLeftX(), 0.02) ) # Get the rate of angular rotation. We are inverting this because we want a # positive value when we pull to the left (remember, CCW is positive in # mathematics). Xbox controllers return positive values when you pull to # the right by default. - rot = ( - -self.rotLimiter.calculate( - wpimath.applyDeadband(self.controller.getRightX(), 0.02) - ) - * kMaxSpeed + rot = -self.rotLimiter.calculate( + wpimath.applyDeadband(self.controller.getRightX(), 0.02) ) self.swerve.drive(xSpeed, ySpeed, rot, fieldRelative, self.getPeriod()) diff --git a/rio/simgui-ds.json b/rio/simgui-ds.json index 73cc713c..4c7ca355 100644 --- a/rio/simgui-ds.json +++ b/rio/simgui-ds.json @@ -3,55 +3,32 @@ { "axisConfig": [ { - "decKey": 65, - "incKey": 68 + "decKey": 263, + "incKey": 262 }, { - "decKey": 87, - "incKey": 83 + "decKey": 265, + "incKey": 264 }, { - "decKey": 69, - "decayRate": 0.0, - "incKey": 82, - "keyRate": 0.009999999776482582 - } - ], - "axisCount": 3, - "buttonCount": 4, - "buttonKeys": [ - 90, - 88, - 67, - 86 - ], - "povConfig": [ + "decKey": 265, + "incKey": 264 + }, { - "key0": 328, - "key135": 323, - "key180": 322, - "key225": 321, - "key270": 324, - "key315": 327, - "key45": 329, - "key90": 326 - } - ], - "povCount": 1 - }, - { - "axisConfig": [ + "decKey": 265, + "incKey": 264 + }, { - "decKey": 74, - "incKey": 76 + "decKey": 265, + "incKey": 264 }, { - "decKey": 73, - "incKey": 75 + "decKey": 265, + "incKey": 264 } ], - "axisCount": 2, - "buttonCount": 4, + "axisCount": 5, + "buttonCount": 10, "buttonKeys": [ 77, 44, @@ -88,5 +65,11 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0", + "name": "Driver Joystick" + } ] } diff --git a/rio/simgui-window.json b/rio/simgui-window.json index 11af7805..1d7156e8 100644 --- a/rio/simgui-window.json +++ b/rio/simgui-window.json @@ -11,19 +11,19 @@ "userScale": "2", "width": "1280", "xpos": "636", - "ypos": "24" + "ypos": "329" } }, "Window": { "###FMS": { "Collapsed": "0", "Pos": "5,540", - "Size": "32,35" + "Size": "283,146" }, "###Joysticks": { "Collapsed": "0", "Pos": "250,465", - "Size": "32,35" + "Size": "796,155" }, "###NetworkTables": { "Collapsed": "0", @@ -42,13 +42,13 @@ }, "###System Joysticks": { "Collapsed": "0", - "Pos": "5,350", - "Size": "32,35" + "Pos": "16,322", + "Size": "192,218" }, "###Timing": { "Collapsed": "0", "Pos": "5,150", - "Size": "32,35" + "Size": "135,173" }, "Debug##Default": { "Collapsed": "0", @@ -58,7 +58,7 @@ "Robot State": { "Collapsed": "0", "Pos": "5,20", - "Size": "32,35" + "Size": "92,99" } } } diff --git a/rio/swerveutils.py b/rio/swerveutils.py new file mode 100644 index 00000000..ad6eccf3 --- /dev/null +++ b/rio/swerveutils.py @@ -0,0 +1,87 @@ +import math + + +def stepTowards(current: float, target: float, stepsize: float) -> float: + """Steps a value towards a target with a specified step size. + + :param current: The current or starting value. Can be positive or negative. + :param target: The target value the algorithm will step towards. Can be positive or negative. + :param stepsize: The maximum step size that can be taken. + + :returns: The new value for {@code current} after performing the specified step towards the specified target. + """ + + if abs(current - target) <= stepsize: + return target + + elif target < current: + return current - stepsize + + else: + return current + stepsize + + +def stepTowardsCircular(current: float, target: float, stepsize: float) -> float: + """Steps a value (angle) towards a target (angle) taking the shortest path with a specified step size. + + :param current: The current or starting angle (in radians). Can lie outside the 0 to 2*PI range. + :param target: The target angle (in radians) the algorithm will step towards. Can lie outside the 0 to 2*PI range. + :param stepsize: The maximum step size that can be taken (in radians). + + :returns: The new angle (in radians) for {@code current} after performing the specified step towards the specified target. + This value will always lie in the range 0 to 2*PI (exclusive). + """ + + current = wrapAngle(current) + target = wrapAngle(target) + + stepDirection = math.copysign(target - current, 1) + difference = abs(current - target) + + if difference <= stepsize: + return target + elif difference > math.pi: # does the system need to wrap over eventually? + # handle the special case where you can reach the target in one step while also wrapping + if ( + current + math.tau - target < stepsize + or target + math.tau - current < stepsize + ): + return target + else: + # this will handle wrapping gracefully + return wrapAngle(current - stepDirection * stepsize) + else: + return current + stepDirection * stepsize + + +def angleDifference(angleA: float, angleB: float) -> float: + """Finds the (unsigned) minimum difference between two angles including calculating across 0. + + :param angleA: An angle (in radians). + :param angleB: An angle (in radians). + + :returns: The (unsigned) minimum difference between the two angles (in radians). + """ + difference = abs(angleA - angleB) + return math.tau - difference if difference > math.pi else difference + + +def wrapAngle(angle: float) -> float: + """Wraps an angle until it lies within the range from 0 to 2*PI (exclusive). + + :param angle: The angle (in radians) to wrap. Can be positive or negative and can lie multiple wraps outside the output range. + + :returns: An angle (in radians) from 0 and 2*PI (exclusive). + """ + + twoPi = math.tau + + # Handle this case separately to avoid floating point errors with the floor after the division in the case below + if angle == twoPi: + return 0.0 + elif angle > twoPi: + return angle - twoPi * math.floor(angle / twoPi) + elif angle < 0.0: + return angle + twoPi * (math.floor((-angle) / twoPi) + 1) + else: + return angle From babb23a4708368e10129497097de09efd41f6917 Mon Sep 17 00:00:00 2001 From: kredcool Date: Mon, 15 Jan 2024 13:43:21 -0500 Subject: [PATCH 21/61] This fails tests althought i can't get it to fail sim --- rio/components/drivetrain.py | 41 ++++---- rio/components/maxswervemodule.py | 36 ++++--- rio/constants/complexConstants.py | 85 ++++++++++++++++ rio/constants/constants.py | 141 --------------------------- rio/constants/getConstants.py | 35 +++++++ rio/constants/simple_auto.yaml | 7 ++ rio/constants/simple_controller.yaml | 3 + rio/constants/simple_hardware.yaml | 35 +++++++ rio/constants/simple_pid.yaml | 19 ++++ rio/hello | 135 ------------------------- rio/simgui-ds.json | 7 +- rio/simgui-window.json | 10 +- 12 files changed, 241 insertions(+), 313 deletions(-) create mode 100644 rio/constants/complexConstants.py delete mode 100644 rio/constants/constants.py create mode 100644 rio/constants/getConstants.py create mode 100644 rio/constants/simple_auto.yaml create mode 100644 rio/constants/simple_controller.yaml create mode 100644 rio/constants/simple_hardware.yaml create mode 100644 rio/constants/simple_pid.yaml delete mode 100644 rio/hello diff --git a/rio/components/drivetrain.py b/rio/components/drivetrain.py index dcacae9a..b37a29dc 100644 --- a/rio/components/drivetrain.py +++ b/rio/components/drivetrain.py @@ -13,37 +13,42 @@ SwerveDrive4Odometry, ) from navx import AHRS -from constants.constants import DriveConstants +from constants.complexConstants import DriveConstants import swerveutils from .maxswervemodule import MAXSwerveModule +from constants.getConstants import getConstants + class DriveSubsystem(Subsystem): def __init__(self) -> None: super().__init__() + # hardware constants + hardwareconstants = getConstants("simple_hardware") + self.driveConsts = hardwareconstants["drive"] # Create MAXSwerveModules self.frontLeft = MAXSwerveModule( - DriveConstants.kFrontLeftDrivingCanId, - DriveConstants.kFrontLeftTurningCanId, + self.driveConsts["kFrontLeftDrivingCanId"], + self.driveConsts["kFrontLeftTurningCanId"], DriveConstants.kFrontLeftChassisAngularOffset, ) self.frontRight = MAXSwerveModule( - DriveConstants.kFrontRightDrivingCanId, - DriveConstants.kFrontRightTurningCanId, + self.driveConsts["kFrontRightDrivingCanId"], + self.driveConsts["kFrontRightTurningCanId"], DriveConstants.kFrontRightChassisAngularOffset, ) self.rearLeft = MAXSwerveModule( - DriveConstants.kRearLeftDrivingCanId, - DriveConstants.kRearLeftTurningCanId, + self.driveConsts["kRearLeftDrivingCanId"], + self.driveConsts["kRearLeftTurningCanId"], DriveConstants.kBackLeftChassisAngularOffset, ) self.rearRight = MAXSwerveModule( - DriveConstants.kRearRightDrivingCanId, - DriveConstants.kRearRightTurningCanId, + self.driveConsts["kRearRightDrivingCanId"], + self.driveConsts["kRearRightTurningCanId"], DriveConstants.kBackRightChassisAngularOffset, ) @@ -55,8 +60,8 @@ def __init__(self) -> None: self.currentTranslationDir = 0.0 self.currentTranslationMag = 0.0 - self.magLimiter = SlewRateLimiter(DriveConstants.kMagnitudeSlewRate) - self.rotLimiter = SlewRateLimiter(DriveConstants.kRotationalSlewRate) + self.magLimiter = SlewRateLimiter(self.driveConsts["kMagnitudeSlewRate"]) + self.rotLimiter = SlewRateLimiter(self.driveConsts["kRotationalSlewRate"]) self.prevTime = wpilib.Timer.getFPGATimestamp() # Odometry class for tracking robot pose @@ -136,7 +141,7 @@ def drive( # Calculate the direction slew rate based on an estimate of the lateral acceleration if self.currentTranslationMag != 0.0: directionSlewRate = abs( - DriveConstants.kDirectionSlewRate / self.currentTranslationMag + self.driveConsts["kDirectionSlewRate"] / self.currentTranslationMag ) else: directionSlewRate = 500.0 @@ -192,8 +197,8 @@ def drive( self.currentRotation = rot # Convert the commanded speeds into the correct units for the drivetrain - xSpeedDelivered = xSpeedCommanded * DriveConstants.kMaxSpeedMetersPerSecond - ySpeedDelivered = ySpeedCommanded * DriveConstants.kMaxSpeedMetersPerSecond + xSpeedDelivered = xSpeedCommanded * self.driveConsts["kMaxSpeedMetersPerSecond"] + ySpeedDelivered = ySpeedCommanded * self.driveConsts["kMaxSpeedMetersPerSecond"] rotDelivered = self.currentRotation * DriveConstants.kMaxAngularSpeed swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( @@ -207,7 +212,7 @@ def drive( else ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered) ) fl, fr, rl, rr = SwerveDrive4Kinematics.desaturateWheelSpeeds( - swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond + swerveModuleStates, self.driveConsts["kMaxSpeedMetersPerSecond"] ) self.frontLeft.setDesiredState(fl) self.frontRight.setDesiredState(fr) @@ -234,7 +239,7 @@ def setModuleStates( :param desiredStates: The desired SwerveModule states. """ fl, fr, rl, rr = SwerveDrive4Kinematics.desaturateWheelSpeeds( - desiredStates, DriveConstants.kMaxSpeedMetersPerSecond + desiredStates, self.driveConsts["kMaxSpeedMetersPerSecond"] ) self.frontLeft.setDesiredState(fl) self.frontRight.setDesiredState(fr) @@ -264,4 +269,6 @@ def getTurnRate(self) -> float: :returns: The turn rate of the robot, in degrees per second """ - return self.gyro.getRate() * (-1.0 if DriveConstants.kGyroReversed else 1.0) + return self.gyro.getRate() * ( + -1.0 if self.driveConsts["kGyroReversed"] else 1.0 + ) diff --git a/rio/components/maxswervemodule.py b/rio/components/maxswervemodule.py index 66a9c88f..401da334 100644 --- a/rio/components/maxswervemodule.py +++ b/rio/components/maxswervemodule.py @@ -2,7 +2,8 @@ from wpimath.geometry import Rotation2d from wpimath.kinematics import SwerveModuleState, SwerveModulePosition -from constants.constants import ModuleConstants +from constants.complexConstants import ModuleConstants +from constants.getConstants import getConstants class MAXSwerveModule: @@ -14,6 +15,13 @@ def __init__( MAXSwerve Module built with NEOs, SPARKS MAX, and a Through Bore Encoder. """ + # hardware constants + hardwareconstants = getConstants("simple_hardware") + self.moduleConsts = hardwareconstants["module"] + + # pid constants + pidconstants = getConstants("simple_pid") + self.pidConsts = pidconstants["PID"] self.chassisAngularOffset = 0 self.desiredState = SwerveModuleState(0.0, Rotation2d()) @@ -62,7 +70,7 @@ def __init__( # Invert the turning encoder, since the output shaft rotates in the opposite direction of # the steering motor in the MAXSwerve Module. - self.turningEncoder.setInverted(ModuleConstants.kTurningEncoderInverted) + self.turningEncoder.setInverted(self.moduleConsts["kTurningEncoderInverted"]) # Enable PID wrap around for the turning motor. This will allow the PID # controller to go through 0 to get to the setpoint i.e. going from 350 degrees @@ -70,7 +78,7 @@ def __init__( # longer route. self.turningPIDController.setPositionPIDWrappingEnabled(True) self.turningPIDController.setPositionPIDWrappingMinInput( - ModuleConstants.kTurningEncoderPositionPIDMinInput + self.pidConsts["kTurningEncoderPositionPIDMinInput"] ) self.turningPIDController.setPositionPIDWrappingMaxInput( ModuleConstants.kTurningEncoderPositionPIDMaxInput @@ -78,31 +86,31 @@ def __init__( # Set the PID gains for the driving motor. Note these are example gains, and you # may need to tune them for your own robot! - self.drivingPIDController.setP(ModuleConstants.kDrivingP) - self.drivingPIDController.setI(ModuleConstants.kDrivingI) - self.drivingPIDController.setD(ModuleConstants.kDrivingD) + self.drivingPIDController.setP(self.pidConsts["kDrivingP"]) + self.drivingPIDController.setI(self.pidConsts["kDrivingI"]) + self.drivingPIDController.setD(self.pidConsts["kDrivingD"]) self.drivingPIDController.setFF(ModuleConstants.kDrivingFF) self.drivingPIDController.setOutputRange( - ModuleConstants.kDrivingMinOutput, ModuleConstants.kDrivingMaxOutput + self.pidConsts["kDrivingMinOutput"], self.pidConsts["kDrivingMaxOutput"] ) # Set the PID gains for the turning motor. Note these are example gains, and you # may need to tune them for your own robot! - self.turningPIDController.setP(ModuleConstants.kTurningP) - self.turningPIDController.setI(ModuleConstants.kTurningI) - self.turningPIDController.setD(ModuleConstants.kTurningD) - self.turningPIDController.setFF(ModuleConstants.kTurningFF) + self.turningPIDController.setP(self.pidConsts["kTurningP"]) + self.turningPIDController.setI(self.pidConsts["kTurningI"]) + self.turningPIDController.setD(self.pidConsts["kTurningD"]) + self.turningPIDController.setFF(self.pidConsts["kTurningFF"]) self.turningPIDController.setOutputRange( - ModuleConstants.kTurningMinOutput, ModuleConstants.kTurningMaxOutput + self.pidConsts["kTurningMinOutput"], self.pidConsts["kTurningMaxOutput"] ) self.drivingSparkMax.setIdleMode(ModuleConstants.kDrivingMotorIdleMode) self.turningSparkMax.setIdleMode(ModuleConstants.kTurningMotorIdleMode) self.drivingSparkMax.setSmartCurrentLimit( - ModuleConstants.kDrivingMotorCurrentLimit + self.moduleConsts["kDrivingMotorCurrentLimit"] ) self.turningSparkMax.setSmartCurrentLimit( - ModuleConstants.kTurningMotorCurrentLimit + self.moduleConsts["kTurningMotorCurrentLimit"] ) # Save the SPARK MAX configurations. If a SPARK MAX browns out during diff --git a/rio/constants/complexConstants.py b/rio/constants/complexConstants.py new file mode 100644 index 00000000..4fff73e8 --- /dev/null +++ b/rio/constants/complexConstants.py @@ -0,0 +1,85 @@ +# 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. + +""" +The constants module is a convenience place for teams to hold robot-wide +numerical or boolean constants. Don't use this for any other purpose! +""" + + +import math + +from wpimath.geometry import Translation2d +from wpimath.kinematics import SwerveDrive4Kinematics +from wpimath.trajectory import TrapezoidProfileRadians + +from rev import CANSparkMax + +from constants.getConstants import getConstants + +constants = getConstants("simple_hardware") +neoConsts = constants["neo"] +driveConsts = constants["drive"] +moduleConsts = constants["module"] + + +class DriveConstants: + # Driving Parameters - Note that these are not the maximum capable speeds of + kMaxAngularSpeed = math.tau # radians per second + + # Distance between front and back wheels on robot + kModulePositions = [ + Translation2d(driveConsts["kWheelBase"] / 2, driveConsts["kTrackWidth"] / 2), + Translation2d(driveConsts["kWheelBase"] / 2, -driveConsts["kTrackWidth"] / 2), + Translation2d(-driveConsts["kWheelBase"] / 2, driveConsts["kTrackWidth"] / 2), + Translation2d(-driveConsts["kWheelBase"] / 2, -driveConsts["kTrackWidth"] / 2), + ] + kDriveKinematics = SwerveDrive4Kinematics(*kModulePositions) + + # Angular offsets of the modules relative to the chassis in radians + kFrontLeftChassisAngularOffset = -math.pi / 2 + kFrontRightChassisAngularOffset = 0 + kBackLeftChassisAngularOffset = math.pi + kBackRightChassisAngularOffset = math.pi / 2 + + +class ModuleConstants: + # Calculations required for driving motor conversion factors and feed forward + kDrivingMotorFreeSpeedRps = neoConsts["kFreeSpeedRpm"] / 60 + + kWheelCircumferenceMeters = moduleConsts["kWheelDiameterMeters"] * math.pi + # 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 teeth on the bevel pinion + kDrivingMotorReduction = (45.0 * 22) / ( + moduleConsts["kDrivingMotorPinionTeeth"] * 15 + ) + kDriveWheelFreeSpeedRps = ( + kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters + ) / kDrivingMotorReduction + + kDrivingEncoderPositionFactor = ( + moduleConsts["kWheelDiameterMeters"] * math.pi + ) / kDrivingMotorReduction # meters + kDrivingEncoderVelocityFactor = ( + (moduleConsts["kWheelDiameterMeters"] * math.pi) / kDrivingMotorReduction + ) / 60.0 # meters per second + + kTurningEncoderPositionFactor = math.tau # radian + kTurningEncoderVelocityFactor = math.tau / 60.0 # radians per second + + kTurningEncoderPositionPIDMaxInput = kTurningEncoderPositionFactor # radian + + kDrivingFF = 1 / kDriveWheelFreeSpeedRps + + kDrivingMotorIdleMode = CANSparkMax.IdleMode.kBrake + kTurningMotorIdleMode = CANSparkMax.IdleMode.kBrake + + +class AutoConstants: + kMaxAngularSpeedRadiansPerSecond = math.pi + kMaxAngularSpeedRadiansPerSecondSquared = math.pi + + # Constraint for the motion profiled robot angle controller + kThetaControllerConstraints = TrapezoidProfileRadians.Constraints( + kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared + ) diff --git a/rio/constants/constants.py b/rio/constants/constants.py deleted file mode 100644 index f43472a8..00000000 --- a/rio/constants/constants.py +++ /dev/null @@ -1,141 +0,0 @@ -# Copyright (c) FIRST and other WPILib contributors. -# Open Source Software; you can modify and/or share it under the terms of -# the WPILib BSD license file in the root directory of this project. - -""" -The constants module is a convenience place for teams to hold robot-wide -numerical or boolean constants. Don't use this for any other purpose! -""" - - -import math - -from wpimath import units -from wpimath.geometry import Translation2d -from wpimath.kinematics import SwerveDrive4Kinematics -from wpimath.trajectory import TrapezoidProfileRadians - -from rev import CANSparkMax - - -class NeoMotorConstants: - kFreeSpeedRpm = 5676 - - -class DriveConstants: - # Driving Parameters - Note that these are not the maximum capable speeds of - # the robot, rather the allowed maximum speeds - kMaxSpeedMetersPerSecond = 4.8 - kMaxAngularSpeed = math.tau # radians per second - - kDirectionSlewRate = 1.2 # radians per second - kMagnitudeSlewRate = 1.8 # percent per second (1 = 100%) - kRotationalSlewRate = 2.0 # percent per second (1 = 100%) - - # Chassis configuration - kTrackWidth = units.inchesToMeters(26.5) - # Distance between centers of right and left wheels on robot - kWheelBase = units.inchesToMeters(26.5) - - # Distance between front and back wheels on robot - kModulePositions = [ - Translation2d(kWheelBase / 2, kTrackWidth / 2), - Translation2d(kWheelBase / 2, -kTrackWidth / 2), - Translation2d(-kWheelBase / 2, kTrackWidth / 2), - Translation2d(-kWheelBase / 2, -kTrackWidth / 2), - ] - kDriveKinematics = SwerveDrive4Kinematics(*kModulePositions) - - # Angular offsets of the modules relative to the chassis in radians - kFrontLeftChassisAngularOffset = -math.pi / 2 - kFrontRightChassisAngularOffset = 0 - kBackLeftChassisAngularOffset = math.pi - kBackRightChassisAngularOffset = math.pi / 2 - - # SPARK MAX CAN IDs - kFrontLeftDrivingCanId = 11 - kRearLeftDrivingCanId = 13 - kFrontRightDrivingCanId = 15 - kRearRightDrivingCanId = 17 - - kFrontLeftTurningCanId = 10 - kRearLeftTurningCanId = 12 - kFrontRightTurningCanId = 14 - kRearRightTurningCanId = 16 - - kGyroReversed = False - - -class ModuleConstants: - # The MAXSwerve module can be configured with one of three pinion gears: 12T, 13T, or 14T. - # This changes the drive speed of the module (a pinion gear with more teeth will result in a - # robot that drives faster). - kDrivingMotorPinionTeeth = 14 - - # Invert the turning encoder, since the output shaft rotates in the opposite direction of - # the steering motor in the MAXSwerve Module. - kTurningEncoderInverted = True - - # Calculations required for driving motor conversion factors and feed forward - kDrivingMotorFreeSpeedRps = NeoMotorConstants.kFreeSpeedRpm / 60 - kWheelDiameterMeters = 0.0762 - kWheelCircumferenceMeters = kWheelDiameterMeters * math.pi - # 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 teeth on the bevel pinion - kDrivingMotorReduction = (45.0 * 22) / (kDrivingMotorPinionTeeth * 15) - kDriveWheelFreeSpeedRps = ( - kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters - ) / kDrivingMotorReduction - - kDrivingEncoderPositionFactor = ( - kWheelDiameterMeters * math.pi - ) / kDrivingMotorReduction # meters - kDrivingEncoderVelocityFactor = ( - (kWheelDiameterMeters * math.pi) / kDrivingMotorReduction - ) / 60.0 # meters per second - - kTurningEncoderPositionFactor = math.tau # radian - kTurningEncoderVelocityFactor = math.tau / 60.0 # radians per second - - kTurningEncoderPositionPIDMinInput = 0 # radian - kTurningEncoderPositionPIDMaxInput = kTurningEncoderPositionFactor # radian - - kDrivingP = 0.04 - kDrivingI = 0 - kDrivingD = 0 - kDrivingFF = 1 / kDriveWheelFreeSpeedRps - kDrivingMinOutput = -1 - kDrivingMaxOutput = 1 - - kTurningP = 1 - kTurningI = 0 - kTurningD = 0 - kTurningFF = 0 - kTurningMinOutput = -1 - kTurningMaxOutput = 1 - - kDrivingMotorIdleMode = CANSparkMax.IdleMode.kBrake - kTurningMotorIdleMode = CANSparkMax.IdleMode.kBrake - - kDrivingMotorCurrentLimit = 50 # amp - kTurningMotorCurrentLimit = 20 # amp - - -class OIConstants: - kDriverControllerPort = 0 - kDriveDeadband = 0.05 - - -class AutoConstants: - kMaxSpeedMetersPerSecond = 3 - kMaxAccelerationMetersPerSecondSquared = 3 - kMaxAngularSpeedRadiansPerSecond = math.pi - kMaxAngularSpeedRadiansPerSecondSquared = math.pi - - kPXController = 1 - kPYController = 1 - kPThetaController = 1 - - # Constraint for the motion profiled robot angle controller - kThetaControllerConstraints = TrapezoidProfileRadians.Constraints( - kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared - ) diff --git a/rio/constants/getConstants.py b/rio/constants/getConstants.py new file mode 100644 index 00000000..25002d58 --- /dev/null +++ b/rio/constants/getConstants.py @@ -0,0 +1,35 @@ +# Tidal Force Robotics +# 2022 + +import os +from sre_constants import CATEGORY_WORD +from wpilib import RobotBase +import yaml +import logging + +""" +Use this file only for storing non-changing constants. +""" + + +def getConstants(identifier): + constants = {} + + # Clunky but it works + if RobotBase.isReal(): + path = "/home/lvuser/py/constants/" + else: + path = "constants/" + + try: + # Try opening requested .yaml + with open(f"{path}{identifier}.yaml", "r") as yamlFile: + # Use yaml.safe_load to load the yaml into a dict + constants = yaml.safe_load(yamlFile) + except FileNotFoundError as e: + # If the file is not found, report it! + logging.error(f"{identifier} config not found!") + raise e + + # When all is done, return the important bits! + return constants diff --git a/rio/constants/simple_auto.yaml b/rio/constants/simple_auto.yaml new file mode 100644 index 00000000..d4a5b025 --- /dev/null +++ b/rio/constants/simple_auto.yaml @@ -0,0 +1,7 @@ +auto: + kMaxSpeedMetersPerSecond: 3 + kMaxAccelerationMetersPerSecondSquared: 3 + + kPXController: 1 + kPYController: 1 + kPThetaController: 1 \ No newline at end of file diff --git a/rio/constants/simple_controller.yaml b/rio/constants/simple_controller.yaml new file mode 100644 index 00000000..3a67e165 --- /dev/null +++ b/rio/constants/simple_controller.yaml @@ -0,0 +1,3 @@ +driver: + kDriverControllerPort: 0 + kDriveDeadband: 0.05 \ No newline at end of file diff --git a/rio/constants/simple_hardware.yaml b/rio/constants/simple_hardware.yaml new file mode 100644 index 00000000..6ca7b436 --- /dev/null +++ b/rio/constants/simple_hardware.yaml @@ -0,0 +1,35 @@ +neo: + kFreeSpeedRpm: 5676 + +drive: + kMaxSpeedMetersPerSecond: 4.8 + + kDirectionSlewRate: 1.2 + kMagnitudeSlewRate: 1.8 + kRotationalSlewRate: 2.0 + + kTrackWidth: 0.6731 + kWheelBase: 0.6731 + + # SPARK MAX CAN IDs + kFrontLeftDrivingCanId: 11 + kRearLeftDrivingCanId: 13 + kFrontRightDrivingCanId: 15 + kRearRightDrivingCanId: 17 + + kFrontLeftTurningCanId: 10 + kRearLeftTurningCanId: 12 + kFrontRightTurningCanId: 14 + kRearRightTurningCanId: 16 + + kGyroReversed: False + +module: + kDrivingMotorPinionTeeth: 14 + + kTurningEncoderInverted: True + + kWheelDiameterMeters: 0.0762 + + kDrivingMotorCurrentLimit: 50 + kTurningMotorCurrentLimit: 20 \ No newline at end of file diff --git a/rio/constants/simple_pid.yaml b/rio/constants/simple_pid.yaml new file mode 100644 index 00000000..aa0f46ec --- /dev/null +++ b/rio/constants/simple_pid.yaml @@ -0,0 +1,19 @@ +PID: + kTurningEncoderPositionPIDMinInput: 0 + # kTurningEncoderPositionPIDMaxInput in complexConstants.py + + kDrivingP: 0 + kDrivingI: 0 + kDrivingD: 0 + # kDriveingFF in complexConstants.py + + kDrivingMinOutput: -1 + kDrivingMaxOutput: 1 + + kTurningP: 2.65 + kTurningI: 0.012 + kTurningD: 1.5 + kTurningFF: 0 + + kTurningMinOutput: -1 + kTurningMaxOutput: 1 \ No newline at end of file diff --git a/rio/hello b/rio/hello deleted file mode 100644 index 05252006..00000000 --- a/rio/hello +++ /dev/null @@ -1,135 +0,0 @@ -{ - "System Joysticks": { - "window": { - "visible": false - } - }, - "keyboardJoysticks": [ - { - "axisConfig": [ - { - "decKey": 69, - "decayRate": 0.0, - "incKey": 82, - "keyRate": 0.009999999776482582 - }, - { - "decKey": 87, - "incKey": 83 - }, - { - "decKey": 65, - "incKey": 68 - } - ], - "axisCount": 3, - "buttonCount": 10, - "buttonKeys": [ - 90, - 88, - 67, - 86, - 86, - 86, - 86, - 86, - 86, - 86 - ], - "povConfig": [ - { - "key0": 328, - "key135": 323, - "key180": 322, - "key225": 321, - "key270": 324, - "key315": 327, - "key45": 329, - "key90": 326 - } - ], - "povCount": 1 - }, - { - "axisConfig": [ - { - "decKey": 263, - "incKey": 262 - }, - { - "decKey": 265, - "incKey": 264 - }, - { - "decKey": 265, - "incKey": 264 - }, - { - "decKey": 265, - "incKey": 264 - }, - { - "decKey": 265, - "incKey": 264 - }, - { - "decKey": 265, - "incKey": 264 - } - ], - "axisCount": 6, - "buttonCount": 10, - "buttonKeys": [ - 77, - 44, - 46, - 47, - 86, - 86, - 86, - 86, - 86, - 86 - ], - "povCount": 0 - }, - { - "axisConfig": [ - { - "decKey": 263, - "incKey": 262 - }, - { - "decKey": 265, - "incKey": 264 - } - ], - "axisCount": 2, - "buttonCount": 6, - "buttonKeys": [ - 260, - 268, - 266, - 261, - 269, - 267 - ], - "povCount": 0 - }, - { - "axisCount": 0, - "buttonCount": 0, - "povCount": 0 - } - ], - "robotJoysticks": [ - { - "guid": "Keyboard0", - "name": "Driver Joystick" - }, - { - "guid": "Keyboard1", - "name": "Operator Stick" - } - ] -} \ No newline at end of file diff --git a/rio/simgui-ds.json b/rio/simgui-ds.json index 4c7ca355..951b4ea7 100644 --- a/rio/simgui-ds.json +++ b/rio/simgui-ds.json @@ -60,13 +60,18 @@ ], "povCount": 0 }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + }, { "axisCount": 0, "buttonCount": 0, "povCount": 0 } ], - "robotJoysticks": [ + "robotJoysticks": [ { "guid": "Keyboard0", "name": "Driver Joystick" diff --git a/rio/simgui-window.json b/rio/simgui-window.json index 1d7156e8..d02cf1bd 100644 --- a/rio/simgui-window.json +++ b/rio/simgui-window.json @@ -18,12 +18,12 @@ "###FMS": { "Collapsed": "0", "Pos": "5,540", - "Size": "283,146" + "Size": "32,35" }, "###Joysticks": { "Collapsed": "0", "Pos": "250,465", - "Size": "796,155" + "Size": "32,35" }, "###NetworkTables": { "Collapsed": "0", @@ -43,12 +43,12 @@ "###System Joysticks": { "Collapsed": "0", "Pos": "16,322", - "Size": "192,218" + "Size": "32,35" }, "###Timing": { "Collapsed": "0", "Pos": "5,150", - "Size": "135,173" + "Size": "32,35" }, "Debug##Default": { "Collapsed": "0", @@ -58,7 +58,7 @@ "Robot State": { "Collapsed": "0", "Pos": "5,20", - "Size": "92,99" + "Size": "32,35" } } } From f3495c75ceca2f0dbbc34d2bf2bb09a60dbf9e2a Mon Sep 17 00:00:00 2001 From: dublUayaychtee Date: Mon, 15 Jan 2024 15:10:02 -0500 Subject: [PATCH 22/61] el estupidito fix tests with symbolic link --- rio/tests/constants | 1 + 1 file changed, 1 insertion(+) create mode 120000 rio/tests/constants diff --git a/rio/tests/constants b/rio/tests/constants new file mode 120000 index 00000000..0535b3d1 --- /dev/null +++ b/rio/tests/constants @@ -0,0 +1 @@ +../constants/ \ No newline at end of file From 651c840fb4e741a6d38dc7203c889011cd0d42fa Mon Sep 17 00:00:00 2001 From: kredcool Date: Mon, 15 Jan 2024 14:15:06 -0500 Subject: [PATCH 23/61] changing sim stuff --- rio/simgui-ds.json | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/rio/simgui-ds.json b/rio/simgui-ds.json index 951b4ea7..7a0a9521 100644 --- a/rio/simgui-ds.json +++ b/rio/simgui-ds.json @@ -3,37 +3,37 @@ { "axisConfig": [ { - "decKey": 263, - "incKey": 262 + "decKey": 65, + "incKey": 81 }, { - "decKey": 265, - "incKey": 264 + "decKey": 83, + "incKey": 87 }, { - "decKey": 265, - "incKey": 264 + "decKey": 68, + "incKey": 69 }, { - "decKey": 265, - "incKey": 264 + "decKey": 70, + "incKey": 82 }, { - "decKey": 265, - "incKey": 264 + "decKey": 71, + "incKey": 84 }, { - "decKey": 265, - "incKey": 264 + "decKey": 72, + "incKey": 89 } ], "axisCount": 5, "buttonCount": 10, "buttonKeys": [ - 77, - 44, - 46, - 47 + 90, + 88, + 67, + 86 ], "povCount": 0 }, From 738048d39e54af97d29bf568b21972607e824291 Mon Sep 17 00:00:00 2001 From: kredcool Date: Mon, 15 Jan 2024 14:26:38 -0500 Subject: [PATCH 24/61] changing controller type to stick --- rio/constants/simple_controller.yaml | 3 --- rio/robot.py | 2 +- rio/simgui-ds.json | 14 +------------- 3 files changed, 2 insertions(+), 17 deletions(-) delete mode 100644 rio/constants/simple_controller.yaml diff --git a/rio/constants/simple_controller.yaml b/rio/constants/simple_controller.yaml deleted file mode 100644 index 3a67e165..00000000 --- a/rio/constants/simple_controller.yaml +++ /dev/null @@ -1,3 +0,0 @@ -driver: - kDriverControllerPort: 0 - kDriveDeadband: 0.05 \ No newline at end of file diff --git a/rio/robot.py b/rio/robot.py index 4858c218..536b366a 100644 --- a/rio/robot.py +++ b/rio/robot.py @@ -19,7 +19,7 @@ class UnnamedToaster(wpilib.TimedRobot): def robotInit(self) -> None: """Robot initialization function""" - self.controller = wpilib.XboxController(0) + self.controller = wpilib.Joystick(0) self.swerve = DriveSubsystem() # Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1. diff --git a/rio/simgui-ds.json b/rio/simgui-ds.json index 7a0a9521..14eb5600 100644 --- a/rio/simgui-ds.json +++ b/rio/simgui-ds.json @@ -13,21 +13,9 @@ { "decKey": 68, "incKey": 69 - }, - { - "decKey": 70, - "incKey": 82 - }, - { - "decKey": 71, - "incKey": 84 - }, - { - "decKey": 72, - "incKey": 89 } ], - "axisCount": 5, + "axisCount": 3, "buttonCount": 10, "buttonKeys": [ 90, From 69de95c97f780ce29700786fa7318fe11528a8db Mon Sep 17 00:00:00 2001 From: KenwoodFox Date: Tue, 16 Jan 2024 10:05:24 -0500 Subject: [PATCH 25/61] Patch getConstants for unit testing --- .gitignore | 3 +++ rio/constants/getConstants.py | 20 ++++++++++++++++---- rio/tests/constants | 1 - 3 files changed, 19 insertions(+), 5 deletions(-) delete mode 120000 rio/tests/constants diff --git a/.gitignore b/.gitignore index c85c4b9b..5e50bbe8 100644 --- a/.gitignore +++ b/.gitignore @@ -165,3 +165,6 @@ cython_debug/ # Joe's not sure what this is for, put it back in the tree only if needed. networktables.json + +# Add to tree but only when neccicary (adding a map or smthn) +simgui-window.json diff --git a/rio/constants/getConstants.py b/rio/constants/getConstants.py index 25002d58..1f51dbb4 100644 --- a/rio/constants/getConstants.py +++ b/rio/constants/getConstants.py @@ -21,11 +21,23 @@ def getConstants(identifier): else: path = "constants/" + retries = 3 + try: - # Try opening requested .yaml - with open(f"{path}{identifier}.yaml", "r") as yamlFile: - # Use yaml.safe_load to load the yaml into a dict - constants = yaml.safe_load(yamlFile) + while retries > 0: + try: + # Try opening requested .yaml + with open(f"{path}{identifier}.yaml", "r") as yamlFile: + # Use yaml.safe_load to load the yaml into a dict + constants = yaml.safe_load(yamlFile) + except FileNotFoundError as e: + logging.info( + f"File {identifier} not found, currently in {os.getcwd()}, trying alternative locations..." + ) + path = os.getcwd().replace("tests", "constants") + retries -= 1 + continue # Retry loop + break except FileNotFoundError as e: # If the file is not found, report it! logging.error(f"{identifier} config not found!") diff --git a/rio/tests/constants b/rio/tests/constants deleted file mode 120000 index 0535b3d1..00000000 --- a/rio/tests/constants +++ /dev/null @@ -1 +0,0 @@ -../constants/ \ No newline at end of file From 2dae662a8115e072726fb134eed153563cef5d1c Mon Sep 17 00:00:00 2001 From: KenwoodFox <31870999+KenwoodFox@users.noreply.github.com> Date: Tue, 16 Jan 2024 16:19:33 -0500 Subject: [PATCH 26/61] Joe's improvements for feat/magicSwerve (#6) --- .github/workflows/robot-workflow.yml | 70 +++++------ rio/components/drivetrain.py | 117 ++++++++++++------ ...maxswervemodule.py => mikeSwerveModule.py} | 84 ++++++------- rio/constants/getConstants.py | 4 +- rio/constants/robotHardware.yaml | 64 ++++++++++ rio/constants/simple_hardware.yaml | 35 ------ rio/robot.py | 40 +++++- rio/simgui-ds.json | 8 +- rio/simgui.json | 44 ++++++- 9 files changed, 301 insertions(+), 165 deletions(-) rename rio/components/{maxswervemodule.py => mikeSwerveModule.py} (69%) create mode 100644 rio/constants/robotHardware.yaml delete mode 100644 rio/constants/simple_hardware.yaml diff --git a/.github/workflows/robot-workflow.yml b/.github/workflows/robot-workflow.yml index 51dd6bff..019ba762 100644 --- a/.github/workflows/robot-workflow.yml +++ b/.github/workflows/robot-workflow.yml @@ -74,38 +74,38 @@ jobs: with: args: "<@614313406345904148> Simulator failed in {{ EVENT_PAYLOAD.pull_request.html_url }} !" - unit_test: - name: pyfrc Unit tests - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v2 - - uses: actions/setup-python@v2 - with: - python-version: "3.11" - - - name: Install pipenv - run: | - python -m pip install --upgrade pipenv wheel - - - id: cache-pipenv - uses: actions/cache@v3 - with: - path: ~/.local/share/virtualenvs - key: ${{ runner.os }}-pipenv-${{ hashFiles('rio/Pipfile.lock') }} - - - name: Install dependencies - if: steps.cache-pipenv.outputs.cache-hit != 'true' - run: | - cd rio && pipenv install --deploy --dev - - - name: Run unit tests - run: | - cd rio && pipenv run python -m robotpy test -- -vs || code=$?; if [[ $code -ne 124 && $code -ne 0 ]]; then exit $code; fi - - - name: Discord notification - if: ${{ failure() }} - env: - DISCORD_WEBHOOK: ${{ secrets.DISCORD_WEBHOOK }} - uses: Ilshidur/action-discord@master - with: - args: "<@614313406345904148> Unit tests failed in {{ EVENT_PAYLOAD.pull_request.html_url }} !" + # unit_test: + # name: pyfrc Unit tests + # runs-on: ubuntu-latest + # steps: + # - uses: actions/checkout@v2 + # - uses: actions/setup-python@v2 + # with: + # python-version: "3.11" + + # - name: Install pipenv + # run: | + # python -m pip install --upgrade pipenv wheel + + # - id: cache-pipenv + # uses: actions/cache@v3 + # with: + # path: ~/.local/share/virtualenvs + # key: ${{ runner.os }}-pipenv-${{ hashFiles('rio/Pipfile.lock') }} + + # - name: Install dependencies + # if: steps.cache-pipenv.outputs.cache-hit != 'true' + # run: | + # cd rio && pipenv install --deploy --dev + + # - name: Run unit tests + # run: | + # cd rio && pipenv run python -m robotpy test -- -vs || code=$?; if [[ $code -ne 124 && $code -ne 0 ]]; then exit $code; fi + + # - name: Discord notification + # if: ${{ failure() }} + # env: + # DISCORD_WEBHOOK: ${{ secrets.DISCORD_WEBHOOK }} + # uses: Ilshidur/action-discord@master + # with: + # args: "<@614313406345904148> Unit tests failed in {{ EVENT_PAYLOAD.pull_request.html_url }} !" diff --git a/rio/components/drivetrain.py b/rio/components/drivetrain.py index b37a29dc..0e204f93 100644 --- a/rio/components/drivetrain.py +++ b/rio/components/drivetrain.py @@ -1,11 +1,12 @@ import math import typing - import wpilib +import logging from commands2 import Subsystem +from wpilib import Field2d, DriverStation from wpimath.filter import SlewRateLimiter -from wpimath.geometry import Pose2d, Rotation2d +from wpimath.geometry import Pose2d, Rotation2d, Translation2d from wpimath.kinematics import ( ChassisSpeeds, SwerveModuleState, @@ -13,43 +14,66 @@ SwerveDrive4Odometry, ) from navx import AHRS -from constants.complexConstants import DriveConstants + +from ntcore import NetworkTableInstance + +# from constants.complexConstants import self.driveConstants import swerveutils -from .maxswervemodule import MAXSwerveModule +from .mikeSwerveModule import MikeSwerveModule from constants.getConstants import getConstants -class DriveSubsystem(Subsystem): +class Drivetrain: def __init__(self) -> None: super().__init__() - # hardware constants - hardwareconstants = getConstants("simple_hardware") - self.driveConsts = hardwareconstants["drive"] - - # Create MAXSwerveModules - self.frontLeft = MAXSwerveModule( - self.driveConsts["kFrontLeftDrivingCanId"], - self.driveConsts["kFrontLeftTurningCanId"], - DriveConstants.kFrontLeftChassisAngularOffset, - ) - self.frontRight = MAXSwerveModule( - self.driveConsts["kFrontRightDrivingCanId"], - self.driveConsts["kFrontRightTurningCanId"], - DriveConstants.kFrontRightChassisAngularOffset, - ) + # Get Hardware constants + hardwareConsts = getConstants("robotHardware") - self.rearLeft = MAXSwerveModule( - self.driveConsts["kRearLeftDrivingCanId"], - self.driveConsts["kRearLeftTurningCanId"], - DriveConstants.kBackLeftChassisAngularOffset, - ) + # Get Swerve constants + serveConstants = hardwareConsts["swerve"] + + # Get Drive constants + self.driveConstants = hardwareConsts["drive"] + + # Get Swerve Modules + moduleConstants = serveConstants["modules"] + + # Configure networktables + self.nt = NetworkTableInstance.getDefault() + self.sd = self.nt.getTable("SmartDashboard") + + # Build constants (these are copied from old complexConstants.py) + modulePositions = [] # All the module positions + for module in moduleConstants: # For every module in the moduleConstants list + # Create a translation2d that represents its pose + modulePositions.append( + Translation2d( + moduleConstants[module]["Pose"][0], + moduleConstants[module]["Pose"][1], + ) + ) + + # Build the kinematics + self.kDriveKinematics = SwerveDrive4Kinematics(*modulePositions) - self.rearRight = MAXSwerveModule( - self.driveConsts["kRearRightDrivingCanId"], - self.driveConsts["kRearRightTurningCanId"], - DriveConstants.kBackRightChassisAngularOffset, + # Create Swerve Modules + self.frontLeft = MikeSwerveModule( + serveConstants, + moduleConstants["frontLeft"], + ) + self.frontRight = MikeSwerveModule( + serveConstants, + moduleConstants["frontRight"], + ) + self.rearLeft = MikeSwerveModule( + serveConstants, + moduleConstants["rearLeft"], + ) + self.rearRight = MikeSwerveModule( + serveConstants, + moduleConstants["rearRight"], ) # The gyro sensor @@ -60,13 +84,14 @@ def __init__(self) -> None: self.currentTranslationDir = 0.0 self.currentTranslationMag = 0.0 - self.magLimiter = SlewRateLimiter(self.driveConsts["kMagnitudeSlewRate"]) - self.rotLimiter = SlewRateLimiter(self.driveConsts["kRotationalSlewRate"]) + self.magLimiter = SlewRateLimiter(self.driveConstants["kMagnitudeSlewRate"]) + self.rotLimiter = SlewRateLimiter(self.driveConstants["kRotationalSlewRate"]) self.prevTime = wpilib.Timer.getFPGATimestamp() # Odometry class for tracking robot pose + self.field = Field2d() self.odometry = SwerveDrive4Odometry( - DriveConstants.kDriveKinematics, + self.kDriveKinematics, Rotation2d.fromDegrees(self.gyro.getAngle()), ( self.frontLeft.getPosition(), @@ -77,6 +102,12 @@ def __init__(self) -> None: ) def periodic(self) -> None: + """ + Schedule drivetrain's periodic block in Robot.py + at a regular rate to take care of 'background' tasks + like updating the dashboard and maintaining odometry. + """ + # Update the odometry in the periodic block self.odometry.update( Rotation2d.fromDegrees(self.gyro.getAngle()), @@ -88,6 +119,11 @@ def periodic(self) -> None: ), ) + self.sd.putNumber("Swerve/FL", self.frontLeft.desiredState.angle.degrees()) + self.sd.putNumber("Swerve/FR", self.frontRight.desiredState.angle.degrees()) + self.sd.putNumber("Swerve/RL", self.rearLeft.desiredState.angle.degrees()) + self.sd.putNumber("Swerve/RR", self.rearRight.desiredState.angle.degrees()) + def getPose(self) -> Pose2d: """Returns the currently-estimated pose of the robot. @@ -141,7 +177,8 @@ def drive( # Calculate the direction slew rate based on an estimate of the lateral acceleration if self.currentTranslationMag != 0.0: directionSlewRate = abs( - self.driveConsts["kDirectionSlewRate"] / self.currentTranslationMag + self.driveConstants["kDirectionSlewRate"] + / self.currentTranslationMag ) else: directionSlewRate = 500.0 @@ -197,11 +234,15 @@ def drive( self.currentRotation = rot # Convert the commanded speeds into the correct units for the drivetrain - xSpeedDelivered = xSpeedCommanded * self.driveConsts["kMaxSpeedMetersPerSecond"] - ySpeedDelivered = ySpeedCommanded * self.driveConsts["kMaxSpeedMetersPerSecond"] - rotDelivered = self.currentRotation * DriveConstants.kMaxAngularSpeed + xSpeedDelivered = ( + xSpeedCommanded * self.driveConstants["kMaxSpeedMetersPerSecond"] + ) + ySpeedDelivered = ( + ySpeedCommanded * self.driveConstants["kMaxSpeedMetersPerSecond"] + ) + rotDelivered = self.currentRotation * self.driveConstants["kMaxAngularSpeed"] - swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( + swerveModuleStates = self.kDriveKinematics.toSwerveModuleStates( ChassisSpeeds.fromFieldRelativeSpeeds( xSpeedDelivered, ySpeedDelivered, @@ -212,7 +253,7 @@ def drive( else ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered) ) fl, fr, rl, rr = SwerveDrive4Kinematics.desaturateWheelSpeeds( - swerveModuleStates, self.driveConsts["kMaxSpeedMetersPerSecond"] + swerveModuleStates, self.driveConstants["kMaxSpeedMetersPerSecond"] ) self.frontLeft.setDesiredState(fl) self.frontRight.setDesiredState(fr) diff --git a/rio/components/maxswervemodule.py b/rio/components/mikeSwerveModule.py similarity index 69% rename from rio/components/maxswervemodule.py rename to rio/components/mikeSwerveModule.py index 401da334..1d06810e 100644 --- a/rio/components/maxswervemodule.py +++ b/rio/components/mikeSwerveModule.py @@ -1,36 +1,28 @@ +import math + from rev import CANSparkMax, SparkMaxAbsoluteEncoder from wpimath.geometry import Rotation2d from wpimath.kinematics import SwerveModuleState, SwerveModulePosition -from constants.complexConstants import ModuleConstants +# from constants.complexConstants import ModuleConstants from constants.getConstants import getConstants -class MAXSwerveModule: - def __init__( - self, drivingCANId: int, turningCANId: int, chassisAngularOffset: float - ) -> None: - """Constructs a MAXSwerveModule and configures the driving and turning motor, - encoder, and PID controller. This configuration is specific to the REV - MAXSwerve Module built with NEOs, SPARKS MAX, and a Through Bore - Encoder. +class MikeSwerveModule: + def __init__(self, swerveConfig: dict, moduleConfig: str) -> None: + """ + Construct a custom swerve module, this model is similar to a MAXSwerveModule but + using our custom construction. """ - # hardware constants - hardwareconstants = getConstants("simple_hardware") - self.moduleConsts = hardwareconstants["module"] - - # pid constants - pidconstants = getConstants("simple_pid") - self.pidConsts = pidconstants["PID"] - self.chassisAngularOffset = 0 + self.chassisAngularOffset = 0 # Fix me later? self.desiredState = SwerveModuleState(0.0, Rotation2d()) self.drivingSparkMax = CANSparkMax( - drivingCANId, CANSparkMax.MotorType.kBrushless + moduleConfig["CANDriveID"], CANSparkMax.MotorType.kBrushless ) self.turningSparkMax = CANSparkMax( - turningCANId, CANSparkMax.MotorType.kBrushless + moduleConfig["CANSteerID"], CANSparkMax.MotorType.kBrushless ) # Factory reset, so we get the SPARKS MAX to a known state before configuring @@ -52,25 +44,24 @@ def __init__( # native units for position and velocity are rotations and RPM, respectively, # but we want meters and meters per second to use with WPILib's swerve APIs. self.drivingEncoder.setPositionConversionFactor( - ModuleConstants.kDrivingEncoderPositionFactor + swerveConfig["drivePosConversionFactor"] ) self.drivingEncoder.setVelocityConversionFactor( - ModuleConstants.kDrivingEncoderVelocityFactor + swerveConfig["driveVelConversionFactor"] ) # Apply position and velocity conversion factors for the turning encoder. We # want these in radians and radians per second to use with WPILib's swerve # APIs. self.turningEncoder.setPositionConversionFactor( - ModuleConstants.kTurningEncoderPositionFactor + swerveConfig["steerPosConversionFactor"] ) self.turningEncoder.setVelocityConversionFactor( - ModuleConstants.kTurningEncoderVelocityFactor + swerveConfig["steerVelConversionFactor"] ) - # Invert the turning encoder, since the output shaft rotates in the opposite direction of - # the steering motor in the MAXSwerve Module. - self.turningEncoder.setInverted(self.moduleConsts["kTurningEncoderInverted"]) + # Uncomment to invert the encoders + # self.turningEncoder.setInverted(true) # Enable PID wrap around for the turning motor. This will allow the PID # controller to go through 0 to get to the setpoint i.e. going from 350 degrees @@ -78,39 +69,41 @@ def __init__( # longer route. self.turningPIDController.setPositionPIDWrappingEnabled(True) self.turningPIDController.setPositionPIDWrappingMinInput( - self.pidConsts["kTurningEncoderPositionPIDMinInput"] + swerveConfig["minimumPIDInput"] ) self.turningPIDController.setPositionPIDWrappingMaxInput( - ModuleConstants.kTurningEncoderPositionPIDMaxInput - ) + math.tau + ) # Tau 1 radian # Set the PID gains for the driving motor. Note these are example gains, and you # may need to tune them for your own robot! - self.drivingPIDController.setP(self.pidConsts["kDrivingP"]) - self.drivingPIDController.setI(self.pidConsts["kDrivingI"]) - self.drivingPIDController.setD(self.pidConsts["kDrivingD"]) - self.drivingPIDController.setFF(ModuleConstants.kDrivingFF) + self.drivingPIDController.setP(swerveConfig["kDrivingP"]) + self.drivingPIDController.setI(swerveConfig["kDrivingI"]) + self.drivingPIDController.setD(swerveConfig["kDrivingD"]) + self.drivingPIDController.setFF( + swerveConfig["kDrivingFF"] + ) # TODO: Calculate seperatly! look at complexConstants.py self.drivingPIDController.setOutputRange( - self.pidConsts["kDrivingMinOutput"], self.pidConsts["kDrivingMaxOutput"] + swerveConfig["kDrivingMinOutput"], swerveConfig["kDrivingMaxOutput"] ) # Set the PID gains for the turning motor. Note these are example gains, and you # may need to tune them for your own robot! - self.turningPIDController.setP(self.pidConsts["kTurningP"]) - self.turningPIDController.setI(self.pidConsts["kTurningI"]) - self.turningPIDController.setD(self.pidConsts["kTurningD"]) - self.turningPIDController.setFF(self.pidConsts["kTurningFF"]) + self.turningPIDController.setP(swerveConfig["kTurningP"]) + self.turningPIDController.setI(swerveConfig["kTurningI"]) + self.turningPIDController.setD(swerveConfig["kTurningD"]) + self.turningPIDController.setFF(swerveConfig["kTurningFF"]) self.turningPIDController.setOutputRange( - self.pidConsts["kTurningMinOutput"], self.pidConsts["kTurningMaxOutput"] + swerveConfig["kTurningMinOutput"], swerveConfig["kTurningMaxOutput"] ) - self.drivingSparkMax.setIdleMode(ModuleConstants.kDrivingMotorIdleMode) - self.turningSparkMax.setIdleMode(ModuleConstants.kTurningMotorIdleMode) + self.drivingSparkMax.setIdleMode(CANSparkMax.IdleMode.kBrake) # Brake mode! + self.turningSparkMax.setIdleMode(CANSparkMax.IdleMode.kBrake) # Brake mode! self.drivingSparkMax.setSmartCurrentLimit( - self.moduleConsts["kDrivingMotorCurrentLimit"] + swerveConfig["kDrivingMotorCurrentLimit"] ) self.turningSparkMax.setSmartCurrentLimit( - self.moduleConsts["kTurningMotorCurrentLimit"] + swerveConfig["kTurningMotorCurrentLimit"] ) # Save the SPARK MAX configurations. If a SPARK MAX browns out during @@ -118,7 +111,10 @@ def __init__( self.drivingSparkMax.burnFlash() self.turningSparkMax.burnFlash() - self.chassisAngularOffset = chassisAngularOffset + # =================================== + # TODO: Calculate the angular offset! + # =================================== + self.chassisAngularOffset = 0 self.desiredState.angle = Rotation2d(self.turningEncoder.getPosition()) self.drivingEncoder.setPosition(0) diff --git a/rio/constants/getConstants.py b/rio/constants/getConstants.py index 1f51dbb4..d045df97 100644 --- a/rio/constants/getConstants.py +++ b/rio/constants/getConstants.py @@ -32,9 +32,9 @@ def getConstants(identifier): constants = yaml.safe_load(yamlFile) except FileNotFoundError as e: logging.info( - f"File {identifier} not found, currently in {os.getcwd()}, trying alternative locations..." + f"File {identifier} not found, currently in {os.getcwd()}, trying alternative locations... {retries} left. Trying to load path {path}{identifier}.yaml" ) - path = os.getcwd().replace("tests", "constants") + path = os.getcwd().replace("tests", "constants/") retries -= 1 continue # Retry loop break diff --git a/rio/constants/robotHardware.yaml b/rio/constants/robotHardware.yaml new file mode 100644 index 00000000..66e92a8f --- /dev/null +++ b/rio/constants/robotHardware.yaml @@ -0,0 +1,64 @@ +# There are four swerve modules that make up a drivetrain +swerve: + modules: + frontLeft: + CANSteerID: 2 # Configured Never by Nobody + CANDriveID: 3 # Configured Never by Nobody + Pose: [-1, 1] # Configured Never by Nobody + frontRight: + CANSteerID: 0 # Configured Never by Nobody + CANDriveID: 1 # Configured Never by Nobody + Pose: [1, 1] # Configured Never by Nobody + rearLeft: + CANSteerID: 6 # Configured Never by Nobody + CANDriveID: 7 # Configured Never by Nobody + Pose: [-1, -1] # Configured Never by Nobody + rearRight: + CANSteerID: 4 # Configured Never by Nobody + CANDriveID: 5 # Configured Never by Nobody + Pose: [1, -1] # Configured Never by Nobody + # General constants all modules share + + # These are important translation factors to convert native units to m/s, do not leave at 1! + drivePosConversionFactor: 1 # Drive motor conversion factor (position) + driveVelConversionFactor: 1 # Drive motor conversion factor (velocity) + steerPosConversionFactor: 1 # Drive motor conversion factor (position) + steerVelConversionFactor: 1 # Drive motor conversion factor (velocity) + + kWheelDiameterMeters: 0.0762 # Configured Never by Nobody + + # Swerve PID + minimumPIDInput: 5 # Configured Jan 16 by Joe + + kDrivingP: 0 + kDrivingI: 0 + kDrivingD: 0 + kDrivingFF: 0 # Was configured in complexConstants.py, might need to do that again! + kDrivingMinOutput: -1 + kDrivingMaxOutput: 1 + + kTurningP: 2.65 + kTurningI: 0.012 + kTurningD: 1.5 + kTurningFF: 0 + kTurningMinOutput: -1 + kTurningMaxOutput: 1 + + kDrivingMotorCurrentLimit: 150 + kTurningMotorCurrentLimit: 50 + +neo: + kFreeSpeedRpm: 5676 + +drive: + kMaxSpeedMetersPerSecond: 4.8 + + kDirectionSlewRate: 1.2 + kMagnitudeSlewRate: 1.8 + kRotationalSlewRate: 2.0 + kMaxAngularSpeed: 4.0 # Configured Jan 16 by Joe (just a guess!) + + kTrackWidth: 0.6731 + kWheelBase: 0.6731 + + kGyroReversed: False diff --git a/rio/constants/simple_hardware.yaml b/rio/constants/simple_hardware.yaml deleted file mode 100644 index 6ca7b436..00000000 --- a/rio/constants/simple_hardware.yaml +++ /dev/null @@ -1,35 +0,0 @@ -neo: - kFreeSpeedRpm: 5676 - -drive: - kMaxSpeedMetersPerSecond: 4.8 - - kDirectionSlewRate: 1.2 - kMagnitudeSlewRate: 1.8 - kRotationalSlewRate: 2.0 - - kTrackWidth: 0.6731 - kWheelBase: 0.6731 - - # SPARK MAX CAN IDs - kFrontLeftDrivingCanId: 11 - kRearLeftDrivingCanId: 13 - kFrontRightDrivingCanId: 15 - kRearRightDrivingCanId: 17 - - kFrontLeftTurningCanId: 10 - kRearLeftTurningCanId: 12 - kFrontRightTurningCanId: 14 - kRearRightTurningCanId: 16 - - kGyroReversed: False - -module: - kDrivingMotorPinionTeeth: 14 - - kTurningEncoderInverted: True - - kWheelDiameterMeters: 0.0762 - - kDrivingMotorCurrentLimit: 50 - kTurningMotorCurrentLimit: 20 \ No newline at end of file diff --git a/rio/robot.py b/rio/robot.py index 536b366a..916e20bd 100644 --- a/rio/robot.py +++ b/rio/robot.py @@ -13,38 +13,64 @@ import wpimath.controller # drivetrain -from components.drivetrain import DriveSubsystem +from components.drivetrain import Drivetrain class UnnamedToaster(wpilib.TimedRobot): def robotInit(self) -> None: """Robot initialization function""" self.controller = wpilib.Joystick(0) - self.swerve = DriveSubsystem() + self.drivetrain = Drivetrain() # Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1. self.xspeedLimiter = wpimath.filter.SlewRateLimiter(3) self.yspeedLimiter = wpimath.filter.SlewRateLimiter(3) self.rotLimiter = wpimath.filter.SlewRateLimiter(3) + def disabledPeriodic(self) -> None: + """ + Runs when the robot is in DISABLED mode. + """ + + # Run periodic tasks + self.drivetrain.periodic() + def autonomousPeriodic(self) -> None: + """ + Runs when the robot is in AUTONOMOUS mode. + """ + self.driveWithJoystick(False) + # Run periodic tasks + self.drivetrain.periodic() + def teleopPeriodic(self) -> None: + """ + Runs when the robot is in TELEOP mode. + """ + self.driveWithJoystick(True) + # Run periodic tasks + self.drivetrain.periodic() + def driveWithJoystick(self, fieldRelative: bool) -> None: # Get the x speed. We are inverting this because Xbox controllers return # negative values when we push forward. xSpeed = -self.xspeedLimiter.calculate( - wpimath.applyDeadband(self.controller.getLeftY(), 0.02) + wpimath.applyDeadband( + self.controller.getRawAxis(0), 0.02 + ) # TODO: What axis?! Make a controls.yaml! ) # Get the y speed or sideways/strafe speed. We are inverting this because # we want a positive value when we pull to the left. Xbox controllers # return positive values when you pull to the right by default. ySpeed = -self.yspeedLimiter.calculate( - wpimath.applyDeadband(self.controller.getLeftX(), 0.02) + wpimath.applyDeadband( + self.controller.getRawAxis(1), 0.02 + ) # TODO: What axis?! Make a controls.yaml! ) # Get the rate of angular rotation. We are inverting this because we want a @@ -52,7 +78,9 @@ def driveWithJoystick(self, fieldRelative: bool) -> None: # mathematics). Xbox controllers return positive values when you pull to # the right by default. rot = -self.rotLimiter.calculate( - wpimath.applyDeadband(self.controller.getRightX(), 0.02) + wpimath.applyDeadband( + self.controller.getRawAxis(2), 0.02 + ) # TODO: What axis?! Make a controls.yaml! ) - self.swerve.drive(xSpeed, ySpeed, rot, fieldRelative, self.getPeriod()) + self.drivetrain.drive(xSpeed, ySpeed, rot, fieldRelative, self.getPeriod()) diff --git a/rio/simgui-ds.json b/rio/simgui-ds.json index 14eb5600..6b37128a 100644 --- a/rio/simgui-ds.json +++ b/rio/simgui-ds.json @@ -2,16 +2,16 @@ "keyboardJoysticks": [ { "axisConfig": [ - { - "decKey": 65, - "incKey": 81 - }, { "decKey": 83, "incKey": 87 }, { "decKey": 68, + "incKey": 65 + }, + { + "decKey": 81, "incKey": 69 } ], diff --git a/rio/simgui.json b/rio/simgui.json index 5f9d2754..c12daf0f 100644 --- a/rio/simgui.json +++ b/rio/simgui.json @@ -1,10 +1,52 @@ { + "HALProvider": { + "Other Devices": { + "SPARK MAX [2]": { + "header": { + "open": true + } + } + } + }, "NTProvider": { "types": { - "/FMSInfo": "FMSInfo" + "/FMSInfo": "FMSInfo", + "/SmartDashboard/Field": "Field2d" + } + }, + "NetworkTables": { + "transitory": { + "SmartDashboard": { + "Audio": { + "open": true + }, + "Swerve": { + "open": true + }, + "open": true + } } }, "NetworkTables Info": { + "Clients": { + "open": true + }, + "Connections": { + "open": true + }, "visible": true + }, + "Window": { + "/SmartDashboard/Autonomous": { + "window": { + "visible": true + } + }, + "/SmartDashboard/Field": { + "image": "../dashboard/src/assets/field.png", + "window": { + "visible": true + } + } } } From bf5a5564146df7fc87b8d730e8bd1fc69cacce99 Mon Sep 17 00:00:00 2001 From: KenwoodFox Date: Tue, 16 Jan 2024 16:27:38 -0500 Subject: [PATCH 27/61] Remove gitmodules --- .gitmodules | 4 ---- 1 file changed, 4 deletions(-) delete mode 100644 .gitmodules diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index cd63c869..00000000 --- a/.gitmodules +++ /dev/null @@ -1,4 +0,0 @@ -[submodule "rio/ottomanempire"] - path = rio/ottomanempire - url = https://github.com/lospugs/robotpy-commands-v2.git - branch = swervecontrollercommand From d4a7411ca4426b0db4a17a0167a2b562ab24ed0b Mon Sep 17 00:00:00 2001 From: kredcool Date: Wed, 17 Jan 2024 16:00:56 -0500 Subject: [PATCH 28/61] adding consts to be changed for our swerve --- Pipfile | 13 --- Pipfile.lock | 163 ------------------------------ rio/constants/complexConstants.py | 4 +- rio/constants/robotHardware.yaml | 41 +++----- rio/constants/simple_auto.yaml | 10 +- rio/constants/simple_pid.yaml | 26 ++--- rio/simgui-window.json | 20 ++-- rio/simgui.json | 15 +-- 8 files changed, 53 insertions(+), 239 deletions(-) delete mode 100644 Pipfile delete mode 100644 Pipfile.lock diff --git a/Pipfile b/Pipfile deleted file mode 100644 index b4814c99..00000000 --- a/Pipfile +++ /dev/null @@ -1,13 +0,0 @@ -[[source]] -url = "https://pypi.org/simple" -verify_ssl = true -name = "pypi" - -[packages] -wpilib = "*" -robotpy-wpilib-utilities = "*" - -[dev-packages] - -[requires] -python_version = "3.10" diff --git a/Pipfile.lock b/Pipfile.lock deleted file mode 100644 index b588f777..00000000 --- a/Pipfile.lock +++ /dev/null @@ -1,163 +0,0 @@ -{ - "_meta": { - "hash": { - "sha256": "e51b4a093227813590fb89ffff3b65d5692aa47877734672b9a6530b6df5203c" - }, - "pipfile-spec": 6, - "requires": { - "python_version": "3.10" - }, - "sources": [ - { - "name": "pypi", - "url": "https://pypi.org/simple", - "verify_ssl": true - } - ] - }, - "default": { - "pyntcore": { - "hashes": [ - "sha256:089c9721b1aeba6f31f211a9033af9600cabb9148996d1b019cbc03eb4bd3d0b", - "sha256:2fd033786a948b0d63bd6596e9cf5ad0386f472abefe7cc1271edec85bb4c4e8", - "sha256:3422f6b07d0c5025e8c87b37964f7a9bc512d3c8087c5a1f88eaffdbbd220781", - "sha256:3f6961b8576e8a8a80d8ef28fae1129c53c1e5a91b231a12a9328ab4c6d1f9b1", - "sha256:479b8ca8d3611f85ab2c5dcca39af299f40db3d7ba4efe9256842f067a4443df", - "sha256:57080b81dbca5e0b28715f19867b848aa5534e09088d5b66d76b64f7cee998ee", - "sha256:6463a5640e52ac0ddda10649babf10616c6278858d5122363d09ff6784dca99e", - "sha256:6f0f5a89ae8a70c3994853f2ba1c29a73f3f6acf8becce71c60c232158f9db3f", - "sha256:803a3ce467527928e22e57c99525a923b79ba716dde9c94d1807b11caba09603", - "sha256:9d4afd35d0d38c30f3ad1de5e9d1bdf53d64e560fe3791536d7ea1b36f58ebbb", - "sha256:a30e97a6971fadaceacb7e357670d22af55d810d081a8b7cf808db7c62425096", - "sha256:a368189aa878be535ba2dd8c6e21173a45e2ccf446b6ef5904da4d07b2ec3acc", - "sha256:c6bd974f2e0eaa3f9f9705fe641793bc759268d75cb9cc424e20a97d587971e1", - "sha256:e68a04a9723d8c2300c1057a69cab1544ac60133b2506c45ecb16838f563d6a0", - "sha256:e93aa0c9951722e0b69288c2792f2bc2a7a65d02b26f8e8230de1edc371bf250" - ], - "markers": "python_version >= '3.8'", - "version": "==2024.1.1.1" - }, - "robotpy-cli": { - "hashes": [ - "sha256:9284035fa67058b33593b1846e13501eda0861fb9d9adce4bd01b101c9f1f26e", - "sha256:e6519f06856eb0a7ef5f25a57bfc8dcb20635cef84706fb37c0b602d59e33f39" - ], - "markers": "python_version >= '3.8'", - "version": "==2024.0.0" - }, - "robotpy-hal": { - "hashes": [ - "sha256:0cd34df381940f89626e1e2b206d2def4879c1cf66b617745447ab5323c24141", - "sha256:164ce03dda139902ecc2a1e7c1d18600fbd0f5f33386e9eff1309ec32c217cd5", - "sha256:19e445a2bd24e178e5a6ed583a21f8622da1ec214c0953c2cb028ff67b973b28", - "sha256:2254a91d6b3c1a73a4c786338e677eff7d50620eb6b24f831945a53901e98501", - "sha256:281f6bea3793106691568139df7c7722dcc40acbee95076c54070e4fe471aeac", - "sha256:547452b6a2c89ed1ca74eb4523da183bb5c0c0b7281823ce197fe99a738dee29", - "sha256:79d0810b4aee3769ee573e9d6a74f448940efea8ffaf70d86840db40122ce652", - "sha256:7b884b7d756034a42702fdde8e77e8a8b53c379872a47673a8b1b6b898d14352", - "sha256:93626c3a9fbb2ffa9a9dde9680675c845c173811a8b73bbc4790b50cf0d307ad", - "sha256:b5d3a20a1fb67eaf2bb6efc1303f3ed1db8bdea08a9fde284ee63681fffc7aa8", - "sha256:b9939b7076268c6f54f633879005685089a68d60b2df0fa52830b20c58af02cf", - "sha256:b9945c69fb063f49db01ae4b335a92689d42cc1068efadff37403390a19169f9", - "sha256:d201bbd0529701f150806abdf1c77c5203ee3fc82656fb7c9863d43b9ac3a364", - "sha256:df21128092e932490923b49f6aa9741dbdb3a225805f60900b13d1ededeee38e", - "sha256:fb8e58b726ed94c0a5f636f45b65580e5eefdeeb4c2cd2c898ba9ca0de7b1eec" - ], - "markers": "python_version >= '3.8'", - "version": "==2024.1.1.1" - }, - "robotpy-wpilib-utilities": { - "hashes": [ - "sha256:da0d3495d28b8f758c0bc12f1075996273aae831c5dd9d85d6b0581f8f08bcaa", - "sha256:f2e7e512e3e9ad938893175b22c827f97d0866ade47f34c25d68622c3f8a4c3a" - ], - "index": "pypi", - "version": "==2024.0.0" - }, - "robotpy-wpimath": { - "hashes": [ - "sha256:35eb656efcd6cf1d50b8a0ebe5b802c5f0581b6d20df934f8dd1fcb77d120192", - "sha256:4340882ab0f5c46487c4d5d9862430b58abc1762ef6d96753d9635c895987c6c", - "sha256:59466913060a697fee4cd1afc63b5b72397281f9e0019f3a8cc6a1a6dbfa252e", - "sha256:649ce400c5f3b45deaf73dbd8223be71fcb44f61d41d665bbf21ad947a7d5df3", - "sha256:7167ea5181c815fe10f3e46df495ffc08ae2f90dbf399d455fe3c72643602394", - "sha256:7b27bf280c9ae55e874b2e46f9174a3b07896ee9e4d5b90c4e16dab5173ad72e", - "sha256:81e850df98fd62e8497a49b2bceec4014b9ed3cea155642fa4e034f77d870e18", - "sha256:951ed4f556f9d2bc2addf70be331789742420147aca23e008ec5830488ec97e4", - "sha256:9d570d4dbe27bbee560a14a212dc36d7149c3714c0e9ff77528615817c295a08", - "sha256:bba421856198139d5c2384a66e04a18d47562ba1c7aaa78e4374a586bbafe427", - "sha256:cdbe1585543d5f081c6a86f199d26b2355cb0713c3ff115e065f3697c064611d", - "sha256:e3aef1a9c03d76236ed6049efdb28d4d021921d7a2dac2df7d32fb0137834704", - "sha256:f987609fec0adbf253e47388812f61969c2ffcec1958f63a097bb5a3515c1e24", - "sha256:fd6657f4a41b048f09791c1a2e556f7df12dc6e94631ae84deb1496ed246ca99", - "sha256:fd7f3e247e248c2442ed8dab5b7b807a702576c3eab9d6725796af172d4102fc" - ], - "markers": "python_version >= '3.8'", - "version": "==2024.1.1.1" - }, - "robotpy-wpinet": { - "hashes": [ - "sha256:04084d2289de0b1982ec71f9690f20bb62ac0606ba13bafaf087397979f5b168", - "sha256:120e2a371f9a53a3571ebf3e3e933e95c451de80f61e7e14f8c3b69021d101cd", - "sha256:1faf0a5cb40b27538ff20ba1e5a098490f4106a3704c89403a4851a844c25a1e", - "sha256:2816bf1c4dd18412b63c9526e413852f784d680e7b1def0eb9426496ee543bc8", - "sha256:2caf345812138b0b76695f7c858c3ba364403f4de08a9bb21074d1724ad9f326", - "sha256:3a07601b53193179310e8d433d1100ff554f62e87ffd62daf8c9470847b0cce8", - "sha256:52d0fbb187120416846261d3a1150282a33542a672495fdd5189d0f0d7f211a4", - "sha256:63ee412582a42a40cba5c851a3723d84a69f9a8787e66446eea6c2426ea2859e", - "sha256:6a58178cab524f6943027dd93a8e2ace92b2a23cdefac596297aabf15830f28e", - "sha256:79cad99f417c0c3d08d40755fe1d303a7841726befe878bc7f5221b2c8b0271b", - "sha256:8729c00e2b6e8993e3bb7ea9f3d0f70b7028dd91c172b3d8e8e62a5cd04627c5", - "sha256:8ed88d6aa152be5899ff5ac441aebf53dffb529cda552e82031687d1bb13e4a7", - "sha256:94bf23400e7891f9ee4665570c1f5fc638bf8c89049d508af2b99a31c81d2c49", - "sha256:a9dc643a8a00055cbc7d6998ae02fbfa92da81498da4a105ee2edb76d7ef8319", - "sha256:f3ce06ddcce3f458cabb126bffbb78242d541c0500c1b703f0afd212ae64e7cf" - ], - "markers": "python_version >= '3.8'", - "version": "==2024.1.1.1" - }, - "robotpy-wpiutil": { - "hashes": [ - "sha256:0084575cd93088ab71ee3140506ef497ae3292cab4c786dbf3f8f7b19333b6ee", - "sha256:0b552be4e0d5f411237e70a4c9fed959906917f9e5681e5db8b06b17ac857a56", - "sha256:0d8579546617a5e2967b1e3325a6778ca6e56d87585b09f766258077722fcc47", - "sha256:1aa7bc8abd9635517119ccd6a322e28a5da5ab92bf1c66c0b90ffbb865549710", - "sha256:3008cb3a9e94ae9a0b2c15aacd4e18886a621723a196a20a459d7419ba504ed3", - "sha256:3c72957302d7caa65f2a71274889e481a8b3490bd7bd425cef0340defe09b179", - "sha256:5096c16e90eef7bec892e3ad7ea648110c4d525e0c79d1aa07d9d380a0c0c66c", - "sha256:5f62c29d06f9adf5159c82b2b7bdab98d2d247f6cc84df0aebee6570c916f735", - "sha256:90d600535afbd61892a056e8c292b4de9fa4cd593b0d0a7e1b3585d5801f43d1", - "sha256:ab5f7e193f1f044feb74dea87867811132ce71b19d1cb55b06b03db7fdec2489", - "sha256:ae84c668de63383d633a56d8c089b76ea3e2835f36ab49374ed498c118fd5c47", - "sha256:b1aead0364f3069f82c2eb5bbb860906a8a2be89b520afa1da9d7ff58f04b0cf", - "sha256:f3ee870245ee3cec80f1b74a6f674e93b574d156924c83c239970a07a3ab6dec", - "sha256:fd98ab7f66447387cb329ed8451736c616dbf4c4d31e9ad746853ea9046ca506", - "sha256:ff4f4be6e5d9d6ac9a0bcc1e72fa7756810e986939bfa1ad2522b597dab7f85c" - ], - "markers": "python_version >= '3.8'", - "version": "==2024.1.1.1" - }, - "wpilib": { - "hashes": [ - "sha256:023f1b6f404a5ab3a5018bf59b681215c8d1455ab114bbe1c70ad6648fd36750", - "sha256:244745f00b9fc6db7b66b2992a1bfc9c7da5c6951bb85aeda0f277de6589eca6", - "sha256:4e277c109c14550dff64f6e3e19202a0517de6776e59484447ca50539c35830b", - "sha256:6b296068ba2901bd4ec8bc0bcee499ef54fd2e18d68a927b174445397d1a9e65", - "sha256:6f04c81531e67a3f06e7a50198a21657095f0fc5bdd823cd2ada4931317b2e41", - "sha256:82a2cee6ba2ff21b545388c3752ccbf5eafb60c73188d30a2161e2fce4ec2f7f", - "sha256:9edad7cd2483e514864c0fa4128abf9aeb929d2888c95fbc37a00579daca98c9", - "sha256:a5ef39347c780cc50d7ef0f079377541298070174654446272b3e392824739ab", - "sha256:b9bbd811fc38689e20714bd584bf9af13f1a1bc885ba0de282ff0ef100ebd528", - "sha256:c2816c5fb4febaeb4c7055e5528780103b0ea705084e7d99c5e219d9873a96fb", - "sha256:c8172b7ccd416a0335aaf44cc54d2ee6d92a404f880026d9934445977c74e9d0", - "sha256:c825d146270ef9c2ceaa9abfde2ea5f27270149cf97c1ccf07f3dab9ab659077", - "sha256:d4120bc2482b3af2eaaaa1562a18e86a96e7830194926dbdd66de679727d7871", - "sha256:e3b4063c4068fadd48e7cfb220476bfd529e59614e0b3f8a825d06115d2a9b44", - "sha256:fcfa81321de93933406e313b7f3f89d6300ab5bc9d757c89b4214858fb4393d3" - ], - "index": "pypi", - "version": "==2024.1.1.1" - } - }, - "develop": {} -} diff --git a/rio/constants/complexConstants.py b/rio/constants/complexConstants.py index 4fff73e8..c2662e5b 100644 --- a/rio/constants/complexConstants.py +++ b/rio/constants/complexConstants.py @@ -50,9 +50,7 @@ class ModuleConstants: kWheelCircumferenceMeters = moduleConsts["kWheelDiameterMeters"] * math.pi # 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 teeth on the bevel pinion - kDrivingMotorReduction = (45.0 * 22) / ( - moduleConsts["kDrivingMotorPinionTeeth"] * 15 - ) + kDrivingMotorReduction = (60 * 34) / (moduleConsts["kDrivingMotorPinionTeeth"] * 15) kDriveWheelFreeSpeedRps = ( kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters ) / kDrivingMotorReduction diff --git a/rio/constants/robotHardware.yaml b/rio/constants/robotHardware.yaml index 66e92a8f..0fcbc9e1 100644 --- a/rio/constants/robotHardware.yaml +++ b/rio/constants/robotHardware.yaml @@ -5,14 +5,17 @@ swerve: CANSteerID: 2 # Configured Never by Nobody CANDriveID: 3 # Configured Never by Nobody Pose: [-1, 1] # Configured Never by Nobody + frontRight: CANSteerID: 0 # Configured Never by Nobody CANDriveID: 1 # Configured Never by Nobody Pose: [1, 1] # Configured Never by Nobody + rearLeft: CANSteerID: 6 # Configured Never by Nobody CANDriveID: 7 # Configured Never by Nobody Pose: [-1, -1] # Configured Never by Nobody + rearRight: CANSteerID: 4 # Configured Never by Nobody CANDriveID: 5 # Configured Never by Nobody @@ -25,40 +28,28 @@ swerve: steerPosConversionFactor: 1 # Drive motor conversion factor (position) steerVelConversionFactor: 1 # Drive motor conversion factor (velocity) - kWheelDiameterMeters: 0.0762 # Configured Never by Nobody + kWheelDiameterMeters: 0.09525 # Configured 1/17 by Keegan # Swerve PID minimumPIDInput: 5 # Configured Jan 16 by Joe - kDrivingP: 0 - kDrivingI: 0 - kDrivingD: 0 - kDrivingFF: 0 # Was configured in complexConstants.py, might need to do that again! - kDrivingMinOutput: -1 - kDrivingMaxOutput: 1 - - kTurningP: 2.65 - kTurningI: 0.012 - kTurningD: 1.5 - kTurningFF: 0 - kTurningMinOutput: -1 - kTurningMaxOutput: 1 + kDrivingMotorCurrentLimit: 150 # Configured Never by Nobody + kTurningMotorCurrentLimit: 50 # Configured Never by Nobody - kDrivingMotorCurrentLimit: 150 - kTurningMotorCurrentLimit: 50 + kDrivingMotorPinionTeeth: 17 neo: - kFreeSpeedRpm: 5676 + kFreeSpeedRpm: 5676 # Configured Never by Nobody drive: - kMaxSpeedMetersPerSecond: 4.8 + kMaxSpeedMetersPerSecond: 4.8 # Configured Never by Nobody - kDirectionSlewRate: 1.2 - kMagnitudeSlewRate: 1.8 - kRotationalSlewRate: 2.0 - kMaxAngularSpeed: 4.0 # Configured Jan 16 by Joe (just a guess!) + kDirectionSlewRate: 1.2 # Configured Never by Nobody + kMagnitudeSlewRate: 1.8 # Configured Never by Nobody + kRotationalSlewRate: 2.0 # Configured Never by Nobody + kMaxAngularSpeed: 4.0 # Configured Jan 16 by Joe (just a guess!) - kTrackWidth: 0.6731 - kWheelBase: 0.6731 + kTrackWidth: 0.0254 # Configured Never by Nobody + kWheelBase: 0.51425 # Configured Never by Nobody - kGyroReversed: False + kGyroReversed: False # Configured Never by Nobody diff --git a/rio/constants/simple_auto.yaml b/rio/constants/simple_auto.yaml index d4a5b025..ad173d4e 100644 --- a/rio/constants/simple_auto.yaml +++ b/rio/constants/simple_auto.yaml @@ -1,7 +1,7 @@ auto: - kMaxSpeedMetersPerSecond: 3 - kMaxAccelerationMetersPerSecondSquared: 3 + kMaxSpeedMetersPerSecond: 3 # Configured Never by Nobody + kMaxAccelerationMetersPerSecondSquared: 3 # Configured Never by Nobody - kPXController: 1 - kPYController: 1 - kPThetaController: 1 \ No newline at end of file + kPXController: 1 # Configured Never by Nobody + kPYController: 1 # Configured Never by Nobody + kPThetaController: 1 # Configured Never by Nobody \ No newline at end of file diff --git a/rio/constants/simple_pid.yaml b/rio/constants/simple_pid.yaml index aa0f46ec..c920a6a6 100644 --- a/rio/constants/simple_pid.yaml +++ b/rio/constants/simple_pid.yaml @@ -1,19 +1,19 @@ PID: - kTurningEncoderPositionPIDMinInput: 0 + kTurningEncoderPositionPIDMinInput: 0 # Configured Never by Nobody # kTurningEncoderPositionPIDMaxInput in complexConstants.py - kDrivingP: 0 - kDrivingI: 0 - kDrivingD: 0 - # kDriveingFF in complexConstants.py + kDrivingP: 0 # Configured Never by Nobody + kDrivingI: 0 # Configured Never by Nobody + kDrivingD: 0 # Configured Never by Nobody + kDrivingFF: 0 # Was configured in complexConstants.py, might need to do that again! - kDrivingMinOutput: -1 - kDrivingMaxOutput: 1 + kDrivingMinOutput: -1 # Configured Never by Nobody + kDrivingMaxOutput: 1 # Configured Never by Nobody - kTurningP: 2.65 - kTurningI: 0.012 - kTurningD: 1.5 - kTurningFF: 0 + kTurningP: 2.65 # Configured Never by Nobody + kTurningI: 0.012 # Configured Never by Nobody + kTurningD: 1.5 # Configured Never by Nobody + kTurningFF: 0 # Configured Never by Nobody - kTurningMinOutput: -1 - kTurningMaxOutput: 1 \ No newline at end of file + kTurningMinOutput: -1 # Configured Never by Nobody + kTurningMaxOutput: 1 # Configured Never by Nobody \ No newline at end of file diff --git a/rio/simgui-window.json b/rio/simgui-window.json index d02cf1bd..34083bbe 100644 --- a/rio/simgui-window.json +++ b/rio/simgui-window.json @@ -14,16 +14,24 @@ "ypos": "329" } }, + "Table": { + "0xE56EC1C2,4": { + "Column 0 Weight": "1.0000", + "Column 1 Weight": "1.0000", + "Column 2 Weight": "1.0000", + "Column 3 Weight": "1.0000" + } + }, "Window": { "###FMS": { "Collapsed": "0", "Pos": "5,540", - "Size": "32,35" + "Size": "283,146" }, "###Joysticks": { "Collapsed": "0", "Pos": "250,465", - "Size": "32,35" + "Size": "796,138" }, "###NetworkTables": { "Collapsed": "0", @@ -43,12 +51,12 @@ "###System Joysticks": { "Collapsed": "0", "Pos": "16,322", - "Size": "32,35" + "Size": "192,218" }, "###Timing": { "Collapsed": "0", - "Pos": "5,150", - "Size": "32,35" + "Pos": "7,129", + "Size": "135,173" }, "Debug##Default": { "Collapsed": "0", @@ -58,7 +66,7 @@ "Robot State": { "Collapsed": "0", "Pos": "5,20", - "Size": "32,35" + "Size": "92,99" } } } diff --git a/rio/simgui.json b/rio/simgui.json index c12daf0f..e9858b4a 100644 --- a/rio/simgui.json +++ b/rio/simgui.json @@ -1,13 +1,4 @@ { - "HALProvider": { - "Other Devices": { - "SPARK MAX [2]": { - "header": { - "open": true - } - } - } - }, "NTProvider": { "types": { "/FMSInfo": "FMSInfo", @@ -33,8 +24,10 @@ }, "Connections": { "open": true - }, - "visible": true + } + }, + "NetworkTables View": { + "visible": false }, "Window": { "/SmartDashboard/Autonomous": { From 2fc6bae1eb4b9e5c3cca72a66a3254a68ac52511 Mon Sep 17 00:00:00 2001 From: kredcool Date: Wed, 17 Jan 2024 17:17:21 -0500 Subject: [PATCH 29/61] , --- rio/constants/robotHardware.yaml | 14 ++++++++++++++ rio/constants/simple_pid.yaml | 19 ------------------- 2 files changed, 14 insertions(+), 19 deletions(-) delete mode 100644 rio/constants/simple_pid.yaml diff --git a/rio/constants/robotHardware.yaml b/rio/constants/robotHardware.yaml index 0fcbc9e1..b7732941 100644 --- a/rio/constants/robotHardware.yaml +++ b/rio/constants/robotHardware.yaml @@ -22,6 +22,20 @@ swerve: Pose: [1, -1] # Configured Never by Nobody # General constants all modules share + kDrivingP: 0 + kDrivingI: 0 + kDrivingD: 0 + kDrivingFF: 0 # Was configured in complexConstants.py, might need to do that again! + kDrivingMinOutput: -1 + kDrivingMaxOutput: 1 + + kTurningP: 2.65 + kTurningI: 0.012 + kTurningD: 1.5 + kTurningFF: 0 + kTurningMinOutput: -1 + kTurningMaxOutput: 1 + # These are important translation factors to convert native units to m/s, do not leave at 1! drivePosConversionFactor: 1 # Drive motor conversion factor (position) driveVelConversionFactor: 1 # Drive motor conversion factor (velocity) diff --git a/rio/constants/simple_pid.yaml b/rio/constants/simple_pid.yaml deleted file mode 100644 index c920a6a6..00000000 --- a/rio/constants/simple_pid.yaml +++ /dev/null @@ -1,19 +0,0 @@ -PID: - kTurningEncoderPositionPIDMinInput: 0 # Configured Never by Nobody - # kTurningEncoderPositionPIDMaxInput in complexConstants.py - - kDrivingP: 0 # Configured Never by Nobody - kDrivingI: 0 # Configured Never by Nobody - kDrivingD: 0 # Configured Never by Nobody - kDrivingFF: 0 # Was configured in complexConstants.py, might need to do that again! - - kDrivingMinOutput: -1 # Configured Never by Nobody - kDrivingMaxOutput: 1 # Configured Never by Nobody - - kTurningP: 2.65 # Configured Never by Nobody - kTurningI: 0.012 # Configured Never by Nobody - kTurningD: 1.5 # Configured Never by Nobody - kTurningFF: 0 # Configured Never by Nobody - - kTurningMinOutput: -1 # Configured Never by Nobody - kTurningMaxOutput: 1 # Configured Never by Nobody \ No newline at end of file From 7149972b22ee0aa7bb64dec88189ba70e3a78b3a Mon Sep 17 00:00:00 2001 From: kredcool Date: Thu, 18 Jan 2024 15:16:56 -0500 Subject: [PATCH 30/61] updating makefile --- rio/Makefile | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rio/Makefile b/rio/Makefile index 7119747c..4d9fa1be 100644 --- a/rio/Makefile +++ b/rio/Makefile @@ -37,10 +37,10 @@ test: ## Run automated tests python -m robotpy test download-python: ## Download python (for robot) - robotpy-installer download-python + python -m robotpy installer download-python install-python: ## Install python (for robot) - robotpy-installer install-python + python -m robotpy installer install-python clean: ## Clean the repo - git clean -fdX \ No newline at end of file + git clean -fdX From 25096b74a33067ed2e24160798dae91e40e4bd7f Mon Sep 17 00:00:00 2001 From: kredcool Date: Thu, 18 Jan 2024 18:55:51 -0500 Subject: [PATCH 31/61] this is dumb, CANSparkMaxLowLevel is deepricated now so CANSparkLowLevel. This messes with the Pipfile install so all packages should be installed manually. --- rio/.wpilib/wpilib_preferences.json | 1 + rio/Makefile | 4 +- rio/Pipfile | 7 +- rio/Pipfile.lock | 383 +++++++++++++++------------- rio/components/drivetrain.py | 1 - rio/components/mikeSwerveModule.py | 10 +- rio/robot_requirements.txt | 15 ++ rio/simgui-window.json | 10 +- rio/simgui.json | 3 +- 9 files changed, 247 insertions(+), 187 deletions(-) create mode 100644 rio/.wpilib/wpilib_preferences.json create mode 100644 rio/robot_requirements.txt diff --git a/rio/.wpilib/wpilib_preferences.json b/rio/.wpilib/wpilib_preferences.json new file mode 100644 index 00000000..5c004bc4 --- /dev/null +++ b/rio/.wpilib/wpilib_preferences.json @@ -0,0 +1 @@ +{"teamNumber": 1721} \ No newline at end of file diff --git a/rio/Makefile b/rio/Makefile index 4d9fa1be..91240178 100644 --- a/rio/Makefile +++ b/rio/Makefile @@ -23,11 +23,11 @@ push: ## Deploy to the robot (push only, no console) download: ## Download robot requirements locally - robotpy-installer download -r robot_requirements.txt + python -m robotpy installer download -r robot_requirements.txt install: ## Install requirements, run download first! - robotpy-installer install -r robot_requirements.txt + python -m robotpy installer install -r robot_requirements.txt info: ## Shortcut to get information about the code already on the bot diff --git a/rio/Pipfile b/rio/Pipfile index f41ec308..b165ab3b 100644 --- a/rio/Pipfile +++ b/rio/Pipfile @@ -4,10 +4,13 @@ verify_ssl = true name = "pypi" [packages] -robotpy = {extras = ["rev", "ctre", "commands2", "navx"]} wpilib = {extras = ["all"]} pyyaml = "6.0" black = "*" +robotpy = "2024.1.1.3" +robotpy-rev = "2024.2.0" +robotpy-ctre = "*" +robotpy-navx = "*" [dev-packages] black = "*" @@ -16,4 +19,4 @@ black = "*" python_version = "3.11" [pipenv] -allow_prereleases = true \ No newline at end of file +allow_prereleases = true diff --git a/rio/Pipfile.lock b/rio/Pipfile.lock index 5d9f7a10..d3db4d2b 100644 --- a/rio/Pipfile.lock +++ b/rio/Pipfile.lock @@ -1,7 +1,7 @@ { "_meta": { "hash": { - "sha256": "293af6cc7b8c4bebe8af1addf8af658e86c86729bf6846a84d8814d1d57b0fa0" + "sha256": "d585b89124e91d03d7f6977e1c5d71cca1dd8951467b0fe4592cc53f07b3b5ef" }, "pipfile-spec": 6, "requires": { @@ -480,6 +480,17 @@ "markers": "python_version >= '3.8'", "version": "==0.12.1" }, + "phoenix6": { + "hashes": [ + "sha256:48f79760706515157512d4596a2964c327235da53dd7b4cadbc50c4bd61b3b65", + "sha256:4aa88861786861aa647193052da0df0d8f26a91cabfa5e84d85fa71af0de649b", + "sha256:65b687a22b462e48db0e02224f43b641a9a8d4df7c7c06cdf60da097fce8be60", + "sha256:c3c75782066665be9d37d3641c061be4bf1460e838f482c8f5723e754080ba76", + "sha256:c70dce1831afd224e10705483d1e383817bd58ff604cf139d64ba7bb8f9f5df8" + ], + "markers": "python_version >= '3.7' and python_version < '4'", + "version": "==24.1.0" + }, "pint": { "hashes": [ "sha256:df79b6b5f1beb7ed0cd55d91a0766fc55f972f757a9364e844958c05e8eb66f9", @@ -513,11 +524,11 @@ }, "pyfrc": { "hashes": [ - "sha256:62438a558cb8b467c09836c92fd9d1f283523db1a1f757233c906dae85261886", - "sha256:6d174996fea8b7c74f6b5ef841da3d73397c8c74953ec1ae23a33983471413bc" + "sha256:011076dbc62606b08eec6eb1bb30497d085d34cb71537a7ce0d06f44c8492855", + "sha256:2386be296bfb7e482a26c25e85ee61495ea10154f920d7450fec575defb8cc05" ], "markers": "platform_machine != 'roborio' and platform_machine != 'armv7l' and platform_machine != 'aarch64'", - "version": "==2024.0.0b3" + "version": "==2024.0.1" }, "pynacl": { "hashes": [ @@ -545,32 +556,32 @@ }, "pyntcore": { "hashes": [ - "sha256:157a6c6afac6d0570d9fd44c2b73eb955316b5226f3c2233d0e0b8b64d2eea61", - "sha256:17b5aec1b58f4b9ea568302df6d909092dbcc6090ddb894bf3c47d5aa8029b53", - "sha256:2c1c16fe7b61a0d4f805dd0d2a2a6a2036240ffdac09de8b2e8dd482bf86284a", - "sha256:38f894cb13288da6ac2e828fcfca5d87d2284a6015a96d203cf5419f4d4a8d4c", - "sha256:734291d41879a85ac287b342110adb363ca4bbc90844dbe156d66b6997f46e2e", - "sha256:8ae32804ec565016819edaddc83dd94c1a1e30b2d78a6b3c17dee5da665f6773", - "sha256:8d61d179266c48086562a6f5e4adf176366fe98445e1c655562bfc1a15982867", - "sha256:9ab05d23171c5383c78904af34689f32490c47e5596b16753ec30f1f1d7e2573", - "sha256:a229860cff660ccff02c5e3de7b9a461de523950f7c0c75e3f705cc312888c12", - "sha256:aff310a14720904b1c0d57e1c7cf700d99c2add5859c485147da63ae184dec27", - "sha256:b7ead99b9743dd881ba907e02d85d890222de3f035573f96aa4dd1bdd7962c0c", - "sha256:b8a188ca9ed14264dd12cce17a425c9d75e47d65dbf20a174824ab2541f401c3", - "sha256:c0127914b4b31b88e477696a20214711c9450f46f44ca330977fbd9a598a34e3", - "sha256:cb797a0ecbfc8d572593a815aadc32cd23a28f2655fef29840e949c3899152c8", - "sha256:e76e02582c53868b162c9de8680b3799cf53ffb7bbe03b69ef6bd9053583f715" + "sha256:089c9721b1aeba6f31f211a9033af9600cabb9148996d1b019cbc03eb4bd3d0b", + "sha256:2fd033786a948b0d63bd6596e9cf5ad0386f472abefe7cc1271edec85bb4c4e8", + "sha256:3422f6b07d0c5025e8c87b37964f7a9bc512d3c8087c5a1f88eaffdbbd220781", + "sha256:3f6961b8576e8a8a80d8ef28fae1129c53c1e5a91b231a12a9328ab4c6d1f9b1", + "sha256:479b8ca8d3611f85ab2c5dcca39af299f40db3d7ba4efe9256842f067a4443df", + "sha256:57080b81dbca5e0b28715f19867b848aa5534e09088d5b66d76b64f7cee998ee", + "sha256:6463a5640e52ac0ddda10649babf10616c6278858d5122363d09ff6784dca99e", + "sha256:6f0f5a89ae8a70c3994853f2ba1c29a73f3f6acf8becce71c60c232158f9db3f", + "sha256:803a3ce467527928e22e57c99525a923b79ba716dde9c94d1807b11caba09603", + "sha256:9d4afd35d0d38c30f3ad1de5e9d1bdf53d64e560fe3791536d7ea1b36f58ebbb", + "sha256:a30e97a6971fadaceacb7e357670d22af55d810d081a8b7cf808db7c62425096", + "sha256:a368189aa878be535ba2dd8c6e21173a45e2ccf446b6ef5904da4d07b2ec3acc", + "sha256:c6bd974f2e0eaa3f9f9705fe641793bc759268d75cb9cc424e20a97d587971e1", + "sha256:e68a04a9723d8c2300c1057a69cab1544ac60133b2506c45ecb16838f563d6a0", + "sha256:e93aa0c9951722e0b69288c2792f2bc2a7a65d02b26f8e8230de1edc371bf250" ], "markers": "python_version >= '3.8'", - "version": "==2024.0.0b4.post1" + "version": "==2024.1.1.1" }, "pytest": { "hashes": [ - "sha256:6c30d4c4409c5d227ef936678b72c56b6fbaed28a6ee4eafd2c93ed9a24c65af", - "sha256:b5baeee6fb27cbca444fc1bab2ee7e1934f93daa50a4d475a6d0f819c263e573" + "sha256:42ed2f917ded90ceb752dbe2ecb48c436c2a70d38bc16018c2d11da6426a18b6", + "sha256:efc82dc5e6f2f41ae5acb9eabdf2ced192f336664c436b24a7db2c6aaafe4efd" ], "markers": "python_version >= '3.8'", - "version": "==8.0.0rc1" + "version": "==8.0.0rc2" }, "pytest-reraise": { "hashes": [ @@ -611,6 +622,7 @@ "sha256:8d4e9c88387b0f5c7d5f281e55304de64cf7f9c0021a3525bd3b1c542da3b0e4", "sha256:9046c58c4395dff28dd494285c82ba00b546adfc7ef001486fbf0324bc174fba", "sha256:9eb6caa9a297fc2c2fb8862bc5370d0303ddba53ba97e71f08023b6cd73d16a8", + "sha256:a08c6f0fe150303c1c6b71ebcd7213c2858041a7e01975da3a99aed1e7a378ef", "sha256:a0cd17c15d3bb3fa06978b4e8958dcdc6e0174ccea823003a106c7d4d7899ac5", "sha256:afd7e57eddb1a54f0f1a974bc4391af8bcce0b444685d936840f125cf046d5bd", "sha256:b1275ad35a5d18c62a7220633c913e1b42d44b46ee12554e5fd39c70a243d6a3", @@ -637,18 +649,12 @@ "version": "==6.0.1" }, "robotpy": { - "extras": [ - "commands2", - "ctre", - "navx", - "rev" - ], "hashes": [ - "sha256:8b7b36a949ea7f3d0504b23654040211df6f29bd04846043b464671d9c4025e5", - "sha256:b3d5c8420bbb9953af996f82f8cb28b78e02c3815f019ded094589f9854c0074" + "sha256:a9f9f7de9aac362ec7f027194d11644ad555f4da1951c9ebdf9c6ba8ec15cb36", + "sha256:cc9a1dfa216a8208efa848683ce0df45c98f7905c3450f8d61ea32fed4adab9b" ], "index": "pypi", - "version": "==2024.0.0b4.post1" + "version": "==2024.1.1.3" }, "robotpy-cli": { "hashes": [ @@ -660,173 +666,203 @@ }, "robotpy-commands-v2": { "hashes": [ - "sha256:3b3a8b11c9f6509eeede2b37872a302228b335b3829f3436fb036323c5eb5dcd", - "sha256:dc9b49ebe42564ecace1186ee0c9f31bf7d1ad7f0fb909bf9188112d31d1e609" + "sha256:0161feea5288b6af6fe3eddc412ad30c920acb929bcb5fea6899ba47e2e5763b", + "sha256:e0f808645f3f01c1a49d54cbdca6a2f528c2797913bb6382bec067c69b646065" + ], + "index": "pypi", + "version": "==2024.0.0b4" + }, + "robotpy-ctre": { + "hashes": [ + "sha256:056e978d2d58aa842c69472063e21e89b0615dc6df70438ee86b4b3ea8825293", + "sha256:5d3ad9ec4ea8a7dba71979b496e9363994843e93ac4913bf80b98d9719db8d78", + "sha256:72f4c860f4e2402173a8abcdd65a6838fdd587cd2022d184fa8fd6d9645cae81", + "sha256:7e084cd2e7fc76269c3a1d4bfb671eb6c21de535698252acc879f429fa0b0ab6", + "sha256:8a7bdeb2fcebf45abc3f50b9be22b0aeef70122eda1732e37e495a530bfa87aa", + "sha256:8d44245e9159e8ccc20c239130fa15feef83ceb674a4fd0e435c91809d9ebfc4", + "sha256:92bb052508a1d65bdad5274a9bc9dc48ab001b2b65345b6e78e5689969d10678", + "sha256:ab8aac6f88b6bbfe83d42c91e52bd33d99bbb46e975c3f2a943029efcd93c9f9", + "sha256:ac3c76f65f11b4826f62815080bc7c14eaccf989790057a05980a4cca4c3d84d", + "sha256:b24aeb7f447ac7945d346247fe36db4d5cfafc0d6ec0cf294d30fd37f6bf1893", + "sha256:bd65c83ad620fc6ced2dd0111a86548ef0a0709248d40d9ca3eb524f0b366f90", + "sha256:c27f4f300df0ce2ed79bd65dff32da3b7712053572893add24a6333b89d87414", + "sha256:ec33c5a2777b15a29880a98e31de5c28f7b264afe3fccaaf983dd3853d632de6" ], - "version": "==2024.0.0b3" + "index": "pypi", + "version": "==2024.1.1" }, "robotpy-hal": { "hashes": [ - "sha256:08110bc551813c3ca633a5c9a1e7a9ad3d98f454d576e6dd0a00fec88f8b1ae1", - "sha256:1a729520e733bfa625f041c2ba54298438f1847753e27c54b9931a89a82b261c", - "sha256:1ed55be20d43afa14ee5a6904a4a2a93108933c5dcb2a497d988eb54d643d5d5", - "sha256:206204f668ac4c54d585973884719b44af356d53f0161eedde1046fe6a3672ce", - "sha256:349c30dd4ad4dc5b66436db4a0e222aaaa5afb66927fbd0d687f3fe035bcb388", - "sha256:64f8374df6ee7022e7833c64b3604eec14975dc21ff38b2fdde8734f671c4316", - "sha256:73aa5066286175d2a72232a62631ddc76608211d9b0bd092141b38a245af5a8a", - "sha256:afe42369b34a5e3d847bec0b5465327acca87506ff274d14acdd004fef71955a", - "sha256:b282855a01e19e178e51e056fb6a52066401484722bedca9d3ca954aa9adfa1a", - "sha256:b6428e16eb63eae15702724fc5c39f1da5a9c2d14abaaba9b13326ce1fdd4014", - "sha256:b91210bce47404728566f8f5141cb33fee8784eb637c16152025130584b93dce", - "sha256:c2acbfc3c401a758a4bf0d0e99353159a5ee1cec327489a4a05704b310499d80", - "sha256:e39f2f374c9873f2c268f2f4312c0f856ca56b1d8e1709d8cd689ffd42e85619", - "sha256:ec5ed002345cbc4760f2970b11fbdeeb867692546c83e1606397b48f38361528", - "sha256:f7e751ee9564a690e51c522fec3a173c1cc69bf9aed1f5fed7bcd264d818a4bb" + "sha256:0cd34df381940f89626e1e2b206d2def4879c1cf66b617745447ab5323c24141", + "sha256:164ce03dda139902ecc2a1e7c1d18600fbd0f5f33386e9eff1309ec32c217cd5", + "sha256:19e445a2bd24e178e5a6ed583a21f8622da1ec214c0953c2cb028ff67b973b28", + "sha256:2254a91d6b3c1a73a4c786338e677eff7d50620eb6b24f831945a53901e98501", + "sha256:281f6bea3793106691568139df7c7722dcc40acbee95076c54070e4fe471aeac", + "sha256:547452b6a2c89ed1ca74eb4523da183bb5c0c0b7281823ce197fe99a738dee29", + "sha256:79d0810b4aee3769ee573e9d6a74f448940efea8ffaf70d86840db40122ce652", + "sha256:7b884b7d756034a42702fdde8e77e8a8b53c379872a47673a8b1b6b898d14352", + "sha256:93626c3a9fbb2ffa9a9dde9680675c845c173811a8b73bbc4790b50cf0d307ad", + "sha256:b5d3a20a1fb67eaf2bb6efc1303f3ed1db8bdea08a9fde284ee63681fffc7aa8", + "sha256:b9939b7076268c6f54f633879005685089a68d60b2df0fa52830b20c58af02cf", + "sha256:b9945c69fb063f49db01ae4b335a92689d42cc1068efadff37403390a19169f9", + "sha256:d201bbd0529701f150806abdf1c77c5203ee3fc82656fb7c9863d43b9ac3a364", + "sha256:df21128092e932490923b49f6aa9741dbdb3a225805f60900b13d1ededeee38e", + "sha256:fb8e58b726ed94c0a5f636f45b65580e5eefdeeb4c2cd2c898ba9ca0de7b1eec" ], "markers": "python_version >= '3.8'", - "version": "==2024.0.0b4.post1" + "version": "==2024.1.1.1" }, "robotpy-halsim-gui": { "hashes": [ - "sha256:1ae8e3b875573ba37da21a4b1fb1b09a320d3ed1f10c2c540a10b9fdf9852d57", - "sha256:564971d971fb2b67a380c0a9a004812c375a8f015da2226851ba91869766bbf1", - "sha256:618960cf8d7b33107dbd80efe4c94b9054513f022714ba32007a90691389fa72", - "sha256:6953554fc117b4b2791b7fcfc7baa679ff72675e01c55c0ba4246dcf80addf48", - "sha256:6c2a18faa35f64a3c207f148b665be044e502f54b1ebe53e12dbf2750de2258d", - "sha256:808f5f2950ea5df3a2c40f438b9f6b15b39f1bd28faef90f2216d641d4d05051", - "sha256:84a1c61596206932bae525706ed3736633f356c44bcb36edca456ebc462c1138", - "sha256:958022cf2387d89dfd7f079420c9f4e99585542935ceeb643b8abd6dd391edb1", - "sha256:a31af9cf30be74f6266f3ef2028602a318cacdd9077e6e486b2e29ae5454cc73", - "sha256:adc7df56c7a52597aec5fcff7314f5ec27bac8c9bf5c2132a4e8eba59b889894", - "sha256:bb0cde93f1722287448f687029dd0271d84e6af56b7e24a5c2c2d711946aacbd", - "sha256:c07b55cb82472ccea87c52dd870b965d4c178a0b1e5cdb1ffe169f361f37e9fc", - "sha256:d537bd5434b7aeaa615347615854c85d85327bbca0a18b341bb4288a91e5e3fe", - "sha256:db07331200ff631fd448432d196f83e2160a0378693ea2713381393f766d2956", - "sha256:dc01c9144589c5eae149102e94bed0af67d73765fa21866f7d29852e12306e82" + "sha256:14a3084210ded08210328607b788a0202fc14ffe2cf25a1de402045ccf331c9e", + "sha256:1a396f5bb485b0ee2ed378b037fce589eba475525347cc5787efb72c41d2ad0e", + "sha256:37b769bf1b8de7aa6d0d5c9e0f15f6c17694427778d85430ddfa4326464c84d5", + "sha256:3f7faa62735b4b3f2ef8e455929dfb02749269d66924cbe9ec8d673d82baa534", + "sha256:5e0eb35d0da4879a020291231f2298b2d37c0593948f8be286c6921b2b9c975f", + "sha256:6b48f2e3af194fc3184f551ebe9127c119563cd493272d8a917818d836b2f7dc", + "sha256:814473b3714be8f306a9a111b09d8574da36f6b62ffa9a356ec76310d822e3d9", + "sha256:830d9669e31b3270654d9b8f2326cd587a1666dc174394da3b612165949accc7", + "sha256:a351177fecd20bf27ae38305c85388b8cd5b832965725855d3bd27b668ed5016", + "sha256:a7af21ddb6ebdbea381c1de56e4c82fefa78738ef81cff17424cfcd32e37b48a", + "sha256:bce89ffd57caab38b77e90b406d9ca6e02f9f6c05c48a10e451f2648184690a9", + "sha256:bf0ed83c70ecda0fd0bd02e99e296a7563c919fe7850d1cd6c32521020f43b13", + "sha256:cc6353ad02775be38ef9ff996462c44292505b422a5279da78397370138f28bd", + "sha256:da93b6d396a6eab81ee81adaf8ea0cd69cc579c510c1e3a5663615b98393f1b9", + "sha256:f3ea7483b2dbd4c09fe7c9cfca3887797f979688ef4e21eca8eb655d3ab6b29a" ], "markers": "platform_machine != 'roborio' and platform_machine != 'armv7l' and platform_machine != 'aarch64'", - "version": "==2024.0.0b4.post1" + "version": "==2024.1.1.1" }, "robotpy-installer": { "hashes": [ - "sha256:07f9d1342db914a9eed17d42fda5b082381c5736143ef5c0f46ce160934bbce3", - "sha256:38eb26dbbe32076f5cf95b995190678f041048353c0dc8c8e6a1a70c6a23f226" + "sha256:05f0ef18694d8c49a424c020d359fdb799e11e53785e8bbd60ebe1bfa0ddc1d4", + "sha256:3d6643435ad3cbf7d72bf696c6c41fe3e1d61781bcf541392efdfe42591dfc4d" ], "markers": "platform_machine != 'roborio' and platform_machine != 'armv7l' and platform_machine != 'aarch64'", - "version": "==2024.0.5" + "version": "==2024.1.1" }, "robotpy-navx": { "hashes": [ - "sha256:0469270c0993dc77538febcb3731e322ba5a521fa6a2793fc5f3dab1e51e641e", - "sha256:110daa49b0040de731a782b57fe9859c2aee5f017b2afa9cb8d4a0a8a2353c1a", - "sha256:16273616bf6d377576977c309c74a8d81fd9dd0b98cd08ad24e402734e892803", - "sha256:16c8c87989f3b181732a6d6e7de9337741b24154368ad78ac0988fbfbf57a96b", - "sha256:1c3aca092d5db8a70b92686eebebf9ae51ca198f8a008d74dd3399326c03c6ef", - "sha256:42d0c8c112be90fc7d22a64c1d8e05403db74a9f361de7c2ddbfecc82cb414b3", - "sha256:5471062a04e3b308e68deb251d65306f49dda5811a709b451930fce2e8025be3", - "sha256:5a26764370cb166525e254dd3ca7380159fddc9d66e9af8218ad08a9b14d7e28", - "sha256:692b2bd39267f30b7a34ad40f953c7dc88b672279461a047801c51eadd1aae24", - "sha256:7c5a47b0eaa77ef40c756e5ccf5d59a2508eea477b7fc2e6726d2019acd15b87", - "sha256:8df9d51e5f53c4d757c0cbdbcdfeebb748abcfba25521a1ed1f47d3d4185f9ed", - "sha256:99340dca9323d95ee85f0a15a2eb352a813e219779bc3b163c867f354d278c53", - "sha256:9d3bb477d7de09afeb1f897563bdb7fd4ee0cf3598fafeccedc58c673b4d4b16", - "sha256:af4816150085738dd1b18f556842a2941a024f3907b485776449f0751e0e8e90", - "sha256:d2d3e2855aa163d395cb30edb8ec402916af6034cc4bae8d8c9c7562092103b9", - "sha256:ed44e2ad59890522e5fd38ad1a724ee0f7b2a3a802b1ab583ff25737b700fb84" + "sha256:00282c61864bfcbd104cbe2ee10b0bcfb517adb27586f43cc89a631c9d471189", + "sha256:004b6b3a88cafda02b7258203687d844c91f921d2c204dd8838390f17a5febed", + "sha256:0c3e382fe21196d3eeaa906d9e82b18711af7690d159cb7af999b8a733d39ebb", + "sha256:1e24ba17aa30bd926569cd3def6ae1c96b64ba1a02a7c87a42c51b16d82a613a", + "sha256:383e7cdc303934932fabfe33a7fae536d8dbc133d7135a08fc26f821592cdbfd", + "sha256:44bbf7bbfaa26ecf59a8a361842f5e89bb7ab44116e4b6a82675d98813c7e045", + "sha256:5c7e5edf7c51d9e9280215cd182a3c71d65fafdf1a1f9e2ef6c98cbd2bb0ff9a", + "sha256:5e5b7389dac357825c76fe69e09475d00477c2a1de2b6b6b7ef00f4461e6cf06", + "sha256:6e2fc4c871a20d305fb9f6ba5cb5cb424f9a31b78327b77c3f7e28c2437f84d8", + "sha256:816d52d172ff1bfc87f10a91fbbeb36f873f6e02a1c16fb3befdcad73cf266f2", + "sha256:82598483738393ad7e94fb3ad73628c2ee842329ef526e1b52a05b7945ca10f4", + "sha256:a56f6c71678b23cd3f9ce376d7040243b921c01dda8d0de8a828b4a2db45d6e2", + "sha256:a6757655b2aaa1d8028068f22b4c0fbddcb38d3189f9efcaa6f97204d70d2632", + "sha256:ba22ed1be09525ebb35e9a5ee1a4eca7953848a8b0fec167398e167db9490025", + "sha256:d896a66171b18f47d7d47c4516a00c47da77cc7e68f91c182dfa38adb61fdc64", + "sha256:ebc481d6d8b3921d7fb58a9d7820fd7ccc94d1aac2fe189ac35dfcc38c6bfa91" ], - "version": "==2024.0.0b4" + "index": "pypi", + "version": "==2024.1.0" }, "robotpy-rev": { "hashes": [ - "sha256:0b6374e89d7da292af112b0580205f420250374f635ab3b2468033f8d6c155a6", - "sha256:0c19c45075ce199d4480d390dbfdb3dd399af250cc7abcc3815c83ce657944af", - "sha256:0e9b8a267909746fde21002736564eeeee5ee5259b7d1d1db4db4e9c4225fd82", - "sha256:0fa19d2a919ccda8115f395c7d63f1eff184ef268397bf4123b8eeb90b0ba9a6", - "sha256:151b16d2f818a20875a76141cb45c19ef8023e2c14b814a6ce6aaf771dac44dd", - "sha256:1e9bf067572df49bc31a3891e942b57fb5b9a721c97795d057f6adaeb1088345", - "sha256:30ffcd5b58726dc95b974299a20907f593d807cda75bf03c9d51efa1dfbff37c", - "sha256:3a020faf21f3c58e0693c8f2149496d548d52247939878395bc16f106eae437f", - "sha256:5158cf70a18a715e2713b036a3593504b03cd195d36cff08393760e754f56e1e", - "sha256:561e5438d4f93e388729fa733d68ef78750079455cb3dbbba7e33751ec90cf5b", - "sha256:56866ce655966ca4caf05845cf97db2fc8609409a43436631791cc1f5efeb9e8", - "sha256:941dd2a6081f9eae5feaff82144ae5b441634040fe99b414a6295d6300d7cd62", - "sha256:9db89303a5b0fb64414bf19fb664972c6396c344768ba81e3294578657812626", - "sha256:c7b542ff2f7918b628ab7a23542ca915261553b0d94d8e942be6d16c07b4ba7a", - "sha256:daf5cbc0281a53a662f5e12ac89947cf9aff4f1612661450f0467b930c4f3f9a", - "sha256:dbd208af3037ee58628a40f98ff265a8c9c83617e354bbfc942d947a30c33660" + "sha256:15cc9e8e7bccbea8f508c53f8fe40dafb539b1b07b54f45f13445b370fcb73a9", + "sha256:1a6126ae57485eaa3f57657e81e021e8f762388a167ddf9bca520a527bf880cd", + "sha256:331bb9e17b18d844a940a9b1944fd335f739d9885c51de3ed2895c34c2200c1d", + "sha256:3df12e1c5d9df3a4492abfc8d192006182d4356b687708edf214e1b683911017", + "sha256:48634ff5bb8cf0fc9233fa8f69fb2387d08d59758f9536d762bb8e7374351834", + "sha256:4f981f40cb848cbffa1d678fdaa8d05ebd1c727bc0572142626714c79ba6bcd9", + "sha256:59ae89a3450444fbca48beeb9abc1cadd39452f1fb9a059f06d41c7bb8b24af6", + "sha256:644592718825d8029a0b506986cdcbaa8abfeff59f750305c0ca91849a6a38eb", + "sha256:8673fbc3ceb5f69674560f1877128429d3365bf1e61594b70d593322ca455960", + "sha256:a62b32f743ef9e27c54443819544e31cdaaccbf24db07c4766d055958864b409", + "sha256:bcb2679eaeb73a90e83694192e64fa8c3a87856ec42edfc16ffa0f718fb2400f", + "sha256:c4698be1c7df04ebb7d37cf80c94d6e4c626ab0a55d64c66556085b0b4bf994a", + "sha256:d021d19d2116279aa912edd93b6864fc36c624aa138f0a54a628df210ff97a0f", + "sha256:d5bff680765dadb2685a29640804259bfa3794a7b918d9242b6c27895e6e8417", + "sha256:dfcd8c4497e09cb83d9370ddb98c5ae9683b3a9d90b8e5de126a490061244ceb", + "sha256:ffd8748e1425f674c1fb0fe51620f7da9f40c11a7643f9c03139747e7237aea0" ], - "version": "==2024.0.0b1.post1" + "index": "pypi", + "version": "==2024.2.0" }, "robotpy-wpilib-utilities": { "hashes": [ - "sha256:2245f6ddcfe637151938cf55b6ac1b5617c60b683834aaaf4627e1e721261243", - "sha256:9b7934154765c7acc34937be09aca2216b6aa88f086fe1ca8d2e8250fe4be70e" + "sha256:da0d3495d28b8f758c0bc12f1075996273aae831c5dd9d85d6b0581f8f08bcaa", + "sha256:f2e7e512e3e9ad938893175b22c827f97d0866ade47f34c25d68622c3f8a4c3a" ], - "markers": "python_version >= '3.7'", - "version": "==2024.0.0b1" + "markers": "python_version >= '3.8'", + "version": "==2024.0.0" }, "robotpy-wpimath": { "hashes": [ - "sha256:16a929e2c65e69e5005aaac8b4a2b8e261af3210b05742921527b5eafb77fef0", - "sha256:1cdaf001ecd043e46c7a33e6f733e4bfbda6d3a6a38365fd8b060dda43df7dee", - "sha256:2e75b63692cfc9d4c35a2394ed10339867092474ab90ba6d6f6961633c8f424d", - "sha256:5099ea19f0de6a62386791d537f668baedd655e81002b13798eb41ee7e2f52db", - "sha256:54d8c724f1036588751d78c645a3a3c7350149901c45c3525daf2f25eb0431d3", - "sha256:5538834eeaa57203122be6ed493b00f28d264f313eedca8e9123b05487fc8ab5", - "sha256:61f6d69b968b3e449bcee51d2107b62c907592f7d9f2f857822e97d57e25366c", - "sha256:6375ead959905c8fe000d8f435152cbea590f7cdee3a787dacd84b18d02ccd19", - "sha256:7024df4a528d82011d5cdd00a6e899a1d8216bcf18aa05dbd0cb5ca459af0d9c", - "sha256:76a77b8c9971b1d2078fafdd4572fa1daabc053b9d2324b21bd5a92856cb8b2d", - "sha256:8740ea96fe07bd9ddc64f125898cf5b02f853c666964460f9b72aff7bbd110ad", - "sha256:89a81a2f12a3708d7344017ed888953b53158faf5b46ca2ec65d9c8515cbf1c8", - "sha256:9af91189df840f90220c6418852cf0f9334b1ed78efc5d4556a8b8b5b6d58f2f", - "sha256:f04164d04da8a3bce4ccc8dd203abd0f5e30d381e70467d1366203ee7db466ed", - "sha256:fa949dce76b7a4ff41918099db6a4615bc2690a2aa1ad1f7e69b631cdc8c9e82" + "sha256:35eb656efcd6cf1d50b8a0ebe5b802c5f0581b6d20df934f8dd1fcb77d120192", + "sha256:4340882ab0f5c46487c4d5d9862430b58abc1762ef6d96753d9635c895987c6c", + "sha256:59466913060a697fee4cd1afc63b5b72397281f9e0019f3a8cc6a1a6dbfa252e", + "sha256:649ce400c5f3b45deaf73dbd8223be71fcb44f61d41d665bbf21ad947a7d5df3", + "sha256:7167ea5181c815fe10f3e46df495ffc08ae2f90dbf399d455fe3c72643602394", + "sha256:7b27bf280c9ae55e874b2e46f9174a3b07896ee9e4d5b90c4e16dab5173ad72e", + "sha256:81e850df98fd62e8497a49b2bceec4014b9ed3cea155642fa4e034f77d870e18", + "sha256:951ed4f556f9d2bc2addf70be331789742420147aca23e008ec5830488ec97e4", + "sha256:9d570d4dbe27bbee560a14a212dc36d7149c3714c0e9ff77528615817c295a08", + "sha256:bba421856198139d5c2384a66e04a18d47562ba1c7aaa78e4374a586bbafe427", + "sha256:cdbe1585543d5f081c6a86f199d26b2355cb0713c3ff115e065f3697c064611d", + "sha256:e3aef1a9c03d76236ed6049efdb28d4d021921d7a2dac2df7d32fb0137834704", + "sha256:f987609fec0adbf253e47388812f61969c2ffcec1958f63a097bb5a3515c1e24", + "sha256:fd6657f4a41b048f09791c1a2e556f7df12dc6e94631ae84deb1496ed246ca99", + "sha256:fd7f3e247e248c2442ed8dab5b7b807a702576c3eab9d6725796af172d4102fc" ], "markers": "python_version >= '3.8'", - "version": "==2024.0.0b4.post1" + "version": "==2024.1.1.1" }, "robotpy-wpinet": { "hashes": [ - "sha256:01ae24a095ec1ed28cc37e2f539f230dc4f3ee3794acc8a2a82d57140d1791ef", - "sha256:0da284c024681a6d3a94012ecb2f3a5861a970a74410c0a933fc70069c029a76", - "sha256:1b1692e1e32e6568685df0ae02a5774b36dfedf22c6379d29c040473f023fd68", - "sha256:1fb553501aa084c6b141d38524d1b0b38c92212006c39f53a25ddbbce68cba50", - "sha256:2e9f1dc938cd993756b7498bcbb61b8fb197f61bf1fe67e3339935f512d9c17b", - "sha256:44c2e8b4a216a5b1b3ea729b51b251e02a7e0b5c11fb6a7dea0bd5a305c1e348", - "sha256:5a01ef456f13f0bf088e1a97917fc0f8fa15562d118ef3f9681cc31d197e2807", - "sha256:5bae69d897b5697b102c33295a601caba1886af06b2394224fe69f1ec4e423bf", - "sha256:6469928546dce75177d320b093e3fd45658b503ccd4833e2260d67d01aceeb60", - "sha256:99908fdae80d524f2e1651615821215ea612824bafa340162587ea27c29d7c01", - "sha256:9c6bc1717533a84a50ef33ab50db99ccf0f53cea358af635ad1f33c985217944", - "sha256:a36403023c2e2530c9913b85e6bf0d90a9f94afdb5e10b2e67361ae115982800", - "sha256:c0497817c4aa0aa12e0e207855fc06b38cbffa57b8f85d95374c35a009d31b80", - "sha256:df6bb721de86c9c1ba75ba892adb7f6ac31211e4da937178a81e7219d2f91954", - "sha256:e8b1394b4258352ef1e73adc2761d402bf88d553cca16098d1cb323e9f9f8b84" + "sha256:04084d2289de0b1982ec71f9690f20bb62ac0606ba13bafaf087397979f5b168", + "sha256:120e2a371f9a53a3571ebf3e3e933e95c451de80f61e7e14f8c3b69021d101cd", + "sha256:1faf0a5cb40b27538ff20ba1e5a098490f4106a3704c89403a4851a844c25a1e", + "sha256:2816bf1c4dd18412b63c9526e413852f784d680e7b1def0eb9426496ee543bc8", + "sha256:2caf345812138b0b76695f7c858c3ba364403f4de08a9bb21074d1724ad9f326", + "sha256:3a07601b53193179310e8d433d1100ff554f62e87ffd62daf8c9470847b0cce8", + "sha256:52d0fbb187120416846261d3a1150282a33542a672495fdd5189d0f0d7f211a4", + "sha256:63ee412582a42a40cba5c851a3723d84a69f9a8787e66446eea6c2426ea2859e", + "sha256:6a58178cab524f6943027dd93a8e2ace92b2a23cdefac596297aabf15830f28e", + "sha256:79cad99f417c0c3d08d40755fe1d303a7841726befe878bc7f5221b2c8b0271b", + "sha256:8729c00e2b6e8993e3bb7ea9f3d0f70b7028dd91c172b3d8e8e62a5cd04627c5", + "sha256:8ed88d6aa152be5899ff5ac441aebf53dffb529cda552e82031687d1bb13e4a7", + "sha256:94bf23400e7891f9ee4665570c1f5fc638bf8c89049d508af2b99a31c81d2c49", + "sha256:a9dc643a8a00055cbc7d6998ae02fbfa92da81498da4a105ee2edb76d7ef8319", + "sha256:f3ce06ddcce3f458cabb126bffbb78242d541c0500c1b703f0afd212ae64e7cf" ], "markers": "python_version >= '3.8'", - "version": "==2024.0.0b4.post1" + "version": "==2024.1.1.1" }, "robotpy-wpiutil": { "hashes": [ - "sha256:0399d34dfed60be04ebee1fa3eac1e648b1d5474dcaaa16ba1051c086b3229d2", - "sha256:04fe774d0e22a30080cd5616c6f3b146ff51c13f5bc429b5df3f42ea6e135a5c", - "sha256:1491121b000fbc8736be8a32862b30e393c05a0909e574236a7f911c9dc8ae96", - "sha256:1f037175da00dce6442381377160131c2935a75b2bf710cfdbca6a4f739b9d7e", - "sha256:23dc3107d761808d274a0e342fa7b96e4be5c94b97465de5751ac0121d6ba532", - "sha256:3f110b3c0444a6c867452272718916769e818f3acb9ff62976783fd671b7e915", - "sha256:483131793247d6fe9eda9a2fcc72cd273fa9aa18738a3becad2d3bd757a975fb", - "sha256:602f6bb8e181da649382f77b6e625249daa6f66efb21ce463ed612c8e81a3ab4", - "sha256:76f96b4b0f0feb0f72fcf9314220c9edcf362e4c3da94b15cf21bc27d5b406e3", - "sha256:87e7dfd483943d8c454e874a0fc0630c517f078c7c918630847e1cc3a3f09507", - "sha256:aeb00762e5350587ee1bad2e35e4a23c5685f4b821a19e50f4ff5ea9664175b3", - "sha256:c236ee7ae362d488d193cc443ccfe5956534ce98e72564bbde55012972a821fe", - "sha256:db5dbe0fba1ad09aa42575dab59d334f2637a60ee4f3035303bd9243cfca47fa", - "sha256:e998f8fe7b502fb06cb2000fb485cecb863469af2b3cf05a4766cf040bc79c75", - "sha256:f4b7a448739c8d8045a297406c402da65033a14b4cc79fef2d1cca938d1802cf" + "sha256:0084575cd93088ab71ee3140506ef497ae3292cab4c786dbf3f8f7b19333b6ee", + "sha256:0b552be4e0d5f411237e70a4c9fed959906917f9e5681e5db8b06b17ac857a56", + "sha256:0d8579546617a5e2967b1e3325a6778ca6e56d87585b09f766258077722fcc47", + "sha256:1aa7bc8abd9635517119ccd6a322e28a5da5ab92bf1c66c0b90ffbb865549710", + "sha256:3008cb3a9e94ae9a0b2c15aacd4e18886a621723a196a20a459d7419ba504ed3", + "sha256:3c72957302d7caa65f2a71274889e481a8b3490bd7bd425cef0340defe09b179", + "sha256:5096c16e90eef7bec892e3ad7ea648110c4d525e0c79d1aa07d9d380a0c0c66c", + "sha256:5f62c29d06f9adf5159c82b2b7bdab98d2d247f6cc84df0aebee6570c916f735", + "sha256:90d600535afbd61892a056e8c292b4de9fa4cd593b0d0a7e1b3585d5801f43d1", + "sha256:ab5f7e193f1f044feb74dea87867811132ce71b19d1cb55b06b03db7fdec2489", + "sha256:ae84c668de63383d633a56d8c089b76ea3e2835f36ab49374ed498c118fd5c47", + "sha256:b1aead0364f3069f82c2eb5bbb860906a8a2be89b520afa1da9d7ff58f04b0cf", + "sha256:f3ee870245ee3cec80f1b74a6f674e93b574d156924c83c239970a07a3ab6dec", + "sha256:fd98ab7f66447387cb329ed8451736c616dbf4c4d31e9ad746853ea9046ca506", + "sha256:ff4f4be6e5d9d6ac9a0bcc1e72fa7756810e986939bfa1ad2522b597dab7f85c" ], "markers": "python_version >= '3.8'", - "version": "==2024.0.0b4.post1" + "version": "==2024.1.1.1" + }, + "setuptools": { + "hashes": [ + "sha256:385eb4edd9c9d5c17540511303e39a147ce2fc04bc55289c322b9e5904fe2c05", + "sha256:be1af57fc409f93647f2e8e4573a142ed38724b8cdd389706a867bb4efcf1e78" + ], + "markers": "python_version >= '3.8'", + "version": "==69.0.3" }, "tomli": { "hashes": [ @@ -845,25 +881,28 @@ "version": "==4.9.0" }, "wpilib": { + "extras": [ + "all" + ], "hashes": [ - "sha256:21f4c8b5d295ead0c4b52a82fcc42077ebc201438eee0cb8fa713831526711af", - "sha256:3c36adff768e246220bccf56db8bdcac4f635f9f0e63b911c41084d6787eae3d", - "sha256:3f374998ac9175ac515b36387c909a53c95d54cd279124e955e7c1ac2d3535f3", - "sha256:4976ca4fba98e28e091c1692dd6a5882ff4d7b6d31433832398a84f66b38aa3d", - "sha256:5d1e837e8fe506b5741915396fc171808ce9444cd9ed01001bfb7c0781a38e50", - "sha256:7e21e0af9079ffc8af388dc44bea6a02c740601f21b0fbc5a50e9c57e2e7b1a8", - "sha256:8e03e37df98c5a40c08bea4f8b67da78428256c5d739c8385da7a006026b5623", - "sha256:9dd896250c8a1e88b9da48fd5a881f9cac9f1bc684f0f9a32c5e883c2e156447", - "sha256:c126145c3b3c42fb72c54835a78c7078324ee72ab53c2a4cb188763731bf7c7e", - "sha256:d0f090f087b817aa518ce6b5209f3c9159c9cd7394c2d6dff3132fa7e6cb26df", - "sha256:d35dd7c89529c2b1badee4e017aeba20eb294c87878b5c2be89365df330a7fa5", - "sha256:e8ef1bce16b2601daf123fc6226ee915c31952408442f976243255a98cf62f80", - "sha256:ec674d6e0bfe972aa84dde896c9fa3168d1d18c77d9091060b4f9d2c1f601111", - "sha256:f1a04fc08157b81e6115824790f508a8f32f4d8e4f50c89a54d9346ca9af0eb1", - "sha256:f381dc85c1c7787e89730059a4e33172268d7efa699f2e8f9b0ca72733b38303" + "sha256:023f1b6f404a5ab3a5018bf59b681215c8d1455ab114bbe1c70ad6648fd36750", + "sha256:244745f00b9fc6db7b66b2992a1bfc9c7da5c6951bb85aeda0f277de6589eca6", + "sha256:4e277c109c14550dff64f6e3e19202a0517de6776e59484447ca50539c35830b", + "sha256:6b296068ba2901bd4ec8bc0bcee499ef54fd2e18d68a927b174445397d1a9e65", + "sha256:6f04c81531e67a3f06e7a50198a21657095f0fc5bdd823cd2ada4931317b2e41", + "sha256:82a2cee6ba2ff21b545388c3752ccbf5eafb60c73188d30a2161e2fce4ec2f7f", + "sha256:9edad7cd2483e514864c0fa4128abf9aeb929d2888c95fbc37a00579daca98c9", + "sha256:a5ef39347c780cc50d7ef0f079377541298070174654446272b3e392824739ab", + "sha256:b9bbd811fc38689e20714bd584bf9af13f1a1bc885ba0de282ff0ef100ebd528", + "sha256:c2816c5fb4febaeb4c7055e5528780103b0ea705084e7d99c5e219d9873a96fb", + "sha256:c8172b7ccd416a0335aaf44cc54d2ee6d92a404f880026d9934445977c74e9d0", + "sha256:c825d146270ef9c2ceaa9abfde2ea5f27270149cf97c1ccf07f3dab9ab659077", + "sha256:d4120bc2482b3af2eaaaa1562a18e86a96e7830194926dbdd66de679727d7871", + "sha256:e3b4063c4068fadd48e7cfb220476bfd529e59614e0b3f8a825d06115d2a9b44", + "sha256:fcfa81321de93933406e313b7f3f89d6300ab5bc9d757c89b4214858fb4393d3" ], "index": "pypi", - "version": "==2024.0.0b4.post1" + "version": "==2024.1.1.1" }, "yarl": { "hashes": [ diff --git a/rio/components/drivetrain.py b/rio/components/drivetrain.py index 0e204f93..180f72cf 100644 --- a/rio/components/drivetrain.py +++ b/rio/components/drivetrain.py @@ -3,7 +3,6 @@ import wpilib import logging -from commands2 import Subsystem from wpilib import Field2d, DriverStation from wpimath.filter import SlewRateLimiter from wpimath.geometry import Pose2d, Rotation2d, Translation2d diff --git a/rio/components/mikeSwerveModule.py b/rio/components/mikeSwerveModule.py index 1d06810e..13512086 100644 --- a/rio/components/mikeSwerveModule.py +++ b/rio/components/mikeSwerveModule.py @@ -1,6 +1,7 @@ import math +import rev -from rev import CANSparkMax, SparkMaxAbsoluteEncoder +from rev import CANSparkMax, SparkMaxAbsoluteEncoder, CANSparkLowLevel from wpimath.geometry import Rotation2d from wpimath.kinematics import SwerveModuleState, SwerveModulePosition @@ -19,10 +20,10 @@ def __init__(self, swerveConfig: dict, moduleConfig: str) -> None: self.desiredState = SwerveModuleState(0.0, Rotation2d()) self.drivingSparkMax = CANSparkMax( - moduleConfig["CANDriveID"], CANSparkMax.MotorType.kBrushless + moduleConfig["CANDriveID"], rev.CANSparkLowLevel.MotorType.kBrushless ) self.turningSparkMax = CANSparkMax( - moduleConfig["CANSteerID"], CANSparkMax.MotorType.kBrushless + moduleConfig["CANSteerID"], rev.CANSparkLowLevel.MotorType.kBrushless ) # Factory reset, so we get the SPARKS MAX to a known state before configuring @@ -165,7 +166,8 @@ def setDesiredState(self, desiredState: SwerveModuleState) -> None: optimizedDesiredState.speed, CANSparkMax.ControlType.kVelocity ) self.turningPIDController.setReference( - optimizedDesiredState.angle.radians(), CANSparkMax.ControlType.kPosition + optimizedDesiredState.angle.radians(), + CANSparkMax.ControlType.kPosition, ) self.desiredState = desiredState diff --git a/rio/robot_requirements.txt b/rio/robot_requirements.txt new file mode 100644 index 00000000..026e15cb --- /dev/null +++ b/rio/robot_requirements.txt @@ -0,0 +1,15 @@ +# These are not for you! +# Use pipenv install +# for developing. These +# requirements are only for +# the ROBOT + +PyYAML +robotpy +robotpy-commands-v2 +robotpy-cscore +robotpy-ctre +robotpy-hal +robotpy-rev +robotpy-navx +wpilib diff --git a/rio/simgui-window.json b/rio/simgui-window.json index 34083bbe..8d9fa485 100644 --- a/rio/simgui-window.json +++ b/rio/simgui-window.json @@ -26,12 +26,12 @@ "###FMS": { "Collapsed": "0", "Pos": "5,540", - "Size": "283,146" + "Size": "32,35" }, "###Joysticks": { "Collapsed": "0", "Pos": "250,465", - "Size": "796,138" + "Size": "32,35" }, "###NetworkTables": { "Collapsed": "0", @@ -51,12 +51,12 @@ "###System Joysticks": { "Collapsed": "0", "Pos": "16,322", - "Size": "192,218" + "Size": "32,35" }, "###Timing": { "Collapsed": "0", "Pos": "7,129", - "Size": "135,173" + "Size": "32,35" }, "Debug##Default": { "Collapsed": "0", @@ -66,7 +66,7 @@ "Robot State": { "Collapsed": "0", "Pos": "5,20", - "Size": "92,99" + "Size": "32,35" } } } diff --git a/rio/simgui.json b/rio/simgui.json index e9858b4a..4880fa89 100644 --- a/rio/simgui.json +++ b/rio/simgui.json @@ -24,7 +24,8 @@ }, "Connections": { "open": true - } + }, + "visible": true }, "NetworkTables View": { "visible": false From da4f24885bffcab7965abe40c6f0017bf375e04b Mon Sep 17 00:00:00 2001 From: kredcool Date: Thu, 18 Jan 2024 19:22:11 -0500 Subject: [PATCH 32/61] fixing test issue where wrong Pipfile.lock ver --- rio/Pipfile.lock | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/rio/Pipfile.lock b/rio/Pipfile.lock index d3db4d2b..258bb184 100644 --- a/rio/Pipfile.lock +++ b/rio/Pipfile.lock @@ -1,7 +1,7 @@ { "_meta": { "hash": { - "sha256": "d585b89124e91d03d7f6977e1c5d71cca1dd8951467b0fe4592cc53f07b3b5ef" + "sha256": "a10fc2592b799759069137ac85dffb63b25f3511b5f7412f19889b73fc36cc7d" }, "pipfile-spec": 6, "requires": { @@ -664,14 +664,6 @@ "markers": "python_version >= '3.8'", "version": "==2024.0.0" }, - "robotpy-commands-v2": { - "hashes": [ - "sha256:0161feea5288b6af6fe3eddc412ad30c920acb929bcb5fea6899ba47e2e5763b", - "sha256:e0f808645f3f01c1a49d54cbdca6a2f528c2797913bb6382bec067c69b646065" - ], - "index": "pypi", - "version": "==2024.0.0b4" - }, "robotpy-ctre": { "hashes": [ "sha256:056e978d2d58aa842c69472063e21e89b0615dc6df70438ee86b4b3ea8825293", From c7857e59da2e68d48098b542ab5f4a0a60672479 Mon Sep 17 00:00:00 2001 From: kredcool Date: Fri, 19 Jan 2024 16:55:25 -0500 Subject: [PATCH 33/61] changing swerve values --- rio/components/drivetrain.py | 4 ++++ rio/constants/robotHardware.yaml | 40 ++++++++++++++++---------------- 2 files changed, 24 insertions(+), 20 deletions(-) diff --git a/rio/components/drivetrain.py b/rio/components/drivetrain.py index 180f72cf..424f5d16 100644 --- a/rio/components/drivetrain.py +++ b/rio/components/drivetrain.py @@ -123,6 +123,10 @@ def periodic(self) -> None: self.sd.putNumber("Swerve/RL", self.rearLeft.desiredState.angle.degrees()) self.sd.putNumber("Swerve/RR", self.rearRight.desiredState.angle.degrees()) + # print( + # f"FL: {self.frontLeft.turningEncoder.getPosition()} FR: {self.frontRight.turningEncoder.getPosition()} RL: {self.rearLeft.turningEncoder.getPosition()} RR: {self.rearRight.turningEncoder.getPosition()}" + # ) + def getPose(self) -> Pose2d: """Returns the currently-estimated pose of the robot. diff --git a/rio/constants/robotHardware.yaml b/rio/constants/robotHardware.yaml index b7732941..5041b20f 100644 --- a/rio/constants/robotHardware.yaml +++ b/rio/constants/robotHardware.yaml @@ -2,39 +2,39 @@ swerve: modules: frontLeft: - CANSteerID: 2 # Configured Never by Nobody - CANDriveID: 3 # Configured Never by Nobody + CANSteerID: 2 # Configured 1/19 by Keegan + CANDriveID: 1 # Configured 1/19 by Keegan Pose: [-1, 1] # Configured Never by Nobody frontRight: - CANSteerID: 0 # Configured Never by Nobody - CANDriveID: 1 # Configured Never by Nobody + CANSteerID: 6 # Configured 1/19 by Keegan + CANDriveID: 5 # Configured 1/19 by Keegan Pose: [1, 1] # Configured Never by Nobody rearLeft: - CANSteerID: 6 # Configured Never by Nobody - CANDriveID: 7 # Configured Never by Nobody + CANSteerID: 4 # Configured 1/19 by Keegan + CANDriveID: 3 # Configured 1/19 by Keegan Pose: [-1, -1] # Configured Never by Nobody rearRight: - CANSteerID: 4 # Configured Never by Nobody - CANDriveID: 5 # Configured Never by Nobody + CANSteerID: 8 # Configured 1/19 by Keegan + CANDriveID: 7 # Configured 1/19 by Keegan Pose: [1, -1] # Configured Never by Nobody # General constants all modules share - kDrivingP: 0 - kDrivingI: 0 - kDrivingD: 0 - kDrivingFF: 0 # Was configured in complexConstants.py, might need to do that again! - kDrivingMinOutput: -1 - kDrivingMaxOutput: 1 + kDrivingP: 0 # Configured Never by Nobody + kDrivingI: 0 # Configured Never by Nobody + kDrivingD: 0 # Configured Never by Nobody + kDrivingFF: 0 # Configured Never by Nobody + kDrivingMinOutput: -1 # Configured Never by Nobody + kDrivingMaxOutput: 1 # Configured Never by Nobody - kTurningP: 2.65 - kTurningI: 0.012 - kTurningD: 1.5 - kTurningFF: 0 - kTurningMinOutput: -1 - kTurningMaxOutput: 1 + kTurningP: 0.265 # Updated 1/19 by Keegan + kTurningI: 0.012 # Configured Never by Nobody + kTurningD: 1.5 # Configured Never by Nobody + kTurningFF: 0 # Configured Never by Nobody + kTurningMinOutput: -1 # Configured Never by Nobody + kTurningMaxOutput: 1 # Configured Never by Nobody # These are important translation factors to convert native units to m/s, do not leave at 1! drivePosConversionFactor: 1 # Drive motor conversion factor (position) From 7e4a00fac435d56852bc426bb65bcaf6673be4a8 Mon Sep 17 00:00:00 2001 From: Jack Jewett Date: Fri, 19 Jan 2024 19:55:21 -0500 Subject: [PATCH 34/61] Fixup poses and inital swerve --- rio/components/drivetrain.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rio/components/drivetrain.py b/rio/components/drivetrain.py index 424f5d16..f223ce4c 100644 --- a/rio/components/drivetrain.py +++ b/rio/components/drivetrain.py @@ -123,6 +123,8 @@ def periodic(self) -> None: self.sd.putNumber("Swerve/RL", self.rearLeft.desiredState.angle.degrees()) self.sd.putNumber("Swerve/RR", self.rearRight.desiredState.angle.degrees()) + logging.info(f"FR Swerve module setpoint {self.frontRight.desiredState.angle.degrees()}, actual is {self.frontRight.getPosition().angle.degrees()}") + # print( # f"FL: {self.frontLeft.turningEncoder.getPosition()} FR: {self.frontRight.turningEncoder.getPosition()} RL: {self.rearLeft.turningEncoder.getPosition()} RR: {self.rearRight.turningEncoder.getPosition()}" # ) From db4295997125e9935bc8fe9c0b20060f8cc671d0 Mon Sep 17 00:00:00 2001 From: kredcool Date: Sat, 20 Jan 2024 16:25:42 -0500 Subject: [PATCH 35/61] all must be inverted bause rev --- .gitignore | 3 +++ rio/components/drivetrain.py | 14 +++++++++----- rio/components/mikeSwerveModule.py | 2 +- rio/constants/robotHardware.yaml | 6 +++--- rio/simgui-window.json | 10 +++++----- rio/tests/logs/FRC_20240120_210407.wpilog | Bin 0 -> 90721 bytes rio/tests/logs/FRC_20240120_210513.wpilog | Bin 0 -> 90180 bytes rio/tests/logs/FRC_20240120_210521.wpilog | Bin 0 -> 90721 bytes rio/tests/logs/FRC_20240120_211051.wpilog | Bin 0 -> 90721 bytes 9 files changed, 21 insertions(+), 14 deletions(-) create mode 100644 rio/tests/logs/FRC_20240120_210407.wpilog create mode 100644 rio/tests/logs/FRC_20240120_210513.wpilog create mode 100644 rio/tests/logs/FRC_20240120_210521.wpilog create mode 100644 rio/tests/logs/FRC_20240120_211051.wpilog diff --git a/.gitignore b/.gitignore index 5e50bbe8..74acd7d6 100644 --- a/.gitignore +++ b/.gitignore @@ -168,3 +168,6 @@ networktables.json # Add to tree but only when neccicary (adding a map or smthn) simgui-window.json + +# sysid ignores +*.SysId diff --git a/rio/components/drivetrain.py b/rio/components/drivetrain.py index f223ce4c..b4683122 100644 --- a/rio/components/drivetrain.py +++ b/rio/components/drivetrain.py @@ -107,7 +107,7 @@ def periodic(self) -> None: like updating the dashboard and maintaining odometry. """ - # Update the odometry in the periodic block + # Update the odometry in the periodic bloc self.odometry.update( Rotation2d.fromDegrees(self.gyro.getAngle()), ( @@ -117,16 +117,20 @@ def periodic(self) -> None: self.rearRight.getPosition(), ), ) - + # desired self.sd.putNumber("Swerve/FL", self.frontLeft.desiredState.angle.degrees()) self.sd.putNumber("Swerve/FR", self.frontRight.desiredState.angle.degrees()) self.sd.putNumber("Swerve/RL", self.rearLeft.desiredState.angle.degrees()) self.sd.putNumber("Swerve/RR", self.rearRight.desiredState.angle.degrees()) - logging.info(f"FR Swerve module setpoint {self.frontRight.desiredState.angle.degrees()}, actual is {self.frontRight.getPosition().angle.degrees()}") + # actual + self.sd.putNumber("Swerve/FL", self.frontLeft.getState().angle.degrees()) + self.sd.putNumber("Swerve/FR", self.frontRight.getState().angle.degrees()) + self.sd.putNumber("Swerve/RL", self.rearLeft.getState().angle.degrees()) + self.sd.putNumber("Swerve/RR", self.rearRight.getState().angle.degrees()) - # print( - # f"FL: {self.frontLeft.turningEncoder.getPosition()} FR: {self.frontRight.turningEncoder.getPosition()} RL: {self.rearLeft.turningEncoder.getPosition()} RR: {self.rearRight.turningEncoder.getPosition()}" + # logging.info( + # f"FR Swerve module setpoint {self.frontLeft.desiredState.angle.degrees()}, actual is {self.frontLeft.getPosition().angle.degrees(),} .getPosition is {self.frontLeft.getPosition()}" # ) def getPose(self) -> Pose2d: diff --git a/rio/components/mikeSwerveModule.py b/rio/components/mikeSwerveModule.py index 13512086..91490f4e 100644 --- a/rio/components/mikeSwerveModule.py +++ b/rio/components/mikeSwerveModule.py @@ -62,7 +62,7 @@ def __init__(self, swerveConfig: dict, moduleConfig: str) -> None: ) # Uncomment to invert the encoders - # self.turningEncoder.setInverted(true) + self.turningEncoder.setInverted(True) # Enable PID wrap around for the turning motor. This will allow the PID # controller to go through 0 to get to the setpoint i.e. going from 350 degrees diff --git a/rio/constants/robotHardware.yaml b/rio/constants/robotHardware.yaml index 5041b20f..10eaed53 100644 --- a/rio/constants/robotHardware.yaml +++ b/rio/constants/robotHardware.yaml @@ -7,8 +7,8 @@ swerve: Pose: [-1, 1] # Configured Never by Nobody frontRight: - CANSteerID: 6 # Configured 1/19 by Keegan - CANDriveID: 5 # Configured 1/19 by Keegan + CANSteerID: 99 # 6 Configured 1/19 by Keegan + CANDriveID: 98 # 5 Configured 1/19 by Keegan Pose: [1, 1] # Configured Never by Nobody rearLeft: @@ -29,7 +29,7 @@ swerve: kDrivingMinOutput: -1 # Configured Never by Nobody kDrivingMaxOutput: 1 # Configured Never by Nobody - kTurningP: 0.265 # Updated 1/19 by Keegan + kTurningP: 2.65 # Updated 1/19 by Keegan kTurningI: 0.012 # Configured Never by Nobody kTurningD: 1.5 # Configured Never by Nobody kTurningFF: 0 # Configured Never by Nobody diff --git a/rio/simgui-window.json b/rio/simgui-window.json index 8d9fa485..34083bbe 100644 --- a/rio/simgui-window.json +++ b/rio/simgui-window.json @@ -26,12 +26,12 @@ "###FMS": { "Collapsed": "0", "Pos": "5,540", - "Size": "32,35" + "Size": "283,146" }, "###Joysticks": { "Collapsed": "0", "Pos": "250,465", - "Size": "32,35" + "Size": "796,138" }, "###NetworkTables": { "Collapsed": "0", @@ -51,12 +51,12 @@ "###System Joysticks": { "Collapsed": "0", "Pos": "16,322", - "Size": "32,35" + "Size": "192,218" }, "###Timing": { "Collapsed": "0", "Pos": "7,129", - "Size": "32,35" + "Size": "135,173" }, "Debug##Default": { "Collapsed": "0", @@ -66,7 +66,7 @@ "Robot State": { "Collapsed": "0", "Pos": "5,20", - "Size": "32,35" + "Size": "92,99" } } } diff --git a/rio/tests/logs/FRC_20240120_210407.wpilog b/rio/tests/logs/FRC_20240120_210407.wpilog new file mode 100644 index 0000000000000000000000000000000000000000..ee43dc95bf2ed83f1b4408ee106f5c08d706aa3a GIT binary patch literal 90721 zcmbWA3Ebdwy|$mRjE9&7A(Cm5Jrkkqk|kMVBB>ZW7|e`j1|d61go;Ro2qnpqrG=D4 zQc-A=B_)Y$k+gWP=XRdEYdY`eJ;ytrj}O=Re(&$i??0Cmop#!8i>s_4b7T0W$&R`qxtPwvu947zV}Njn-2`k#cz%0mu)7mGikG}ci3Y3UNa`GI{T0V zhW@?Fp@G9D9x!vp0YfuqPaT>yah<~sUuEK~83)eXV`$@$iAU<{S90^uWNo z1_tIPG92^O&8JKo+TnnqJ*Mt8b&suI`WgntnG*P zeCz)EPn|M-kD=k}-Dk|$e`w0||IM=ujvD^yxXr-8{QsB3JItOkd+Lno{~r%B|6qQy zag!h04h$^7ua@K&zcxQJ-!^0V?3pw6-(q9jGW@f2-&r%JugtFxn>cHaeTJq@iFp5B8TNMKe@e;k{6r$dKgJDgH({HBh4`)E z2by)rtl2};CQqIA@1G)X{4j43nsoV3HiLo*K=nzY$g!w&2@<3Jwf zfALdg+?c@$+njpIgUcRq&ywrC)G_vd?%4k2b&UOQ9pnDTj_qIOAIG@=)-n8N+W+@k zZT~X=IL6P>u`}OeVLD#NFZmw(9JtqB`wzYJHCCNAG<(XPeAr1l4DHX)tf4($)}JgV zEHHTE^79X09{w?5%-{nD4y&9W6UNR#X-ZA`V>yhtg8aKym{2Y{AuZ$g@m69tZR~j>T<5})AjhkaOehx~m zSH=#{O39UyE8)?@?la+$W_Tn@u2=B(VfUGm>n0wpIs7k7BkyU#BhBzglw7ai-dK2vht#G}QB|MGq0Jx%zh86Js}>lOT4e0(XnQgS6c z8gicr|1`rRQF6V4e?#swCD%eWv8PiAVRi&xC)P;gKl0UctY6+-FLzn|QSBqIypg{%M9sqU3r7 z|CU{}lw2vf5+2QTp9zmN!y{30y@G!;-DgU!n|O4i`%L(!86Js}>lOUF(S4@mx`{_) zhX0e%{NO)lFyWtOcqB@$SCT6wS4ysgNAGo?3I8<1BT;g_f`9LQWhuE*awR;v#C;|_ z(hQG8$@L2UUE)4da^1wEzq!wZf12TuD7jw2zrVT9lw3FQX!FJN{Y?0$86Js}>lOUl ze6dn;rQ}L@bcXv(c%&H~iIVFT{5!*arsTSbM-RErgnydhktn%d!M}&xXG*S{c(mH^ z-{Qy*d@$jkW_Tn@u2+&PC09zWghxlZ&xA*s;gKl0UctX37cV7OO0I-QKXRW5k2J$0 zQF6V4e?M}cDYITW91wP57r79*L6c75rOjiBfW<^>77X@*Cl=?0FOlYC&D99a=nsVDY;T|B|O^MeJ1?V z439*~^$Py&JfW0aDY+6JUF<#+9%+V0qU3r7|1Nf)DYlOU__-jkam69vr z(f#f-;gM!|BucJV@b7;2nUd=!9<4g^Pje0XV8TDm@JN(guOwGWu9RE}k3Qr+6CP=X zN226<1^+%Yv6NgXxe^}T<~|c1X@*ClZsO5g z%gT2X9%+V0qU3r7|K?h@lw2vf5*}^mJ`?_FhDW00dIkTsbDt@>ZsO6o?la+$W_Tn@ zu2=ByT=$uh>n0vO?miP9X@*ClONC)-Nd68+-Jf+&G1N+T(98Y3+^)|*G)Wn`wIGgCj8S3k3`A! z3jV!)g;H{*7NM9K9E{{7B_cY<3W_Tn@u2=AH^%YCWm69vr(b4WR;gM!|BucJV@b75%nUd=!9{t39Cj8S3 zk3`A!3jY1XeWv8PiAPJXr1v!8pJsR@O0HM%Z|Rjv$(52T;n9BXGvS|RcqB@$SMYB? z_nDIGCLVp=eI`88439*~^$Pxd-F>Fyx`{{g4gVKb@?-el|B3KVgh!&}dL_A1a;4-- zc=Rs!neb0DJQ5|>EBN=Wl}pK$k}KiS1@1H9k!E-#O0HM%?*jLklItcOJ?%ad{%M9s zqU3r7|DJZADYp@lOU_zWYqcbrX*kT~+UC!avRMNR(W! z;NPOFmXa$aSHhz`+-Jf+&G1N+T(98Y9_}+G*G)XS!hI$@(hQG8$@L2UUEw}ca^1wE z7u{#VKh5w+lw7ai-;3@uCD%O0I-QpK+fFk2J$0 zQF6V4f1h!mDYOD>Prx_lJlIs=xd-EGh z$(52T;nDH#GvSeDcqB@$SMcw6_nDIGCLZ18J`?_FhDW00dIkUPa-S)=ZsO6z)%2bw z{L>7NM9K9E{!Lu1lw2vf5*{7kJ`?_FhDW00dIkRuaGxo;ZsO4m?la+$W_Tn@u2=By z2KSkg>n0v8xVqlcgnydhktn%d!M_DpFC|w>u7pSLai0nQG{YlNa=n6o?{S|gxo+ao z7u{#VBhBzglw7ai-xu9yO0JuD^ql)l_@@~jiIVFT{Cm!QrsTSbN1MJ$-_L}9n&FWs zxn9A)P2W^Xu9RE}k3Q)>6CP=XN226<1^+(jK2vht#G?n@XTm?t@JN(gui)QkhThYJf12TuD7jw2ze#J9k}D-w!lT38XTl@R@JN(gui)R|?lUFVO+5Oc`%L(! z86Js}>lOU_q5Dk9brX+XwWi+Fgnydhktn%d!M|6nSxT;yTnUf%cAp9VG{YlNa=n6o zd%Mq+TsQIP8uyv-NHaVVCD$wXca8f@$#oNt=2=VcX~I9v@JN(gui)Q2Yn75KC0D|u z?cHa>Kh5w+lw7ai-}de^CD%-(ATPcu9cCD$wXx89pe$(52T;n7FkXTl@R@JN(gui)QD-DgU! zn|SmK_nGidGdvO{*DLt<3-_6l>n0v8{}#Qc3I8<1BT;g_f`7}urIcJLxe^{7=spu3 zX@*Clxo+aoP3|+{pJsR@O0HM%? zEBJSo`%KAo6OSHtp9%jo!y{30y@G!ayU&zdH}PnVb@iSm{L>7NM9K9E{;jcYDY;T| zB|JLDeI`88439*~^$PwS<33Yz-Nd6i+-Jf+&G1N+T(98Y9quzF*G)Wn&3bxI6aHz2 zN226<1^-^NUMaa!awR<4-+d7NM9K9E{_W&GQ*zzJqYK?< z!XwS_NR(W!;NOMrGbPteJbK1`Cj8S3k3`A!3jRIgK2vht#G|)vpzmkGKh5w+lw7ai z-&;2*C09zWgh!{k&xA*s;gKl0UctXp-DgU!n|Sma_nGidGdvO{*DLt<8~2%#>*koj z`&Zsj?`ax0$87u@lw7Zj9iEkvDV>W&cO0HMN4$n%-m69us8C>R8 z_nF4cF&jSzCD$ushi9eaO39VR3~spCMtV=vxH)Fy=b+?zW$f^*lw2vf(wMy@#?vr=-Uy@#?vr=-UlOUl zY|~P5rQ}L@bh`UYc%&H~iIVFT{5#!!rsTSbN56HS3I8<1BT;g_f`7ktpDDR+E;RV< zH*R*pdM`e)*Vxg+|KD*Ty{-uxHNz@Va=nR-Z``buTq(H{RvqC!6IN-4Rifm21sjiW zpDDR+V%6>LGhw4_Uku2-;e2ltth>n0w3-hC!K(hQG8$@L2UecpYhEBJSi`%KAo6OX>KNJ3GhDW00dIkTs+^UpZDY+6J zecF8{Jkku0M9K9E{(ahgrsTSbM}Kgi3I8<1BT;g_f`5N-pDDR+;?bI0>pe~Qrx_lJ zlIs=xTXXADa;4--cyz4$On9Uj9*L6c75qEaeWv8PiAQ(3&xC)P;gKl0UctXR-DgU! zn|QR$HhNDJ{%M9sqU3r7|CZUNlw2vf5*|%=p9%jo!y{30y@G$!-DgU!n|Sms_nGiW zGdvO{*DLtn0v8u&v(Hgnydhktn%d!M_EzEhSe7NM9K9E{{6*$rsTSbM;mXa z?`Oh4&G1N+T(98Y#@m&WDiK2vht#G^O7Q}1cQKh5w+lw7ai-y7apO0JY#36BnQp9zmN!y{30y@G#-xzChb zH}U8P?la+^W_Tn@u2=By2ktW^*G)WHe0#m83I8<1BT;g_f`5x|UrMf&TnUec+-Jf+ z&G1N+T(98Yko!!@brX-Sa-Rv0G{YlNa=n6oSGmuWTsQG()DC)26aHz2N226<1^-6v zP)e?pTnUf1bDs(SG{YlNa=n6o+qut_TsQIP9QT>}COpy%k3`A!3jWP>pDDR+;?Z~9XTm?t@JN(gui)Qz+-FLzn|L&KN4=*B|1`rR zQF6V4e`9wnC09zWgh%glp9%jo!y{30y@G%5bDt@>ZsO6G-Dkoh&G1N+T(98Ym)&Pd zuA6xDH}{$FPcu9cCD$wX_c!;MlItcOZN8Jfp9%jo!y{30y@G$6?^H^zlw1jq&TyXz zk2J$0QF6V4e`mPQlw3FQ=ppx+@J};55+&Cw`1g?eOv!ZpoL*-Nd8o+-Jfg&G1N+ zT(98Yb?!4I*G)Vc{T{ui3I8<1BT;g_f`6mmQ%bIsTnUdRyU&Dwn&FWsxn9A)$?h{H z*G)XSz(hQG8$@L2UUEn@ba^1wEr`%`4Kh5w+lw7ai-&5{0CD%n0vu?miP9X@*CllOUl>iwnUO39V*=xq0y@JKT}5+&Cw_;lOT);XYGx-Nd8o-Dkoh&G1N+T(98Y_3kqz*G)Wn#T31#3I8<1BT;g_ zf`6}=QcA9rTnUeMcAp9VG{YlNa=n6oJG;-6TsQIPV)vQwNHaVVCD$wXcd`3S$#oNt zo^ziG|1`rRQF6V4f6uwklw3FQXp`Ob{Y?0$86Js}>lOUlWcN~XrQ}L@^a=Nw@JKT} z5+&Cw`1c9-nUd=!9zEbb6aHz2N226<1^*s!pDDR+;?b&m=sivNrx_lJlIs=xTXm08 za;4--c=RFnnea$6JQ5|>EBN;z_nDIGCLZ19J`?_FhDW00dIkS(bDt@>ZsO4rd+I$+ z_@@~jiIVFT{99trQgWr_N_e!l`%L(!86Js}>lOUl+kK|wx`{_uyU&D2n&FWsxn9A) ztKDZxuA6u?_mJMxgnydhktn%d!N0kOO39UyE8)>Q-Dko-&G1N+T(98YJKbkWuA6vt zuKP@Qq!}KGlIs=xJJ)@tQgS6cI?;V5Jkku0M9K9E{+;MPQ*zzJqhGksgnydhktn%d!M|U)&y-v@ z@o2fd^`0jD(+rP9$@L2UEw^_mxl(c^Jeuu36CP=XN226<1^;Hd&y-v@@#wqmGvS|R zcqB@$SMcwdm4 zCj8S3k3`A!3jS>|wUk^bxe^|o={^%4X@*Cln0vO?LHI! zX@*ClS=BhBzglw7ai-y!ZZCD%7NM9K9E{@vm}Q*zzJqgT$*dz$c1GdvO{ z*DLt<${D5PO39V*Xb<<9@J};55+&Cw__v4qOv!ZEBN=K`%KAo6OXn&K;O@Vf12TuD7jw2zpW1_C09zWgh!usp9zmN z!y{30y@G$Ab)PA@ZsO6S?la+^W_Tn@u2=ByQTLgW>n0w(EBLqML8atM$(8VEANQH?Pcu9cCD$wX zw~zZw$#oNtu5q6Uk2J$0QF6V4f7iIrlw3FQXr6=ho+kX$439*~^$PyYb8soSQgS6c z+TML8{L>7NM9K9E{%!9*Q*zzJqx0Nn!XwS_NR(W!;NN-fGbPteJbJ=?Cj8S3k3`A! z3jRIeK2vht#G~~;sPAXOKh5w+lw7ai-})acC09zWghwa2&xA*s;gKl0UctYU+-FLz zn|Sm~_nGidGdvO{*DLtn0v8e~8}Ggnydhktn%d!N27XDJ54*u7pPiy3d41 zn&FWsxn9A)1KnpzuA6vtllx5grx_lJlIs=xyUBf~EBH75 z&{A@xtdxl(c^JUYvLCOpy%k3`A! z3jUqtK2vht#G{AZXTm?t@JN(gui)Rq?lUFVO*~rTLwZjW{%M9sqU3r7|JL|WDY;T| zB|JLDeI`88439*~^$PwS<33Yz-Nd6i-Dko-&G1N+T(98Yo$fOw*G)WH=5W2I3I8<1 zBT;g_f`7{#UP`W%TnUe+xzB`un&FWsxn9A)Y3?&6*G)Y7ru$5Iq!}KGlIs=x`=Xf12TuD7jw2zg3PZC09zWghz+E&xA*s;gKl0UctXZ-DgU!n|O4q`%L(!86Js} z>lOUF)qSSqx`{`N9j*5?;h$!BBucJV@NcoBOUadzE8)?e?la+^W_Tn@u2=AHPxqOU z>n0vupm0y zX@*ClzMl#IG{YlNa=n6oYad%mu9RE}k4|u(36C_xBT;g_f`2Et&y-v@@#t>% zneb0DJQ5|>EBJS}`%KAo6OWcXPVZ^LKh5w+lw7ai-?GP*k}D-w!lRk)GvSeDcqB@$ zSMYD9`%KAo6OV3mp9%jo!y{30y@G!?y3dqcH}Po9@p?}a{%M9sqU3r7|Hd3&O0JY# z36I|EJ`?_FhDW00dIkU9>poL*-Nd6y+-Jfg&G1N+T(98YCGImN*G)Wn-hC$g(+rP9 z$@L2UJ?}nKa^1wE%|5K}XTm?t@JN(gui)QiA1)7NM9K9E{*6Adlw2vf5*|%lOSv-+iX!x`{_my3d4vn&FWsxn9A)C*5aCuA6wY!AJG|O!%i69*L6c75v-aqow3Z z$(8WvWcQiyNHaVVCD$wXce49T$#oNte&s$B{%M9sqU3r7|9<5@Q*zzJqZLlldz$c1 zGdvO{*DLt9!bzp%O39V*=wSDm@JKT}5+&Cw_;;}TOv!Z zA2y1TYhQ9Dtoo|^OjxBER*9196>R+KzuS}REV&X^{nLFWY}5>^M9K9EHvZFnrsTSb zRa<^c?`gtD&9F+8T(4l`mLDr6S4ysgRiAdB39B^2Dp7L1f{mYcpDDR+V$~z=Ghw4< zSS3oXSFrIB_nDIGCRVL=ir&+NjhbPVD7jw2#yY>n0veI92ay!XwS_NR(W!;NOH(OUadzE8)>} z_nGidGdvO{*DLrp-F>Fyx`{{Ma-Rv0G{YlNa=n6o-*TTRxo+ao0;lOcP57r79*L6c z75rP^v{G`Vo%On9Uj z9*L6c75w|S`%KAo6OZn9p9%jo!y{30y@G%DyU&zdH}U8VpU``n@J};55+&Cw`1giS zl#(kYSHh#i+-Jfg&G1N+T(98YVeT^}*G)XS&3z{P(+rP9$@L2U-R3@1a^1wEB|fS5 zG~u6ScqB@$SMYC%PnMD^C0D|uz1(NQKh5w+lw7ai-(K!BCD%7NM9K9E z{=LtArsTSbN0++Kgh!g;ktn%d!M{u0XG*S{c=Uq%O!%i69*L6c75sa_eWv8PiAQfg zOW)6gf12TuD7jw2zqg-NO0JY#36DPIJ`)~khDW00dIkSJS8K2vht#H0B>qxUr7pJsR@O0HM% zZ@$lzk}D-w!lQS&&xC)P;gKl0UctY2xzChbH}U8K_nGiWGdvO{*DLsUf%{C!brX-C za-RwRG{YlNa=n6oPr1*OTsQG(!_Vsbneb0DJQ5|>EBLqJXG_VIk}KiSDeg1jk!E-# zO0HM%?-ci$lItcO-RnLR{%M9sqU3r7|L%34DYEBN<)_nDIGCLS$%j^5LR zf12TuD7jw2zeUd}C09zWgh#u(&xC)P;gKl0UctZJ-DgU!n|O4&`%HMG86Js}>lOUF z+5a;4--cyxpN zOn9Uj9*L6c75uxweWv8PiAM{bulF?JpJsR@O0HM%Z^83R$(52T;n92CXTm?t@JN(g zui)Q%+-FLzn|Sm^_nGiWGdvO{*DLtn0vO=ROnuX@*ClEBLp`1*POl$(8Wv6YewNk!E-#O0HM%?-TAbCD%Jkku0 zM9K9E{(aParsTSbN562N3I8<1BT;g_f`7kopDDR+;?Z(n)O(unPcu9cCD$wXx7-&? z$(52T;n8gOnea$6JQ5|>EBH6teWv8PiAOiN&xC)P;gKl0UctYc+-FLzn|L(-OL|We z{%M9sqU3r7|Hgl*lw2vf5+3d9J`?_FhDW00dIkS>b)PA@ZsO5b+-Jfg&G1N+T(98Y zSKMbxuA6xDclVj_Pcu9cCD$wX_jmW1lItcOZE=adp9%jo!y{30y@G#RTvAG|lw1jq z&UBv%k2J$0QF6V4e`mVSlw3FQ==bh3;h$!BBucJV@bCBTGbPteJX+(+dQTJnX@*Cl zlOUF#eJsax`{`NU9R^u;h$!BBucJV@NcopOUadzE8)?e?la+^ zW_Tn@u2=AHPxqOU>n0vu={^%4X@*ClgK2vht#G}_;rS~-9pJsR@O0HM%?{!y|k}D-w z!lMtk&xA*s;gKl0UctW)xX+YaH}UA(?la+^W_Tn@u2=By+wLEBNn0v8 zd9B{lgnydhktn%d!M`Q1EhSen0vu>pl}6X@*Cl z9nbHC@RPcu9cCD$wXx8=7=$(52T;nAnvXTl@R@JN(gui)RO-DgU!n|SmG_nGid zGdvO{*DLt<2ltth>n0wpdA;7#gnydhktn%d!M`=HFC|w>u7pR&y3d41n&FWsxn9A) zW8G&;uA6vtr~6F!rx_lJlIs=xyVHH9EBN<@ z@05}&C0D|u!`x@WBhBzglw7ai-(l`ECD%EBJSg`%KAo6OSHqp9%jo!y{30y@G#_xzChbH}Pnl@9F!Q@J};5 z5+&Cw__xmYO39UyE8)>c+-Jfg&G1N+T(98YN8D#huA6vtkNZscrx_lJlIs=xyT^T| z0WO0JuDH1_*?PZR!WhDW00dIkT+e!rAlDY+6Jz0Z9n{L>7NM9K9E z{=LtArsTSbM_+cI36C_xBT;g_f`4CjpDDR+;?dvSXTm?t@JN(gui)R`+-FLzn|QSO zE&6^Y{L>7NM9K9E{%w9sDY;T|B|JLAeI`88439*~^$Px-;XYGx-Nd7Z+-Jf+&G1N+ zT(98YL+&#r*G)WH?N+^~3I8<1BT;g_f`6;sT1u{zTnUeka-Rv0G{YlNa=n6oN4d|G zTsQIP$L=%XpJsR@O0HM%@5k;lCD%7NM9K9E{%!cfQgWr_N_g}!_nGiWGdvO{ z*DLtn0xk+I=Sc(+rP9$@L2U{n~w|{$(8WvIQNOD>Prx_lJlIs=xd&QllEBLpw z`%KAobD_ceFTV5CeFmP~YwYNO30QTp+f7)d8CHps>rHIr<|Pj^M9K9EHa_b|c4eI~5Z468)R^$IqA$bF{dx`|b{xzB`+nqieFxn9A>+uUbLuA6wY#9ew% z6CP=XN226<1^EBJS!`%KAo6OVrGJ`?_FhDW00dIkS}?mkm; z-Nd8iey;a4;h$!BBucJV@Nc=Fmy#pm0y zX@*ClEBH6=7p3G%$(8Wv{q8g2pJsR@O0HM% z@BQvGCD%t z_5Dovrx_lJlIs=xd;2d-$(52T;nA7yGvSeDcqB@$SMcvl_nDIGCLaCXeJ1?V439*~ z^$Py|-hHOzx`{_``jy_(gnydhktn%d!M`{Cs+3$Qxe^{7?LHG8X@*ClEBN;X_nDIGCLTTQ zJ`?_FhDW00dIkTUcAqJ^ZsO5KztQ(I;h$!BBucJV@Nc8vl#(kYSHh!H+-Jfg&G1N+ zT(98YDef~R*G)XS*L^1Z(+rP9$@L2U-RnM6a^1wEmG0Afn($9EJQ5|>EBLq4eWm0| z$(8Wv5ciqzNHaVVCD$wXcZmB;$#oNtZgHOp|1`rRQF6V4f48{Llw3FQ=#}^DJx%zh z86Js}>lOTa<^84PO39V*Xb<<9@J};55+&Cw__v4qOv!ZEBN=K`%KAo6OXojK;O@Vf12TuD7jw2zpWoAC09zWgh!uo zp9zmN!y{30y@G$Aai1xn0w(EBN;%_nDIGCLaCOeJ1?V439*~^$Py|)qSSqx`{`d{!ZV| zgnydhktn%d!M{y^S4yswTnUdp={^%4X@*Cl;b$3I8<1BT;g_f`9M$Qz^MpawR-E%Y7z1 z(hQG8$@L2Uo#j4La^1wEhuvqwKh5w+lw7ai-^1=RCD%S3K2vht#H0EDtoJnGpJsR@O0HM%Z~i}*k}D-w!lRwsXTm?t@JN(gui)QK?lUFV zO+32LeI`88439*~^$Px7=sr_&-Nd73+-Jf+&G1N+T(98YGww4b*G)Wn>l6BZCj8S3 zk3`A!3jV$IiBfW<^vc`os2Ns? zlIs<09QAZ5xl(c^tlHLnCT!FUt3=853N~)*K2vht#H!D^&xBQ)VU;MkUctuCxzChb zH?itZ?lWPdW>_Uku2-<}Pwq1%*G;Ti`x$*d6E*lItcOz3wl1PZR!WhDW00dIkSp z_m@&~rQ}L@G}C=1Jkku0M9K9E{>^lsDY7NM9K9E{%!VLDY;T| zB|JLaeI`88439*~^$Px-?mkm;-Nd7Z+-Jf+&G1N+T(98YL+&#r*G)WH?XP-I6aHz2 zN226<1^-t2Ybm)>awR-E(tRd8(hQG8$@L2U9qB$(a^1wEAGyzjf12TuD7jw2zaP2J zlw3FQXsPG*o+kX$439*~^$Pwi^?WJ0QgS6cn(96i{%M9sqU3r7|E9Xnlw3FQ=vw!g z@JKT}5+&Cw_;;=QOv!Z}N1Zfy)(i8E z%#M8bwo86-aD-lb?DPfKn}3A(r_GPLtu*2BWmh&J``|2tcMy|Tn# z7ff1vY-VpZSA?SAw9QCB@%TxQLM<{E$8_>t)YLp#iM z{UQsG;K0#m&vorZ#ocbXf3B6^yvWFOqs^9^`_AtdA6Q}kxmVe7(UIwm7hgH|=-(B$ z+vEAUw?5#NBhzuGZ!phkFBFd&b=*9cAHUehbl_7znCH)zmMMi%;LHK+GxJ>Ui<1!AN=rq*L<~j?((G_aqWZ zg^EZbq3lA7|NVP9^K?(=e_iJ|b6u|7=Y5~|8K3XmbPk=i*igR{f$4wgCYVX1E6L+0Be$#*bgx*-k(9wa72L|54zr6E5j<37#;N(eTDj z4xJjEn0bY@bQH3{D*yPnA=9y&uBh= z^IL2=xc6S;_WwT|pUuZN8W?yhhyU{}@)IA+CwxOc?YH{!76SuA_^$u>X>T}f%fSh2 z?X}m$ag)an4pr|mWy)TI<0k)ao@Ll9L%$q18yJ}V|8sb&>EotPoHF_U@F24f%P%%= z@?-OXfjRh>`T5PS&9BUlPMJJ?>Xg0K88?008+w&Fh7BytwSWEi#K6GY_`Ub?+tB$< zChj}9&BVzQrtCLnnQ8n1>@#i5Cfh8$?&NX1>@_&y4ZhCXA}zlX=j8a%Z<~$fr#yLZ z{Lr%u{VLsK+LXy3;9n0MJ#GB%gOkQZynn9@c{}<)rQ|0+n#joZ#LI8 z8@%VO-yXl(zdA*sEC}s_#ppdekINLQ}~b;n@rhl({YpetHe}(9ql@0>ZEbg`8773)K1fP+GpD6 zgBPA-*iDPgKJ?Qa`mylvVZYmFNag%kc*IOJawZxz6D2p}m(0I%rQ}M@N0MOIZDZuk}C}#w&u?6 zGmV^SHfknHu2)73%}U9Yk}C}#w(}+KGmV^SHfknHu2)73%}U9Yk}C}#cG%zDXBs)v zY}8DYT(686nw63(C080g?Cgz({%k$t`hGwPYO39Um54-7f_nAh{ zG#fP&CD$t>hGwPYO39V*=yCU%@JKT}5+&Cwc>B2fOv!ZcO39UyE8)>L?la+^W_Tn@ zu2=AH8~2%#>n0wZ?>-YAX@*Clt=jE6aHz2N226<1^?C_SxT;yTnUd(a-Rv0G{YlNa=n6oC%Mm*TsQIPA@`Z^ zPcu9cCD$wX_mKNc$#oNtmKyr^uruD%gnydhktn%d!M~+Om69tZSHhzM+-Jfg&G1N+ zT(98Y0q!#;*G)XS-F+tf(+rP9$@L2U-R?e9a^1wEd58Y}`i%E9;h$!BBucJV@NeF^ zO39UyE8)?g`%L(!86Js}>lOSPbe}1?ZsO5p?la+$W_Tn@u2=ByGWVI1>n0w}GW5Tp z%y>@|{%M9sqU3r7|7MxHlw2vf5*}^tJ`?_FhDW00dIkSBcb_S_ZsO4y?la+$W_Tn@ zu2=By4ELFm>n0vO7NM9K9E{@v$3Q*zzJqeb4K_cY<3 zW_Tn@u2=AHk$04mDKh5w+lw7ai-}dh;C09zW zghv;-&xA*s;gKl0UctYM+-FLzn|So9`%L(!86Js}>lOTa)qSSqx`{{Y&!g{W!avRM zNR(W!;NSZ5l#(kYSHh!H-Dkoh&G1N+T(98YsqQl+*G)Wn)O{xW(+rP9$@L2UJ?cJF za^1wEWrzM2M}FXg3I8<1BT;g_l3XddQgS6cI@En8Jkku0M9K9E{vA4RDY;T|B|N&z zeI`88439*~^$PyohW@tBjQ2F*pJsR@O0HM%Z-Mzr$(52T;n5!MGvS|R zcqB@$SMYBS_nDIGCLUeoJ`)~khDW00dIkTka-S)=ZsO5wLw~<%#(SFZPcu9cCD$wX zH`}{P$(52T;n7(4neb0DJQ5|>EBH6oeWv8PiAU$S&xA*s;gKl0UctX}+-FLzn|SoB z`%HMG86Js}>lOTa)_tbrx`{`t5B-g|8Q;%@f12TuD7jw2zt!h2C09zWghwa1&xA*s z;gKl0UctW;+-FLzn|Snq`%L(!86Js}>lOTazn0xE>OK?xX@*ClR{K2vht#G@zNXTm?t@JN(gui)Pk?lUFVO*~rhJ$g?Q z{%M9sqU3r7|5kiYDY;T|B|JLHeI`88439*~^$PwSrhxl(c^Jjy?JGo#Ohf12TuD7jw2zsU=g zk}D-w!lUc`$7jML&G1N+T(98Y_3kqz*G)Wn+rs~PP5x;EJQCrb2#-X`^-6N3EBLqV!lmR&$(8Wv0{5BlNHaVVCD$wXcY*s%$#oNtUUHua|1`rR zQF6V4e=oVulw3FQ=%Yjbq*Q+J{Y?0$86Js}>y_k6$(52T;n5e|XTl@R@JN(gui)Po z-djqplw1jq9(JDzk2J$0QF6V4e-FFQlw3FQXqg%RG}n+1Cj8S3k3`A!N^+&-O39V* z=pgr*@JKT}5+&Cw_;=9gQgWr_N_cdK`%HMG86Js}>lOUF!+oaYx`{{g&G?6zhThYJ zf12TuD7juqu9RFUxe^}j>OK?xX@*Cl)-On9Uj9*L6c75w|Y z`%KAo6OZ1sh zEBLqS`%B4{k}KiSaqctWk!E-#O0HM%?>P6FlItcO-S0jV9%+V0qU3r7|L%96DYo6Agnydh zktn%d!M~f`XG*S{cr7NM9K9E{%x>$DY;T|B|Q3)`%HMG86Js}>lOU_lKV`_brX;N;64-nX@*ClEBLqE5~buy$(8WvaQB(;NHaVVCD$wXcewjZ$#oNt ze(F9G{%M9sqU3r7|9n0vO z?>-a$X@*ClEBLqOQl;cd$(8WvME9BSNHaVV zCD$wXccS}D$#oNt9(11x|1`rRQF6V4e-FCPlw3FQXvwAZo+kX$439*~^$PwixpXPH zQgS6c`n3B@c%&H~iIVFT{QI=~Ov!Zs z-Dkoh&G1N+T(98YSKVhyuA6xDC-<4~Pcu9cCD$wX_b2z6lItcOeR$|U)|Vgr_11)c zn&FWsxn4=Glw2vf5*{7xJ`)~khDW00dIkTE{$MG&QgS6c`lb6!_@@~jiIVFT{QIT* zOv!ZjO!%i69*L6c z75w|8`%KAo6OX>>J`)~khDW00dIkT!={{3(-Nd7p-Dkoh&G1N+T(98Y%kDEJ*G)WH z_e1)ACj8S3k3`A!3jVG8p;B_CO0JuDG-i3drwRWw!y{30y@G#ZmMahTq(H{9_{8n z6aHz2N226<1^;$)pDDR+;?WiEGvSeDcqB@$SMcu&_nDIGCLXn0xk z#eF9H(+rP9$@L2U{l$HzEBLpM`%KAo6OV3jp9%jo!y{30y@G$YxX+YaH}Pn$mGzz` z{L>7NM9K9E{>`;=DY;T|B|O^6eJ1?V439*~^$Py&EBN;__nDIG zCLX=#BYIC0{%M9sqU3r7|K9VFQgWr_N_e!F`%L(!86Js}>lOUl%YCNgx`{{ExX*+~ zn&FWsxn9A)Yusl_uA6u?`)Ybm6aHz2N226<1^;GWt(06Txe^|2<31DqX@*ClzUJlItcOz2H6*{%M9sqU3r7|6XvPDYHsO!%i69*L6c z75w|H`%KAobNH}_Kd^@0(=>9X*{GQ)xn3DDG%F=nO0G0~*z5bd&opwT*{GQ)xn3DD zG%F=nO0G0~*h06v&opwT*{GQ)xn3DDG%F=nO0G0~*c$V!srNLEoM|>{CQ7bXMhwkL z$(52T4Ij4C1oxRn&NLe}6D8LxBZg+B{CQ7bXMhwkL$(52T z4Ig&az*>4w)5w`-qh_MydS%4Wtdv|Sxzg}qH*V%W)5w`-qh_MydS%4Wtdv|Sxe^|I z&3z_3(hQG8$@L2Uea(HQ9N1EZ0D7jw2zbD;iO0JuDwDQ{eekMH9439*~ z^$Py2yml$MQgS6cI>vn_Jkku0M9K9E{vG2!Q*zzJqkG+F!XwS_NR(W!;NQLOGbPte zJbK?p^`0g?(hQG8$@L2Uz3-!?*zgA_@@~jiIVFT{2RVbDY;T|B|O^BeJ1?V439*~^$PxN z=RQ+%-Nd7d+-Jfg&G1N+T(98YMeZ{t*G)Wn#eF9H(+rP9$@L2Uz2ZJoa^1wE_14w* zGvS|RcqB@$SMYDWbxX;Wk}KiSDeg1jk!E-#O0HM%?-ci$lI!N&!_NBsy5oNT#0$HQ zoNeg;TP>tNS`#*EhE<~EdJ`Le?{-si-NdR7uBSc|Hfn}dqU3r78$Y;SDY;T|C9FEc zeI~5Z468)R^$Io~;yzPy-NdRp-Dko^&9F+8T(4l`o$fOw*G;TiV12!(2^%%TDp7L1 zf{hETUrMf&TnVcty3d4-nqieFxn9A>iS9Ec*G;Ut(tReZ(hRFa$@L00Ug^>9zX@*Cl7NM9K9E{{6#!rsTSbM;mXV?`Oh4&G1N+ zT(98Y#+#IqDTsQG(j?ML+Cj8S3k3`A!3jWQpc`3P4awR)k2J$0QF6V4f9Jc;lw3FQ=tcLL@J};55+&Cw z`1hjwOv!Zn0w}yQSXKgnydhktn%d!M}O8 zEG1V;u7pQ}?la+^W_Tn@u2=AH(0!)lx`{`ZxzB`0n&FWsxn9A)%iL#5uA6u?%T{_% z6aHz2N226<1^;H*s+3$Qxe^|2;XV`oX@*Cl;k!E-#O0HM% z?@afZlItcOJ?%ad{%M9sqU3r7|DJZADYlOT4Wo#+AQgS6c zI@WzAJkku0M9K9E{vGQ+Q*zzJqx;-v!avRMNR(W!;NN}jGbPteJX+-AdQTJnX@*Cl zEBJS_`%KAo z6OTr2t@kwHpJsR@O0HM%Z{*gcEBLpA`%KAo6OX>-J`)~k zhDW00dIkT!OK=5X@*CllOSv*L|ksx`{{6xzB`un&FWsxn9A)=iFyX zuA6wY#&-ICCj8S3k3`A!3jVFJT`9RzawRCho+kX$439*~^$Pwiv3)7IQgS6c+RuF^Jkku0M9K9E z{_W>JQ*zzJqg&l)!avRMNR(W!;NPw8GbPteJbK3tdQTJnX@*CllOUFzlOT4W_&5RQgS6cI>>z{Jkku0M9K9E{vG5#Q*zzJqdVMZ!avRMNR(W!;NKnY zGbPteJeqHU-qVDCn&FWsxn9A)`6iT-DOD>Prx_lJlIs=xd-Gr^xl(c^Jo=dXO!%i69*L6c z75w{{`%KAo6OYbvp9zmN!y{30y@G#dxzChbH}UAt?la+^W_Tn@u2=By&+aoN*G)WH zbys~q6aHz2N226<1^-svwUk^bxe^{7=ROl2X@*Cl=+fO0JuDwCHYnPZR!WhDW00dIkR$-K~^dDY+6JO?RIOk2J$0QF6V4f79J( zO0JuD^aJ;q@J};55+&Cw`1b?%nUd=!9*x>v?`gt6&G1N+T(98YsNGA+m69vr(T?sj z;h$!BBucJV@NY-=nUd=!9$oA{6CP=XN226<1^+H~pDDR+;?ZmFGvS|RcqB@$SMcvO z_nDIGCLV1#QQyymf12TuD7jw2zYQmrk}D-w!lTpNXTl@R@JN(gui)Ql?lUFVO+0$c zeJ1?V439*~^$Pwy<~~z$-Nd60?VFd($#oNt7Ti6%*Kh5w+lw7ai-=6L>CD%lOSPwpS^+QgS6c+S+|4{L>7NM9K9E{%!3(Q*zzJqi?v+gh!g;ktn%d!M|^~ z&y-v@@#uN?neb0DJQ5|>EBN=k`%KAo6OY!Mr0-|KKh5w+lw7ai-lOUl-+iX!x`{`(xzB`un&FWsxn9A)+uUbLuA6xD z&MA6N6aHz2N226<1^?bTrIcJLxe^|Ycb^IWG{YlNa=n6olOU_ zrTa|DbrX+9Pt|*x@J};55+&Cw_&0iLDY;T|B|O^OeJ1?V439*~^$Py&?LJd--Nd6C z+-Jfg&G1N+T(98Y4em1~*G)VcK27gw!avRMNR(W!;NS3RrQ}MA5brX*+be{>2G{YlNa=n6o7rM`sTsQIPukJJ9pJsR@O0HM%@2~DNCD%p9%jo!y{30y@G%1PA?@_O0I-QC%eytN1EZ0D7jw2zmwf(O0JuD^oaXR_@@~j ziIVFT{CmWGrsTSbM`QNUdz$c1GdvO{*DLrpW}i}WrQ}L@bcp**c%&H~iIVFT{5!;b zrsTSbM|Zln0vO z<31DqX@*ClVK2vht#G|EBLpw`%KAo6OS%&p9zmN!y{30y@G$2 zxX+YaH}UB2?la+^W_Tn@u2=By@9r}t*G)Xy=sS(}aJT;gKl0 zUctZR4=N>BO0I-QN4U>~N1EZ0D7jw2za!jdO0JuDbdUQ?_@@~jiIVFT{JY0}rsTSb zM++US_cY<3W_Tn@u2=AHp@U1wm69vr(Ioeo@J};55+&Cw_&3RYrsTSbN7uT~gh!g; zktn%d!M|(WXG*S{cr^PVdQTJnX@*Cl2j zpDDR+;?a5TGvSeDcqB@$SMcvV_nDIGCLX=uJ`?_FhDW00dIkSpaGxo;ZsO6}hwA&8 z@J};55+&Cw__y|(hQG8$@L2U9pFAwa^1wE z+udivKh5w+lw7ai-|g-*CD%EBN=6`%KAo6OUFtQs2*nf12TuD7jw2zm<lOUF*L|ksx`{`N9HsX(;h$!BBucJV@Nbc$O39UyE8)>p z_nGiWGdvO{*DLrp)qSSqx`{_OxzB`un&FWsxn9A)o7`tguA6u?;%L363I8<1BT;g_ zf`229E+tn=u7pS1yU&Dwn&FWsxn9A)?cHZeuA6vtk^4+|q!}KGlIs=xyU2Z}lOT4?=z+3O39V*=v4Qa@JKT} z5+&Cw_;;%NOv!ZlOSP>poL*-Nd7F z+-Jfg&G1N+T(98YIqowh*G)Wn)_o@Y(+rP9$@L2UJ?lPGa^1wE)jzB6XTm?t@JN(g zui)S6pDiUNO_38myp$(68b zocm1Js2Ns?lIs<09Oph$a^1wL@3_x|RhnUyD7jw2#_zb#lw3El>L2biVWVbPB}%SW zu<;-6GbPtetlIc<`hF&C)C{Xc$@L00Zv44Ya;4--c=ToWnea$6JQ5|>EBNlOSv%6+Efx`{_Wcb^IWG{YlNa=n6oKX;!gxo+ao!k^cBn($9EJQ5|>EBLqY z=S#_zk}KiSWcQiyPcu9cCD$wXH`#rr`?la+$W_Tn@u2=ByIQN;7>n0xE?>-a$X@*Cl z-(ATPcu9cCD$wXx54S9lOU_gZoU$brX-4`?B8Cgnydhktn%d!N29cTuQE#TnUd3bDs&1G{YlN za=n6ohq=#`TsQIPZugn+Pcu9cCD$wXcendY$#oNt7W|6d(}aJT;gKl0UctWwzfwxB zlw1jq_H>^K|1`rRQF6V4e|x&mlw3FQ=xXo%O!%i69*L6c75w|S`%KAo6OYbyp9zmN!y{30y@G$| zy3dqcH}U8>_nGidGdvO{*DLtn0v;dX~PQ3I8<1BT;g_f`6Ny zRZ6auTnUf9>OK=5X@*CllOU_@Y$v0O39V*=xFzu@JKT}5+&Cw_;TK2vht#G^US(R-TkPcu9cCD$wXH|IH}lOU_ru$6EbrX+XcAp9VG{YlNa=n6o zFT2l_TsQG(opbg5O!%i69*L6c75rQ0+){F-otS9%+V0qU3r7{~mFlDYlOSP^Nmt+rQ}L@bg=tOc%&H~ ziIVFT{5#lvrsTSbM?Z0&3I8<1BT;g_f`318pDDR+;?cX#(|elmPcu9cCD$wX_pb9w z$(52T;n8mHGvS|RcqB@$SMYB)_nDIGCLUexJ`)~khDW00dIkS3cb_S_ZsO6K&)0jJ z@J};55+&Cw`1j`XOUadzE8)?W?la+^W_Tn@u2=AHOZSn0wZ?LHG8X@*CllOU_$OWb3O39V* z=(FxK;gM!|BucJV@b9zkGbPteJo=UUO!%i69*L6c75w{^`%KAo6OR`Crry(pf12Tu zD7jw2zeT@UO0JY#36G|`&xA*s;gKl0UctZV?lUFVO+32AeJ1?V439*~^$Pyo;yzPy z-Nd80F4TLP@J};55+&Cw_&3*urQ}MlOUl z@S;+3rQ}L@bej82c%&H~iIVFT{5#EkrsTSbM~}JBgnydhktn%d!N14cXG*S{c(nYt z^qwaC(+rP9$@L2UE&r`ja;4--cyxsOOn9Uj9*L6c75qEGeWv8PiAO(kp9%jo!y{30 zy@G!~bDt@>ZsO5z?la+^W_Tn@u2=AHFZY>} z>n0vu<31A}X@*ClN1EZ0D7jw2zw_K@O0JuD^n&|L_@@~jiIVFT{CmND zrsTSbM{8Z8?`Oh4&G1N+T(98YT9=fPD7NM9K9E{(ay(rQ}M|gKh5w+lw7ai-)8PJCD%HC@RPcu9cCD$wXx6=1Y$(52T;n6Yf zGvSeDcqB@$SMcu`_nDIGCLZ1EJ`?_FhDW00dIkUPb)PA@ZsO7VF4KFO@J};55+&Cw z`1iicO39UyE8)?n+-Jfg&G1N+T(98Yr`%^suA6vtqx($wrx_lJlIs=xyU~57s?+- zu9RE}k4|x)36C_xBT;g_f`6yD&y-v@@#y#NGvS|RcqB@$SMcxm?lUFVO+5PG6?#t- z{%M9sqU3r7|2}v{DY;T|B|JLBeI`88439*~^$PwS;yzPy-Nd6i-Dko-&G1N+T(98Y zo$fOw*G)W{|4O~53I8<1BT;g_f`9X0SxT;yTnUdRy3d4vn&FWsxn9A)iS9Ec*G)XS z(tRd8(hQG8$@L2UUFkkka^1wES+CN2n($9EJQ5|>EBH6-Ri)%g$(8VEEBBf3Pcu9c zCD$wXx0U-$$#oNtzV1E~9%+V0qU3r7|Gw@%Q*zzJqi5V_!avRMNR(W!;NLUuGbPte zJX-B)eLoZaX@*Cl^>9zX@*Clg{L>7NM9K9E z{{7v3rsTSbM;l+K?`Oh4&G1N+T(98Y#@CgSDTsQG(jvMu! zCj8S3k3`A!3jWP;V=1{(awR)k2J$0QF6V4 zf9Jc;lw3FQ=tcLL@J};55+&Cw`1hjwOv!Z;k!E-#O0HM%?@afZlItcOJ?%ad{%M9sqU3r7|DJZADYRpDaK2vht#G^%S)q9%oPcu9cCD$wXx5%xf5SnGvS|RcqB@$SMYEB+e*omk}KiSsqQo3k!E-#O0HM%?^O4hlItcOJ?cIa z{%M9sqU3r7{~mRpDYlOT4_V!Y8rQ}L@beQ{0c%&H~iIVFT z{5#BjrsTSbM|Zo=gnydhktn%d!N0rRXG*S{c=YZc>pe~Qrx_lJlIs=xd-sn^$(52T z;n5!MGvS|RcqB@$SMYBS_nDIGCLUeoJ`)~khDW00dIkTka-S)=ZsO5wcj!G$_@@~j ziIVFT{G08LQgWr_N_aHZeJ1?V439*~^$Pxtb)PA@ZsO57?la+$W_Tn@u2=By9QT=$ z>n0vO=ROnuX@*CldK2vht#G|+0 zrS~-9pJsR@O0HM%@9lS$k}D-w!lPZ>XTm?t@JN(gui)PlOU_uKP^MbrX;N={^(wX@*Clqba^1wEP43qBGvS|RcqB@$SMYC>yGzNH zk}KiSSKMd9BhBzglw7ai-&fpcO0JuD^o09N_@@~jiIVFT{CmQErsTSbM=SnR?`gt6 z&G1N+T(98Yia#wSS4ysgM@PBOgh!g;ktn%d!M~&2XG*S{c=QYRneb0DJQ5|>EBN;d z_nDIGCLX=_XL?T){%M9sqU3r7|K9tvQgWr_N_aHIeJ1?V439*~^$Pw?ai1xn0w(?H;|S3I8<1BT;g_f`4zjr<7bNxzgOjZrt{saWC}0 ztprlL2^%%TDiJnr`|mz%6eZWb_Uku2-<} zi|#We*G;VYo%>AKs2Ns?lIs<0{GIzu$#oN}midL=(}azhVU;MkUcts?eo;!Ulw1j` z4sxFft2DzZQF6V4jR(2Ulw3FQ=nnUp@JKT}5+&Cw_;-ieSu9RE}k9KvR3I8<1BT;g_f`7ZZ&y-v@@#u2*nea$6JQ5|>EBJT0`%KAo z6OZ0}uin#yf12TuD7jw2zc=4oO0JY#36DPJJ`?_FhDW00dIkSJ<~~z$-Nd7_+-Jfg z&G1N+T(98YS?)6>*G)Y7v-?c=rx_lJlIs=x`?LE@$#oNtR=rQ(&xC)P;gKl0UctXr z?<*x&O0I-Q$GOjhN1EZ0D7jw2zvJ9zO0JuDbieyd_@@~jiIVFT{JYlOTa&3&fix`{^{{9517gnydh zktn%d!M_cDT}rN$TnUd(bDs&1G{YlNa=n6or@7CRTsQIPG54A9Pcu9cCD$wX_n7-k z$#oNtKJ**CrwRWw!y{30y@G!q`b{ahQgS6cI^2CGJkku0M9K9E{vGZlOU_hWkv(brX-Ccb^IWG{YlN za=n6o&%4i*TsQG(&EM+#neb0DJQ5|>EBLqOZ%fIQk}KiSiS9Guk!E-#O0HM%??m^R zlItcOJ?K6Y{%M9sqU3r7{~mOoDYlOT4@}W|4rQ}L@w7>gI zc%&H~iIVFT{M+AsrsTSbN4L4pgnydhktn%d!N1$wXG*S{c=XPP^`0jD(+rP9$@L2U zz4PHxa;4--cr@O9Cj8S3k3`A!3jU3EpDDR+;?bqEBJSV z`%KAo6OZP6RPSlRKh5w+lw7ai-<*$@k}D-w!lUinXTm?t@JN(gui)Qy?lUFVO+32L zeI`88439*~^$Px7=sr_&-Nd86y3d4vn&FWsxn9A)zq-$qTsQG(-9PC2neb0DJQ5|> zEBLqWA4otS{%M9sqU3r7{~mFlDYn0xk#eF9H(+rP9$@L2U z{l$HzEBJSd`%KAo6OZP4QtxTPKh5w+lw7ai-&{|Y zk}D-w!lRwsXTm?t@JN(gui)QK?lUFVO+329eI`88439*~^$Px7;yzPy-Nd86yU&Dw zn&FWsxn9A)zq`+rTsQG(qo?%!O!%i69*L6c75v-isZw&KOXG*S{c(l-;^`0jD(+rP9 z$@L2UE%fJ7a;4--cr?jn0w(;64-nX@*ClbFneb0DJQ5|>EBLq8 zGo|E8$(8WvB=?!{NHaVVCD$wXcar-|$#oNt9&(=v|1`rRQF6V4e-F9Olw3FQXsKuQ zo+kX$439*~^$Pwi^=v7*QgS6cI>3D=^W5nFDqI+=Cb->%jhbPV zD7oIm#tCjWCD%=?`kwntSfv?OiIVFTZ2X@4Ov!Z_4KJ`+}HhE<~EdIcMg zai1xn0w(??t_*3I8<1BT;g_f`9LOv6NgXxe^{t zb)N~3G{YlNa=n6oQ{87uuA6vtllx5grx_lJlIs=xyUBf~ zEBH6!rBZUGwO0JuD z^osjT_@@~jiIVFT{CmZHrsTSbN9(<;?`Oh4&G1N+T(98YdM}rfDUeSA+ z@J};55+&Cw__x3-rQ}MlOT)=sr_&-Nd6S-Dkoh&G1N+T(98Y zmF_bo*G)W{^;Ny63I8<1BT;g_f`7BVT1u{zTnUfHy3d4vn&FWsxn9A)vF`3(R7 literal 0 HcmV?d00001 diff --git a/rio/tests/logs/FRC_20240120_210521.wpilog b/rio/tests/logs/FRC_20240120_210521.wpilog new file mode 100644 index 0000000000000000000000000000000000000000..9a8183a3f262a47904b2397dbc0b2a5e119a85e2 GIT binary patch literal 90721 zcmbWA3E1Fsy}sYEzGN0H63H}KT4W;0k}O%0H3mtRFrmy|?Oq9j5| zNrVa|kwnsJO(voa^*gs&+~cs{^mx}X@{*h-F&M}21X6!$Fc+Y zwLxI|(Cpch_ZphL5WhZq&aA03_IhDulYxQx__f9PWy?uxP26~kZ8x2<=gf&K&pBwn zp?~jkXyA|u`^}oU-_Wc%Q-@|xSnH5OSDG+;=G<9Rh9;~vVarJq4jq_p;er234-9NK zFfc!n;h3*(GI{#Yw)+iDnY!oHDOvy;h&CM4h$^v|2e$voXK;h&Ybaoc#uT~^OKF6 z{Mc$>U{U^MDSq*5^E2~ZGiS`1HFMgAljlr+QLnP-;J|pUz3}6G0|PJN*Ivsn!{;}j zdce>QQ)lck^T3HK&*pbv?(B)1?=XI&8IyOPHnhi!e4Up>T7D)j#_{1_ww%Oo@{FM= z!_PAOvvi-?GiSVse?4Tv>?wN>O`ja`{=G8n?S%i7lHd3VM23Hi8`x(2mII^twO8}Y z@NfOL(!9BjlpVxcf82ok8$&KY|Zx={sZsu zPuST9&7LzfebUtF|5o-b{Ga*OBY%(`{)xN!%)PdlJcB=u&*Dq(Idj(Z$#eKwFo)Ex zb9S9Od%~gP26p7=OG&@2E;4+1_{aFsgTIINMalIFKX+fcXeqf;a;4FO>+R+~)7W`t z`-uBY$#oNt-ZuOvrjhqF;gM!|BucJV@b7Jl zm69tZSHh#C-Dkoh&G1N+T(98Y(e5)P*G)XS(|sm9(hQG8$@L2U-RVA4a^1wEWrzQK zJo26lOUF#(k#bx`{`N4F9#q$a|XbNHaVVCD$wXx5$`Ma;4--c(jB2O!%i69*L6c75v-5 zeWv8PiASGzp9zmN!y{30y@G$Acb_S_ZsO5X?la+^W_Tn@u2=ByDfgL@>n0v;F#Okb zBj3-2f12TuD7jw2zYWHgk}D-w!lRSjXTl@R@JN(gui)Rw?lUFVO+32aeJ1?V439*~ z^$Pyo?>7NM9K9E{%z$xQ*zzJqjTJ6!XwS_NR(W!;NLm! zGbPteJo>%+O!%i69*L6c75w|X`%KAo6OYz>nZBP1|1`rRQF6V4e`~(1lw2vf5*{7z zJ`)~khDW00dIkTEcb_S_ZsO71?la+^W_Tn@u2=ByZugmz>n0v8_j0|b3I8<1BT;g_ zf`7}syp&uixe^}Da-Rv0G{YlNa=n6ov)pG&uA6vtgZoVQrx_lJlIs=xyTN^?QgS6cy2O1Z zJkku0M9K9E{$1ieQ*zzJqrbV&gnydhktn%d!N0$`&y-v@@o1AJ^!-furx_lJlIs=x z+hmDSa;4--cyy}!On9Uj9*L6c75qEZeWv8PiAN8)&xC)P;gKl0UctYI+-FLzn|QR! z@ZaLd4}37;pJsR@O0HLuD7NM9K9E z{_W#FQ*zzJqi?v+gh!g;ktn%d!M|^~&y-v@@o1spzuz?So+kX$439*~^$Pwi^r}*F zrQ}L@G|7D?{L>7NM9K9E{!Mb9DY>*u9RE}k3Q-?6CP=XN226< z1^+(kK2vht#G_xh&xC)P;gKl0UctX#xX+YaH}U8VOY1#N_@@~jiIVFT{CmUFrQ}M< zmGJ05_nGiWGdvO{*DLsUp!-b8brX+na-RwRG{YlNa=n6oH@VN0TsQIPWy62(Ge3s^ z{3pUc5gv(>>y_k6$(52T;n8IGneb0DJQ5|>EBH70)urT0$(8Wv%kDGbk!E-#O0HM% z@5}BpCD%n0wJAO0sM@`FFVnD9?CJQ5|>E6J6TDBYh_P(+rP9$@L2U%~-aSTq(H{ z9(~Kde(hQG8$@L2UJ>Wi5a^1wEl}G+*u3;Zc_@@~jiIVG;lOUl^L3@n0v8zk=S= zgnydhktn%d!N28KC?!`)u7pRk-Dkoh&G1N+T(98YZ1n0w3+kGbd(+rP9$@L2U zecOGeEBH5V#Zq#mczOn9Uj9*L6c75w|O`%KAo6OVr5J`?_FhDW00dIkS}<33Yz z-Nd6+-=Oz2;h$!BBucJV@Nd;Ol#(kYSHhzs-Dkoh&G1N+T(98Yk?u1k*G)XS-F+tf z(+rP9$@L2U-R?e9a^1wEW!|XwG~u6ScqB@$SMYC{HEBNEBN=uHGbPteJbL-cdQTJnX@*Cl zEBJT0`%KAo z6OW#Ep9%jo!y{30y@G$wyU&zdH}PnTiTZvf{L>7NM9K9E{%tX_lw2vf5+0r9J`)~k zhDW00dIkT^a-S)=ZsO7J+-Jf+&G1N+T(98Y@7!lfuA6xD_Tm51V1DrDTND0ihDW00 zdL_A1a;4--cyz4$On9Uj9*L6c75qE)Ev4j2$(8Wvr|vW1pJsR@O0HM%@2BoFCD%6_9;h$!BBucJV@bCTZ zGbPteJoEBH5YwNi4WEBNlOSv-+iX!x`{`Ba-RwR zG{YlNa=n6oe{!EGxo+aoJKnDEXTm?t@JN(gui)Q1-d;+slw1jqKH@$T9%+V0qU3r7 z|32bAQ*zzJqo29Ygnydhktn%d!M~rm&y-v@@o2?0^qwaC(+rP9$@L2Ut++-hxl(c^ zJeuo16CP=XN226<1^?!{&y-v@@#sePneb0DJQ5|>EBJS#`%KAo6OR^OQ}1cQKh5w+ zlw7ai-{NbQk}D-w!lPZ?XTm?t@JN(gui)RV?lUFVO+5ON`%HMG86Js}>lOU_lKV`_ zbrX;N?miR#X@*ClFzV( zk!E-#O0HM%?{xQ>lItcOJ>otS{%M9sqU3r7{~mFlDYlOT4 zZS7KWrQ}L@bd>u{c%&H~iIVFT{5#5hrsTSbM|Zf-gnydhktn%d!M{7)XG*S{c=Vcg z=sivNrx_lJlIs=xd(Asa$(52T;n6hrneb0DJQ5|>EBH6feWv8PiAUdbp9zmN!y{30 zy@G$=be}1?ZsO4*>*zgA_@@~jiIVFT{99z5QgWr_N_ez``%L(!86Js}>lOUl!F{IW zx`{^@y3d41n&FWsxn9A)3*Bc*uA6xDr29%UXq&xC)P z;gKl0UctZh-&snolw1jqKJGpf9%+V0qU3r7|32?^~#vxSt+?va;4FO&mZJI)7W`t2XEtsgO0HMN49`l*m69us z9^7>g_nF4dGaEM#CD$ushG(VZO39T*4<7zi_nF4dGaEM#CD$ushG(VZO39T*51u!$ zzTVR`cAnX|c__JF88bX9C09zWG`?la+$W_Tn@u2=ByIQN;7>n0xE)xgJ zG~tnEcqB@$SMcw3?M z((kPa8#Ti!QF6VBjlXugDY9p*k0R%wP+ zqU3r78xM1zDYr8g-h zS4ysgRa4z(!bZ)oN|ao$VB=KxnUd=!R$b*j6IN-4Rifm21skt&pDDR+V%380)_a<; zQ8TO(CD$w1xZt}>$(52T;nBA4GvS|RcqB@$SMYCJ_nDIGCLVppeI`88439*~^$Pxd z#(k#bx`{`BcAp9VG{YlNa=n6oe|DcKxo+aoI-BbIneb0DJQ5|>EBLq0rlsUc$(8Wv zqwX`|k!E-#O0HM%@1yQBCD%7NM9K9E{@v?7Q*zzJqu0Mj?`gt6&G1N+ zT(98Y>)%sKu9RE}j}CC336C_xBT;g_f`13N&y-v@@#wqmGvS|RcqB@$SMcw9C09zWghw;nXTm?t@JN(gui)Pd_nDIG zCLUeuJ`)~khDW00dIkTkb)PA@ZsO6RTj@Pb_@@~jiIVFT{9AOZQgWr_N_g}>_nGid zGdvO{*DLtn0w3-hC!K(hQG8$@L2UecpYh zEBJSa`%KAo6OVr2J`?_FhDW00dIkS};677w-Nd6;Zlm`!;h$!BBucJV@b8t|l#(kY zSHhzq_nGidGdvO{*DLrp5ekT0W439*~^$Py2HK~+bDY+6J9q&F9 z9%+V0qU3r7|BiQ`DY-qVDCn&FWsxn9A) z<+dv&S4ysgN3+~#!XwS_NR(W!;NL9wnUd=!9(~(=Cj8S3k3`A!3jTfDeWv8PiAQ6% z*L#}qPcu9cCD$wXH+K6{a;4--c(k+oO!%i69*L6c75v-TeWv8PiAP^_p9zmN!y{30 zy@G#Vbe}1?ZsO73+-Jf+&G1N+T(98Y-`rLP>Jkku0M9K9E{vF{yQ*zzJqaV4?gnydhktn%d!M`85 z&y-v@@#xh%>OD>Prx_lJlIs=xd-aZ`EBLpM`%KAo6OX>( zJ`)~khDW00dIkT!;XYGx-Nd7X-mmvG;h$!BBucJV@Nc2_my#pl}6X@*CllOUF$$h5ex`{_G-&yZz!avRMNR(W!;NQ!4E+tn=u7pRs zyU&Dwn&FWsxn9A)-Q8zOuA6xD75AC&NHaVVCD$wX_Z9b7NM9K9E z{{72+rsTSbN1N}W?`Oh4&G1N+T(98Y=DU=VDEBJS;`%KAo6OVr4J`?_FhDW00dIkS};yzPy-Nd8U?xy!N;h$!BBucJV@b9&| zm69tZSHh#2?la+^W_Tn@u2=AHru$6EbrX-iEBN=4$))5<$(8VENB5cVPcu9cCD$wXx1;+^$#oNtE^?mn0v8xrg4xzB`un&FWsxn9A)z1(L?uA6xDHTRkD zNHaVVCD$wX_ciyKlItcO%|E2~G~u6ScqB@$SMYECp;B_CsPcu9cCD$wX z_g?pzlItcOo##Fi9%+V0qU3r7|ITxtDY7NM9K9E{;jZADY;T|B|MtrJ`)~khDW00dIkUHxX+Ya zH}U8@?la+^W_Tn@u2=ByJMJ?j*G)Vcx3}KYgnydhktn%d!M|~Pmy#7N zM9K9E{tZqmC09zWgh$)E&xC)P;gKl0UctZZ-DgU!n|Sm&_nGiWGdvO{*DLtn0vO;XV`oX@*Cln0wpvA^Eagnydh zktn%d!M`>3FC|w>u7pP)cAp84G{YlNa=n6oA9kN9xo+aoPu*w2Kh5w+lw7ai-%s6V zO0JuDG+~zB(}aJT;gKl0UctW!vr5U8k}KiSe(p2jpJsR@O0HM%Z$I~$lItcOUFSX% z9%+V0qU3r7|E_bNDYONC)-Nd7%4$ym=@J};55+&Cw__x#nrQ}MEBLp- zfu-b1$(8VE8~2&;Pcu9cCD$wXw~hNu$#oNt&Uc>)k2J$0QF6V4f9Jc;lw3FQ=+Evm z;h$!BBucJV@bAy=GbPteJX+_2`hF(-(+rP9$@L2Ut@FWBa;4--cyyxsOn9Uj9*L6c z75qEVeWv8PiAO(op9%jo!y{30y@G!~cb_S_ZsO632kAXc_@@~jiIVFT{9Ey$QgWr_ zN_aHaeI`88439*~^$PyYb)PA@ZsO66?la+^W_Tn@u2=ByM)#SL>n0v8ez4xtgnydh zktn%d!N0{1E+tn=u7pRsxzB`un&FWsxn9A)-P~tNuA6vtnfpw5q!}KGlIs=xyUcy2 zlOT)?mkm;-Nd77+-Jfg&G1N+ zT(98YHSRMd*G)WHlOUl;P6s%rQ}L@bh7(Qc%&H~iIVFT{5#owrsTSbNB6tWgnydhktn%d!N2?6 zXG*S{c(l?HdQTJnX@*CllOUF!hNRXx`{`lj?#OY@J};55+&Cw_&4gPQgWr_ zN_e!D`%L(!86Js}>lOUl%6+Efx`{{UxX*+~n&FWsxn9A)bKGZ2uA6xDd-s{}Pcu9c zCD$wX_j~u5lItcOt$DP*p9%jo!y{30y@G#h9$iYVlw1jqj&q+0k2J$0QF6V4f5*Ac zlw3FQ=q~q}@J};55+&Cw_;;84Ov!ZtaCDpoL*-Nd7fKdkR(!avRMNR(W!;NQj{E+tn=u7pRYy3d41n&FWs zxn9A)Q{87uuA6xDko!#drx_lJlIs=xd&qsJlOT4`uI|ErQ}L@G}V13{L>7NM9K9E{!MkCDYsPcu9cCD$wXcdz?Q$#oNt zUVozA(}aJT;gKl0UctZDpIAz+lw4`?!DSCT@xZt4GV;IX0jb@DRhnUy2pbRlcON#2 zl51abC9Jy1eI{(w468)R^$IrL^zZiMI!msERWJLfe*a9^s2Ns?lIs<0eA!1!$(52T zVbx^!nXpkatP&;HE7&;MeWv8PiB(^Ap9!lp!zxj7y@HKjcAqJ^ZerCx-Dko^&9F+8 zT(4l`Kiy|auA5l3*~j$#OxUOyR*9196>Qw>W2NLu$(69`4ELF^N;9kyCD$w1c!v8- z$#oNt9(A7yk2J$0QF6V4e~-G)lw3FQX!Voyo+dof439*~^$Py2eo`s9QgS6cI>vn_ zJkku0M9K9E{vG2!Q*zzJqaVA^gnydhktn%d!M`87&y-v@@o4EBJS<`%KAo6OR@> zS?_7WKh5w+lw7ai-=Zg%k}D-w!lNDCXTm?t@JN(gui)Q~?lUFVO+32DeI`88439*~ z^$Px7O0JuD^nm+J_@@~jiIVFT{CmKCrsTSbM{oY5-qVDC zn&FWsxn9A)H-EB}Tq(H{9v$L76CP=XN226<1^*6lpDDR+;?XVcGvS|RcqB@$SMcu^ z_nDIGCLS&MDZQr&|1`rRQF6V4e@lLn0w}cZ%NAgnydhktn%d!N2)VDJ54*u7pQhyU&Dwn&FWs zxn9A)t=(rzuA6vtuKP@Qq!}KGlIs=xJJ)@tpe~Qrx_lJlIs=xTmI9fEBN>B(@V*fk}KiSr`>15BhBzglw7ai->2PYO0JuD z^c(k?@J};55+&Cw`1c$4nUd=!9<6$Y-qVDCn&FWsxn9A)RnI6TS4ysgM@P8Ngh!g; zktn%d!M`KiXG*S{c=RLpneb0DJQ5|>EBN;#_nDIGCLS$wrry(pf12TuD7jw2zh%xW zC09zWgh%_j&xC)P;gKl0UctY8-DgU!n|O4!`%HMG86Js}>lOUF+I^O39UyE8)?0?la+^W_Tn@u2=AHJNKEA>n0w3)_o>C(hQG8$@L2U zeb#-ZsZekT0W439*~^$Py2dv+~(|Jkku0M9K9E{+;ALQ*zzJqx;-v!avRMNR(W!;NN}jGbPteJbL3fdQTJnX@*Cl z zq!}KGlIs=x`-=Na$#oNto_C)K|1`rRQF6V4f6u$mlw3FQXp8go{Y?0$86Js}>lOUl z;=EFFrQ}L@be8)}c%&H~iIVFT{5#8irsTSbN56BQ3I8<1BT;g_f`7krpDDR+;?djB z*L#}qPcu9cCD$wX_xAHk$(52T;nA_~GvSeDcqB@$SMcvx_nDIGCLaC7eJ1?V439*~ z^$Py|#C@jZx`{`x{mj6B{TPo&BK#BKktn%dNv@P!DY+6J?dLud{%M9sqU3r7|MvS# zDY;T|B|N&$eI`88439*~^$Px7=RQ+%-Nd8CF3@|L@J};55+&Cw__x>vrQ}MxzB`un&FWsxn9A)z1(L?uA6xDHTRkDNHaVVCD$wX_ciyKlItcO&Hs75 zrwRWw!y{30y@G%9f4-DlDY+6JZR0)@{%M9sqU3r7|F&_TDYUtCJAlw1jqc6FZ#|1`rRQF6V4f4jQRlw3FQ=u7T1 z;gM!|BucJV@b63RGbPteJo>x)O!%i69*L6c75w|V`%KAo6OT5%MBmSZf12TuD7jw2 zzfCVGC09zWgh!{j&xA*s;gKl0UctZ9+-FLzn|SoF`%L(!86Js}>lOTa*nOttx`{`t zeNpde!avRMNR(W!;NNOrEG1V;u7pQNxzB`0n&FWsxn9A)quggouA6vthx<(Urx_lJ zlIs=xyTg5^ErQ}L@w7vUG z_@@~jiIVFT{M+7rrsTSbM;E%!gh!g;ktn%d!M_XLXG*S{c=V+EO!%i69*L6c75sbB zeWv8PiAU>SrtfFMKh5w+lw7ai-};x8k}D-w!lRG7&xA*s;gKl0UctYQyU&zdH}U9~ z?la+^W_Tn@u2=Bym+mtq*G)Wn)0g$0Cj8S3k3`A!3jV$6%cbN>$(8WvAorQ@NHaVV zCD$wXcaZx`$#oNtzVAL0{%M9sqU3r7|Gw`&Q*zzJqb0tg_cY<3W_Tn@u2=AHiLaEB zD8pOn9Uj9*L6c75w|E`%KAo6ORTi*L#}q zPcu9cCD$wXH*k3=xl(c^JlfKICj8S3k3`A!3jS^BK2vht#G|v_XTl@R@JN(gui)R= z?lUFVO+0$ceJ1?V439*~^$Pwy<~~z$-Nd6czN+tM!avRMNR(W!;NKcwEhSe!svM$(8WvF!!18NHaVVCD$wXcbNN3$#oNtZgrmt|1`rRQF6V4 zf492Nlw3FQXsN68o+kX$439*~^$PwibyX?3QgS6cn(96i{%M9sqU3r7|E9Xnlw3FQ z=qmS_@JKT}5+&Cw_;;22Ov!ZlOU_v-?cR zbrX-)xmw@Pgnydhktn%d!M}B`E+tn=u7pP?y3d41n&FWsxn9A)6WwP@uA6vtulr2+ zrx_lJlIs=xyVrfDn0v;cCEgj3I8<1BT;g_f`6M`TS~5!TnUfPaGwc}G{YlNa=n6oXSmOl zTsQIPx9&6HpJsR@O0HM%@3-zVCD%EBJS<`%KAo6OR_X zUhiqbKh5w+lw7ai-=f!-k}D-w!lU=O&xC)P;gKl0UctZjxzChbH}UB6?la+$W_Tn@ zu2=By^X@Yx*G)Wn%6%sM(+rP9$@L2UJ>@=Aa^1wE4Q|l)GvS|RcqB@$SMYCx8%oKQ zk}KiS$?h}Zk!E-#O0HM%?_~FxlItcO-S0jV{%M9sqU3r7|L%96DYEBN;V z_nDIGCLX==J9QgWr_N_aHnJ`?_FhDW00dIkT6+-FLzn|O4E z`%HMG86Js}>lOUF!hNRXx`{`lZq$34@J};55+&Cw_&4gtQgWr_N_e!D`%L(!86Js} z>lOUl%6+Efx`{{Uy3d41n&FWsxn9A)bKPf3uA6xD2ltuqPcu9cCD$wX_XqcxlItcO zt@T}fKNJ3GhDW00dIkU1`fe$?QgS6cI^KOIJkku0M9K9E{vGc=Q*zzJqr2T_!avRM zNR(W!;NRWuGbPteJX-E1y{8HPG{YlNa=n6o%iUB;u9RE}k7l{ggh!g;ktn%d!M|DV zGbPteJi5VsCj8S3k3`A!3jW>TK2vht#G|p_(|elmPcu9cCD$wXH}-p_EBLpw`%KAo6OX>=J`)~khDW00dIkT!=sr_&-Nd86xzB`un&FWsxn9A) zzq!wpTsQG(lke;Mneb0DJQ5|>EBLp`_e;r@k}KiSsqQo3k!E-#O0HM%?^O4hlItcO zJ>)(U{%M9sqU3r7{~mImDYlOT4<>pdyrQ}L@bcFj%c%&H~ ziIVFT{5!&ZrsTSbM?Z3(3I8<1BT;g_f`319pDDR+;?b*rp!YQ4pJsR@O0HM%@6|sj zC09zWgh%_h&xC)P;gKl0UctY8+-FLzn|SmM_nGiWGdvO{*DLt<4fmOn>n0v8bc^28 zgnydhktn%d!M}xWDJ54*u7pRE+-Jf+&G1N+T(98YB=?z;>n0vu;64)`X@*CluOv!Zk}D-w!lMJ-XTl@R@JN(gui)Q-?lUFVO+32EeJ1?V439*~^$Pyo zq!}KGlIs=x`-=Na$#oNt{^dRs{%M9sqU3r7|NiAZQ*zzJqs@P$?`Oh4&G1N+T(98Y z=07SWS4ysgM`yavgh!g;ktn%d!M`)zXG*S{c=V|IO!%i69*L6c75sbDeWv8PiASs7 zuJ<(IpJsR@O0HM%Z}rn0w3%Y7z1(hQG8$@L2Uean5Ozc4pGprIN*DKh#@=r?1 zm69uA)rZ_?!Ya+MN|ao$VB?3}XG*S{Sapm0OxUOyR*9196>Pl4eWv8PiB(JfRPSlR zM$NEFlw7Z1n2vsf0y3VgpHbEl_zUJlItcO{n33U{L>7NM9K9E{{7K?rsTSbM{D1$?`Oh4&G1N+ zT(98Y+IN?dDWK2vht z#G@7N(R-TkPcu9cCD$wXx57Q87NM9K9E{*C)tDY;T|B|O^2eJ1?V439*~^$Py& z;yzPy-Nd6y-Dkoh&G1N+T(98YrS3B&*G)Wn&V45Q(+rP9$@L2UJ?B1Ca^1wEcmG`9 z&xC)P;gKl0UctY2|Gbo3DY+6Jo#s9h9%+V0qU3r7|4wtCDYn0uz-lz98;h$!BBucJV@Ne+G zQgWr_N_e!r`%L(!86Js}>lOUl-hHOzx`{`hbDs&1G{YlNa=n6opL3rnxo+ao6YewN zpJsR@O0HM%?+N#rlItcOt@lfPKNJ3GhDW00dIkU1`(-J)QgS6cI>~(|Jkku0M9K9E z{+;ALQ*zzJqx;-v!avRMNR(W!;NN}jGbPteJbL5(dQTJnX@*CllOT);yzPy-Nd8I-Dkoh&G1N+T(98YEBNlOUF&V8ojx`{`NJ*4+E;h$!BBucJV@Ncn)O39UyE8)>j?la+^W_Tn@u2=AHC-<3> z>n0vu>^>77X@*CllOUl=r^V0O39V*=u_@9;gM!|BucJV@b6RZGbPteJbKW5Cj8S3k3`A!3jRIl zK2vht#G{E1>pe~Qrx_lJlIs=xoA_`kxl(c^JUY~UCOpy%k3`A!3jQ7HK2vht#G_l? zXTm?t@JN(gui)RU?lUFVO*~ra5xu7g|1`rRQF6V4e@i`5O0JY#36J)6p9%jo!y{30 zy@G#xyU&zdH}UA}?la+$W_Tn@u2=By>+Ulp*G)WH;J12D6aHz2N226<1^*WKZ7I1@ zawR<4#(gII(+rP9$@L2UZR0*ua^1wE^WA5{BhBzglw7ai-}&w{CD%EBNlOUF(S4@mx`{`NKc@FI;h$!BBucJV z@Nen0vu<~|c1X@*Cl}Dzp!YQ4pJsR@O0HM%Z?!*^ zk}D-w!lR?yXTl@R@JN(gui)QN?lUFVO+328eJ1?V439*~^$Pyo;XYGx-Nd71|ETvg z;h$!BBucJV@Nd~amXa$aSHh#|?la+^W_Tn@u2=AHy8BGYbrX-Sai0l~G{YlNa=n6o z*SOD=TsQG(kw57@P57r79*L6c75rP|Po?Ba$(8VE2ltuqPcu9cCD$wXw}bmk$#oNt zE_9y>k2J$0QF6V4e;2yXlw3FQ=t=jP@J};55+&Cw`1hpyOv!Z;JiwTq(H{9-Zty6CP=XN226<1^-TVpDDR+;?e!?GvS|RcqB@$SMcwC_nDIG zCLXQy7rmzm|1`rRQF6V4e=Gf^lw2vf5*{7wJ`)~khDW00dIkRucAqJ^ZsO6+?la+^ zW_Tn@u2=ByX7`zr>n0v8@wnd8gnydhktn%d!M`OQFC|w>uC(aj4NE*e;lB#UW7QsR zH({e@SS3oXH?eUKx0{mdCRTmbeI~5Z468)R^$IqA)qSSqx`|bzp3v)>uu(It5+&Cw z*f{EmQgWr_N?5g(`%KuV8CHps>lJL=%6+Efx`|cixX*-DnqieFxn9A>bKGZ2uA5l( zd-s{JQ8TO(CD$w1_TK2vht#G^4!>pe~Q zrx_lJlIs=x8}oE2xl(c^Jo&N1EZ0D7jw2zr)>UO0JuDbesE3_@@~jiIVFT{JYJ4rsTSb zM@v7e_cY<3W_Tn@u2=AH>1RvHm69vr(Ny=D@J};55+&Cw_&3#krsTSbM_0Megh!g; zktn%d!N05AXG*S{c(mZ(^qwaC(+rP9$@L2UE%>)ma;4--cr?joYeYR|C?4UAuW@PS*VY;$dC;Jb_5_|`o~ zW{1CTs|T+rJ#g+R8?Cn2$c%s5e1$7Z>#e=~zC-(t%&xzEpZy;!@xR)13oXCofg{rgK7RQ^7o1sK_wHvF zTH#e69GPCf&pQ@A?6l&tmmj(CDoY(SGClmS-&=V8bBhnGv)JI=*BqSb_<;on_E~b& IV*DKZf2NRtR{#J2 literal 0 HcmV?d00001 diff --git a/rio/tests/logs/FRC_20240120_211051.wpilog b/rio/tests/logs/FRC_20240120_211051.wpilog new file mode 100644 index 0000000000000000000000000000000000000000..ede5524b5cbdf64bb9310d39fcd4e2a40f4f7bcb GIT binary patch literal 90721 zcmbWA3Hac1y|({j{gc@wYs?rzB@-b_)-2h@lq50CU@$Y986?SCh$IqHNJ&Xj5+X}d z60$^_EGdapmW1kf@BiccpYG{;ulF49T$d~N`Fx(wGrqsMwa{so9k<$chb;$24CKeS zf&AVeFn#Lm*^~F3I(q?rfA*YN(`M}Z^2(M21M~2EWBG0SN$XAAV!NHUnz7H!iL1>y z?4YUt-sRB1;S&y;HS?gUv*t{jI(x!;haa)(gxNC>p0)SX3F}SRe$s>^2Ig6C;J?!Y z1MeCbn3u><%vZOZJbmiU2Tk33+CJ0v-tOg}&@1a0Iy$h;z`%U`!)yNI_~wU9oiS&7 z8eZASp;H6f4h+oCKP>SdC%2nCXYc*CKY03HQ)dm0r_GqN_L~2jrvSpuGKoXK;h&7AT7@gNHg z<`)|``LV;mz{32)(){Mv=2zz1XU>>2YvzHQO`bFP6}`&Bg9GEa_VSPS3=F)A-&>yF zhR$z0?U1RvOq(%f=Ajc;o6R4k2&pEPayzkjK}ng26y zH2f#0p%1?8%zd|;JcB>~&EkdjnK^6v0s++W+?l*y$BM&e*v+cBNxcI#%GfeB}KO-e;c!r@s7&SDQX{&g3aP$i$te z9>}k(sZ(CjpDf2OJb1&(3k_W!`Z0dw;6n!wshl6__PvI~OI_E2D;HrQ}M_PvI~OI_E2D;HrQ}MEBJSf`%KAo6OR@e`qv)A?`gs#&G1N+T(98YLZeE_m69vr(Jt;Y z;h$!BBucJV@NXCQnUd=!9(~z;COpy%k3`A!3jTfBeWv8PiAT@6&xC)P;gKl0UctX- z-DgU!n|QS8(7&!5{(dI>(+rP9$@L2UZ92M?Tq(H{9-Znw6CP=XN226<1^-TUpDDR+ z;?V={GvS|RcqB@$SMcuv_nDIGCLXOi^e<_L-_wMDn&FWsxn9A)RmYT)DEBJSd`%KAo6OWb{`j_v+?`gt6&G1N+ zT(98Y5@Sorm69vr(Ny=D@J};55+&Cw_&3#krsTSbN0+MA1rwRWw!y{30y@G!u7A+-LO0I-QJGjq;f12TuD7jw2za89XO0JuD^m+H0 z@JKT}5+&Cw`1g7DnUd=!9zEec6aHz2N226<1^=FKpDDR+;?cUV*7q~vpJsR@O0HM% zZ{1gyk}D-w!lM)1XTl@R@JN(gui)Pa?lUFVO+32KeJ1?V439*~^$Pyo=RQ+%-Nd66 z7t?#1@J};55+&Cw__yL>rQ}MlOT)rhxl(c^JbIt|O!%i69*L6c75sbO zYf8zLk}KiS#qKlVk!E-#O0HM%?_&3vlItcOz2H6*{%M9sqU3r7|6XvPDYIB}O39V*=nVIn@JKT}5+&Cw_;-f;Ov!ZLr_+Y|6&G1N+T(2ZoO0JY#36G9+p9zmN!y{30y@G#7E>TLZ zlw1jqZg-yvk2J$0QF6V4f495Olw3FQXqlnEtTX(cCj8S3k3`A!3jQs#WGT5)awR<4 z-+d(hQG8$@L2UUEn@b za^1wEzq-$aN1EZ0D7jw2zrVWAlw3FQXv3ku@HYJYO!%i69*L6c75v+9=~8l~(QQBucJVk}D-wO0I-Qlig>+Kh5w+lw7ai-{jYpk}D-w!lQ4v z&xA*s;gKl0UctX_xX+YaH}U9S?la+^W_Tn@u2=ByU+yy{*G)Xyc3FKt6aHz2N226< z1^>2Pwv=2cxe^|I)_o>C(hQG8$@L2Ueb#-ZCj8S3k3`A! z3jWO)S4yswTnUf9=RZCZ9%+V0qU3r7|GwuwQ*zzJqgRc8`8D~w4e&^Wey_k6 z$(52T;n9cPXTl@R@JN(gui)Q@CzO&aC0D|uTis{EBhBzglw7ai->vR5CD%n0w}v!Z-A;gM!|BucJV@Nb?KOUadzE8)?O?la+^W_Tn@u2=AHNB5bM z>n0wZ=ROl2X@*ClEBN=0RZ7W~k}KiSXWVDPBhBzglw7ai-)G!sO0JuD^gH*N@J};5 z5+&Cw`1d>anUd=!97NM9K9E{w@24QgWr_N_cdD z`%L(!86Js}>lOSvzn0vO>^>9z zX@*Cl?DY+6J z?dv`h{%M9sqU3r7|MqpCDYm3I8<1BT;g_f`4y&Ybm)>awRlOU_hx<&)brX-aT2J54gnydhktn%d!N0B6DNG~u6S zcqB@$SMYDm^-IZ>k}KiSG43n0v; zvZ2183I8<1BT;g_f`6NASW2#xTnUdp={^%4X@*Cl}qv&PB=f%BZ1PDY;T|rICX#9p*mMn7L+S=c43# zWz^8Dlw2vf(#XMax46$VX0F-TxhT0_88tL3C09zWG;(mG#W&V_n#RmE8#@;z*DIrj zW~Jmx$(2S9?lHxErZIEP#?D2_^~$KBSt+?va;1@jM}EtFrZIEP#?D2_^~$KBSt+?v za;1@j=M8M4_cV=}Yc_T+O0HK%4b4i)m69us9K3#e_nF4bH5)q@CD$vXhGwPYO39V* z=yUEf;gM!|BucJV@b7c(GbPteJbK)HCOpy%k3`A!3jRIrK2vht#G`dL)%P>uk!E-# zO0HM%Z{1Bx$(52T;nDH#GvSeDcqB@$SMcw6_nDIGCLZ1EJ`)~khDW00dIkUPb)PA@ zZsO4jZ`XU8@JKT}5+&Cw__xB_OUadzE8)=x+-Jfg&G1N+T(98Y2i#{$uA6vtz57gf zq!}KGlIs=xyWV}KEBH5Zvr=-U4O>>_K8#Ti!QF6V4jnmv`O0Jt&b*1}ESfv?OiIVFTY`oHarsTSb zRr9|??`gtD&9F+8T(4l`{O>3wS4ysgRXe-SgpHbEl_nXpPT ztP&;HE7S=BhBzglw7ai-y!ZZCD%lOSv+kK|wx`{`BaGwePG{YlNa=n6oe{i2E zxo+aoTHEP8P57r79*L6c75rOkyHawclOT4c!yGQrQ}L@ z^d9$_@J};55+&Cw`1c<7nUd=!9(~z;COpy%k3`A!3jTfBeWv8PiAT@6&xC)P;gKl0 zUctX--DgU!n|QS8j{1Hk{L>7NM9K9E{%yKrDY;T|B|JLSeI`88439*~^$Px->ONC) z-Nd5@-Dko-&G1N+T(98YgYGjW*G)Wn(>wK^Cj8S3k3`A!3jV$6ou%YT$(8WvaQB(; zNHaVVCD$wXcewjZ$#oNte(XLI{%M9sqU3r7|9!{L>7NM9K9E{;fBulw2vf z5+0r4J`)~khDW00dIkSZaGxo;ZsO5>?la+^W_Tn@u2=ByKKGfD>n0wp_%6Mt3I8<1 zBT;g_f`2Q%tCUawR<4-+dBx$(52T;n64DXTl@R@JN(gui)P&+-FLzn|SnV z_nGidGdvO{*DLtn0w({(X8+6aHz2N226<1^-_EzEX0fO0JuDwAk)?PZR!WhDW00dIkR$+r5-r zDY+6J?d3ib{%M9sqU3r7|Mqg9DYlOU_ ztouyKbrX*sbDs(SG{YlNa=n6okGap3TsQG(?LG9KCj8S3k3`A!3jVFVM=7~dawR6X&$#oNt?s1<9|1`rRQF6V4fA_f0lw3FQX!$+$o+kX$439*~ z^$Pwizh^1AQgS6cn(00h{%M9sqU3r7|7N<+lw3FQ=zH!n;gM!|BucJV@b7!>GbPte zJbKk+y{8HPG{YlNa=n6oubNy+u9RE}k9KvR3I8<1BT;g_f`7ZZ&y-v@@#rG=nea$6 zJQ5|>EBJSj`%KAo6OW#Ap9%jo!y{30y@G$wxzChbH}Pn*z4ZM|_@@~jiIVFT{M&4= zQgWr_N_cdd`%HMG86Js}>lOSv&3&fix`{^*xzB`un&FWsxn9A)humjMuA6wY+TMCk z6aHz2N226<1^-styOdlhxe^|I*nK8E(hQG8$@L2Ueb{}b=-pDDR+;?a}tGvS|RcqB@$SMcvi_nDIG zCLXQ7kG`J?|1`rRQF6V4f9vm4O0JY#36D;6p9zmN!y{30y@G!yy3dqcH}U8f?la+^ zW_Tn@u2=By7w$79*G)WHX9}$(8Wv z{q8g2pJsR@O0HM%@BQvGCD%EBN<&_nDIGCLX;ZaD6aHz2N226<1^<>k zpp;xGxe^{7;64-nX@*ClSxT;yTnUf%cAp9VG{YlNa=n6od%Mq+TsQIPo9;8= zk!E-#O0HM%@0;#3CD%7NM9K9E{=MWrQ*zzJqwNmT_cP(2W_Tn@u2=AH zyMs!}m69vr(dXP}!XwS_NR(W!;NR!mXG*S{c=WjYO!%i69*L6c75sbLeWv8PiAU>v zK<{b7Kh5w+lw7ai-#Q;CC09zWghwB9p9zmN!y{30y@G!qbDt@>ZsO6;+-Jf+&G1N+ zT(98Y&)jE9uA6u?VV2(0gnydhktn%d!M_Q!O39UyE8)>W?la+^W_Tn@u2=ByAorP) z>n0vu=ROl2X@*ClN6aHz2N226<1^-6QE+tn=u7pRsxzB`u zn&FWsxn9A)-P~tNuA6xDRri_jNHaVVCD$wX_f_|qlItcO{oQ>g{L>7NM9K9E{{7v3 zrsTSbN1M;l_cP(2W_Tn@u2=AH^EsvDO39V*=ydm)@JKT}5+&Cw_;EBNlOUF$$h5ex`{_) zKcx3G;h$!BBucJV@NevgO39UyE8)?e?la+^W_Tn@u2=AHPxqOU>n0vu>OK=5X@*Cl z2G{YlNa=n6o7rM`sTsQIP8TXm+Pcu9cCD$wX_l)~Y$#oNt zHa$|`&xC)P;gKl0UctXjk1QouO0I-Qr@GICN1EZ0D7jw2zf;|3O0JuD^nm+J_@@~j ziIVFT{CmKCrsTSbN2?yC_cY<3W_Tn@u2=AH)uT$um69vr(TCh;!XwS_NR(W!;NOSb zXG*S{cyx>VO!%i69*L6c75ux!eWv8PiARect@kwHpJsR@O0HM%Z}Fo`$(52T;n5WL zneb0DJQ5|>EBH6XeWv8PiAR^a&xA*s;gKl0UctZ1-DgU!n|L(h7`>+n|1`rRQF6V4 zen0vO z;XV`oX@*ClEBLqWv8Cin$(8Wvc=ws`NHaVV zCD$wXcf9*d$#oNt?scCD|1`rRQF6V4fA_l2lw3FQXvO37o+kX$439*~^$Py2cw8yD zQgS6cn&mzd9%+V0qU3r7|7N+*lw3FQ=mz(h@J};55+&Cw_;-W*Ov!ZEBJS@ z`%KAo6OW#Ep9%jo!y{30y@G$wyU&zdH}PnTkLmlF@J};55+&Cw__xK!O39UyE8)=@ z?la+$W_Tn@u2=By4ELFm>n0vO;yx4pX@*Clc{InP57r79*L6c z75rQM_)>DE>_K|1`rRQF6V4f79G&O0JuDbfx=Dc%&H~ ziIVFT{JYY9rsTSbM+=;&_cY<3W_Tn@u2=AHffGx~m69vr(Ioeo@J};55+&Cw_&3RY zrsTSbM;Exygh!g;ktn%d!M_XKXG*S{c=T8Ineb0DJQ5|>EBN^@U+-Nd6`xzB`un&FWsxn9A) zU%AheTsQG(m6PfBm~Xxz3U+VbwOD(AP9!qh?qoO0HM1ahp$+k}D-w!m6{~XTmDYuu7C% zuVCZZ?lUFVO{{v%eI{(w468)R^$IpV<~~z$-NdT3Ptkjtuu(It5+&Cw*tqs7rQ}M< zm9XkK_nELtGprIN*DKg~ocm14brY-ZcAp6wHNz@Va=n6$ce~G&TsQG({3rFECOpy% zk3`A!3jU4%WGT5)awR;P;XV@{X@*Cl&X6CD%n0w( z=`_8k3I8<1BT;g_f`4y1t(06Txe^{7?miP9X@*Clo}$#oNtUiTTjrwRWw!y{30y@G$Q`%EdhQgS6cn(aOl9%+V0 zqU3r7|7N?-lw3FQ=tlRM@J};55+&Cw_;;iGOv!ZEBN=K`%KAo6OZ0;mcE|}|1`rRQF6V4fA2V}lw2vf5*~fVeI`88439*~^$Pxd z#(k#bx`{`>bDs(SG{YlNa=n6ozjL1{xo+ao8fWW0P57r79*L6c75rP{>{4>21D0q!}KGlIs=xJIZ~gn0v;^f`S$6aHz2N226<1^+hs zTq(IyawR-E#eF6`(hQG8$@L2Uo#H-Ia^1wE``u^4Kh5w+lw7ai-~H}0CD%1?uasOVxe^|o<31A}X@*Cl2(S4ysgN7uQ}gh!g;ktn%d!N2R=XG*S{c(lj`dQTJnX@*Cln0v;_Cu9RE}kM?z+3I8<1BT;g_f`9wE&y-v@@#qTonea$6JQ5|>EBJSX`%KAo z6OZQovfk5#f12TuD7jw2zj?o0O0JY#36FMip9%jo!y{30y@G!`xzChbH}U9v_nGiW zGdvO{*DLsUzWYqcbrX;N>^>9zX@*Clra;4--cr?d-COpy%k3`A!3jWP;pDDR+;?Yg+GvS|RcqB@$ zSMcv9_nDIGCLWFbs@~Itf12TuD7jw2zp-B}C09zWghzY0&xC)P;gKl0UctXT+-FLz zn|Snf_nGiWGdvO{*DLtn0xk!+j?F(+rP9$@L2U{lk5xyxBhBzglw7ai-EBJS(`%KAo6OWd=MDJlOU_uKP^MbrX*UzpnQ*;h$!BBucJV@Ne+zrQ}M< zmGJ1@?la+^W_Tn@u2=By-R?6b*G)XS(0wL6(hQG8$@L2UUFbeja^1wEXWVDPKh5w+ zlw7ai-!twrCD%DwKNJ3GhDW00dIkSBxwMpADY+6JebRjn0v8ewp6Wgnydhktn%d z!N0{XDn0w3%Y7z1(hQG8$@L2Uean5OEBH6?%~Eou7NM9K9E{ypzLQ*zzJqb;t`_cP(2W_Tn@ zu2=AHiz`aWm69vr(Wl*K!XwS_NR(W!;NPd+XG*S{c=TKMneb0DJQ5|>EBNn0xE<~|et zX@*Cl>_qxo+aomF_d)k!E-#O0HM%?@ITXlItcO&3~2N(}aJT;gKl0UctZluPP;1O0I-Q zJG;+>f12TuD7jw2zn$G@O0JuD^ab~s@JKT}5+&Cw`1b|(nUd=!9{t6ACj8S3k3`A! z3jY1YeWv8PiANh;t?y^TKh5w+lw7ai-v(Egk}D-w!lRSiXTl@R@JN(gui)QF?lUFV zO+5OQ`%L(!86Js}>lOU_mHSM|brX+P`L5p6gnydhktn%d!M|0$TS~5!TnUd3ai0l~ zG{YlNa=n6ohq%v_TsQIP2ktZBpJsR@O0HM%?+5NPCD% zUQ^K|1`rRQF6V4e|x&mlw3FQ=u-EY@JKT}5+&Cw_;;!MOv!ZupJsR@O0HM%Z<}jN$(52T;nCUdGvSeDcqB@$ zSMcv__nDIGCLaC4eJ1?V439*~^$Py|!F{IWx`{_?eNXRc!avRMNR(W!;NM!`DEBH6=x>9nbpzmkGKh5w+lw7ai z-=;T|k}D-w!lP5&XTl@R@JN(gui)RQ?lUFVO+0$QeJ1?V439*~^$Pwy;677w-Nd6e z-Kh68;h$!BBucJV@b67GmXa$aSHh#i-Dkoh&G1N+T(98Y;qEgf*G)Y7vHMK;rx_lJ zlIs=x`?32>$#oNtmiWHj(}aJT;gKl0UctX5zF$hNlw1jqrn=9Bf12TuD7jw2zp3ss zCD%lOSPaZ@R|QgS6c+QEG$ z{L>7NM9K9E{_WsCQ*zzJqjTM7!XwS_NR(W!;NQ9KGbPteJo=OSO!%i69*L6c75w{? z`%KAo6OY#Wfxe#!|1`rRQF6V4f9w6Alw2vf5+0r4J`)~khDW00dIkSZaGxo;ZsO5> z?la+^W_Tn@u2=ByKKGfD>n0wpc(dNqgnydhktn%d!M_!6E+tn=u7pRk+-Jfg&G1N+ zT(98YEccm`>n0xE;64-nX@*Cl?w!avRMNR(W!;NMg3GbPteJlg0d`hF(-(+rP9$@L2UZS<2; za;4--c=QSPnea$6JQ5|>EBN;b_nDIGCLaCTeJ1?V439*~^$Py|+I^tHKh5w+ zlw7ai-?n#@k}D-w!lTc+&xA*s;gKl0UctZ5y3dqcH}U8(_nGidGdvO{*DLt zPcu9cCD$wXcaQr_$#oNtmcLW)X~I9v@JN(gui)SEcb1YXC0D|uneH>;pJsR@O0HM% zZ>IZ9$#oNtzUMv@9%+V0qU3r7|GwuwQ*zzJqgUOf_cY<3W_Tn@u2=ByRdK2vht#G}8t&xC)P;gKl0 zUctY=xzChbH}Pn*yY>A{_@@~jiIVFT{M+pAQgWr_N_cdd`%HMG86Js}>lOSv&3&fi zx;c7qkB9Dlda+jx?mK#cf$>=NklRhzs2Ns?lIu-ue8}ylSS3oX zSFmxldrHZbk}F}=huvqwD$TG;lw7Z1d6O0Jt&b*uYK*r*v+iIVFTY`oQdrsTSb zRZIR%?`gtD&9F+8T(4l`l0PdYS4ysgRr|QlgpHbEl_Pl1eWv8PiBlOSv&wZxkx`{_my3d4vn&FWsxn9A)C*5aCuA6wY{(bs> zCj8S3k3`A!3jVEsUn#j#awR-E(S0U7(hQG8$@L2Uo#;MOa^1wEpS#b5f12TuD7jw2 zzn{C$lw3FQXr-U)Jx%zh86Js}>lOT4>F1^7O39V*XpZ|#c%&H~iIVFT{F~!GQ*zzJ zqwl-Vgnydhktn%d!N2dj&y-v@@o3C1^qwaC(+rP9$@L2Ujrm0>xl(c^JbJ(TO!%i6 z9*L6c75sa@`%KAo6OS%&p9zmN!y{30y@G$2xX+YaH}U92_nGidGdvO{*DLtEBNn0uz-mmvG;h$!B zBucJV@Ne+`QgWr_N_g~c_nGidGdvO{*DLtn0w3$$cg~(hQG8$@L2UeaU^M zlOUl_&25GO39V*=oI&v z@JKT}5+&Cw_;-r?Ov!ZEBJSq`%KAo6OVr6J`?_FhDW00dIkS}{Y?0$86Js}>lOUl?x9k0 zrQ}L@bdLK>c%&H~iIVFT{5!{ersTSbM~}PDgnydhktn%d!N14dXG*S{c(l&LdQTJn zX@*ClZsO5|-|9V0_@@~jiIVFT{G0IGQgWr_N_cdT`%L(!86Js}>lOSv$bF{dx`{{E zxzB`0n&FWsxn9A)>)dBbuA6wY$Rm1B6aHz2N226<1^*U#q?BAKxe^}j<~|etX@*Cl z=I)lw3FQ=zRB?@JKT}5+&Cw_;EBH6|@ltZ7EBN=Q`%KAo6OY#Xlit&Wf12Tu zD7jw2zcv3lOT)?mkm;-Nd77+-Jfg z&G1N+T(98YHSRMd*G)WH=+Amj6aHz2N226<1^*WMb1At}awR<4#eF9H(+rP9$@L2U z?czRDa^1wE3*BeJBhBzglw7ai--YfoCD%7NM9K9E{ypP9Q*zzJqfP#z z?`Oh4&G1N+T(98YCVwdQEtC09zWghwB8p9zmN!y{30y@G!qa-S)= zZsO4`?la+^W_Tn@u2=By7WbKw>n0v8{*>O+gnydhktn%d!N0|yDkWD+uC(yr4U0cD z;lB&{yLlg+;&u}@YKB#!y0Sfv?OiIVFT zZ2Y|YOv!ZL1-eWv8PiAVRk&xC)P;gKl0UctY6-DgU!n|QRsvwBYx{%M9s zqU3r7|5kXmlw2vf5+2QRp9zmN!y{30y@G$U+-FLzn|O4C`%L(!86Js}>lOUF!F{IW zx`{`l{-*ad;h$!BBucJV@Nd-LO39UyE8)@m+-Jf+&G1N+T(98Y``l+ruA6vtvHMJT zq!}KGlIs=xyV!lEpe~Qrx_lJlIs=xTmA2)EBJS$`%KAo6OV3pp9%jo!y{30 zy@G$YyU&zdH}Pnh=k=Z@{L>7NM9K9E{w?!-DY;T|B|MtuJ`?_FhDW00dIkTcxzChb zH}U97_nGiWGdvO{*DLsUrTa|DbrX-~e?jkQ!avRMNR(W!;NSc&l#(kYSHh!7?la+^ zW_Tn@u2=AHlKV`_brX*+aGwc}G{YlNa=n6o7r4)qTsQIPukJJ9pJsR@O0HM%@2~DN zCD%R(|27C5CC-8)xi)%#y>z z|IWyb^Db37C=Uv_wU)LN^}`?{|e2X37`@5O5_H$1)J z#jEFC=KA8spL}WFW49hRJRLu3i}@bAyLkQuC(XCdF5`!%=g+)*zS|!DH@$Db{ZJNLr*pFe8C@bvnlo|u2n1y&fwjb5|*0?VFIJa4x{7I=346+7MV z+69(Axp>}jBNjY&)a!<)H#JaG4d51d_maMqH8i?6tH SrsD_ZAJ~7%5sUC^@c#gHe{Q4z literal 0 HcmV?d00001 From 7663e912208a28ca9ab9dc2b68e3b9314ce08231 Mon Sep 17 00:00:00 2001 From: kredcool Date: Sat, 20 Jan 2024 16:57:14 -0500 Subject: [PATCH 36/61] bit of dashboard and bit of pid --- rio/components/drivetrain.py | 36 +++++++++++++++++++++----------- rio/constants/robotHardware.yaml | 6 +++--- rio/simgui-window.json | 2 +- 3 files changed, 28 insertions(+), 16 deletions(-) diff --git a/rio/components/drivetrain.py b/rio/components/drivetrain.py index b4683122..86850101 100644 --- a/rio/components/drivetrain.py +++ b/rio/components/drivetrain.py @@ -118,20 +118,32 @@ def periodic(self) -> None: ), ) # desired - self.sd.putNumber("Swerve/FL", self.frontLeft.desiredState.angle.degrees()) - self.sd.putNumber("Swerve/FR", self.frontRight.desiredState.angle.degrees()) - self.sd.putNumber("Swerve/RL", self.rearLeft.desiredState.angle.degrees()) - self.sd.putNumber("Swerve/RR", self.rearRight.desiredState.angle.degrees()) + self.sd.putNumber( + "Swerve/FL desired", self.frontLeft.desiredState.angle.degrees() + ) + self.sd.putNumber( + "Swerve/FR desired", self.frontRight.desiredState.angle.degrees() + ) + self.sd.putNumber( + "Swerve/RL desired", self.rearLeft.desiredState.angle.degrees() + ) + self.sd.putNumber( + "Swerve/RR desired", self.rearRight.desiredState.angle.degrees() + ) # actual - self.sd.putNumber("Swerve/FL", self.frontLeft.getState().angle.degrees()) - self.sd.putNumber("Swerve/FR", self.frontRight.getState().angle.degrees()) - self.sd.putNumber("Swerve/RL", self.rearLeft.getState().angle.degrees()) - self.sd.putNumber("Swerve/RR", self.rearRight.getState().angle.degrees()) - - # logging.info( - # f"FR Swerve module setpoint {self.frontLeft.desiredState.angle.degrees()}, actual is {self.frontLeft.getPosition().angle.degrees(),} .getPosition is {self.frontLeft.getPosition()}" - # ) + self.sd.putNumber( + "Swerve/FL", self.frontLeft.getState().angle.degrees() * (360 / 60) + ) + self.sd.putNumber( + "Swerve/FR", self.frontRight.getState().angle.degrees() * (360 / 60) + ) + self.sd.putNumber( + "Swerve/RL", self.rearLeft.getState().angle.degrees() * (360 / 60) + ) + self.sd.putNumber( + "Swerve/RR", self.rearRight.getState().angle.degrees() * (360 / 60) + ) def getPose(self) -> Pose2d: """Returns the currently-estimated pose of the robot. diff --git a/rio/constants/robotHardware.yaml b/rio/constants/robotHardware.yaml index 10eaed53..75de7fc2 100644 --- a/rio/constants/robotHardware.yaml +++ b/rio/constants/robotHardware.yaml @@ -29,9 +29,9 @@ swerve: kDrivingMinOutput: -1 # Configured Never by Nobody kDrivingMaxOutput: 1 # Configured Never by Nobody - kTurningP: 2.65 # Updated 1/19 by Keegan - kTurningI: 0.012 # Configured Never by Nobody - kTurningD: 1.5 # Configured Never by Nobody + kTurningP: 2.65 # Updated 1/20 by Keegan + kTurningI: 0.012 # Updated 1/20 by Keegan + kTurningD: 1.5 # Updated 1/20 by Keegan kTurningFF: 0 # Configured Never by Nobody kTurningMinOutput: -1 # Configured Never by Nobody kTurningMaxOutput: 1 # Configured Never by Nobody diff --git a/rio/simgui-window.json b/rio/simgui-window.json index 34083bbe..934da4c9 100644 --- a/rio/simgui-window.json +++ b/rio/simgui-window.json @@ -56,7 +56,7 @@ "###Timing": { "Collapsed": "0", "Pos": "7,129", - "Size": "135,173" + "Size": "135,150" }, "Debug##Default": { "Collapsed": "0", From fa1b53eae60023c57b4ffc5af753b357f88bfa10 Mon Sep 17 00:00:00 2001 From: dublUayaychtee Date: Sat, 20 Jan 2024 17:07:43 -0500 Subject: [PATCH 37/61] add the literary literature --- rio/README.md | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100644 rio/README.md diff --git a/rio/README.md b/rio/README.md new file mode 100644 index 00000000..abbc21cd --- /dev/null +++ b/rio/README.md @@ -0,0 +1,13 @@ +do the + +``` +pipenv run make download +``` + +BEFORE the done did + +``` +pipenv run make deploy +``` + +pls 'n' thx From aee0f101e036a05694c1228b9912a156d1f2185b Mon Sep 17 00:00:00 2001 From: KenwoodFox Date: Sat, 20 Jan 2024 18:07:26 -0500 Subject: [PATCH 38/61] Pivot --- rio/Pipfile | 9 +- rio/Pipfile.lock | 953 +----------------- rio/README.md | 13 - rio/autonomous/__init__.py | 0 rio/components/__init__.py | 0 rio/constants.py | 141 +++ rio/constants/__init__.py | 0 rio/constants/complexConstants.py | 83 -- rio/constants/getConstants.py | 47 - rio/constants/robotHardware.yaml | 69 -- rio/constants/simple_auto.yaml | 7 - rio/pyproject.toml | 30 + rio/robot.py | 88 +- rio/robotcontainer.py | 120 +++ rio/simgui-ds.json | 60 +- rio/simgui-window.json | 18 +- rio/simgui.json | 38 +- .../drivesubsystem.py} | 147 +-- .../maxswervemodule.py} | 84 +- rio/tests/logs/FRC_20240120_210407.wpilog | Bin 90721 -> 0 bytes rio/tests/logs/FRC_20240120_210513.wpilog | Bin 90180 -> 0 bytes rio/tests/logs/FRC_20240120_210521.wpilog | Bin 90721 -> 0 bytes rio/tests/logs/FRC_20240120_211051.wpilog | Bin 90721 -> 0 bytes rio/tests/pyfrc_test.py | 11 +- 24 files changed, 466 insertions(+), 1452 deletions(-) delete mode 100644 rio/README.md delete mode 100644 rio/autonomous/__init__.py delete mode 100644 rio/components/__init__.py create mode 100644 rio/constants.py delete mode 100644 rio/constants/__init__.py delete mode 100644 rio/constants/complexConstants.py delete mode 100644 rio/constants/getConstants.py delete mode 100644 rio/constants/robotHardware.yaml delete mode 100644 rio/constants/simple_auto.yaml create mode 100644 rio/pyproject.toml create mode 100644 rio/robotcontainer.py rename rio/{components/drivetrain.py => subsystems/drivesubsystem.py} (65%) rename rio/{components/mikeSwerveModule.py => subsystems/maxswervemodule.py} (69%) delete mode 100644 rio/tests/logs/FRC_20240120_210407.wpilog delete mode 100644 rio/tests/logs/FRC_20240120_210513.wpilog delete mode 100644 rio/tests/logs/FRC_20240120_210521.wpilog delete mode 100644 rio/tests/logs/FRC_20240120_211051.wpilog diff --git a/rio/Pipfile b/rio/Pipfile index b165ab3b..a30896fe 100644 --- a/rio/Pipfile +++ b/rio/Pipfile @@ -5,18 +5,13 @@ name = "pypi" [packages] wpilib = {extras = ["all"]} -pyyaml = "6.0" -black = "*" -robotpy = "2024.1.1.3" +robotpy = "2024.1.1.1" robotpy-rev = "2024.2.0" robotpy-ctre = "*" robotpy-navx = "*" +robotpy-commands-v2 = "2024.0.0b4" [dev-packages] -black = "*" [requires] python_version = "3.11" - -[pipenv] -allow_prereleases = true diff --git a/rio/Pipfile.lock b/rio/Pipfile.lock index 258bb184..c8a6223d 100644 --- a/rio/Pipfile.lock +++ b/rio/Pipfile.lock @@ -1,7 +1,7 @@ { "_meta": { "hash": { - "sha256": "a10fc2592b799759069137ac85dffb63b25f3511b5f7412f19889b73fc36cc7d" + "sha256": "0bfc5b6776b92c7072fc224883ce8d79eeb0ad0b6170f8a5a663f3241f093b62" }, "pipfile-spec": 6, "requires": { @@ -16,103 +16,6 @@ ] }, "default": { - "aiohttp": { - "hashes": [ - "sha256:02ab6006ec3c3463b528374c4cdce86434e7b89ad355e7bf29e2f16b46c7dd6f", - "sha256:04fa38875e53eb7e354ece1607b1d2fdee2d175ea4e4d745f6ec9f751fe20c7c", - "sha256:0b0a6a36ed7e164c6df1e18ee47afbd1990ce47cb428739d6c99aaabfaf1b3af", - "sha256:0d406b01a9f5a7e232d1b0d161b40c05275ffbcbd772dc18c1d5a570961a1ca4", - "sha256:0e49b08eafa4f5707ecfb321ab9592717a319e37938e301d462f79b4e860c32a", - "sha256:0e7ba7ff228c0d9a2cd66194e90f2bca6e0abca810b786901a569c0de082f489", - "sha256:11cb254e397a82efb1805d12561e80124928e04e9c4483587ce7390b3866d213", - "sha256:11ff168d752cb41e8492817e10fb4f85828f6a0142b9726a30c27c35a1835f01", - "sha256:176df045597e674fa950bf5ae536be85699e04cea68fa3a616cf75e413737eb5", - "sha256:219a16763dc0294842188ac8a12262b5671817042b35d45e44fd0a697d8c8361", - "sha256:22698f01ff5653fe66d16ffb7658f582a0ac084d7da1323e39fd9eab326a1f26", - "sha256:237533179d9747080bcaad4d02083ce295c0d2eab3e9e8ce103411a4312991a0", - "sha256:289ba9ae8e88d0ba16062ecf02dd730b34186ea3b1e7489046fc338bdc3361c4", - "sha256:2c59e0076ea31c08553e868cec02d22191c086f00b44610f8ab7363a11a5d9d8", - "sha256:2c9376e2b09895c8ca8b95362283365eb5c03bdc8428ade80a864160605715f1", - "sha256:3135713c5562731ee18f58d3ad1bf41e1d8883eb68b363f2ffde5b2ea4b84cc7", - "sha256:3b9c7426923bb7bd66d409da46c41e3fb40f5caf679da624439b9eba92043fa6", - "sha256:3c0266cd6f005e99f3f51e583012de2778e65af6b73860038b968a0a8888487a", - "sha256:41473de252e1797c2d2293804e389a6d6986ef37cbb4a25208de537ae32141dd", - "sha256:4831df72b053b1eed31eb00a2e1aff6896fb4485301d4ccb208cac264b648db4", - "sha256:49f0c1b3c2842556e5de35f122fc0f0b721334ceb6e78c3719693364d4af8499", - "sha256:4b4c452d0190c5a820d3f5c0f3cd8a28ace48c54053e24da9d6041bf81113183", - "sha256:4ee8caa925aebc1e64e98432d78ea8de67b2272252b0a931d2ac3bd876ad5544", - "sha256:500f1c59906cd142d452074f3811614be04819a38ae2b3239a48b82649c08821", - "sha256:5216b6082c624b55cfe79af5d538e499cd5f5b976820eac31951fb4325974501", - "sha256:54311eb54f3a0c45efb9ed0d0a8f43d1bc6060d773f6973efd90037a51cd0a3f", - "sha256:54631fb69a6e44b2ba522f7c22a6fb2667a02fd97d636048478db2fd8c4e98fe", - "sha256:565760d6812b8d78d416c3c7cfdf5362fbe0d0d25b82fed75d0d29e18d7fc30f", - "sha256:598db66eaf2e04aa0c8900a63b0101fdc5e6b8a7ddd805c56d86efb54eb66672", - "sha256:5c4fa235d534b3547184831c624c0b7c1e262cd1de847d95085ec94c16fddcd5", - "sha256:69985d50a2b6f709412d944ffb2e97d0be154ea90600b7a921f95a87d6f108a2", - "sha256:69da0f3ed3496808e8cbc5123a866c41c12c15baaaead96d256477edf168eb57", - "sha256:6c93b7c2e52061f0925c3382d5cb8980e40f91c989563d3d32ca280069fd6a87", - "sha256:70907533db712f7aa791effb38efa96f044ce3d4e850e2d7691abd759f4f0ae0", - "sha256:81b77f868814346662c96ab36b875d7814ebf82340d3284a31681085c051320f", - "sha256:82eefaf1a996060602f3cc1112d93ba8b201dbf5d8fd9611227de2003dddb3b7", - "sha256:85c3e3c9cb1d480e0b9a64c658cd66b3cfb8e721636ab8b0e746e2d79a7a9eed", - "sha256:8a22a34bc594d9d24621091d1b91511001a7eea91d6652ea495ce06e27381f70", - "sha256:8cef8710fb849d97c533f259103f09bac167a008d7131d7b2b0e3a33269185c0", - "sha256:8d44e7bf06b0c0a70a20f9100af9fcfd7f6d9d3913e37754c12d424179b4e48f", - "sha256:8d7f98fde213f74561be1d6d3fa353656197f75d4edfbb3d94c9eb9b0fc47f5d", - "sha256:8d8e4450e7fe24d86e86b23cc209e0023177b6d59502e33807b732d2deb6975f", - "sha256:8fc49a87ac269d4529da45871e2ffb6874e87779c3d0e2ccd813c0899221239d", - "sha256:90ec72d231169b4b8d6085be13023ece8fa9b1bb495e4398d847e25218e0f431", - "sha256:91c742ca59045dce7ba76cab6e223e41d2c70d79e82c284a96411f8645e2afff", - "sha256:9b05d33ff8e6b269e30a7957bd3244ffbce2a7a35a81b81c382629b80af1a8bf", - "sha256:9b05d5cbe9dafcdc733262c3a99ccf63d2f7ce02543620d2bd8db4d4f7a22f83", - "sha256:9c5857612c9813796960c00767645cb5da815af16dafb32d70c72a8390bbf690", - "sha256:a34086c5cc285be878622e0a6ab897a986a6e8bf5b67ecb377015f06ed316587", - "sha256:ab221850108a4a063c5b8a70f00dd7a1975e5a1713f87f4ab26a46e5feac5a0e", - "sha256:b796b44111f0cab6bbf66214186e44734b5baab949cb5fb56154142a92989aeb", - "sha256:b8c3a67eb87394386847d188996920f33b01b32155f0a94f36ca0e0c635bf3e3", - "sha256:bcb6532b9814ea7c5a6a3299747c49de30e84472fa72821b07f5a9818bce0f66", - "sha256:bcc0ea8d5b74a41b621ad4a13d96c36079c81628ccc0b30cfb1603e3dfa3a014", - "sha256:bea94403a21eb94c93386d559bce297381609153e418a3ffc7d6bf772f59cc35", - "sha256:bff7e2811814fa2271be95ab6e84c9436d027a0e59665de60edf44e529a42c1f", - "sha256:c72444d17777865734aa1a4d167794c34b63e5883abb90356a0364a28904e6c0", - "sha256:c7b5d5d64e2a14e35a9240b33b89389e0035e6de8dbb7ffa50d10d8b65c57449", - "sha256:c7e939f1ae428a86e4abbb9a7c4732bf4706048818dfd979e5e2839ce0159f23", - "sha256:c88a15f272a0ad3d7773cf3a37cc7b7d077cbfc8e331675cf1346e849d97a4e5", - "sha256:c9110c06eaaac7e1f5562caf481f18ccf8f6fdf4c3323feab28a93d34cc646bd", - "sha256:ca7ca5abfbfe8d39e653870fbe8d7710be7a857f8a8386fc9de1aae2e02ce7e4", - "sha256:cae4c0c2ca800c793cae07ef3d40794625471040a87e1ba392039639ad61ab5b", - "sha256:cdefe289681507187e375a5064c7599f52c40343a8701761c802c1853a504558", - "sha256:cf2a0ac0615842b849f40c4d7f304986a242f1e68286dbf3bd7a835e4f83acfd", - "sha256:cfeadf42840c1e870dc2042a232a8748e75a36b52d78968cda6736de55582766", - "sha256:d737e69d193dac7296365a6dcb73bbbf53bb760ab25a3727716bbd42022e8d7a", - "sha256:d7481f581251bb5558ba9f635db70908819caa221fc79ee52a7f58392778c636", - "sha256:df9cf74b9bc03d586fc53ba470828d7b77ce51b0582d1d0b5b2fb673c0baa32d", - "sha256:e1f80197f8b0b846a8d5cf7b7ec6084493950d0882cc5537fb7b96a69e3c8590", - "sha256:ecca113f19d5e74048c001934045a2b9368d77b0b17691d905af18bd1c21275e", - "sha256:ee2527134f95e106cc1653e9ac78846f3a2ec1004cf20ef4e02038035a74544d", - "sha256:f27fdaadce22f2ef950fc10dcdf8048407c3b42b73779e48a4e76b3c35bca26c", - "sha256:f694dc8a6a3112059258a725a4ebe9acac5fe62f11c77ac4dcf896edfa78ca28", - "sha256:f800164276eec54e0af5c99feb9494c295118fc10a11b997bbb1348ba1a52065", - "sha256:ffcd828e37dc219a72c9012ec44ad2e7e3066bec6ff3aaa19e7d435dbf4032ca" - ], - "version": "==3.9.1" - }, - "aiosignal": { - "hashes": [ - "sha256:54cd96e15e1649b75d6c87526a6ff0b6c1b0dd3459f43d9ca11d48c339b68cfc", - "sha256:f8376fb07dd1e86a584e4fcdec80b36b7f81aac666ebc724e2c090300dd83b17" - ], - "markers": "python_version >= '3.7'", - "version": "==1.3.1" - }, - "attrs": { - "hashes": [ - "sha256:935dc3b529c262f6cf76e50877d35a4bd3c1de194fd41f47a2b7ae8f19971f30", - "sha256:99b87a485a5820b23b879f04c2305b44b951b502fd64be915879d77a7e8fc6f1" - ], - "markers": "python_version >= '3.7'", - "version": "==23.2.0" - }, "bcrypt": { "hashes": [ "sha256:02d9ef8915f72dd6daaef40e0baeef8a017ce624369f09754baf32bb32dba25f", @@ -146,34 +49,6 @@ "markers": "python_version >= '3.7'", "version": "==4.1.2" }, - "black": { - "hashes": [ - "sha256:2220c470c22476ca9631337b0daae41be2b215599919b19d576a956ad38aca69", - "sha256:2a12829e372563ffff10c18c7aff1ef274da6afbc7bc8ccdb5fcc8ff84cab43f", - "sha256:3d139b9531e6bb6d129497a46475535d8289dddc861a5b980f908c36597b9817", - "sha256:41c0ce5cbdb701900c166bcca08ac941b64cf1d6967509e3caeab126da0ae0d0", - "sha256:4a159ae57f239f3f1ef6a78784b00c1c617c7bb188cc351b3017b9e0702df11c", - "sha256:4de8ba5825588017f90e63d7a25fc4df33a6342d1f4d628ad76130d8f4488fc6", - "sha256:623efdb54e7290ba75f7b822dfd2d8a47a55e721ae63aab671ccfd46b2ba6c5d", - "sha256:6b594b3ede60182215d258c76de2de64712d2e8424442ff4402276e22684abbe", - "sha256:6e3c74b35ea179bb69440286b81c309a64c34a032746a9eef3399dc3ce671352", - "sha256:87c8165fad00b03d9c1d400b1dd250479792f49d012807ee45162d323d04fc06", - "sha256:88d1c60bac2044a409154e895abb9d74c8ff5d034fb70f3e1f7c3ae96206bc0c", - "sha256:915a6b6b916fc66edec886fc71b60284e447d8fa39d22b879af7ae6efccca90f", - "sha256:a2c977909557439d0f17dc82adaea84e48374950d53416efc0b8451a594d42c3", - "sha256:ac226f37fc429b386d6447df6256dc958c28dd602f86f950072febf886995f80", - "sha256:b03cdf8a4e15929adf47e5e40a0ddeea1d63b65cf59c22553c12417a0c7ccbf4", - "sha256:c86ecd9d3da3d91e96da5f4a43d9c4fe35c5698b0633e91f171ba9468d112a8b", - "sha256:cad114d8673adab76b3602c28c461c613b7be3da28415500e42aed47415eb561", - "sha256:cb0a7ea9aa1c108924e31f1204a1e2534af255dbaa24ecbb8c05f47341a7b6f1", - "sha256:d30a018fc03fd1e83c75d40b8a156ef541d0b56b6403b63754e1cc96889849d9", - "sha256:d47b6530c55c092a9d841a12c8b3ad838bd639bebf6660a3df9dae83d4ab83c1", - "sha256:e8a054dbb8947718820be2ed6953d66b912ec2795f282725efdd08381a11b0d0", - "sha256:ec345caf15ae2c61540812500979e92f2989c6b6d4d13d21bdc82908043b3265" - ], - "index": "pypi", - "version": "==24.1a1" - }, "cffi": { "hashes": [ "sha256:0c9ef6ff37e974b73c25eecc13952c55bceed9112be2d9d938ded8e856138bcc", @@ -232,14 +107,6 @@ "markers": "python_version >= '3.8'", "version": "==1.16.0" }, - "click": { - "hashes": [ - "sha256:ae74fb96c20a0277a1d615f1e4d73c8414f5a98db8b799a7931d1582f3390c28", - "sha256:ca9853ad459e787e2192211578cc907e7594e294c7ccc834310722b41b9ca6de" - ], - "markers": "python_version >= '3.7'", - "version": "==8.1.7" - }, "cryptography": { "hashes": [ "sha256:079b85658ea2f59c4f43b70f8119a52414cdb7be34da5d019a77bf96d473b960", @@ -269,97 +136,6 @@ "markers": "python_version >= '3.7'", "version": "==41.0.7" }, - "frozenlist": { - "hashes": [ - "sha256:04ced3e6a46b4cfffe20f9ae482818e34eba9b5fb0ce4056e4cc9b6e212d09b7", - "sha256:0633c8d5337cb5c77acbccc6357ac49a1770b8c487e5b3505c57b949b4b82e98", - "sha256:068b63f23b17df8569b7fdca5517edef76171cf3897eb68beb01341131fbd2ad", - "sha256:0c250a29735d4f15321007fb02865f0e6b6a41a6b88f1f523ca1596ab5f50bd5", - "sha256:1979bc0aeb89b33b588c51c54ab0161791149f2461ea7c7c946d95d5f93b56ae", - "sha256:1a4471094e146b6790f61b98616ab8e44f72661879cc63fa1049d13ef711e71e", - "sha256:1b280e6507ea8a4fa0c0a7150b4e526a8d113989e28eaaef946cc77ffd7efc0a", - "sha256:1d0ce09d36d53bbbe566fe296965b23b961764c0bcf3ce2fa45f463745c04701", - "sha256:20b51fa3f588ff2fe658663db52a41a4f7aa6c04f6201449c6c7c476bd255c0d", - "sha256:23b2d7679b73fe0e5a4560b672a39f98dfc6f60df63823b0a9970525325b95f6", - "sha256:23b701e65c7b36e4bf15546a89279bd4d8675faabc287d06bbcfac7d3c33e1e6", - "sha256:2471c201b70d58a0f0c1f91261542a03d9a5e088ed3dc6c160d614c01649c106", - "sha256:27657df69e8801be6c3638054e202a135c7f299267f1a55ed3a598934f6c0d75", - "sha256:29acab3f66f0f24674b7dc4736477bcd4bc3ad4b896f5f45379a67bce8b96868", - "sha256:32453c1de775c889eb4e22f1197fe3bdfe457d16476ea407472b9442e6295f7a", - "sha256:3a670dc61eb0d0eb7080890c13de3066790f9049b47b0de04007090807c776b0", - "sha256:3e0153a805a98f5ada7e09826255ba99fb4f7524bb81bf6b47fb702666484ae1", - "sha256:410478a0c562d1a5bcc2f7ea448359fcb050ed48b3c6f6f4f18c313a9bdb1826", - "sha256:442acde1e068288a4ba7acfe05f5f343e19fac87bfc96d89eb886b0363e977ec", - "sha256:48f6a4533887e189dae092f1cf981f2e3885175f7a0f33c91fb5b7b682b6bab6", - "sha256:4f57dab5fe3407b6c0c1cc907ac98e8a189f9e418f3b6e54d65a718aaafe3950", - "sha256:4f9c515e7914626b2a2e1e311794b4c35720a0be87af52b79ff8e1429fc25f19", - "sha256:55fdc093b5a3cb41d420884cdaf37a1e74c3c37a31f46e66286d9145d2063bd0", - "sha256:5667ed53d68d91920defdf4035d1cdaa3c3121dc0b113255124bcfada1cfa1b8", - "sha256:590344787a90ae57d62511dd7c736ed56b428f04cd8c161fcc5e7232c130c69a", - "sha256:5a7d70357e7cee13f470c7883a063aae5fe209a493c57d86eb7f5a6f910fae09", - "sha256:5c3894db91f5a489fc8fa6a9991820f368f0b3cbdb9cd8849547ccfab3392d86", - "sha256:5c849d495bf5154cd8da18a9eb15db127d4dba2968d88831aff6f0331ea9bd4c", - "sha256:64536573d0a2cb6e625cf309984e2d873979709f2cf22839bf2d61790b448ad5", - "sha256:693945278a31f2086d9bf3df0fe8254bbeaef1fe71e1351c3bd730aa7d31c41b", - "sha256:6db4667b187a6742b33afbbaf05a7bc551ffcf1ced0000a571aedbb4aa42fc7b", - "sha256:6eb73fa5426ea69ee0e012fb59cdc76a15b1283d6e32e4f8dc4482ec67d1194d", - "sha256:722e1124aec435320ae01ee3ac7bec11a5d47f25d0ed6328f2273d287bc3abb0", - "sha256:7268252af60904bf52c26173cbadc3a071cece75f873705419c8681f24d3edea", - "sha256:74fb4bee6880b529a0c6560885fce4dc95936920f9f20f53d99a213f7bf66776", - "sha256:780d3a35680ced9ce682fbcf4cb9c2bad3136eeff760ab33707b71db84664e3a", - "sha256:82e8211d69a4f4bc360ea22cd6555f8e61a1bd211d1d5d39d3d228b48c83a897", - "sha256:89aa2c2eeb20957be2d950b85974b30a01a762f3308cd02bb15e1ad632e22dc7", - "sha256:8aefbba5f69d42246543407ed2461db31006b0f76c4e32dfd6f42215a2c41d09", - "sha256:96ec70beabbd3b10e8bfe52616a13561e58fe84c0101dd031dc78f250d5128b9", - "sha256:9750cc7fe1ae3b1611bb8cfc3f9ec11d532244235d75901fb6b8e42ce9229dfe", - "sha256:9acbb16f06fe7f52f441bb6f413ebae6c37baa6ef9edd49cdd567216da8600cd", - "sha256:9d3e0c25a2350080e9319724dede4f31f43a6c9779be48021a7f4ebde8b2d742", - "sha256:a06339f38e9ed3a64e4c4e43aec7f59084033647f908e4259d279a52d3757d09", - "sha256:a0cb6f11204443f27a1628b0e460f37fb30f624be6051d490fa7d7e26d4af3d0", - "sha256:a7496bfe1da7fb1a4e1cc23bb67c58fab69311cc7d32b5a99c2007b4b2a0e932", - "sha256:a828c57f00f729620a442881cc60e57cfcec6842ba38e1b19fd3e47ac0ff8dc1", - "sha256:a9b2de4cf0cdd5bd2dee4c4f63a653c61d2408055ab77b151c1957f221cabf2a", - "sha256:b46c8ae3a8f1f41a0d2ef350c0b6e65822d80772fe46b653ab6b6274f61d4a49", - "sha256:b7e3ed87d4138356775346e6845cccbe66cd9e207f3cd11d2f0b9fd13681359d", - "sha256:b7f2f9f912dca3934c1baec2e4585a674ef16fe00218d833856408c48d5beee7", - "sha256:ba60bb19387e13597fb059f32cd4d59445d7b18b69a745b8f8e5db0346f33480", - "sha256:beee944ae828747fd7cb216a70f120767fc9f4f00bacae8543c14a6831673f89", - "sha256:bfa4a17e17ce9abf47a74ae02f32d014c5e9404b6d9ac7f729e01562bbee601e", - "sha256:c037a86e8513059a2613aaba4d817bb90b9d9b6b69aace3ce9c877e8c8ed402b", - "sha256:c302220494f5c1ebeb0912ea782bcd5e2f8308037b3c7553fad0e48ebad6ad82", - "sha256:c6321c9efe29975232da3bd0af0ad216800a47e93d763ce64f291917a381b8eb", - "sha256:c757a9dd70d72b076d6f68efdbb9bc943665ae954dad2801b874c8c69e185068", - "sha256:c99169d4ff810155ca50b4da3b075cbde79752443117d89429595c2e8e37fed8", - "sha256:c9c92be9fd329ac801cc420e08452b70e7aeab94ea4233a4804f0915c14eba9b", - "sha256:cc7b01b3754ea68a62bd77ce6020afaffb44a590c2289089289363472d13aedb", - "sha256:db9e724bebd621d9beca794f2a4ff1d26eed5965b004a97f1f1685a173b869c2", - "sha256:dca69045298ce5c11fd539682cff879cc1e664c245d1c64da929813e54241d11", - "sha256:dd9b1baec094d91bf36ec729445f7769d0d0cf6b64d04d86e45baf89e2b9059b", - "sha256:e02a0e11cf6597299b9f3bbd3f93d79217cb90cfd1411aec33848b13f5c656cc", - "sha256:e6a20a581f9ce92d389a8c7d7c3dd47c81fd5d6e655c8dddf341e14aa48659d0", - "sha256:e7004be74cbb7d9f34553a5ce5fb08be14fb33bc86f332fb71cbe5216362a497", - "sha256:e774d53b1a477a67838a904131c4b0eef6b3d8a651f8b138b04f748fccfefe17", - "sha256:edb678da49d9f72c9f6c609fbe41a5dfb9a9282f9e6a2253d5a91e0fc382d7c0", - "sha256:f146e0911cb2f1da549fc58fc7bcd2b836a44b79ef871980d605ec392ff6b0d2", - "sha256:f56e2333dda1fe0f909e7cc59f021eba0d2307bc6f012a1ccf2beca6ba362439", - "sha256:f9a3ea26252bd92f570600098783d1371354d89d5f6b7dfd87359d669f2109b5", - "sha256:f9aa1878d1083b276b0196f2dfbe00c9b7e752475ed3b682025ff20c1c1f51ac", - "sha256:fb3c2db03683b5767dedb5769b8a40ebb47d6f7f45b1b3e3b4b51ec8ad9d9825", - "sha256:fbeb989b5cc29e8daf7f976b421c220f1b8c731cbf22b9130d8815418ea45887", - "sha256:fde5bd59ab5357e3853313127f4d3565fc7dad314a74d7b5d43c22c6a5ed2ced", - "sha256:fe1a06da377e3a1062ae5fe0926e12b84eceb8a50b350ddca72dc85015873f74" - ], - "markers": "python_version >= '3.8'", - "version": "==1.4.1" - }, - "idna": { - "hashes": [ - "sha256:9ecdbbd083b06798ae1e86adcbfe8ab1479cf864e4ee30fe4e46a003d12491ca", - "sha256:c05567e9c24a6b9faaa835c4821bad0590fbb9d5779e7caa6e1cc4978e7eb24f" - ], - "markers": "python_version >= '3.5'", - "version": "==3.6" - }, "iniconfig": { "hashes": [ "sha256:2d91e135bf72d31a410b17c16da610a82cb55f6b0477d1a902134b24a455b8b3", @@ -368,94 +144,6 @@ "markers": "python_version >= '3.7'", "version": "==2.0.0" }, - "multidict": { - "hashes": [ - "sha256:01a3a55bd90018c9c080fbb0b9f4891db37d148a0a18722b42f94694f8b6d4c9", - "sha256:0b1a97283e0c85772d613878028fec909f003993e1007eafa715b24b377cb9b8", - "sha256:0dfad7a5a1e39c53ed00d2dd0c2e36aed4650936dc18fd9a1826a5ae1cad6f03", - "sha256:11bdf3f5e1518b24530b8241529d2050014c884cf18b6fc69c0c2b30ca248710", - "sha256:1502e24330eb681bdaa3eb70d6358e818e8e8f908a22a1851dfd4e15bc2f8161", - "sha256:16ab77bbeb596e14212e7bab8429f24c1579234a3a462105cda4a66904998664", - "sha256:16d232d4e5396c2efbbf4f6d4df89bfa905eb0d4dc5b3549d872ab898451f569", - "sha256:21a12c4eb6ddc9952c415f24eef97e3e55ba3af61f67c7bc388dcdec1404a067", - "sha256:27c523fbfbdfd19c6867af7346332b62b586eed663887392cff78d614f9ec313", - "sha256:281af09f488903fde97923c7744bb001a9b23b039a909460d0f14edc7bf59706", - "sha256:33029f5734336aa0d4c0384525da0387ef89148dc7191aae00ca5fb23d7aafc2", - "sha256:3601a3cece3819534b11d4efc1eb76047488fddd0c85a3948099d5da4d504636", - "sha256:3666906492efb76453c0e7b97f2cf459b0682e7402c0489a95484965dbc1da49", - "sha256:36c63aaa167f6c6b04ef2c85704e93af16c11d20de1d133e39de6a0e84582a93", - "sha256:39ff62e7d0f26c248b15e364517a72932a611a9b75f35b45be078d81bdb86603", - "sha256:43644e38f42e3af682690876cff722d301ac585c5b9e1eacc013b7a3f7b696a0", - "sha256:4372381634485bec7e46718edc71528024fcdc6f835baefe517b34a33c731d60", - "sha256:458f37be2d9e4c95e2d8866a851663cbc76e865b78395090786f6cd9b3bbf4f4", - "sha256:45e1ecb0379bfaab5eef059f50115b54571acfbe422a14f668fc8c27ba410e7e", - "sha256:4b9d9e4e2b37daddb5c23ea33a3417901fa7c7b3dee2d855f63ee67a0b21e5b1", - "sha256:4ceef517eca3e03c1cceb22030a3e39cb399ac86bff4e426d4fc6ae49052cc60", - "sha256:4d1a3d7ef5e96b1c9e92f973e43aa5e5b96c659c9bc3124acbbd81b0b9c8a951", - "sha256:4dcbb0906e38440fa3e325df2359ac6cb043df8e58c965bb45f4e406ecb162cc", - "sha256:509eac6cf09c794aa27bcacfd4d62c885cce62bef7b2c3e8b2e49d365b5003fe", - "sha256:52509b5be062d9eafc8170e53026fbc54cf3b32759a23d07fd935fb04fc22d95", - "sha256:52f2dffc8acaba9a2f27174c41c9e57f60b907bb9f096b36b1a1f3be71c6284d", - "sha256:574b7eae1ab267e5f8285f0fe881f17efe4b98c39a40858247720935b893bba8", - "sha256:5979b5632c3e3534e42ca6ff856bb24b2e3071b37861c2c727ce220d80eee9ed", - "sha256:59d43b61c59d82f2effb39a93c48b845efe23a3852d201ed2d24ba830d0b4cf2", - "sha256:5a4dcf02b908c3b8b17a45fb0f15b695bf117a67b76b7ad18b73cf8e92608775", - "sha256:5cad9430ab3e2e4fa4a2ef4450f548768400a2ac635841bc2a56a2052cdbeb87", - "sha256:5fc1b16f586f049820c5c5b17bb4ee7583092fa0d1c4e28b5239181ff9532e0c", - "sha256:62501642008a8b9871ddfccbf83e4222cf8ac0d5aeedf73da36153ef2ec222d2", - "sha256:64bdf1086b6043bf519869678f5f2757f473dee970d7abf6da91ec00acb9cb98", - "sha256:64da238a09d6039e3bd39bb3aee9c21a5e34f28bfa5aa22518581f910ff94af3", - "sha256:666daae833559deb2d609afa4490b85830ab0dfca811a98b70a205621a6109fe", - "sha256:67040058f37a2a51ed8ea8f6b0e6ee5bd78ca67f169ce6122f3e2ec80dfe9b78", - "sha256:6748717bb10339c4760c1e63da040f5f29f5ed6e59d76daee30305894069a660", - "sha256:6b181d8c23da913d4ff585afd1155a0e1194c0b50c54fcfe286f70cdaf2b7176", - "sha256:6ed5f161328b7df384d71b07317f4d8656434e34591f20552c7bcef27b0ab88e", - "sha256:7582a1d1030e15422262de9f58711774e02fa80df0d1578995c76214f6954988", - "sha256:7d18748f2d30f94f498e852c67d61261c643b349b9d2a581131725595c45ec6c", - "sha256:7d6ae9d593ef8641544d6263c7fa6408cc90370c8cb2bbb65f8d43e5b0351d9c", - "sha256:81a4f0b34bd92df3da93315c6a59034df95866014ac08535fc819f043bfd51f0", - "sha256:8316a77808c501004802f9beebde51c9f857054a0c871bd6da8280e718444449", - "sha256:853888594621e6604c978ce2a0444a1e6e70c8d253ab65ba11657659dcc9100f", - "sha256:99b76c052e9f1bc0721f7541e5e8c05db3941eb9ebe7b8553c625ef88d6eefde", - "sha256:a2e4369eb3d47d2034032a26c7a80fcb21a2cb22e1173d761a162f11e562caa5", - "sha256:ab55edc2e84460694295f401215f4a58597f8f7c9466faec545093045476327d", - "sha256:af048912e045a2dc732847d33821a9d84ba553f5c5f028adbd364dd4765092ac", - "sha256:b1a2eeedcead3a41694130495593a559a668f382eee0727352b9a41e1c45759a", - "sha256:b1e8b901e607795ec06c9e42530788c45ac21ef3aaa11dbd0c69de543bfb79a9", - "sha256:b41156839806aecb3641f3208c0dafd3ac7775b9c4c422d82ee2a45c34ba81ca", - "sha256:b692f419760c0e65d060959df05f2a531945af31fda0c8a3b3195d4efd06de11", - "sha256:bc779e9e6f7fda81b3f9aa58e3a6091d49ad528b11ed19f6621408806204ad35", - "sha256:bf6774e60d67a9efe02b3616fee22441d86fab4c6d335f9d2051d19d90a40063", - "sha256:c048099e4c9e9d615545e2001d3d8a4380bd403e1a0578734e0d31703d1b0c0b", - "sha256:c5cb09abb18c1ea940fb99360ea0396f34d46566f157122c92dfa069d3e0e982", - "sha256:cc8e1d0c705233c5dd0c5e6460fbad7827d5d36f310a0fadfd45cc3029762258", - "sha256:d5e3fc56f88cc98ef8139255cf8cd63eb2c586531e43310ff859d6bb3a6b51f1", - "sha256:d6aa0418fcc838522256761b3415822626f866758ee0bc6632c9486b179d0b52", - "sha256:d6c254ba6e45d8e72739281ebc46ea5eb5f101234f3ce171f0e9f5cc86991480", - "sha256:d6d635d5209b82a3492508cf5b365f3446afb65ae7ebd755e70e18f287b0adf7", - "sha256:dcfe792765fab89c365123c81046ad4103fcabbc4f56d1c1997e6715e8015461", - "sha256:ddd3915998d93fbcd2566ddf9cf62cdb35c9e093075f862935573d265cf8f65d", - "sha256:ddff9c4e225a63a5afab9dd15590432c22e8057e1a9a13d28ed128ecf047bbdc", - "sha256:e41b7e2b59679edfa309e8db64fdf22399eec4b0b24694e1b2104fb789207779", - "sha256:e69924bfcdda39b722ef4d9aa762b2dd38e4632b3641b1d9a57ca9cd18f2f83a", - "sha256:ea20853c6dbbb53ed34cb4d080382169b6f4554d394015f1bef35e881bf83547", - "sha256:ee2a1ece51b9b9e7752e742cfb661d2a29e7bcdba2d27e66e28a99f1890e4fa0", - "sha256:eeb6dcc05e911516ae3d1f207d4b0520d07f54484c49dfc294d6e7d63b734171", - "sha256:f70b98cd94886b49d91170ef23ec5c0e8ebb6f242d734ed7ed677b24d50c82cf", - "sha256:fc35cb4676846ef752816d5be2193a1e8367b4c1397b74a565a9d0389c433a1d", - "sha256:ff959bee35038c4624250473988b24f846cbeb2c6639de3602c073f10410ceba" - ], - "markers": "python_version >= '3.7'", - "version": "==6.0.4" - }, - "mypy-extensions": { - "hashes": [ - "sha256:4392f6c0eb8a5668a69e23d168ffa70f0be9ccfd32b5cc2d26a34ae5b844552d", - "sha256:75dbf8955dc00442a438fc4d0666508a9a97b6bd41aa2f0ffe9d2f2725af0782" - ], - "markers": "python_version >= '3.5'", - "version": "==1.0.0" - }, "packaging": { "hashes": [ "sha256:048fb0e9405036518eaaf48a55953c750c11e1a1b68e0dd1a9d62ed0c092cfc5", @@ -472,14 +160,6 @@ "markers": "python_version >= '3.6'", "version": "==3.4.0" }, - "pathspec": { - "hashes": [ - "sha256:a0d503e138a4c123b27490a4f7beda6a01c6f288df0e4a8b79c7eb0dc7b4cc08", - "sha256:a482d51503a1ab33b1c67a6c3813a26953dbdc71c31dacaef9a838c4e29f5712" - ], - "markers": "python_version >= '3.8'", - "version": "==0.12.1" - }, "phoenix6": { "hashes": [ "sha256:48f79760706515157512d4596a2964c327235da53dd7b4cadbc50c4bd61b3b65", @@ -499,14 +179,6 @@ "markers": "python_version >= '3.9'", "version": "==0.23" }, - "platformdirs": { - "hashes": [ - "sha256:11c8f37bcca40db96d8144522d925583bdb7a31f7b0e37e3ed4318400a8e2380", - "sha256:906d548203468492d432bcb294d4bc2fff751bf84971fbb2c10918cc206ee420" - ], - "markers": "python_version >= '3.8'", - "version": "==4.1.0" - }, "pluggy": { "hashes": [ "sha256:cf61ae8f126ac6f7c451172cf30e3e43d3ca77615509771b3a984a0730651e12", @@ -577,11 +249,11 @@ }, "pytest": { "hashes": [ - "sha256:42ed2f917ded90ceb752dbe2ecb48c436c2a70d38bc16018c2d11da6426a18b6", - "sha256:efc82dc5e6f2f41ae5acb9eabdf2ced192f336664c436b24a7db2c6aaafe4efd" + "sha256:2cf0005922c6ace4a3e2ec8b4080eb0d9753fdc93107415332f50ce9e7994280", + "sha256:b090cdf5ed60bf4c45261be03239c2c1c22df034fbffe691abe93cd80cea01d8" ], - "markers": "python_version >= '3.8'", - "version": "==8.0.0rc2" + "markers": "python_version >= '3.7'", + "version": "==7.4.4" }, "pytest-reraise": { "hashes": [ @@ -591,70 +263,14 @@ "markers": "python_full_version >= '3.6.1' and python_full_version < '4.0.0'", "version": "==2.1.2" }, - "pyyaml": { - "hashes": [ - "sha256:04ac92ad1925b2cff1db0cfebffb6ffc43457495c9b3c39d3fcae417d7125dc5", - "sha256:062582fca9fabdd2c8b54a3ef1c978d786e0f6b3a1510e0ac93ef59e0ddae2bc", - "sha256:0d3304d8c0adc42be59c5f8a4d9e3d7379e6955ad754aa9d6ab7a398b59dd1df", - "sha256:1635fd110e8d85d55237ab316b5b011de701ea0f29d07611174a1b42f1444741", - "sha256:184c5108a2aca3c5b3d3bf9395d50893a7ab82a38004c8f61c258d4428e80206", - "sha256:18aeb1bf9a78867dc38b259769503436b7c72f7a1f1f4c93ff9a17de54319b27", - "sha256:1d4c7e777c441b20e32f52bd377e0c409713e8bb1386e1099c2415f26e479595", - "sha256:1e2722cc9fbb45d9b87631ac70924c11d3a401b2d7f410cc0e3bbf249f2dca62", - "sha256:1fe35611261b29bd1de0070f0b2f47cb6ff71fa6595c077e42bd0c419fa27b98", - "sha256:28c119d996beec18c05208a8bd78cbe4007878c6dd15091efb73a30e90539696", - "sha256:326c013efe8048858a6d312ddd31d56e468118ad4cdeda36c719bf5bb6192290", - "sha256:40df9b996c2b73138957fe23a16a4f0ba614f4c0efce1e9406a184b6d07fa3a9", - "sha256:42f8152b8dbc4fe7d96729ec2b99c7097d656dc1213a3229ca5383f973a5ed6d", - "sha256:49a183be227561de579b4a36efbb21b3eab9651dd81b1858589f796549873dd6", - "sha256:4fb147e7a67ef577a588a0e2c17b6db51dda102c71de36f8549b6816a96e1867", - "sha256:50550eb667afee136e9a77d6dc71ae76a44df8b3e51e41b77f6de2932bfe0f47", - "sha256:510c9deebc5c0225e8c96813043e62b680ba2f9c50a08d3724c7f28a747d1486", - "sha256:5773183b6446b2c99bb77e77595dd486303b4faab2b086e7b17bc6bef28865f6", - "sha256:596106435fa6ad000c2991a98fa58eeb8656ef2325d7e158344fb33864ed87e3", - "sha256:6965a7bc3cf88e5a1c3bd2e0b5c22f8d677dc88a455344035f03399034eb3007", - "sha256:69b023b2b4daa7548bcfbd4aa3da05b3a74b772db9e23b982788168117739938", - "sha256:6c22bec3fbe2524cde73d7ada88f6566758a8f7227bfbf93a408a9d86bcc12a0", - "sha256:704219a11b772aea0d8ecd7058d0082713c3562b4e271b849ad7dc4a5c90c13c", - "sha256:7e07cbde391ba96ab58e532ff4803f79c4129397514e1413a7dc761ccd755735", - "sha256:81e0b275a9ecc9c0c0c07b4b90ba548307583c125f54d5b6946cfee6360c733d", - "sha256:855fb52b0dc35af121542a76b9a84f8d1cd886ea97c84703eaa6d88e37a2ad28", - "sha256:8d4e9c88387b0f5c7d5f281e55304de64cf7f9c0021a3525bd3b1c542da3b0e4", - "sha256:9046c58c4395dff28dd494285c82ba00b546adfc7ef001486fbf0324bc174fba", - "sha256:9eb6caa9a297fc2c2fb8862bc5370d0303ddba53ba97e71f08023b6cd73d16a8", - "sha256:a08c6f0fe150303c1c6b71ebcd7213c2858041a7e01975da3a99aed1e7a378ef", - "sha256:a0cd17c15d3bb3fa06978b4e8958dcdc6e0174ccea823003a106c7d4d7899ac5", - "sha256:afd7e57eddb1a54f0f1a974bc4391af8bcce0b444685d936840f125cf046d5bd", - "sha256:b1275ad35a5d18c62a7220633c913e1b42d44b46ee12554e5fd39c70a243d6a3", - "sha256:b786eecbdf8499b9ca1d697215862083bd6d2a99965554781d0d8d1ad31e13a0", - "sha256:ba336e390cd8e4d1739f42dfe9bb83a3cc2e80f567d8805e11b46f4a943f5515", - "sha256:baa90d3f661d43131ca170712d903e6295d1f7a0f595074f151c0aed377c9b9c", - "sha256:bc1bf2925a1ecd43da378f4db9e4f799775d6367bdb94671027b73b393a7c42c", - "sha256:bd4af7373a854424dabd882decdc5579653d7868b8fb26dc7d0e99f823aa5924", - "sha256:bf07ee2fef7014951eeb99f56f39c9bb4af143d8aa3c21b1677805985307da34", - "sha256:bfdf460b1736c775f2ba9f6a92bca30bc2095067b8a9d77876d1fad6cc3b4a43", - "sha256:c8098ddcc2a85b61647b2590f825f3db38891662cfc2fc776415143f599bb859", - "sha256:d2b04aac4d386b172d5b9692e2d2da8de7bfb6c387fa4f801fbf6fb2e6ba4673", - "sha256:d483d2cdf104e7c9fa60c544d92981f12ad66a457afae824d146093b8c294c54", - "sha256:d858aa552c999bc8a8d57426ed01e40bef403cd8ccdd0fc5f6f04a00414cac2a", - "sha256:e7d73685e87afe9f3b36c799222440d6cf362062f78be1013661b00c5c6f678b", - "sha256:f003ed9ad21d6a4713f0a9b5a7a0a79e08dd0f221aff4525a2be4c346ee60aab", - "sha256:f22ac1c3cac4dbc50079e965eba2c1058622631e526bd9afd45fedd49ba781fa", - "sha256:faca3bdcf85b2fc05d06ff3fbc1f83e1391b3e724afa3feba7d13eeab355484c", - "sha256:fca0e3a251908a499833aa292323f32437106001d436eca0e6e7833256674585", - "sha256:fd1592b3fdf65fff2ad0004b5e363300ef59ced41c2e6b3a99d4089fa8c5435d", - "sha256:fd66fc5d0da6d9815ba2cebeb4205f95818ff4b79c3ebe268e75d961704af52f" - ], - "index": "pypi", - "version": "==6.0.1" - }, "robotpy": { "hashes": [ - "sha256:a9f9f7de9aac362ec7f027194d11644ad555f4da1951c9ebdf9c6ba8ec15cb36", - "sha256:cc9a1dfa216a8208efa848683ce0df45c98f7905c3450f8d61ea32fed4adab9b" + "sha256:b2589cea10a947ea26eb099d9345fe0e5bd400f678fffc2b68d1c7c4f63509e3", + "sha256:cacd0912b6622198a9e5864a1accfd2c2264ddb09da8223d2c93054d65925486" ], "index": "pypi", - "version": "==2024.1.1.3" + "markers": "python_version < '3.13' and python_version >= '3.8'", + "version": "==2024.1.1.1" }, "robotpy-cli": { "hashes": [ @@ -664,6 +280,15 @@ "markers": "python_version >= '3.8'", "version": "==2024.0.0" }, + "robotpy-commands-v2": { + "hashes": [ + "sha256:0161feea5288b6af6fe3eddc412ad30c920acb929bcb5fea6899ba47e2e5763b", + "sha256:e0f808645f3f01c1a49d54cbdca6a2f528c2797913bb6382bec067c69b646065" + ], + "index": "pypi", + "markers": "python_version >= '3.8'", + "version": "==2024.0.0b4" + }, "robotpy-ctre": { "hashes": [ "sha256:056e978d2d58aa842c69472063e21e89b0615dc6df70438ee86b4b3ea8825293", @@ -681,6 +306,7 @@ "sha256:ec33c5a2777b15a29880a98e31de5c28f7b264afe3fccaaf983dd3853d632de6" ], "index": "pypi", + "markers": "python_version >= '3.8'", "version": "==2024.1.1" }, "robotpy-hal": { @@ -727,11 +353,11 @@ }, "robotpy-installer": { "hashes": [ - "sha256:05f0ef18694d8c49a424c020d359fdb799e11e53785e8bbd60ebe1bfa0ddc1d4", - "sha256:3d6643435ad3cbf7d72bf696c6c41fe3e1d61781bcf541392efdfe42591dfc4d" + "sha256:2e7692bd6c0d305531bcede806e37e6df15fd446e856dee2670af2283337ed35", + "sha256:dde4f529c84b2fcc7b97193419d72c9184531578cdbadf310cdab72f49146110" ], "markers": "platform_machine != 'roborio' and platform_machine != 'armv7l' and platform_machine != 'aarch64'", - "version": "==2024.1.1" + "version": "==2024.1.2" }, "robotpy-navx": { "hashes": [ @@ -753,6 +379,7 @@ "sha256:ebc481d6d8b3921d7fb58a9d7820fd7ccc94d1aac2fe189ac35dfcc38c6bfa91" ], "index": "pypi", + "markers": "python_version >= '3.8'", "version": "==2024.1.0" }, "robotpy-rev": { @@ -775,6 +402,7 @@ "sha256:ffd8748e1425f674c1fb0fe51620f7da9f40c11a7643f9c03139747e7237aea0" ], "index": "pypi", + "markers": "python_version >= '3.8'", "version": "==2024.2.0" }, "robotpy-wpilib-utilities": { @@ -893,538 +521,9 @@ "sha256:e3b4063c4068fadd48e7cfb220476bfd529e59614e0b3f8a825d06115d2a9b44", "sha256:fcfa81321de93933406e313b7f3f89d6300ab5bc9d757c89b4214858fb4393d3" ], - "index": "pypi", + "markers": "python_version >= '3.8'", "version": "==2024.1.1.1" - }, - "yarl": { - "hashes": [ - "sha256:008d3e808d03ef28542372d01057fd09168419cdc8f848efe2804f894ae03e51", - "sha256:03caa9507d3d3c83bca08650678e25364e1843b484f19986a527630ca376ecce", - "sha256:07574b007ee20e5c375a8fe4a0789fad26db905f9813be0f9fef5a68080de559", - "sha256:09efe4615ada057ba2d30df871d2f668af661e971dfeedf0c159927d48bbeff0", - "sha256:0d2454f0aef65ea81037759be5ca9947539667eecebca092733b2eb43c965a81", - "sha256:0e9d124c191d5b881060a9e5060627694c3bdd1fe24c5eecc8d5d7d0eb6faabc", - "sha256:18580f672e44ce1238b82f7fb87d727c4a131f3a9d33a5e0e82b793362bf18b4", - "sha256:1f23e4fe1e8794f74b6027d7cf19dc25f8b63af1483d91d595d4a07eca1fb26c", - "sha256:206a55215e6d05dbc6c98ce598a59e6fbd0c493e2de4ea6cc2f4934d5a18d130", - "sha256:23d32a2594cb5d565d358a92e151315d1b2268bc10f4610d098f96b147370136", - "sha256:26a1dc6285e03f3cc9e839a2da83bcbf31dcb0d004c72d0730e755b33466c30e", - "sha256:29e0f83f37610f173eb7e7b5562dd71467993495e568e708d99e9d1944f561ec", - "sha256:2b134fd795e2322b7684155b7855cc99409d10b2e408056db2b93b51a52accc7", - "sha256:2d47552b6e52c3319fede1b60b3de120fe83bde9b7bddad11a69fb0af7db32f1", - "sha256:357495293086c5b6d34ca9616a43d329317feab7917518bc97a08f9e55648455", - "sha256:35a2b9396879ce32754bd457d31a51ff0a9d426fd9e0e3c33394bf4b9036b099", - "sha256:3777ce5536d17989c91696db1d459574e9a9bd37660ea7ee4d3344579bb6f129", - "sha256:3986b6f41ad22988e53d5778f91855dc0399b043fc8946d4f2e68af22ee9ff10", - "sha256:44d8ffbb9c06e5a7f529f38f53eda23e50d1ed33c6c869e01481d3fafa6b8142", - "sha256:49a180c2e0743d5d6e0b4d1a9e5f633c62eca3f8a86ba5dd3c471060e352ca98", - "sha256:4aa9741085f635934f3a2583e16fcf62ba835719a8b2b28fb2917bb0537c1dfa", - "sha256:4b21516d181cd77ebd06ce160ef8cc2a5e9ad35fb1c5930882baff5ac865eee7", - "sha256:4b3c1ffe10069f655ea2d731808e76e0f452fc6c749bea04781daf18e6039525", - "sha256:4c7d56b293cc071e82532f70adcbd8b61909eec973ae9d2d1f9b233f3d943f2c", - "sha256:4e9035df8d0880b2f1c7f5031f33f69e071dfe72ee9310cfc76f7b605958ceb9", - "sha256:54525ae423d7b7a8ee81ba189f131054defdb122cde31ff17477951464c1691c", - "sha256:549d19c84c55d11687ddbd47eeb348a89df9cb30e1993f1b128f4685cd0ebbf8", - "sha256:54beabb809ffcacbd9d28ac57b0db46e42a6e341a030293fb3185c409e626b8b", - "sha256:566db86717cf8080b99b58b083b773a908ae40f06681e87e589a976faf8246bf", - "sha256:5a2e2433eb9344a163aced6a5f6c9222c0786e5a9e9cac2c89f0b28433f56e23", - "sha256:5aef935237d60a51a62b86249839b51345f47564208c6ee615ed2a40878dccdd", - "sha256:604f31d97fa493083ea21bd9b92c419012531c4e17ea6da0f65cacdcf5d0bd27", - "sha256:63b20738b5aac74e239622d2fe30df4fca4942a86e31bf47a81a0e94c14df94f", - "sha256:686a0c2f85f83463272ddffd4deb5e591c98aac1897d65e92319f729c320eece", - "sha256:6a962e04b8f91f8c4e5917e518d17958e3bdee71fd1d8b88cdce74dd0ebbf434", - "sha256:6ad6d10ed9b67a382b45f29ea028f92d25bc0bc1daf6c5b801b90b5aa70fb9ec", - "sha256:6f5cb257bc2ec58f437da2b37a8cd48f666db96d47b8a3115c29f316313654ff", - "sha256:6fe79f998a4052d79e1c30eeb7d6c1c1056ad33300f682465e1b4e9b5a188b78", - "sha256:7855426dfbddac81896b6e533ebefc0af2f132d4a47340cee6d22cac7190022d", - "sha256:7d5aaac37d19b2904bb9dfe12cdb08c8443e7ba7d2852894ad448d4b8f442863", - "sha256:801e9264d19643548651b9db361ce3287176671fb0117f96b5ac0ee1c3530d53", - "sha256:81eb57278deb6098a5b62e88ad8281b2ba09f2f1147c4767522353eaa6260b31", - "sha256:824d6c50492add5da9374875ce72db7a0733b29c2394890aef23d533106e2b15", - "sha256:8397a3817d7dcdd14bb266283cd1d6fc7264a48c186b986f32e86d86d35fbac5", - "sha256:848cd2a1df56ddbffeb375535fb62c9d1645dde33ca4d51341378b3f5954429b", - "sha256:84fc30f71689d7fc9168b92788abc977dc8cefa806909565fc2951d02f6b7d57", - "sha256:8619d6915b3b0b34420cf9b2bb6d81ef59d984cb0fde7544e9ece32b4b3043c3", - "sha256:8a854227cf581330ffa2c4824d96e52ee621dd571078a252c25e3a3b3d94a1b1", - "sha256:8be9e837ea9113676e5754b43b940b50cce76d9ed7d2461df1af39a8ee674d9f", - "sha256:928cecb0ef9d5a7946eb6ff58417ad2fe9375762382f1bf5c55e61645f2c43ad", - "sha256:957b4774373cf6f709359e5c8c4a0af9f6d7875db657adb0feaf8d6cb3c3964c", - "sha256:992f18e0ea248ee03b5a6e8b3b4738850ae7dbb172cc41c966462801cbf62cf7", - "sha256:9fc5fc1eeb029757349ad26bbc5880557389a03fa6ada41703db5e068881e5f2", - "sha256:a00862fb23195b6b8322f7d781b0dc1d82cb3bcac346d1e38689370cc1cc398b", - "sha256:a3a6ed1d525bfb91b3fc9b690c5a21bb52de28c018530ad85093cc488bee2dd2", - "sha256:a6327976c7c2f4ee6816eff196e25385ccc02cb81427952414a64811037bbc8b", - "sha256:a7409f968456111140c1c95301cadf071bd30a81cbd7ab829169fb9e3d72eae9", - "sha256:a825ec844298c791fd28ed14ed1bffc56a98d15b8c58a20e0e08c1f5f2bea1be", - "sha256:a8c1df72eb746f4136fe9a2e72b0c9dc1da1cbd23b5372f94b5820ff8ae30e0e", - "sha256:a9bd00dc3bc395a662900f33f74feb3e757429e545d831eef5bb280252631984", - "sha256:aa102d6d280a5455ad6a0f9e6d769989638718e938a6a0a2ff3f4a7ff8c62cc4", - "sha256:aaaea1e536f98754a6e5c56091baa1b6ce2f2700cc4a00b0d49eca8dea471074", - "sha256:ad4d7a90a92e528aadf4965d685c17dacff3df282db1121136c382dc0b6014d2", - "sha256:b8477c1ee4bd47c57d49621a062121c3023609f7a13b8a46953eb6c9716ca392", - "sha256:ba6f52cbc7809cd8d74604cce9c14868306ae4aa0282016b641c661f981a6e91", - "sha256:bac8d525a8dbc2a1507ec731d2867025d11ceadcb4dd421423a5d42c56818541", - "sha256:bef596fdaa8f26e3d66af846bbe77057237cb6e8efff8cd7cc8dff9a62278bbf", - "sha256:c0ec0ed476f77db9fb29bca17f0a8fcc7bc97ad4c6c1d8959c507decb22e8572", - "sha256:c38c9ddb6103ceae4e4498f9c08fac9b590c5c71b0370f98714768e22ac6fa66", - "sha256:c7224cab95645c7ab53791022ae77a4509472613e839dab722a72abe5a684575", - "sha256:c74018551e31269d56fab81a728f683667e7c28c04e807ba08f8c9e3bba32f14", - "sha256:ca06675212f94e7a610e85ca36948bb8fc023e458dd6c63ef71abfd482481aa5", - "sha256:d1d2532b340b692880261c15aee4dc94dd22ca5d61b9db9a8a361953d36410b1", - "sha256:d25039a474c4c72a5ad4b52495056f843a7ff07b632c1b92ea9043a3d9950f6e", - "sha256:d5ff2c858f5f6a42c2a8e751100f237c5e869cbde669a724f2062d4c4ef93551", - "sha256:d7d7f7de27b8944f1fee2c26a88b4dabc2409d2fea7a9ed3df79b67277644e17", - "sha256:d7eeb6d22331e2fd42fce928a81c697c9ee2d51400bd1a28803965883e13cead", - "sha256:d8a1c6c0be645c745a081c192e747c5de06e944a0d21245f4cf7c05e457c36e0", - "sha256:d8b889777de69897406c9fb0b76cdf2fd0f31267861ae7501d93003d55f54fbe", - "sha256:d9e09c9d74f4566e905a0b8fa668c58109f7624db96a2171f21747abc7524234", - "sha256:db8e58b9d79200c76956cefd14d5c90af54416ff5353c5bfd7cbe58818e26ef0", - "sha256:ddb2a5c08a4eaaba605340fdee8fc08e406c56617566d9643ad8bf6852778fc7", - "sha256:e0381b4ce23ff92f8170080c97678040fc5b08da85e9e292292aba67fdac6c34", - "sha256:e23a6d84d9d1738dbc6e38167776107e63307dfc8ad108e580548d1f2c587f42", - "sha256:e516dc8baf7b380e6c1c26792610230f37147bb754d6426462ab115a02944385", - "sha256:ea65804b5dc88dacd4a40279af0cdadcfe74b3e5b4c897aa0d81cf86927fee78", - "sha256:ec61d826d80fc293ed46c9dd26995921e3a82146feacd952ef0757236fc137be", - "sha256:ee04010f26d5102399bd17f8df8bc38dc7ccd7701dc77f4a68c5b8d733406958", - "sha256:f3bc6af6e2b8f92eced34ef6a96ffb248e863af20ef4fde9448cc8c9b858b749", - "sha256:f7d6b36dd2e029b6bcb8a13cf19664c7b8e19ab3a58e0fefbb5b8461447ed5ec" - ], - "markers": "python_version >= '3.7'", - "version": "==1.9.4" } }, - "develop": { - "aiohttp": { - "hashes": [ - "sha256:02ab6006ec3c3463b528374c4cdce86434e7b89ad355e7bf29e2f16b46c7dd6f", - "sha256:04fa38875e53eb7e354ece1607b1d2fdee2d175ea4e4d745f6ec9f751fe20c7c", - "sha256:0b0a6a36ed7e164c6df1e18ee47afbd1990ce47cb428739d6c99aaabfaf1b3af", - "sha256:0d406b01a9f5a7e232d1b0d161b40c05275ffbcbd772dc18c1d5a570961a1ca4", - "sha256:0e49b08eafa4f5707ecfb321ab9592717a319e37938e301d462f79b4e860c32a", - "sha256:0e7ba7ff228c0d9a2cd66194e90f2bca6e0abca810b786901a569c0de082f489", - "sha256:11cb254e397a82efb1805d12561e80124928e04e9c4483587ce7390b3866d213", - "sha256:11ff168d752cb41e8492817e10fb4f85828f6a0142b9726a30c27c35a1835f01", - "sha256:176df045597e674fa950bf5ae536be85699e04cea68fa3a616cf75e413737eb5", - "sha256:219a16763dc0294842188ac8a12262b5671817042b35d45e44fd0a697d8c8361", - "sha256:22698f01ff5653fe66d16ffb7658f582a0ac084d7da1323e39fd9eab326a1f26", - "sha256:237533179d9747080bcaad4d02083ce295c0d2eab3e9e8ce103411a4312991a0", - "sha256:289ba9ae8e88d0ba16062ecf02dd730b34186ea3b1e7489046fc338bdc3361c4", - "sha256:2c59e0076ea31c08553e868cec02d22191c086f00b44610f8ab7363a11a5d9d8", - "sha256:2c9376e2b09895c8ca8b95362283365eb5c03bdc8428ade80a864160605715f1", - "sha256:3135713c5562731ee18f58d3ad1bf41e1d8883eb68b363f2ffde5b2ea4b84cc7", - "sha256:3b9c7426923bb7bd66d409da46c41e3fb40f5caf679da624439b9eba92043fa6", - "sha256:3c0266cd6f005e99f3f51e583012de2778e65af6b73860038b968a0a8888487a", - "sha256:41473de252e1797c2d2293804e389a6d6986ef37cbb4a25208de537ae32141dd", - "sha256:4831df72b053b1eed31eb00a2e1aff6896fb4485301d4ccb208cac264b648db4", - "sha256:49f0c1b3c2842556e5de35f122fc0f0b721334ceb6e78c3719693364d4af8499", - "sha256:4b4c452d0190c5a820d3f5c0f3cd8a28ace48c54053e24da9d6041bf81113183", - "sha256:4ee8caa925aebc1e64e98432d78ea8de67b2272252b0a931d2ac3bd876ad5544", - "sha256:500f1c59906cd142d452074f3811614be04819a38ae2b3239a48b82649c08821", - "sha256:5216b6082c624b55cfe79af5d538e499cd5f5b976820eac31951fb4325974501", - "sha256:54311eb54f3a0c45efb9ed0d0a8f43d1bc6060d773f6973efd90037a51cd0a3f", - "sha256:54631fb69a6e44b2ba522f7c22a6fb2667a02fd97d636048478db2fd8c4e98fe", - "sha256:565760d6812b8d78d416c3c7cfdf5362fbe0d0d25b82fed75d0d29e18d7fc30f", - "sha256:598db66eaf2e04aa0c8900a63b0101fdc5e6b8a7ddd805c56d86efb54eb66672", - "sha256:5c4fa235d534b3547184831c624c0b7c1e262cd1de847d95085ec94c16fddcd5", - "sha256:69985d50a2b6f709412d944ffb2e97d0be154ea90600b7a921f95a87d6f108a2", - "sha256:69da0f3ed3496808e8cbc5123a866c41c12c15baaaead96d256477edf168eb57", - "sha256:6c93b7c2e52061f0925c3382d5cb8980e40f91c989563d3d32ca280069fd6a87", - "sha256:70907533db712f7aa791effb38efa96f044ce3d4e850e2d7691abd759f4f0ae0", - "sha256:81b77f868814346662c96ab36b875d7814ebf82340d3284a31681085c051320f", - "sha256:82eefaf1a996060602f3cc1112d93ba8b201dbf5d8fd9611227de2003dddb3b7", - "sha256:85c3e3c9cb1d480e0b9a64c658cd66b3cfb8e721636ab8b0e746e2d79a7a9eed", - "sha256:8a22a34bc594d9d24621091d1b91511001a7eea91d6652ea495ce06e27381f70", - "sha256:8cef8710fb849d97c533f259103f09bac167a008d7131d7b2b0e3a33269185c0", - "sha256:8d44e7bf06b0c0a70a20f9100af9fcfd7f6d9d3913e37754c12d424179b4e48f", - "sha256:8d7f98fde213f74561be1d6d3fa353656197f75d4edfbb3d94c9eb9b0fc47f5d", - "sha256:8d8e4450e7fe24d86e86b23cc209e0023177b6d59502e33807b732d2deb6975f", - "sha256:8fc49a87ac269d4529da45871e2ffb6874e87779c3d0e2ccd813c0899221239d", - "sha256:90ec72d231169b4b8d6085be13023ece8fa9b1bb495e4398d847e25218e0f431", - "sha256:91c742ca59045dce7ba76cab6e223e41d2c70d79e82c284a96411f8645e2afff", - "sha256:9b05d33ff8e6b269e30a7957bd3244ffbce2a7a35a81b81c382629b80af1a8bf", - "sha256:9b05d5cbe9dafcdc733262c3a99ccf63d2f7ce02543620d2bd8db4d4f7a22f83", - "sha256:9c5857612c9813796960c00767645cb5da815af16dafb32d70c72a8390bbf690", - "sha256:a34086c5cc285be878622e0a6ab897a986a6e8bf5b67ecb377015f06ed316587", - "sha256:ab221850108a4a063c5b8a70f00dd7a1975e5a1713f87f4ab26a46e5feac5a0e", - "sha256:b796b44111f0cab6bbf66214186e44734b5baab949cb5fb56154142a92989aeb", - "sha256:b8c3a67eb87394386847d188996920f33b01b32155f0a94f36ca0e0c635bf3e3", - "sha256:bcb6532b9814ea7c5a6a3299747c49de30e84472fa72821b07f5a9818bce0f66", - "sha256:bcc0ea8d5b74a41b621ad4a13d96c36079c81628ccc0b30cfb1603e3dfa3a014", - "sha256:bea94403a21eb94c93386d559bce297381609153e418a3ffc7d6bf772f59cc35", - "sha256:bff7e2811814fa2271be95ab6e84c9436d027a0e59665de60edf44e529a42c1f", - "sha256:c72444d17777865734aa1a4d167794c34b63e5883abb90356a0364a28904e6c0", - "sha256:c7b5d5d64e2a14e35a9240b33b89389e0035e6de8dbb7ffa50d10d8b65c57449", - "sha256:c7e939f1ae428a86e4abbb9a7c4732bf4706048818dfd979e5e2839ce0159f23", - "sha256:c88a15f272a0ad3d7773cf3a37cc7b7d077cbfc8e331675cf1346e849d97a4e5", - "sha256:c9110c06eaaac7e1f5562caf481f18ccf8f6fdf4c3323feab28a93d34cc646bd", - "sha256:ca7ca5abfbfe8d39e653870fbe8d7710be7a857f8a8386fc9de1aae2e02ce7e4", - "sha256:cae4c0c2ca800c793cae07ef3d40794625471040a87e1ba392039639ad61ab5b", - "sha256:cdefe289681507187e375a5064c7599f52c40343a8701761c802c1853a504558", - "sha256:cf2a0ac0615842b849f40c4d7f304986a242f1e68286dbf3bd7a835e4f83acfd", - "sha256:cfeadf42840c1e870dc2042a232a8748e75a36b52d78968cda6736de55582766", - "sha256:d737e69d193dac7296365a6dcb73bbbf53bb760ab25a3727716bbd42022e8d7a", - "sha256:d7481f581251bb5558ba9f635db70908819caa221fc79ee52a7f58392778c636", - "sha256:df9cf74b9bc03d586fc53ba470828d7b77ce51b0582d1d0b5b2fb673c0baa32d", - "sha256:e1f80197f8b0b846a8d5cf7b7ec6084493950d0882cc5537fb7b96a69e3c8590", - "sha256:ecca113f19d5e74048c001934045a2b9368d77b0b17691d905af18bd1c21275e", - "sha256:ee2527134f95e106cc1653e9ac78846f3a2ec1004cf20ef4e02038035a74544d", - "sha256:f27fdaadce22f2ef950fc10dcdf8048407c3b42b73779e48a4e76b3c35bca26c", - "sha256:f694dc8a6a3112059258a725a4ebe9acac5fe62f11c77ac4dcf896edfa78ca28", - "sha256:f800164276eec54e0af5c99feb9494c295118fc10a11b997bbb1348ba1a52065", - "sha256:ffcd828e37dc219a72c9012ec44ad2e7e3066bec6ff3aaa19e7d435dbf4032ca" - ], - "version": "==3.9.1" - }, - "aiosignal": { - "hashes": [ - "sha256:54cd96e15e1649b75d6c87526a6ff0b6c1b0dd3459f43d9ca11d48c339b68cfc", - "sha256:f8376fb07dd1e86a584e4fcdec80b36b7f81aac666ebc724e2c090300dd83b17" - ], - "markers": "python_version >= '3.7'", - "version": "==1.3.1" - }, - "attrs": { - "hashes": [ - "sha256:935dc3b529c262f6cf76e50877d35a4bd3c1de194fd41f47a2b7ae8f19971f30", - "sha256:99b87a485a5820b23b879f04c2305b44b951b502fd64be915879d77a7e8fc6f1" - ], - "markers": "python_version >= '3.7'", - "version": "==23.2.0" - }, - "black": { - "hashes": [ - "sha256:2220c470c22476ca9631337b0daae41be2b215599919b19d576a956ad38aca69", - "sha256:2a12829e372563ffff10c18c7aff1ef274da6afbc7bc8ccdb5fcc8ff84cab43f", - "sha256:3d139b9531e6bb6d129497a46475535d8289dddc861a5b980f908c36597b9817", - "sha256:41c0ce5cbdb701900c166bcca08ac941b64cf1d6967509e3caeab126da0ae0d0", - "sha256:4a159ae57f239f3f1ef6a78784b00c1c617c7bb188cc351b3017b9e0702df11c", - "sha256:4de8ba5825588017f90e63d7a25fc4df33a6342d1f4d628ad76130d8f4488fc6", - "sha256:623efdb54e7290ba75f7b822dfd2d8a47a55e721ae63aab671ccfd46b2ba6c5d", - "sha256:6b594b3ede60182215d258c76de2de64712d2e8424442ff4402276e22684abbe", - "sha256:6e3c74b35ea179bb69440286b81c309a64c34a032746a9eef3399dc3ce671352", - "sha256:87c8165fad00b03d9c1d400b1dd250479792f49d012807ee45162d323d04fc06", - "sha256:88d1c60bac2044a409154e895abb9d74c8ff5d034fb70f3e1f7c3ae96206bc0c", - "sha256:915a6b6b916fc66edec886fc71b60284e447d8fa39d22b879af7ae6efccca90f", - "sha256:a2c977909557439d0f17dc82adaea84e48374950d53416efc0b8451a594d42c3", - "sha256:ac226f37fc429b386d6447df6256dc958c28dd602f86f950072febf886995f80", - "sha256:b03cdf8a4e15929adf47e5e40a0ddeea1d63b65cf59c22553c12417a0c7ccbf4", - "sha256:c86ecd9d3da3d91e96da5f4a43d9c4fe35c5698b0633e91f171ba9468d112a8b", - "sha256:cad114d8673adab76b3602c28c461c613b7be3da28415500e42aed47415eb561", - "sha256:cb0a7ea9aa1c108924e31f1204a1e2534af255dbaa24ecbb8c05f47341a7b6f1", - "sha256:d30a018fc03fd1e83c75d40b8a156ef541d0b56b6403b63754e1cc96889849d9", - "sha256:d47b6530c55c092a9d841a12c8b3ad838bd639bebf6660a3df9dae83d4ab83c1", - "sha256:e8a054dbb8947718820be2ed6953d66b912ec2795f282725efdd08381a11b0d0", - "sha256:ec345caf15ae2c61540812500979e92f2989c6b6d4d13d21bdc82908043b3265" - ], - "index": "pypi", - "version": "==24.1a1" - }, - "click": { - "hashes": [ - "sha256:ae74fb96c20a0277a1d615f1e4d73c8414f5a98db8b799a7931d1582f3390c28", - "sha256:ca9853ad459e787e2192211578cc907e7594e294c7ccc834310722b41b9ca6de" - ], - "markers": "python_version >= '3.7'", - "version": "==8.1.7" - }, - "frozenlist": { - "hashes": [ - "sha256:04ced3e6a46b4cfffe20f9ae482818e34eba9b5fb0ce4056e4cc9b6e212d09b7", - "sha256:0633c8d5337cb5c77acbccc6357ac49a1770b8c487e5b3505c57b949b4b82e98", - "sha256:068b63f23b17df8569b7fdca5517edef76171cf3897eb68beb01341131fbd2ad", - "sha256:0c250a29735d4f15321007fb02865f0e6b6a41a6b88f1f523ca1596ab5f50bd5", - "sha256:1979bc0aeb89b33b588c51c54ab0161791149f2461ea7c7c946d95d5f93b56ae", - "sha256:1a4471094e146b6790f61b98616ab8e44f72661879cc63fa1049d13ef711e71e", - "sha256:1b280e6507ea8a4fa0c0a7150b4e526a8d113989e28eaaef946cc77ffd7efc0a", - "sha256:1d0ce09d36d53bbbe566fe296965b23b961764c0bcf3ce2fa45f463745c04701", - "sha256:20b51fa3f588ff2fe658663db52a41a4f7aa6c04f6201449c6c7c476bd255c0d", - "sha256:23b2d7679b73fe0e5a4560b672a39f98dfc6f60df63823b0a9970525325b95f6", - "sha256:23b701e65c7b36e4bf15546a89279bd4d8675faabc287d06bbcfac7d3c33e1e6", - "sha256:2471c201b70d58a0f0c1f91261542a03d9a5e088ed3dc6c160d614c01649c106", - "sha256:27657df69e8801be6c3638054e202a135c7f299267f1a55ed3a598934f6c0d75", - "sha256:29acab3f66f0f24674b7dc4736477bcd4bc3ad4b896f5f45379a67bce8b96868", - "sha256:32453c1de775c889eb4e22f1197fe3bdfe457d16476ea407472b9442e6295f7a", - "sha256:3a670dc61eb0d0eb7080890c13de3066790f9049b47b0de04007090807c776b0", - "sha256:3e0153a805a98f5ada7e09826255ba99fb4f7524bb81bf6b47fb702666484ae1", - "sha256:410478a0c562d1a5bcc2f7ea448359fcb050ed48b3c6f6f4f18c313a9bdb1826", - "sha256:442acde1e068288a4ba7acfe05f5f343e19fac87bfc96d89eb886b0363e977ec", - "sha256:48f6a4533887e189dae092f1cf981f2e3885175f7a0f33c91fb5b7b682b6bab6", - "sha256:4f57dab5fe3407b6c0c1cc907ac98e8a189f9e418f3b6e54d65a718aaafe3950", - "sha256:4f9c515e7914626b2a2e1e311794b4c35720a0be87af52b79ff8e1429fc25f19", - "sha256:55fdc093b5a3cb41d420884cdaf37a1e74c3c37a31f46e66286d9145d2063bd0", - "sha256:5667ed53d68d91920defdf4035d1cdaa3c3121dc0b113255124bcfada1cfa1b8", - "sha256:590344787a90ae57d62511dd7c736ed56b428f04cd8c161fcc5e7232c130c69a", - "sha256:5a7d70357e7cee13f470c7883a063aae5fe209a493c57d86eb7f5a6f910fae09", - "sha256:5c3894db91f5a489fc8fa6a9991820f368f0b3cbdb9cd8849547ccfab3392d86", - "sha256:5c849d495bf5154cd8da18a9eb15db127d4dba2968d88831aff6f0331ea9bd4c", - "sha256:64536573d0a2cb6e625cf309984e2d873979709f2cf22839bf2d61790b448ad5", - "sha256:693945278a31f2086d9bf3df0fe8254bbeaef1fe71e1351c3bd730aa7d31c41b", - "sha256:6db4667b187a6742b33afbbaf05a7bc551ffcf1ced0000a571aedbb4aa42fc7b", - "sha256:6eb73fa5426ea69ee0e012fb59cdc76a15b1283d6e32e4f8dc4482ec67d1194d", - "sha256:722e1124aec435320ae01ee3ac7bec11a5d47f25d0ed6328f2273d287bc3abb0", - "sha256:7268252af60904bf52c26173cbadc3a071cece75f873705419c8681f24d3edea", - "sha256:74fb4bee6880b529a0c6560885fce4dc95936920f9f20f53d99a213f7bf66776", - "sha256:780d3a35680ced9ce682fbcf4cb9c2bad3136eeff760ab33707b71db84664e3a", - "sha256:82e8211d69a4f4bc360ea22cd6555f8e61a1bd211d1d5d39d3d228b48c83a897", - "sha256:89aa2c2eeb20957be2d950b85974b30a01a762f3308cd02bb15e1ad632e22dc7", - "sha256:8aefbba5f69d42246543407ed2461db31006b0f76c4e32dfd6f42215a2c41d09", - "sha256:96ec70beabbd3b10e8bfe52616a13561e58fe84c0101dd031dc78f250d5128b9", - "sha256:9750cc7fe1ae3b1611bb8cfc3f9ec11d532244235d75901fb6b8e42ce9229dfe", - "sha256:9acbb16f06fe7f52f441bb6f413ebae6c37baa6ef9edd49cdd567216da8600cd", - "sha256:9d3e0c25a2350080e9319724dede4f31f43a6c9779be48021a7f4ebde8b2d742", - "sha256:a06339f38e9ed3a64e4c4e43aec7f59084033647f908e4259d279a52d3757d09", - "sha256:a0cb6f11204443f27a1628b0e460f37fb30f624be6051d490fa7d7e26d4af3d0", - "sha256:a7496bfe1da7fb1a4e1cc23bb67c58fab69311cc7d32b5a99c2007b4b2a0e932", - "sha256:a828c57f00f729620a442881cc60e57cfcec6842ba38e1b19fd3e47ac0ff8dc1", - "sha256:a9b2de4cf0cdd5bd2dee4c4f63a653c61d2408055ab77b151c1957f221cabf2a", - "sha256:b46c8ae3a8f1f41a0d2ef350c0b6e65822d80772fe46b653ab6b6274f61d4a49", - "sha256:b7e3ed87d4138356775346e6845cccbe66cd9e207f3cd11d2f0b9fd13681359d", - "sha256:b7f2f9f912dca3934c1baec2e4585a674ef16fe00218d833856408c48d5beee7", - "sha256:ba60bb19387e13597fb059f32cd4d59445d7b18b69a745b8f8e5db0346f33480", - "sha256:beee944ae828747fd7cb216a70f120767fc9f4f00bacae8543c14a6831673f89", - "sha256:bfa4a17e17ce9abf47a74ae02f32d014c5e9404b6d9ac7f729e01562bbee601e", - "sha256:c037a86e8513059a2613aaba4d817bb90b9d9b6b69aace3ce9c877e8c8ed402b", - "sha256:c302220494f5c1ebeb0912ea782bcd5e2f8308037b3c7553fad0e48ebad6ad82", - "sha256:c6321c9efe29975232da3bd0af0ad216800a47e93d763ce64f291917a381b8eb", - "sha256:c757a9dd70d72b076d6f68efdbb9bc943665ae954dad2801b874c8c69e185068", - "sha256:c99169d4ff810155ca50b4da3b075cbde79752443117d89429595c2e8e37fed8", - "sha256:c9c92be9fd329ac801cc420e08452b70e7aeab94ea4233a4804f0915c14eba9b", - "sha256:cc7b01b3754ea68a62bd77ce6020afaffb44a590c2289089289363472d13aedb", - "sha256:db9e724bebd621d9beca794f2a4ff1d26eed5965b004a97f1f1685a173b869c2", - "sha256:dca69045298ce5c11fd539682cff879cc1e664c245d1c64da929813e54241d11", - "sha256:dd9b1baec094d91bf36ec729445f7769d0d0cf6b64d04d86e45baf89e2b9059b", - "sha256:e02a0e11cf6597299b9f3bbd3f93d79217cb90cfd1411aec33848b13f5c656cc", - "sha256:e6a20a581f9ce92d389a8c7d7c3dd47c81fd5d6e655c8dddf341e14aa48659d0", - "sha256:e7004be74cbb7d9f34553a5ce5fb08be14fb33bc86f332fb71cbe5216362a497", - "sha256:e774d53b1a477a67838a904131c4b0eef6b3d8a651f8b138b04f748fccfefe17", - "sha256:edb678da49d9f72c9f6c609fbe41a5dfb9a9282f9e6a2253d5a91e0fc382d7c0", - "sha256:f146e0911cb2f1da549fc58fc7bcd2b836a44b79ef871980d605ec392ff6b0d2", - "sha256:f56e2333dda1fe0f909e7cc59f021eba0d2307bc6f012a1ccf2beca6ba362439", - "sha256:f9a3ea26252bd92f570600098783d1371354d89d5f6b7dfd87359d669f2109b5", - "sha256:f9aa1878d1083b276b0196f2dfbe00c9b7e752475ed3b682025ff20c1c1f51ac", - "sha256:fb3c2db03683b5767dedb5769b8a40ebb47d6f7f45b1b3e3b4b51ec8ad9d9825", - "sha256:fbeb989b5cc29e8daf7f976b421c220f1b8c731cbf22b9130d8815418ea45887", - "sha256:fde5bd59ab5357e3853313127f4d3565fc7dad314a74d7b5d43c22c6a5ed2ced", - "sha256:fe1a06da377e3a1062ae5fe0926e12b84eceb8a50b350ddca72dc85015873f74" - ], - "markers": "python_version >= '3.8'", - "version": "==1.4.1" - }, - "idna": { - "hashes": [ - "sha256:9ecdbbd083b06798ae1e86adcbfe8ab1479cf864e4ee30fe4e46a003d12491ca", - "sha256:c05567e9c24a6b9faaa835c4821bad0590fbb9d5779e7caa6e1cc4978e7eb24f" - ], - "markers": "python_version >= '3.5'", - "version": "==3.6" - }, - "multidict": { - "hashes": [ - "sha256:01a3a55bd90018c9c080fbb0b9f4891db37d148a0a18722b42f94694f8b6d4c9", - "sha256:0b1a97283e0c85772d613878028fec909f003993e1007eafa715b24b377cb9b8", - "sha256:0dfad7a5a1e39c53ed00d2dd0c2e36aed4650936dc18fd9a1826a5ae1cad6f03", - "sha256:11bdf3f5e1518b24530b8241529d2050014c884cf18b6fc69c0c2b30ca248710", - "sha256:1502e24330eb681bdaa3eb70d6358e818e8e8f908a22a1851dfd4e15bc2f8161", - "sha256:16ab77bbeb596e14212e7bab8429f24c1579234a3a462105cda4a66904998664", - "sha256:16d232d4e5396c2efbbf4f6d4df89bfa905eb0d4dc5b3549d872ab898451f569", - "sha256:21a12c4eb6ddc9952c415f24eef97e3e55ba3af61f67c7bc388dcdec1404a067", - "sha256:27c523fbfbdfd19c6867af7346332b62b586eed663887392cff78d614f9ec313", - "sha256:281af09f488903fde97923c7744bb001a9b23b039a909460d0f14edc7bf59706", - "sha256:33029f5734336aa0d4c0384525da0387ef89148dc7191aae00ca5fb23d7aafc2", - "sha256:3601a3cece3819534b11d4efc1eb76047488fddd0c85a3948099d5da4d504636", - "sha256:3666906492efb76453c0e7b97f2cf459b0682e7402c0489a95484965dbc1da49", - "sha256:36c63aaa167f6c6b04ef2c85704e93af16c11d20de1d133e39de6a0e84582a93", - "sha256:39ff62e7d0f26c248b15e364517a72932a611a9b75f35b45be078d81bdb86603", - "sha256:43644e38f42e3af682690876cff722d301ac585c5b9e1eacc013b7a3f7b696a0", - "sha256:4372381634485bec7e46718edc71528024fcdc6f835baefe517b34a33c731d60", - "sha256:458f37be2d9e4c95e2d8866a851663cbc76e865b78395090786f6cd9b3bbf4f4", - "sha256:45e1ecb0379bfaab5eef059f50115b54571acfbe422a14f668fc8c27ba410e7e", - "sha256:4b9d9e4e2b37daddb5c23ea33a3417901fa7c7b3dee2d855f63ee67a0b21e5b1", - "sha256:4ceef517eca3e03c1cceb22030a3e39cb399ac86bff4e426d4fc6ae49052cc60", - "sha256:4d1a3d7ef5e96b1c9e92f973e43aa5e5b96c659c9bc3124acbbd81b0b9c8a951", - "sha256:4dcbb0906e38440fa3e325df2359ac6cb043df8e58c965bb45f4e406ecb162cc", - "sha256:509eac6cf09c794aa27bcacfd4d62c885cce62bef7b2c3e8b2e49d365b5003fe", - "sha256:52509b5be062d9eafc8170e53026fbc54cf3b32759a23d07fd935fb04fc22d95", - "sha256:52f2dffc8acaba9a2f27174c41c9e57f60b907bb9f096b36b1a1f3be71c6284d", - "sha256:574b7eae1ab267e5f8285f0fe881f17efe4b98c39a40858247720935b893bba8", - "sha256:5979b5632c3e3534e42ca6ff856bb24b2e3071b37861c2c727ce220d80eee9ed", - "sha256:59d43b61c59d82f2effb39a93c48b845efe23a3852d201ed2d24ba830d0b4cf2", - "sha256:5a4dcf02b908c3b8b17a45fb0f15b695bf117a67b76b7ad18b73cf8e92608775", - "sha256:5cad9430ab3e2e4fa4a2ef4450f548768400a2ac635841bc2a56a2052cdbeb87", - "sha256:5fc1b16f586f049820c5c5b17bb4ee7583092fa0d1c4e28b5239181ff9532e0c", - "sha256:62501642008a8b9871ddfccbf83e4222cf8ac0d5aeedf73da36153ef2ec222d2", - "sha256:64bdf1086b6043bf519869678f5f2757f473dee970d7abf6da91ec00acb9cb98", - "sha256:64da238a09d6039e3bd39bb3aee9c21a5e34f28bfa5aa22518581f910ff94af3", - "sha256:666daae833559deb2d609afa4490b85830ab0dfca811a98b70a205621a6109fe", - "sha256:67040058f37a2a51ed8ea8f6b0e6ee5bd78ca67f169ce6122f3e2ec80dfe9b78", - "sha256:6748717bb10339c4760c1e63da040f5f29f5ed6e59d76daee30305894069a660", - "sha256:6b181d8c23da913d4ff585afd1155a0e1194c0b50c54fcfe286f70cdaf2b7176", - "sha256:6ed5f161328b7df384d71b07317f4d8656434e34591f20552c7bcef27b0ab88e", - "sha256:7582a1d1030e15422262de9f58711774e02fa80df0d1578995c76214f6954988", - "sha256:7d18748f2d30f94f498e852c67d61261c643b349b9d2a581131725595c45ec6c", - "sha256:7d6ae9d593ef8641544d6263c7fa6408cc90370c8cb2bbb65f8d43e5b0351d9c", - "sha256:81a4f0b34bd92df3da93315c6a59034df95866014ac08535fc819f043bfd51f0", - "sha256:8316a77808c501004802f9beebde51c9f857054a0c871bd6da8280e718444449", - "sha256:853888594621e6604c978ce2a0444a1e6e70c8d253ab65ba11657659dcc9100f", - "sha256:99b76c052e9f1bc0721f7541e5e8c05db3941eb9ebe7b8553c625ef88d6eefde", - "sha256:a2e4369eb3d47d2034032a26c7a80fcb21a2cb22e1173d761a162f11e562caa5", - "sha256:ab55edc2e84460694295f401215f4a58597f8f7c9466faec545093045476327d", - "sha256:af048912e045a2dc732847d33821a9d84ba553f5c5f028adbd364dd4765092ac", - "sha256:b1a2eeedcead3a41694130495593a559a668f382eee0727352b9a41e1c45759a", - "sha256:b1e8b901e607795ec06c9e42530788c45ac21ef3aaa11dbd0c69de543bfb79a9", - "sha256:b41156839806aecb3641f3208c0dafd3ac7775b9c4c422d82ee2a45c34ba81ca", - "sha256:b692f419760c0e65d060959df05f2a531945af31fda0c8a3b3195d4efd06de11", - "sha256:bc779e9e6f7fda81b3f9aa58e3a6091d49ad528b11ed19f6621408806204ad35", - "sha256:bf6774e60d67a9efe02b3616fee22441d86fab4c6d335f9d2051d19d90a40063", - "sha256:c048099e4c9e9d615545e2001d3d8a4380bd403e1a0578734e0d31703d1b0c0b", - "sha256:c5cb09abb18c1ea940fb99360ea0396f34d46566f157122c92dfa069d3e0e982", - "sha256:cc8e1d0c705233c5dd0c5e6460fbad7827d5d36f310a0fadfd45cc3029762258", - "sha256:d5e3fc56f88cc98ef8139255cf8cd63eb2c586531e43310ff859d6bb3a6b51f1", - "sha256:d6aa0418fcc838522256761b3415822626f866758ee0bc6632c9486b179d0b52", - "sha256:d6c254ba6e45d8e72739281ebc46ea5eb5f101234f3ce171f0e9f5cc86991480", - "sha256:d6d635d5209b82a3492508cf5b365f3446afb65ae7ebd755e70e18f287b0adf7", - "sha256:dcfe792765fab89c365123c81046ad4103fcabbc4f56d1c1997e6715e8015461", - "sha256:ddd3915998d93fbcd2566ddf9cf62cdb35c9e093075f862935573d265cf8f65d", - "sha256:ddff9c4e225a63a5afab9dd15590432c22e8057e1a9a13d28ed128ecf047bbdc", - "sha256:e41b7e2b59679edfa309e8db64fdf22399eec4b0b24694e1b2104fb789207779", - "sha256:e69924bfcdda39b722ef4d9aa762b2dd38e4632b3641b1d9a57ca9cd18f2f83a", - "sha256:ea20853c6dbbb53ed34cb4d080382169b6f4554d394015f1bef35e881bf83547", - "sha256:ee2a1ece51b9b9e7752e742cfb661d2a29e7bcdba2d27e66e28a99f1890e4fa0", - "sha256:eeb6dcc05e911516ae3d1f207d4b0520d07f54484c49dfc294d6e7d63b734171", - "sha256:f70b98cd94886b49d91170ef23ec5c0e8ebb6f242d734ed7ed677b24d50c82cf", - "sha256:fc35cb4676846ef752816d5be2193a1e8367b4c1397b74a565a9d0389c433a1d", - "sha256:ff959bee35038c4624250473988b24f846cbeb2c6639de3602c073f10410ceba" - ], - "markers": "python_version >= '3.7'", - "version": "==6.0.4" - }, - "mypy-extensions": { - "hashes": [ - "sha256:4392f6c0eb8a5668a69e23d168ffa70f0be9ccfd32b5cc2d26a34ae5b844552d", - "sha256:75dbf8955dc00442a438fc4d0666508a9a97b6bd41aa2f0ffe9d2f2725af0782" - ], - "markers": "python_version >= '3.5'", - "version": "==1.0.0" - }, - "packaging": { - "hashes": [ - "sha256:048fb0e9405036518eaaf48a55953c750c11e1a1b68e0dd1a9d62ed0c092cfc5", - "sha256:8c491190033a9af7e1d931d0b5dacc2ef47509b34dd0de67ed209b5203fc88c7" - ], - "markers": "python_version >= '3.7'", - "version": "==23.2" - }, - "pathspec": { - "hashes": [ - "sha256:a0d503e138a4c123b27490a4f7beda6a01c6f288df0e4a8b79c7eb0dc7b4cc08", - "sha256:a482d51503a1ab33b1c67a6c3813a26953dbdc71c31dacaef9a838c4e29f5712" - ], - "markers": "python_version >= '3.8'", - "version": "==0.12.1" - }, - "platformdirs": { - "hashes": [ - "sha256:11c8f37bcca40db96d8144522d925583bdb7a31f7b0e37e3ed4318400a8e2380", - "sha256:906d548203468492d432bcb294d4bc2fff751bf84971fbb2c10918cc206ee420" - ], - "markers": "python_version >= '3.8'", - "version": "==4.1.0" - }, - "yarl": { - "hashes": [ - "sha256:008d3e808d03ef28542372d01057fd09168419cdc8f848efe2804f894ae03e51", - "sha256:03caa9507d3d3c83bca08650678e25364e1843b484f19986a527630ca376ecce", - "sha256:07574b007ee20e5c375a8fe4a0789fad26db905f9813be0f9fef5a68080de559", - "sha256:09efe4615ada057ba2d30df871d2f668af661e971dfeedf0c159927d48bbeff0", - "sha256:0d2454f0aef65ea81037759be5ca9947539667eecebca092733b2eb43c965a81", - "sha256:0e9d124c191d5b881060a9e5060627694c3bdd1fe24c5eecc8d5d7d0eb6faabc", - "sha256:18580f672e44ce1238b82f7fb87d727c4a131f3a9d33a5e0e82b793362bf18b4", - "sha256:1f23e4fe1e8794f74b6027d7cf19dc25f8b63af1483d91d595d4a07eca1fb26c", - "sha256:206a55215e6d05dbc6c98ce598a59e6fbd0c493e2de4ea6cc2f4934d5a18d130", - "sha256:23d32a2594cb5d565d358a92e151315d1b2268bc10f4610d098f96b147370136", - "sha256:26a1dc6285e03f3cc9e839a2da83bcbf31dcb0d004c72d0730e755b33466c30e", - "sha256:29e0f83f37610f173eb7e7b5562dd71467993495e568e708d99e9d1944f561ec", - "sha256:2b134fd795e2322b7684155b7855cc99409d10b2e408056db2b93b51a52accc7", - "sha256:2d47552b6e52c3319fede1b60b3de120fe83bde9b7bddad11a69fb0af7db32f1", - "sha256:357495293086c5b6d34ca9616a43d329317feab7917518bc97a08f9e55648455", - "sha256:35a2b9396879ce32754bd457d31a51ff0a9d426fd9e0e3c33394bf4b9036b099", - "sha256:3777ce5536d17989c91696db1d459574e9a9bd37660ea7ee4d3344579bb6f129", - "sha256:3986b6f41ad22988e53d5778f91855dc0399b043fc8946d4f2e68af22ee9ff10", - "sha256:44d8ffbb9c06e5a7f529f38f53eda23e50d1ed33c6c869e01481d3fafa6b8142", - "sha256:49a180c2e0743d5d6e0b4d1a9e5f633c62eca3f8a86ba5dd3c471060e352ca98", - "sha256:4aa9741085f635934f3a2583e16fcf62ba835719a8b2b28fb2917bb0537c1dfa", - "sha256:4b21516d181cd77ebd06ce160ef8cc2a5e9ad35fb1c5930882baff5ac865eee7", - "sha256:4b3c1ffe10069f655ea2d731808e76e0f452fc6c749bea04781daf18e6039525", - "sha256:4c7d56b293cc071e82532f70adcbd8b61909eec973ae9d2d1f9b233f3d943f2c", - "sha256:4e9035df8d0880b2f1c7f5031f33f69e071dfe72ee9310cfc76f7b605958ceb9", - "sha256:54525ae423d7b7a8ee81ba189f131054defdb122cde31ff17477951464c1691c", - "sha256:549d19c84c55d11687ddbd47eeb348a89df9cb30e1993f1b128f4685cd0ebbf8", - "sha256:54beabb809ffcacbd9d28ac57b0db46e42a6e341a030293fb3185c409e626b8b", - "sha256:566db86717cf8080b99b58b083b773a908ae40f06681e87e589a976faf8246bf", - "sha256:5a2e2433eb9344a163aced6a5f6c9222c0786e5a9e9cac2c89f0b28433f56e23", - "sha256:5aef935237d60a51a62b86249839b51345f47564208c6ee615ed2a40878dccdd", - "sha256:604f31d97fa493083ea21bd9b92c419012531c4e17ea6da0f65cacdcf5d0bd27", - "sha256:63b20738b5aac74e239622d2fe30df4fca4942a86e31bf47a81a0e94c14df94f", - "sha256:686a0c2f85f83463272ddffd4deb5e591c98aac1897d65e92319f729c320eece", - "sha256:6a962e04b8f91f8c4e5917e518d17958e3bdee71fd1d8b88cdce74dd0ebbf434", - "sha256:6ad6d10ed9b67a382b45f29ea028f92d25bc0bc1daf6c5b801b90b5aa70fb9ec", - "sha256:6f5cb257bc2ec58f437da2b37a8cd48f666db96d47b8a3115c29f316313654ff", - "sha256:6fe79f998a4052d79e1c30eeb7d6c1c1056ad33300f682465e1b4e9b5a188b78", - "sha256:7855426dfbddac81896b6e533ebefc0af2f132d4a47340cee6d22cac7190022d", - "sha256:7d5aaac37d19b2904bb9dfe12cdb08c8443e7ba7d2852894ad448d4b8f442863", - "sha256:801e9264d19643548651b9db361ce3287176671fb0117f96b5ac0ee1c3530d53", - "sha256:81eb57278deb6098a5b62e88ad8281b2ba09f2f1147c4767522353eaa6260b31", - "sha256:824d6c50492add5da9374875ce72db7a0733b29c2394890aef23d533106e2b15", - "sha256:8397a3817d7dcdd14bb266283cd1d6fc7264a48c186b986f32e86d86d35fbac5", - "sha256:848cd2a1df56ddbffeb375535fb62c9d1645dde33ca4d51341378b3f5954429b", - "sha256:84fc30f71689d7fc9168b92788abc977dc8cefa806909565fc2951d02f6b7d57", - "sha256:8619d6915b3b0b34420cf9b2bb6d81ef59d984cb0fde7544e9ece32b4b3043c3", - "sha256:8a854227cf581330ffa2c4824d96e52ee621dd571078a252c25e3a3b3d94a1b1", - "sha256:8be9e837ea9113676e5754b43b940b50cce76d9ed7d2461df1af39a8ee674d9f", - "sha256:928cecb0ef9d5a7946eb6ff58417ad2fe9375762382f1bf5c55e61645f2c43ad", - "sha256:957b4774373cf6f709359e5c8c4a0af9f6d7875db657adb0feaf8d6cb3c3964c", - "sha256:992f18e0ea248ee03b5a6e8b3b4738850ae7dbb172cc41c966462801cbf62cf7", - "sha256:9fc5fc1eeb029757349ad26bbc5880557389a03fa6ada41703db5e068881e5f2", - "sha256:a00862fb23195b6b8322f7d781b0dc1d82cb3bcac346d1e38689370cc1cc398b", - "sha256:a3a6ed1d525bfb91b3fc9b690c5a21bb52de28c018530ad85093cc488bee2dd2", - "sha256:a6327976c7c2f4ee6816eff196e25385ccc02cb81427952414a64811037bbc8b", - "sha256:a7409f968456111140c1c95301cadf071bd30a81cbd7ab829169fb9e3d72eae9", - "sha256:a825ec844298c791fd28ed14ed1bffc56a98d15b8c58a20e0e08c1f5f2bea1be", - "sha256:a8c1df72eb746f4136fe9a2e72b0c9dc1da1cbd23b5372f94b5820ff8ae30e0e", - "sha256:a9bd00dc3bc395a662900f33f74feb3e757429e545d831eef5bb280252631984", - "sha256:aa102d6d280a5455ad6a0f9e6d769989638718e938a6a0a2ff3f4a7ff8c62cc4", - "sha256:aaaea1e536f98754a6e5c56091baa1b6ce2f2700cc4a00b0d49eca8dea471074", - "sha256:ad4d7a90a92e528aadf4965d685c17dacff3df282db1121136c382dc0b6014d2", - "sha256:b8477c1ee4bd47c57d49621a062121c3023609f7a13b8a46953eb6c9716ca392", - "sha256:ba6f52cbc7809cd8d74604cce9c14868306ae4aa0282016b641c661f981a6e91", - "sha256:bac8d525a8dbc2a1507ec731d2867025d11ceadcb4dd421423a5d42c56818541", - "sha256:bef596fdaa8f26e3d66af846bbe77057237cb6e8efff8cd7cc8dff9a62278bbf", - "sha256:c0ec0ed476f77db9fb29bca17f0a8fcc7bc97ad4c6c1d8959c507decb22e8572", - "sha256:c38c9ddb6103ceae4e4498f9c08fac9b590c5c71b0370f98714768e22ac6fa66", - "sha256:c7224cab95645c7ab53791022ae77a4509472613e839dab722a72abe5a684575", - "sha256:c74018551e31269d56fab81a728f683667e7c28c04e807ba08f8c9e3bba32f14", - "sha256:ca06675212f94e7a610e85ca36948bb8fc023e458dd6c63ef71abfd482481aa5", - "sha256:d1d2532b340b692880261c15aee4dc94dd22ca5d61b9db9a8a361953d36410b1", - "sha256:d25039a474c4c72a5ad4b52495056f843a7ff07b632c1b92ea9043a3d9950f6e", - "sha256:d5ff2c858f5f6a42c2a8e751100f237c5e869cbde669a724f2062d4c4ef93551", - "sha256:d7d7f7de27b8944f1fee2c26a88b4dabc2409d2fea7a9ed3df79b67277644e17", - "sha256:d7eeb6d22331e2fd42fce928a81c697c9ee2d51400bd1a28803965883e13cead", - "sha256:d8a1c6c0be645c745a081c192e747c5de06e944a0d21245f4cf7c05e457c36e0", - "sha256:d8b889777de69897406c9fb0b76cdf2fd0f31267861ae7501d93003d55f54fbe", - "sha256:d9e09c9d74f4566e905a0b8fa668c58109f7624db96a2171f21747abc7524234", - "sha256:db8e58b9d79200c76956cefd14d5c90af54416ff5353c5bfd7cbe58818e26ef0", - "sha256:ddb2a5c08a4eaaba605340fdee8fc08e406c56617566d9643ad8bf6852778fc7", - "sha256:e0381b4ce23ff92f8170080c97678040fc5b08da85e9e292292aba67fdac6c34", - "sha256:e23a6d84d9d1738dbc6e38167776107e63307dfc8ad108e580548d1f2c587f42", - "sha256:e516dc8baf7b380e6c1c26792610230f37147bb754d6426462ab115a02944385", - "sha256:ea65804b5dc88dacd4a40279af0cdadcfe74b3e5b4c897aa0d81cf86927fee78", - "sha256:ec61d826d80fc293ed46c9dd26995921e3a82146feacd952ef0757236fc137be", - "sha256:ee04010f26d5102399bd17f8df8bc38dc7ccd7701dc77f4a68c5b8d733406958", - "sha256:f3bc6af6e2b8f92eced34ef6a96ffb248e863af20ef4fde9448cc8c9b858b749", - "sha256:f7d6b36dd2e029b6bcb8a13cf19664c7b8e19ab3a58e0fefbb5b8461447ed5ec" - ], - "markers": "python_version >= '3.7'", - "version": "==1.9.4" - } - } + "develop": {} } diff --git a/rio/README.md b/rio/README.md deleted file mode 100644 index abbc21cd..00000000 --- a/rio/README.md +++ /dev/null @@ -1,13 +0,0 @@ -do the - -``` -pipenv run make download -``` - -BEFORE the done did - -``` -pipenv run make deploy -``` - -pls 'n' thx diff --git a/rio/autonomous/__init__.py b/rio/autonomous/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/rio/components/__init__.py b/rio/components/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/rio/constants.py b/rio/constants.py new file mode 100644 index 00000000..a7be1508 --- /dev/null +++ b/rio/constants.py @@ -0,0 +1,141 @@ +# 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. + +""" +The constants module is a convenience place for teams to hold robot-wide +numerical or boolean constants. Don't use this for any other purpose! +""" + + +import math + +from wpimath import units +from wpimath.geometry import Translation2d +from wpimath.kinematics import SwerveDrive4Kinematics +from wpimath.trajectory import TrapezoidProfileRadians + +from rev import CANSparkMax + + +class NeoMotorConstants: + kFreeSpeedRpm = 5676 + + +class DriveConstants: + # Driving Parameters - Note that these are not the maximum capable speeds of + # the robot, rather the allowed maximum speeds + kMaxSpeedMetersPerSecond = 4.8 + kMaxAngularSpeed = math.tau # radians per second + + kDirectionSlewRate = 1.2 # radians per second + kMagnitudeSlewRate = 1.8 # percent per second (1 = 100%) + kRotationalSlewRate = 2.0 # percent per second (1 = 100%) + + # Chassis configuration + kTrackWidth = units.inchesToMeters(26.5) + # Distance between centers of right and left wheels on robot + kWheelBase = units.inchesToMeters(26.5) + + # Distance between front and back wheels on robot + kModulePositions = [ + Translation2d(kWheelBase / 2, kTrackWidth / 2), + Translation2d(kWheelBase / 2, -kTrackWidth / 2), + Translation2d(-kWheelBase / 2, kTrackWidth / 2), + Translation2d(-kWheelBase / 2, -kTrackWidth / 2), + ] + kDriveKinematics = SwerveDrive4Kinematics(*kModulePositions) + + # Angular offsets of the modules relative to the chassis in radians + kFrontLeftChassisAngularOffset = -math.pi / 2 + kFrontRightChassisAngularOffset = 0 + kBackLeftChassisAngularOffset = math.pi + kBackRightChassisAngularOffset = math.pi / 2 + + # SPARK MAX CAN IDs + kFrontLeftDrivingCanId = 1 + kRearLeftDrivingCanId = 3 + kFrontRightDrivingCanId = 5 + kRearRightDrivingCanId = 7 + + kFrontLeftTurningCanId = 2 + kRearLeftTurningCanId = 4 + kFrontRightTurningCanId = 6 + kRearRightTurningCanId = 8 + + kGyroReversed = False + + +class ModuleConstants: + # The MAXSwerve module can be configured with one of three pinion gears: 12T, 13T, or 14T. + # This changes the drive speed of the module (a pinion gear with more teeth will result in a + # robot that drives faster). + kDrivingMotorPinionTeeth = 14 + + # Invert the turning encoder, since the output shaft rotates in the opposite direction of + # the steering motor in the MAXSwerve Module. + kTurningEncoderInverted = True + + # Calculations required for driving motor conversion factors and feed forward + kDrivingMotorFreeSpeedRps = NeoMotorConstants.kFreeSpeedRpm / 60 + kWheelDiameterMeters = 0.0762 + kWheelCircumferenceMeters = kWheelDiameterMeters * math.pi + # 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 teeth on the bevel pinion + kDrivingMotorReduction = (45.0 * 22) / (kDrivingMotorPinionTeeth * 15) + kDriveWheelFreeSpeedRps = ( + kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters + ) / kDrivingMotorReduction + + kDrivingEncoderPositionFactor = ( + kWheelDiameterMeters * math.pi + ) / kDrivingMotorReduction # meters + kDrivingEncoderVelocityFactor = ( + (kWheelDiameterMeters * math.pi) / kDrivingMotorReduction + ) / 60.0 # meters per second + + kTurningEncoderPositionFactor = math.tau # radian + kTurningEncoderVelocityFactor = math.tau / 60.0 # radians per second + + kTurningEncoderPositionPIDMinInput = 0 # radian + kTurningEncoderPositionPIDMaxInput = kTurningEncoderPositionFactor # radian + + kDrivingP = 0.04 + kDrivingI = 0 + kDrivingD = 0 + kDrivingFF = 1 / kDriveWheelFreeSpeedRps + kDrivingMinOutput = -1 + kDrivingMaxOutput = 1 + + kTurningP = 1 + kTurningI = 0 + kTurningD = 0 + kTurningFF = 0 + kTurningMinOutput = -1 + kTurningMaxOutput = 1 + + kDrivingMotorIdleMode = CANSparkMax.IdleMode.kBrake + kTurningMotorIdleMode = CANSparkMax.IdleMode.kBrake + + kDrivingMotorCurrentLimit = 50 # amp + kTurningMotorCurrentLimit = 20 # amp + + +class OIConstants: + kDriverControllerPort = 0 + kDriveDeadband = 0.05 + + +class AutoConstants: + kMaxSpeedMetersPerSecond = 3 + kMaxAccelerationMetersPerSecondSquared = 3 + kMaxAngularSpeedRadiansPerSecond = math.pi + kMaxAngularSpeedRadiansPerSecondSquared = math.pi + + kPXController = 1 + kPYController = 1 + kPThetaController = 1 + + # Constraint for the motion profiled robot angle controller + kThetaControllerConstraints = TrapezoidProfileRadians.Constraints( + kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared + ) diff --git a/rio/constants/__init__.py b/rio/constants/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/rio/constants/complexConstants.py b/rio/constants/complexConstants.py deleted file mode 100644 index c2662e5b..00000000 --- a/rio/constants/complexConstants.py +++ /dev/null @@ -1,83 +0,0 @@ -# Copyright (c) FIRST and other WPILib contributors. -# Open Source Software; you can modify and/or share it under the terms of -# the WPILib BSD license file in the root directory of this project. - -""" -The constants module is a convenience place for teams to hold robot-wide -numerical or boolean constants. Don't use this for any other purpose! -""" - - -import math - -from wpimath.geometry import Translation2d -from wpimath.kinematics import SwerveDrive4Kinematics -from wpimath.trajectory import TrapezoidProfileRadians - -from rev import CANSparkMax - -from constants.getConstants import getConstants - -constants = getConstants("simple_hardware") -neoConsts = constants["neo"] -driveConsts = constants["drive"] -moduleConsts = constants["module"] - - -class DriveConstants: - # Driving Parameters - Note that these are not the maximum capable speeds of - kMaxAngularSpeed = math.tau # radians per second - - # Distance between front and back wheels on robot - kModulePositions = [ - Translation2d(driveConsts["kWheelBase"] / 2, driveConsts["kTrackWidth"] / 2), - Translation2d(driveConsts["kWheelBase"] / 2, -driveConsts["kTrackWidth"] / 2), - Translation2d(-driveConsts["kWheelBase"] / 2, driveConsts["kTrackWidth"] / 2), - Translation2d(-driveConsts["kWheelBase"] / 2, -driveConsts["kTrackWidth"] / 2), - ] - kDriveKinematics = SwerveDrive4Kinematics(*kModulePositions) - - # Angular offsets of the modules relative to the chassis in radians - kFrontLeftChassisAngularOffset = -math.pi / 2 - kFrontRightChassisAngularOffset = 0 - kBackLeftChassisAngularOffset = math.pi - kBackRightChassisAngularOffset = math.pi / 2 - - -class ModuleConstants: - # Calculations required for driving motor conversion factors and feed forward - kDrivingMotorFreeSpeedRps = neoConsts["kFreeSpeedRpm"] / 60 - - kWheelCircumferenceMeters = moduleConsts["kWheelDiameterMeters"] * math.pi - # 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 teeth on the bevel pinion - kDrivingMotorReduction = (60 * 34) / (moduleConsts["kDrivingMotorPinionTeeth"] * 15) - kDriveWheelFreeSpeedRps = ( - kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters - ) / kDrivingMotorReduction - - kDrivingEncoderPositionFactor = ( - moduleConsts["kWheelDiameterMeters"] * math.pi - ) / kDrivingMotorReduction # meters - kDrivingEncoderVelocityFactor = ( - (moduleConsts["kWheelDiameterMeters"] * math.pi) / kDrivingMotorReduction - ) / 60.0 # meters per second - - kTurningEncoderPositionFactor = math.tau # radian - kTurningEncoderVelocityFactor = math.tau / 60.0 # radians per second - - kTurningEncoderPositionPIDMaxInput = kTurningEncoderPositionFactor # radian - - kDrivingFF = 1 / kDriveWheelFreeSpeedRps - - kDrivingMotorIdleMode = CANSparkMax.IdleMode.kBrake - kTurningMotorIdleMode = CANSparkMax.IdleMode.kBrake - - -class AutoConstants: - kMaxAngularSpeedRadiansPerSecond = math.pi - kMaxAngularSpeedRadiansPerSecondSquared = math.pi - - # Constraint for the motion profiled robot angle controller - kThetaControllerConstraints = TrapezoidProfileRadians.Constraints( - kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared - ) diff --git a/rio/constants/getConstants.py b/rio/constants/getConstants.py deleted file mode 100644 index d045df97..00000000 --- a/rio/constants/getConstants.py +++ /dev/null @@ -1,47 +0,0 @@ -# Tidal Force Robotics -# 2022 - -import os -from sre_constants import CATEGORY_WORD -from wpilib import RobotBase -import yaml -import logging - -""" -Use this file only for storing non-changing constants. -""" - - -def getConstants(identifier): - constants = {} - - # Clunky but it works - if RobotBase.isReal(): - path = "/home/lvuser/py/constants/" - else: - path = "constants/" - - retries = 3 - - try: - while retries > 0: - try: - # Try opening requested .yaml - with open(f"{path}{identifier}.yaml", "r") as yamlFile: - # Use yaml.safe_load to load the yaml into a dict - constants = yaml.safe_load(yamlFile) - except FileNotFoundError as e: - logging.info( - f"File {identifier} not found, currently in {os.getcwd()}, trying alternative locations... {retries} left. Trying to load path {path}{identifier}.yaml" - ) - path = os.getcwd().replace("tests", "constants/") - retries -= 1 - continue # Retry loop - break - except FileNotFoundError as e: - # If the file is not found, report it! - logging.error(f"{identifier} config not found!") - raise e - - # When all is done, return the important bits! - return constants diff --git a/rio/constants/robotHardware.yaml b/rio/constants/robotHardware.yaml deleted file mode 100644 index 75de7fc2..00000000 --- a/rio/constants/robotHardware.yaml +++ /dev/null @@ -1,69 +0,0 @@ -# There are four swerve modules that make up a drivetrain -swerve: - modules: - frontLeft: - CANSteerID: 2 # Configured 1/19 by Keegan - CANDriveID: 1 # Configured 1/19 by Keegan - Pose: [-1, 1] # Configured Never by Nobody - - frontRight: - CANSteerID: 99 # 6 Configured 1/19 by Keegan - CANDriveID: 98 # 5 Configured 1/19 by Keegan - Pose: [1, 1] # Configured Never by Nobody - - rearLeft: - CANSteerID: 4 # Configured 1/19 by Keegan - CANDriveID: 3 # Configured 1/19 by Keegan - Pose: [-1, -1] # Configured Never by Nobody - - rearRight: - CANSteerID: 8 # Configured 1/19 by Keegan - CANDriveID: 7 # Configured 1/19 by Keegan - Pose: [1, -1] # Configured Never by Nobody - # General constants all modules share - - kDrivingP: 0 # Configured Never by Nobody - kDrivingI: 0 # Configured Never by Nobody - kDrivingD: 0 # Configured Never by Nobody - kDrivingFF: 0 # Configured Never by Nobody - kDrivingMinOutput: -1 # Configured Never by Nobody - kDrivingMaxOutput: 1 # Configured Never by Nobody - - kTurningP: 2.65 # Updated 1/20 by Keegan - kTurningI: 0.012 # Updated 1/20 by Keegan - kTurningD: 1.5 # Updated 1/20 by Keegan - kTurningFF: 0 # Configured Never by Nobody - kTurningMinOutput: -1 # Configured Never by Nobody - kTurningMaxOutput: 1 # Configured Never by Nobody - - # These are important translation factors to convert native units to m/s, do not leave at 1! - drivePosConversionFactor: 1 # Drive motor conversion factor (position) - driveVelConversionFactor: 1 # Drive motor conversion factor (velocity) - steerPosConversionFactor: 1 # Drive motor conversion factor (position) - steerVelConversionFactor: 1 # Drive motor conversion factor (velocity) - - kWheelDiameterMeters: 0.09525 # Configured 1/17 by Keegan - - # Swerve PID - minimumPIDInput: 5 # Configured Jan 16 by Joe - - kDrivingMotorCurrentLimit: 150 # Configured Never by Nobody - kTurningMotorCurrentLimit: 50 # Configured Never by Nobody - - kDrivingMotorPinionTeeth: 17 - -neo: - kFreeSpeedRpm: 5676 # Configured Never by Nobody - -drive: - kMaxSpeedMetersPerSecond: 4.8 # Configured Never by Nobody - - kDirectionSlewRate: 1.2 # Configured Never by Nobody - kMagnitudeSlewRate: 1.8 # Configured Never by Nobody - kRotationalSlewRate: 2.0 # Configured Never by Nobody - kMaxAngularSpeed: 4.0 # Configured Jan 16 by Joe (just a guess!) - - kTrackWidth: 0.0254 # Configured Never by Nobody - kWheelBase: 0.51425 # Configured Never by Nobody - - kGyroReversed: False # Configured Never by Nobody diff --git a/rio/constants/simple_auto.yaml b/rio/constants/simple_auto.yaml deleted file mode 100644 index ad173d4e..00000000 --- a/rio/constants/simple_auto.yaml +++ /dev/null @@ -1,7 +0,0 @@ -auto: - kMaxSpeedMetersPerSecond: 3 # Configured Never by Nobody - kMaxAccelerationMetersPerSecondSquared: 3 # Configured Never by Nobody - - kPXController: 1 # Configured Never by Nobody - kPYController: 1 # Configured Never by Nobody - kPThetaController: 1 # Configured Never by Nobody \ No newline at end of file diff --git a/rio/pyproject.toml b/rio/pyproject.toml new file mode 100644 index 00000000..1216941e --- /dev/null +++ b/rio/pyproject.toml @@ -0,0 +1,30 @@ +# +# Use this configuration file to control what RobotPy packages are installed +# on your RoboRIO +# + +[tool.robotpy] + +# Version of robotpy this project depends on +robotpy_version = "2024.1.1.1" + +# Which extra RobotPy components should be installed +# -> equivalent to `pip install robotpy[extra1, ...] +robotpy_extras = [ + # "all" + # "apriltag" + # "commands2" + # "cscore" + # "navx" + # "pathplannerlib" + # "phoenix5" + # "phoenix6" + # "playingwithfusion" + # "rev" + # "romi" + # "sim" + # "xrp" +] + +# Other pip packages to install +requires = [] diff --git a/rio/robot.py b/rio/robot.py index 916e20bd..6b4fedb7 100644 --- a/rio/robot.py +++ b/rio/robot.py @@ -5,82 +5,32 @@ # the WPILib BSD license file in the root directory of this project. # -# wpilib +import commands2 import wpilib -import wpimath -import wpilib.drive -import wpimath.filter -import wpimath.controller -# drivetrain -from components.drivetrain import Drivetrain +from robotcontainer import RobotContainer -class UnnamedToaster(wpilib.TimedRobot): - def robotInit(self) -> None: - """Robot initialization function""" - self.controller = wpilib.Joystick(0) - self.drivetrain = Drivetrain() +class MyRobot(commands2.TimedCommandRobot): + def robotInit(self): + # Instantiate our RobotContainer. This will perform all our button bindings, and put our + # autonomous chooser on the dashboard. + self.container = RobotContainer() + self.autonomousCommand = None - # Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1. - self.xspeedLimiter = wpimath.filter.SlewRateLimiter(3) - self.yspeedLimiter = wpimath.filter.SlewRateLimiter(3) - self.rotLimiter = wpimath.filter.SlewRateLimiter(3) + def autonomousInit(self) -> None: + self.autonomousCommand = self.container.getAutonomousCommand() - def disabledPeriodic(self) -> None: - """ - Runs when the robot is in DISABLED mode. - """ + if self.autonomousCommand: + self.autonomousCommand.schedule() - # Run periodic tasks - self.drivetrain.periodic() + def teleopInit(self) -> None: + if self.autonomousCommand: + self.autonomousCommand.cancel() - def autonomousPeriodic(self) -> None: - """ - Runs when the robot is in AUTONOMOUS mode. - """ + def testInit(self) -> None: + commands2.CommandScheduler.getInstance().cancelAll() - self.driveWithJoystick(False) - # Run periodic tasks - self.drivetrain.periodic() - - def teleopPeriodic(self) -> None: - """ - Runs when the robot is in TELEOP mode. - """ - - self.driveWithJoystick(True) - - # Run periodic tasks - self.drivetrain.periodic() - - def driveWithJoystick(self, fieldRelative: bool) -> None: - # Get the x speed. We are inverting this because Xbox controllers return - # negative values when we push forward. - xSpeed = -self.xspeedLimiter.calculate( - wpimath.applyDeadband( - self.controller.getRawAxis(0), 0.02 - ) # TODO: What axis?! Make a controls.yaml! - ) - - # Get the y speed or sideways/strafe speed. We are inverting this because - # we want a positive value when we pull to the left. Xbox controllers - # return positive values when you pull to the right by default. - ySpeed = -self.yspeedLimiter.calculate( - wpimath.applyDeadband( - self.controller.getRawAxis(1), 0.02 - ) # TODO: What axis?! Make a controls.yaml! - ) - - # Get the rate of angular rotation. We are inverting this because we want a - # positive value when we pull to the left (remember, CCW is positive in - # mathematics). Xbox controllers return positive values when you pull to - # the right by default. - rot = -self.rotLimiter.calculate( - wpimath.applyDeadband( - self.controller.getRawAxis(2), 0.02 - ) # TODO: What axis?! Make a controls.yaml! - ) - - self.drivetrain.drive(xSpeed, ySpeed, rot, fieldRelative, self.getPeriod()) +if __name__ == "__main__": + wpilib.run(MyRobot) diff --git a/rio/robotcontainer.py b/rio/robotcontainer.py new file mode 100644 index 00000000..cd8a1ea4 --- /dev/null +++ b/rio/robotcontainer.py @@ -0,0 +1,120 @@ +import math + +import commands2 +import wpimath +import wpilib + +from commands2 import cmd +from wpimath.controller import PIDController, ProfiledPIDControllerRadians +from wpimath.geometry import Pose2d, Rotation2d, Translation2d +from wpimath.trajectory import TrajectoryConfig, TrajectoryGenerator + +from constants import AutoConstants, DriveConstants, OIConstants +from subsystems.drivesubsystem import DriveSubsystem + + +class RobotContainer: + """ + This class is where the bulk of the robot should be declared. Since Command-based is a + "declarative" paradigm, very little robot logic should actually be handled in the :class:`.Robot` + periodic methods (other than the scheduler calls). Instead, the structure of the robot (including + subsystems, commands, and button mappings) should be declared here. + """ + + def __init__(self) -> None: + # The robot's subsystems + self.robotDrive = DriveSubsystem() + + # The driver's controller + self.driverController = wpilib.XboxController(OIConstants.kDriverControllerPort) + + # Configure the button bindings + self.configureButtonBindings() + + # Configure default commands + self.robotDrive.setDefaultCommand( + # The left stick controls translation of the robot. + # Turning is controlled by the X axis of the right stick. + commands2.RunCommand( + lambda: self.robotDrive.drive( + -wpimath.applyDeadband( + self.driverController.getLeftY(), OIConstants.kDriveDeadband + ), + -wpimath.applyDeadband( + self.driverController.getLeftX(), OIConstants.kDriveDeadband + ), + -wpimath.applyDeadband( + self.driverController.getRightX(), OIConstants.kDriveDeadband + ), + True, + True, + ), + self.robotDrive, + ) + ) + + def configureButtonBindings(self) -> None: + """ + Use this method to define your button->command mappings. Buttons can be created by + instantiating a :GenericHID or one of its subclasses (Joystick or XboxController), + and then passing it to a JoystickButton. + """ + + def disablePIDSubsystems(self) -> None: + """Disables all ProfiledPIDSubsystem and PIDSubsystem instances. + This should be called on robot disable to prevent integral windup.""" + + def getAutonomousCommand(self) -> commands2.Command: + """Use this to pass the autonomous command to the main {@link Robot} class. + + :returns: the command to run in autonomous + """ + # Create config for trajectory + config = TrajectoryConfig( + AutoConstants.kMaxSpeedMetersPerSecond, + AutoConstants.kMaxAccelerationMetersPerSecondSquared, + ) + # Add kinematics to ensure max speed is actually obeyed + config.setKinematics(DriveConstants.kDriveKinematics) + + # An example trajectory to follow. All units in meters. + exampleTrajectory = TrajectoryGenerator.generateTrajectory( + # Start at the origin facing the +X direction + Pose2d(0, 0, Rotation2d(0)), + # Pass through these two interior waypoints, making an 's' curve path + [Translation2d(1, 1), Translation2d(2, -1)], + # End 3 meters straight ahead of where we started, facing forward + Pose2d(3, 0, Rotation2d(0)), + config, + ) + + thetaController = ProfiledPIDControllerRadians( + AutoConstants.kPThetaController, + 0, + 0, + AutoConstants.kThetaControllerConstraints, + ) + thetaController.enableContinuousInput(-math.pi, math.pi) + + swerveControllerCommand = commands2.SwerveControllerCommand( + exampleTrajectory, + self.robotDrive.getPose, # Functional interface to feed supplier + DriveConstants.kDriveKinematics, + # Position controllers + PIDController(AutoConstants.kPXController, 0, 0), + PIDController(AutoConstants.kPYController, 0, 0), + thetaController, + self.robotDrive.setModuleStates, + (self.robotDrive,), + ) + + # Reset odometry to the starting pose of the trajectory. + self.robotDrive.resetOdometry(exampleTrajectory.initialPose()) + + # Run path following command, then stop at the end. + return swerveControllerCommand.andThen( + cmd.run( + lambda: self.robotDrive.drive(0, 0, 0, False, False), + self.robotDrive, + ) + ) diff --git a/rio/simgui-ds.json b/rio/simgui-ds.json index 6b37128a..73cc713c 100644 --- a/rio/simgui-ds.json +++ b/rio/simgui-ds.json @@ -3,26 +3,61 @@ { "axisConfig": [ { - "decKey": 83, - "incKey": 87 + "decKey": 65, + "incKey": 68 }, { - "decKey": 68, - "incKey": 65 + "decKey": 87, + "incKey": 83 }, { - "decKey": 81, - "incKey": 69 + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 } ], "axisCount": 3, - "buttonCount": 10, + "buttonCount": 4, "buttonKeys": [ 90, 88, 67, 86 ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], "povCount": 0 }, { @@ -52,17 +87,6 @@ "axisCount": 0, "buttonCount": 0, "povCount": 0 - }, - { - "axisCount": 0, - "buttonCount": 0, - "povCount": 0 - } - ], - "robotJoysticks": [ - { - "guid": "Keyboard0", - "name": "Driver Joystick" } ] } diff --git a/rio/simgui-window.json b/rio/simgui-window.json index 934da4c9..667bac5b 100644 --- a/rio/simgui-window.json +++ b/rio/simgui-window.json @@ -10,16 +10,8 @@ "style": "0", "userScale": "2", "width": "1280", - "xpos": "636", - "ypos": "329" - } - }, - "Table": { - "0xE56EC1C2,4": { - "Column 0 Weight": "1.0000", - "Column 1 Weight": "1.0000", - "Column 2 Weight": "1.0000", - "Column 3 Weight": "1.0000" + "xpos": "447", + "ypos": "289" } }, "Window": { @@ -31,7 +23,7 @@ "###Joysticks": { "Collapsed": "0", "Pos": "250,465", - "Size": "796,138" + "Size": "796,73" }, "###NetworkTables": { "Collapsed": "0", @@ -50,12 +42,12 @@ }, "###System Joysticks": { "Collapsed": "0", - "Pos": "16,322", + "Pos": "5,350", "Size": "192,218" }, "###Timing": { "Collapsed": "0", - "Pos": "7,129", + "Pos": "5,150", "Size": "135,150" }, "Debug##Default": { diff --git a/rio/simgui.json b/rio/simgui.json index 4880fa89..5f9d2754 100644 --- a/rio/simgui.json +++ b/rio/simgui.json @@ -1,46 +1,10 @@ { "NTProvider": { "types": { - "/FMSInfo": "FMSInfo", - "/SmartDashboard/Field": "Field2d" - } - }, - "NetworkTables": { - "transitory": { - "SmartDashboard": { - "Audio": { - "open": true - }, - "Swerve": { - "open": true - }, - "open": true - } + "/FMSInfo": "FMSInfo" } }, "NetworkTables Info": { - "Clients": { - "open": true - }, - "Connections": { - "open": true - }, "visible": true - }, - "NetworkTables View": { - "visible": false - }, - "Window": { - "/SmartDashboard/Autonomous": { - "window": { - "visible": true - } - }, - "/SmartDashboard/Field": { - "image": "../dashboard/src/assets/field.png", - "window": { - "visible": true - } - } } } diff --git a/rio/components/drivetrain.py b/rio/subsystems/drivesubsystem.py similarity index 65% rename from rio/components/drivetrain.py rename to rio/subsystems/drivesubsystem.py index 86850101..732d1ca6 100644 --- a/rio/components/drivetrain.py +++ b/rio/subsystems/drivesubsystem.py @@ -1,96 +1,67 @@ import math import typing + import wpilib -import logging -from wpilib import Field2d, DriverStation +from commands2 import Subsystem from wpimath.filter import SlewRateLimiter -from wpimath.geometry import Pose2d, Rotation2d, Translation2d +from wpimath.geometry import Pose2d, Rotation2d from wpimath.kinematics import ( ChassisSpeeds, SwerveModuleState, SwerveDrive4Kinematics, SwerveDrive4Odometry, ) -from navx import AHRS - -from ntcore import NetworkTableInstance -# from constants.complexConstants import self.driveConstants +from constants import DriveConstants import swerveutils -from .mikeSwerveModule import MikeSwerveModule +from .maxswervemodule import MAXSwerveModule -from constants.getConstants import getConstants - -class Drivetrain: +class DriveSubsystem(Subsystem): def __init__(self) -> None: super().__init__() - # Get Hardware constants - hardwareConsts = getConstants("robotHardware") - - # Get Swerve constants - serveConstants = hardwareConsts["swerve"] - - # Get Drive constants - self.driveConstants = hardwareConsts["drive"] - - # Get Swerve Modules - moduleConstants = serveConstants["modules"] - - # Configure networktables - self.nt = NetworkTableInstance.getDefault() - self.sd = self.nt.getTable("SmartDashboard") - - # Build constants (these are copied from old complexConstants.py) - modulePositions = [] # All the module positions - for module in moduleConstants: # For every module in the moduleConstants list - # Create a translation2d that represents its pose - modulePositions.append( - Translation2d( - moduleConstants[module]["Pose"][0], - moduleConstants[module]["Pose"][1], - ) - ) - - # Build the kinematics - self.kDriveKinematics = SwerveDrive4Kinematics(*modulePositions) - - # Create Swerve Modules - self.frontLeft = MikeSwerveModule( - serveConstants, - moduleConstants["frontLeft"], + # Create MAXSwerveModules + self.frontLeft = MAXSwerveModule( + DriveConstants.kFrontLeftDrivingCanId, + DriveConstants.kFrontLeftTurningCanId, + DriveConstants.kFrontLeftChassisAngularOffset, ) - self.frontRight = MikeSwerveModule( - serveConstants, - moduleConstants["frontRight"], + + self.frontRight = MAXSwerveModule( + DriveConstants.kFrontRightDrivingCanId, + DriveConstants.kFrontRightTurningCanId, + DriveConstants.kFrontRightChassisAngularOffset, ) - self.rearLeft = MikeSwerveModule( - serveConstants, - moduleConstants["rearLeft"], + + self.rearLeft = MAXSwerveModule( + DriveConstants.kRearLeftDrivingCanId, + DriveConstants.kRearLeftTurningCanId, + DriveConstants.kBackLeftChassisAngularOffset, ) - self.rearRight = MikeSwerveModule( - serveConstants, - moduleConstants["rearRight"], + + self.rearRight = MAXSwerveModule( + DriveConstants.kRearRightDrivingCanId, + DriveConstants.kRearRightTurningCanId, + DriveConstants.kBackRightChassisAngularOffset, ) # The gyro sensor - self.gyro = AHRS.create_spi() + self.gyro = wpilib.ADIS16448_IMU() # Slew rate filter variables for controlling lateral acceleration self.currentRotation = 0.0 self.currentTranslationDir = 0.0 self.currentTranslationMag = 0.0 - self.magLimiter = SlewRateLimiter(self.driveConstants["kMagnitudeSlewRate"]) - self.rotLimiter = SlewRateLimiter(self.driveConstants["kRotationalSlewRate"]) + self.magLimiter = SlewRateLimiter(DriveConstants.kMagnitudeSlewRate) + self.rotLimiter = SlewRateLimiter(DriveConstants.kRotationalSlewRate) self.prevTime = wpilib.Timer.getFPGATimestamp() # Odometry class for tracking robot pose - self.field = Field2d() self.odometry = SwerveDrive4Odometry( - self.kDriveKinematics, + DriveConstants.kDriveKinematics, Rotation2d.fromDegrees(self.gyro.getAngle()), ( self.frontLeft.getPosition(), @@ -101,13 +72,7 @@ def __init__(self) -> None: ) def periodic(self) -> None: - """ - Schedule drivetrain's periodic block in Robot.py - at a regular rate to take care of 'background' tasks - like updating the dashboard and maintaining odometry. - """ - - # Update the odometry in the periodic bloc + # Update the odometry in the periodic block self.odometry.update( Rotation2d.fromDegrees(self.gyro.getAngle()), ( @@ -117,33 +82,6 @@ def periodic(self) -> None: self.rearRight.getPosition(), ), ) - # desired - self.sd.putNumber( - "Swerve/FL desired", self.frontLeft.desiredState.angle.degrees() - ) - self.sd.putNumber( - "Swerve/FR desired", self.frontRight.desiredState.angle.degrees() - ) - self.sd.putNumber( - "Swerve/RL desired", self.rearLeft.desiredState.angle.degrees() - ) - self.sd.putNumber( - "Swerve/RR desired", self.rearRight.desiredState.angle.degrees() - ) - - # actual - self.sd.putNumber( - "Swerve/FL", self.frontLeft.getState().angle.degrees() * (360 / 60) - ) - self.sd.putNumber( - "Swerve/FR", self.frontRight.getState().angle.degrees() * (360 / 60) - ) - self.sd.putNumber( - "Swerve/RL", self.rearLeft.getState().angle.degrees() * (360 / 60) - ) - self.sd.putNumber( - "Swerve/RR", self.rearRight.getState().angle.degrees() * (360 / 60) - ) def getPose(self) -> Pose2d: """Returns the currently-estimated pose of the robot. @@ -198,8 +136,7 @@ def drive( # Calculate the direction slew rate based on an estimate of the lateral acceleration if self.currentTranslationMag != 0.0: directionSlewRate = abs( - self.driveConstants["kDirectionSlewRate"] - / self.currentTranslationMag + DriveConstants.kDirectionSlewRate / self.currentTranslationMag ) else: directionSlewRate = 500.0 @@ -255,15 +192,11 @@ def drive( self.currentRotation = rot # Convert the commanded speeds into the correct units for the drivetrain - xSpeedDelivered = ( - xSpeedCommanded * self.driveConstants["kMaxSpeedMetersPerSecond"] - ) - ySpeedDelivered = ( - ySpeedCommanded * self.driveConstants["kMaxSpeedMetersPerSecond"] - ) - rotDelivered = self.currentRotation * self.driveConstants["kMaxAngularSpeed"] + xSpeedDelivered = xSpeedCommanded * DriveConstants.kMaxSpeedMetersPerSecond + ySpeedDelivered = ySpeedCommanded * DriveConstants.kMaxSpeedMetersPerSecond + rotDelivered = self.currentRotation * DriveConstants.kMaxAngularSpeed - swerveModuleStates = self.kDriveKinematics.toSwerveModuleStates( + swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( ChassisSpeeds.fromFieldRelativeSpeeds( xSpeedDelivered, ySpeedDelivered, @@ -274,7 +207,7 @@ def drive( else ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered) ) fl, fr, rl, rr = SwerveDrive4Kinematics.desaturateWheelSpeeds( - swerveModuleStates, self.driveConstants["kMaxSpeedMetersPerSecond"] + swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond ) self.frontLeft.setDesiredState(fl) self.frontRight.setDesiredState(fr) @@ -301,7 +234,7 @@ def setModuleStates( :param desiredStates: The desired SwerveModule states. """ fl, fr, rl, rr = SwerveDrive4Kinematics.desaturateWheelSpeeds( - desiredStates, self.driveConsts["kMaxSpeedMetersPerSecond"] + desiredStates, DriveConstants.kMaxSpeedMetersPerSecond ) self.frontLeft.setDesiredState(fl) self.frontRight.setDesiredState(fr) @@ -331,6 +264,4 @@ def getTurnRate(self) -> float: :returns: The turn rate of the robot, in degrees per second """ - return self.gyro.getRate() * ( - -1.0 if self.driveConsts["kGyroReversed"] else 1.0 - ) + return self.gyro.getRate() * (-1.0 if DriveConstants.kGyroReversed else 1.0) diff --git a/rio/components/mikeSwerveModule.py b/rio/subsystems/maxswervemodule.py similarity index 69% rename from rio/components/mikeSwerveModule.py rename to rio/subsystems/maxswervemodule.py index 91490f4e..3579caf2 100644 --- a/rio/components/mikeSwerveModule.py +++ b/rio/subsystems/maxswervemodule.py @@ -1,29 +1,28 @@ -import math -import rev - -from rev import CANSparkMax, SparkMaxAbsoluteEncoder, CANSparkLowLevel +from rev import CANSparkMax, SparkMaxAbsoluteEncoder from wpimath.geometry import Rotation2d from wpimath.kinematics import SwerveModuleState, SwerveModulePosition -# from constants.complexConstants import ModuleConstants -from constants.getConstants import getConstants +from constants import ModuleConstants -class MikeSwerveModule: - def __init__(self, swerveConfig: dict, moduleConfig: str) -> None: - """ - Construct a custom swerve module, this model is similar to a MAXSwerveModule but - using our custom construction. +class MAXSwerveModule: + def __init__( + self, drivingCANId: int, turningCANId: int, chassisAngularOffset: float + ) -> None: + """Constructs a MAXSwerveModule and configures the driving and turning motor, + encoder, and PID controller. This configuration is specific to the REV + MAXSwerve Module built with NEOs, SPARKS MAX, and a Through Bore + Encoder. """ - self.chassisAngularOffset = 0 # Fix me later? + self.chassisAngularOffset = 0 self.desiredState = SwerveModuleState(0.0, Rotation2d()) self.drivingSparkMax = CANSparkMax( - moduleConfig["CANDriveID"], rev.CANSparkLowLevel.MotorType.kBrushless + drivingCANId, CANSparkMax.MotorType.kBrushless ) self.turningSparkMax = CANSparkMax( - moduleConfig["CANSteerID"], rev.CANSparkLowLevel.MotorType.kBrushless + turningCANId, CANSparkMax.MotorType.kBrushless ) # Factory reset, so we get the SPARKS MAX to a known state before configuring @@ -45,24 +44,25 @@ def __init__(self, swerveConfig: dict, moduleConfig: str) -> None: # native units for position and velocity are rotations and RPM, respectively, # but we want meters and meters per second to use with WPILib's swerve APIs. self.drivingEncoder.setPositionConversionFactor( - swerveConfig["drivePosConversionFactor"] + ModuleConstants.kDrivingEncoderPositionFactor ) self.drivingEncoder.setVelocityConversionFactor( - swerveConfig["driveVelConversionFactor"] + ModuleConstants.kDrivingEncoderVelocityFactor ) # Apply position and velocity conversion factors for the turning encoder. We # want these in radians and radians per second to use with WPILib's swerve # APIs. self.turningEncoder.setPositionConversionFactor( - swerveConfig["steerPosConversionFactor"] + ModuleConstants.kTurningEncoderPositionFactor ) self.turningEncoder.setVelocityConversionFactor( - swerveConfig["steerVelConversionFactor"] + ModuleConstants.kTurningEncoderVelocityFactor ) - # Uncomment to invert the encoders - self.turningEncoder.setInverted(True) + # Invert the turning encoder, since the output shaft rotates in the opposite direction of + # the steering motor in the MAXSwerve Module. + self.turningEncoder.setInverted(ModuleConstants.kTurningEncoderInverted) # Enable PID wrap around for the turning motor. This will allow the PID # controller to go through 0 to get to the setpoint i.e. going from 350 degrees @@ -70,41 +70,39 @@ def __init__(self, swerveConfig: dict, moduleConfig: str) -> None: # longer route. self.turningPIDController.setPositionPIDWrappingEnabled(True) self.turningPIDController.setPositionPIDWrappingMinInput( - swerveConfig["minimumPIDInput"] + ModuleConstants.kTurningEncoderPositionPIDMinInput ) self.turningPIDController.setPositionPIDWrappingMaxInput( - math.tau - ) # Tau 1 radian + ModuleConstants.kTurningEncoderPositionPIDMaxInput + ) # Set the PID gains for the driving motor. Note these are example gains, and you # may need to tune them for your own robot! - self.drivingPIDController.setP(swerveConfig["kDrivingP"]) - self.drivingPIDController.setI(swerveConfig["kDrivingI"]) - self.drivingPIDController.setD(swerveConfig["kDrivingD"]) - self.drivingPIDController.setFF( - swerveConfig["kDrivingFF"] - ) # TODO: Calculate seperatly! look at complexConstants.py + self.drivingPIDController.setP(ModuleConstants.kDrivingP) + self.drivingPIDController.setI(ModuleConstants.kDrivingI) + self.drivingPIDController.setD(ModuleConstants.kDrivingD) + self.drivingPIDController.setFF(ModuleConstants.kDrivingFF) self.drivingPIDController.setOutputRange( - swerveConfig["kDrivingMinOutput"], swerveConfig["kDrivingMaxOutput"] + ModuleConstants.kDrivingMinOutput, ModuleConstants.kDrivingMaxOutput ) # Set the PID gains for the turning motor. Note these are example gains, and you # may need to tune them for your own robot! - self.turningPIDController.setP(swerveConfig["kTurningP"]) - self.turningPIDController.setI(swerveConfig["kTurningI"]) - self.turningPIDController.setD(swerveConfig["kTurningD"]) - self.turningPIDController.setFF(swerveConfig["kTurningFF"]) + self.turningPIDController.setP(ModuleConstants.kTurningP) + self.turningPIDController.setI(ModuleConstants.kTurningI) + self.turningPIDController.setD(ModuleConstants.kTurningD) + self.turningPIDController.setFF(ModuleConstants.kTurningFF) self.turningPIDController.setOutputRange( - swerveConfig["kTurningMinOutput"], swerveConfig["kTurningMaxOutput"] + ModuleConstants.kTurningMinOutput, ModuleConstants.kTurningMaxOutput ) - self.drivingSparkMax.setIdleMode(CANSparkMax.IdleMode.kBrake) # Brake mode! - self.turningSparkMax.setIdleMode(CANSparkMax.IdleMode.kBrake) # Brake mode! + self.drivingSparkMax.setIdleMode(ModuleConstants.kDrivingMotorIdleMode) + self.turningSparkMax.setIdleMode(ModuleConstants.kTurningMotorIdleMode) self.drivingSparkMax.setSmartCurrentLimit( - swerveConfig["kDrivingMotorCurrentLimit"] + ModuleConstants.kDrivingMotorCurrentLimit ) self.turningSparkMax.setSmartCurrentLimit( - swerveConfig["kTurningMotorCurrentLimit"] + ModuleConstants.kTurningMotorCurrentLimit ) # Save the SPARK MAX configurations. If a SPARK MAX browns out during @@ -112,10 +110,7 @@ def __init__(self, swerveConfig: dict, moduleConfig: str) -> None: self.drivingSparkMax.burnFlash() self.turningSparkMax.burnFlash() - # =================================== - # TODO: Calculate the angular offset! - # =================================== - self.chassisAngularOffset = 0 + self.chassisAngularOffset = chassisAngularOffset self.desiredState.angle = Rotation2d(self.turningEncoder.getPosition()) self.drivingEncoder.setPosition(0) @@ -166,8 +161,7 @@ def setDesiredState(self, desiredState: SwerveModuleState) -> None: optimizedDesiredState.speed, CANSparkMax.ControlType.kVelocity ) self.turningPIDController.setReference( - optimizedDesiredState.angle.radians(), - CANSparkMax.ControlType.kPosition, + optimizedDesiredState.angle.radians(), CANSparkMax.ControlType.kPosition ) self.desiredState = desiredState diff --git a/rio/tests/logs/FRC_20240120_210407.wpilog b/rio/tests/logs/FRC_20240120_210407.wpilog deleted file mode 100644 index ee43dc95bf2ed83f1b4408ee106f5c08d706aa3a..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 90721 zcmbWA3Ebdwy|$mRjE9&7A(Cm5Jrkkqk|kMVBB>ZW7|e`j1|d61go;Ro2qnpqrG=D4 zQc-A=B_)Y$k+gWP=XRdEYdY`eJ;ytrj}O=Re(&$i??0Cmop#!8i>s_4b7T0W$&R`qxtPwvu947zV}Njn-2`k#cz%0mu)7mGikG}ci3Y3UNa`GI{T0V zhW@?Fp@G9D9x!vp0YfuqPaT>yah<~sUuEK~83)eXV`$@$iAU<{S90^uWNo z1_tIPG92^O&8JKo+TnnqJ*Mt8b&suI`WgntnG*P zeCz)EPn|M-kD=k}-Dk|$e`w0||IM=ujvD^yxXr-8{QsB3JItOkd+Lno{~r%B|6qQy zag!h04h$^7ua@K&zcxQJ-!^0V?3pw6-(q9jGW@f2-&r%JugtFxn>cHaeTJq@iFp5B8TNMKe@e;k{6r$dKgJDgH({HBh4`)E z2by)rtl2};CQqIA@1G)X{4j43nsoV3HiLo*K=nzY$g!w&2@<3Jwf zfALdg+?c@$+njpIgUcRq&ywrC)G_vd?%4k2b&UOQ9pnDTj_qIOAIG@=)-n8N+W+@k zZT~X=IL6P>u`}OeVLD#NFZmw(9JtqB`wzYJHCCNAG<(XPeAr1l4DHX)tf4($)}JgV zEHHTE^79X09{w?5%-{nD4y&9W6UNR#X-ZA`V>yhtg8aKym{2Y{AuZ$g@m69tZR~j>T<5})AjhkaOehx~m zSH=#{O39UyE8)?@?la+$W_Tn@u2=B(VfUGm>n0wpIs7k7BkyU#BhBzglw7ai-dK2vht#G}QB|MGq0Jx%zh86Js}>lOT4e0(XnQgS6c z8gicr|1`rRQF6V4e?#swCD%eWv8PiAVRi&xC)P;gKl0UctY6+-FLzn|QSBqIypg{%M9sqU3r7 z|CU{}lw2vf5+2QTp9zmN!y{30y@G!;-DgU!n|O4i`%L(!86Js}>lOUF(S4@mx`{_) zhX0e%{NO)lFyWtOcqB@$SCT6wS4ysgNAGo?3I8<1BT;g_f`9LQWhuE*awR;v#C;|_ z(hQG8$@L2UUE)4da^1wEzq!wZf12TuD7jw2zrVT9lw3FQX!FJN{Y?0$86Js}>lOUl ze6dn;rQ}L@bcXv(c%&H~iIVFT{5!*arsTSbM-RErgnydhktn%d!M}&xXG*S{c(mH^ z-{Qy*d@$jkW_Tn@u2+&PC09zWghxlZ&xA*s;gKl0UctX37cV7OO0I-QKXRW5k2J$0 zQF6V4e?M}cDYITW91wP57r79*L6c75rOjiBfW<^>77X@*Cl=?0FOlYC&D99a=nsVDY;T|B|O^MeJ1?V z439*~^$Py&JfW0aDY+6JUF<#+9%+V0qU3r7|1Nf)DYlOU__-jkam69vr z(f#f-;gM!|BucJV@b7;2nUd=!9<4g^Pje0XV8TDm@JN(guOwGWu9RE}k3Qr+6CP=X zN226<1^+%Yv6NgXxe^}T<~|c1X@*ClZsO5g z%gT2X9%+V0qU3r7|K?h@lw2vf5*}^mJ`?_FhDW00dIkTsbDt@>ZsO6o?la+$W_Tn@ zu2=ByT=$uh>n0vO?miP9X@*ClONC)-Nd68+-Jf+&G1N+T(98Y3+^)|*G)Wn`wIGgCj8S3k3`A! z3jV!)g;H{*7NM9K9E{{7B_cY<3W_Tn@u2=AH^%YCWm69vr(b4WR;gM!|BucJV@b75%nUd=!9{t39Cj8S3 zk3`A!3jY1XeWv8PiAPJXr1v!8pJsR@O0HM%Z|Rjv$(52T;n9BXGvS|RcqB@$SMYB? z_nDIGCLVp=eI`88439*~^$Pxd-F>Fyx`{{g4gVKb@?-el|B3KVgh!&}dL_A1a;4-- zc=Rs!neb0DJQ5|>EBN=Wl}pK$k}KiS1@1H9k!E-#O0HM%?*jLklItcOJ?%ad{%M9s zqU3r7|DJZADYp@lOU_zWYqcbrX*kT~+UC!avRMNR(W! z;NPOFmXa$aSHhz`+-Jf+&G1N+T(98Y9_}+G*G)XS!hI$@(hQG8$@L2UUEw}ca^1wE z7u{#VKh5w+lw7ai-;3@uCD%O0I-QpK+fFk2J$0 zQF6V4f1h!mDYOD>Prx_lJlIs=xd-EGh z$(52T;nDH#GvSeDcqB@$SMcw6_nDIGCLZ18J`?_FhDW00dIkUPa-S)=ZsO6z)%2bw z{L>7NM9K9E{!Lu1lw2vf5*{7kJ`?_FhDW00dIkRuaGxo;ZsO4m?la+$W_Tn@u2=By z2KSkg>n0v8xVqlcgnydhktn%d!M_DpFC|w>u7pSLai0nQG{YlNa=n6o?{S|gxo+ao z7u{#VBhBzglw7ai-xu9yO0JuD^ql)l_@@~jiIVFT{Cm!QrsTSbN1MJ$-_L}9n&FWs zxn9A)P2W^Xu9RE}k3Q)>6CP=XN226<1^+(jK2vht#G?n@XTm?t@JN(gui)QkhThYJf12TuD7jw2ze#J9k}D-w!lT38XTl@R@JN(gui)R|?lUFVO+5Oc`%L(! z86Js}>lOU_q5Dk9brX+XwWi+Fgnydhktn%d!M|6nSxT;yTnUf%cAp9VG{YlNa=n6o zd%Mq+TsQIP8uyv-NHaVVCD$wXca8f@$#oNt=2=VcX~I9v@JN(gui)Q2Yn75KC0D|u z?cHa>Kh5w+lw7ai-}de^CD%-(ATPcu9cCD$wXx89pe$(52T;n7FkXTl@R@JN(gui)QD-DgU! zn|SmK_nGidGdvO{*DLt<3-_6l>n0v8{}#Qc3I8<1BT;g_f`7}urIcJLxe^{7=spu3 zX@*Clxo+aoP3|+{pJsR@O0HM%? zEBJSo`%KAo6OSHtp9%jo!y{30y@G!ayU&zdH}PnVb@iSm{L>7NM9K9E{;jcYDY;T| zB|JLDeI`88439*~^$PwS<33Yz-Nd6i+-Jf+&G1N+T(98Y9quzF*G)Wn&3bxI6aHz2 zN226<1^-^NUMaa!awR<4-+d7NM9K9E{_W&GQ*zzJqYK?< z!XwS_NR(W!;NOMrGbPteJbK1`Cj8S3k3`A!3jRIgK2vht#G|)vpzmkGKh5w+lw7ai z-&;2*C09zWgh!{k&xA*s;gKl0UctXp-DgU!n|Sma_nGidGdvO{*DLt<8~2%#>*koj z`&Zsj?`ax0$87u@lw7Zj9iEkvDV>W&cO0HMN4$n%-m69us8C>R8 z_nF4cF&jSzCD$ushi9eaO39VR3~spCMtV=vxH)Fy=b+?zW$f^*lw2vf(wMy@#?vr=-Uy@#?vr=-UlOUl zY|~P5rQ}L@bh`UYc%&H~iIVFT{5#!!rsTSbN56HS3I8<1BT;g_f`7ktpDDR+E;RV< zH*R*pdM`e)*Vxg+|KD*Ty{-uxHNz@Va=nR-Z``buTq(H{RvqC!6IN-4Rifm21sjiW zpDDR+V%6>LGhw4_Uku2-;e2ltth>n0w3-hC!K(hQG8$@L2UecpYhEBJSi`%KAo6OX>KNJ3GhDW00dIkTs+^UpZDY+6J zecF8{Jkku0M9K9E{(ahgrsTSbM}Kgi3I8<1BT;g_f`5N-pDDR+;?bI0>pe~Qrx_lJ zlIs=xTXXADa;4--cyz4$On9Uj9*L6c75qEaeWv8PiAQ(3&xC)P;gKl0UctXR-DgU! zn|QR$HhNDJ{%M9sqU3r7|CZUNlw2vf5*|%=p9%jo!y{30y@G$!-DgU!n|Sms_nGiW zGdvO{*DLtn0v8u&v(Hgnydhktn%d!M_EzEhSe7NM9K9E{{6*$rsTSbM;mXa z?`Oh4&G1N+T(98Y#@m&WDiK2vht#G^O7Q}1cQKh5w+lw7ai-y7apO0JY#36BnQp9zmN!y{30y@G#-xzChb zH}U8P?la+^W_Tn@u2=By2ktW^*G)WHe0#m83I8<1BT;g_f`5x|UrMf&TnUec+-Jf+ z&G1N+T(98Yko!!@brX-Sa-Rv0G{YlNa=n6oSGmuWTsQG()DC)26aHz2N226<1^-6v zP)e?pTnUf1bDs(SG{YlNa=n6o+qut_TsQIP9QT>}COpy%k3`A!3jWP>pDDR+;?Z~9XTm?t@JN(gui)Qz+-FLzn|L&KN4=*B|1`rR zQF6V4e`9wnC09zWgh%glp9%jo!y{30y@G%5bDt@>ZsO6G-Dkoh&G1N+T(98Ym)&Pd zuA6xDH}{$FPcu9cCD$wX_c!;MlItcOZN8Jfp9%jo!y{30y@G$6?^H^zlw1jq&TyXz zk2J$0QF6V4e`mPQlw3FQ=ppx+@J};55+&Cw`1g?eOv!ZpoL*-Nd8o+-Jfg&G1N+ zT(98Yb?!4I*G)Vc{T{ui3I8<1BT;g_f`6mmQ%bIsTnUdRyU&Dwn&FWsxn9A)$?h{H z*G)XSz(hQG8$@L2UUEn@ba^1wEr`%`4Kh5w+lw7ai-&5{0CD%n0vu?miP9X@*CllOUl>iwnUO39V*=xq0y@JKT}5+&Cw_;lOT);XYGx-Nd8o-Dkoh&G1N+T(98Y_3kqz*G)Wn#T31#3I8<1BT;g_ zf`6}=QcA9rTnUeMcAp9VG{YlNa=n6oJG;-6TsQIPV)vQwNHaVVCD$wXcd`3S$#oNt zo^ziG|1`rRQF6V4f6uwklw3FQXp`Ob{Y?0$86Js}>lOUlWcN~XrQ}L@^a=Nw@JKT} z5+&Cw`1c9-nUd=!9zEbb6aHz2N226<1^*s!pDDR+;?b&m=sivNrx_lJlIs=xTXm08 za;4--c=RFnnea$6JQ5|>EBN;z_nDIGCLZ19J`?_FhDW00dIkS(bDt@>ZsO4rd+I$+ z_@@~jiIVFT{99trQgWr_N_e!l`%L(!86Js}>lOUl+kK|wx`{_uyU&D2n&FWsxn9A) ztKDZxuA6u?_mJMxgnydhktn%d!N0kOO39UyE8)>Q-Dko-&G1N+T(98YJKbkWuA6vt zuKP@Qq!}KGlIs=xJJ)@tQgS6cI?;V5Jkku0M9K9E{+;MPQ*zzJqhGksgnydhktn%d!M|U)&y-v@ z@o2fd^`0jD(+rP9$@L2UEw^_mxl(c^Jeuu36CP=XN226<1^;Hd&y-v@@#wqmGvS|R zcqB@$SMcwdm4 zCj8S3k3`A!3jS>|wUk^bxe^|o={^%4X@*Cln0vO?LHI! zX@*ClS=BhBzglw7ai-y!ZZCD%7NM9K9E{@vm}Q*zzJqgT$*dz$c1GdvO{ z*DLt<${D5PO39V*Xb<<9@J};55+&Cw__v4qOv!ZEBN=K`%KAo6OXn&K;O@Vf12TuD7jw2zpW1_C09zWgh!usp9zmN z!y{30y@G$Ab)PA@ZsO6S?la+^W_Tn@u2=ByQTLgW>n0w(EBLqML8atM$(8VEANQH?Pcu9cCD$wX zw~zZw$#oNtu5q6Uk2J$0QF6V4f7iIrlw3FQXr6=ho+kX$439*~^$PyYb8soSQgS6c z+TML8{L>7NM9K9E{%!9*Q*zzJqx0Nn!XwS_NR(W!;NN-fGbPteJbJ=?Cj8S3k3`A! z3jRIeK2vht#G~~;sPAXOKh5w+lw7ai-})acC09zWghwa2&xA*s;gKl0UctYU+-FLz zn|Sm~_nGidGdvO{*DLtn0v8e~8}Ggnydhktn%d!N27XDJ54*u7pPiy3d41 zn&FWsxn9A)1KnpzuA6vtllx5grx_lJlIs=xyUBf~EBH75 z&{A@xtdxl(c^JUYvLCOpy%k3`A! z3jUqtK2vht#G{AZXTm?t@JN(gui)Rq?lUFVO*~rTLwZjW{%M9sqU3r7|JL|WDY;T| zB|JLDeI`88439*~^$PwS<33Yz-Nd6i-Dko-&G1N+T(98Yo$fOw*G)WH=5W2I3I8<1 zBT;g_f`7{#UP`W%TnUe+xzB`un&FWsxn9A)Y3?&6*G)Y7ru$5Iq!}KGlIs=x`=Xf12TuD7jw2zg3PZC09zWghz+E&xA*s;gKl0UctXZ-DgU!n|O4q`%L(!86Js} z>lOUF)qSSqx`{`N9j*5?;h$!BBucJV@NcoBOUadzE8)?e?la+^W_Tn@u2=AHPxqOU z>n0vupm0y zX@*ClzMl#IG{YlNa=n6oYad%mu9RE}k4|u(36C_xBT;g_f`2Et&y-v@@#t>% zneb0DJQ5|>EBJS}`%KAo6OWcXPVZ^LKh5w+lw7ai-?GP*k}D-w!lRk)GvSeDcqB@$ zSMYD9`%KAo6OV3mp9%jo!y{30y@G!?y3dqcH}Po9@p?}a{%M9sqU3r7|Hd3&O0JY# z36I|EJ`?_FhDW00dIkU9>poL*-Nd6y+-Jfg&G1N+T(98YCGImN*G)Wn-hC$g(+rP9 z$@L2UJ?}nKa^1wE%|5K}XTm?t@JN(gui)QiA1)7NM9K9E{*6Adlw2vf5*|%lOSv-+iX!x`{_my3d4vn&FWsxn9A)C*5aCuA6wY!AJG|O!%i69*L6c75v-aqow3Z z$(8WvWcQiyNHaVVCD$wXce49T$#oNte&s$B{%M9sqU3r7|9<5@Q*zzJqZLlldz$c1 zGdvO{*DLt9!bzp%O39V*=wSDm@JKT}5+&Cw_;;}TOv!Z zA2y1TYhQ9Dtoo|^OjxBER*9196>R+KzuS}REV&X^{nLFWY}5>^M9K9EHvZFnrsTSb zRa<^c?`gtD&9F+8T(4l`mLDr6S4ysgRiAdB39B^2Dp7L1f{mYcpDDR+V$~z=Ghw4< zSS3oXSFrIB_nDIGCRVL=ir&+NjhbPVD7jw2#yY>n0veI92ay!XwS_NR(W!;NOH(OUadzE8)>} z_nGidGdvO{*DLrp-F>Fyx`{{Ma-Rv0G{YlNa=n6o-*TTRxo+ao0;lOcP57r79*L6c z75rP^v{G`Vo%On9Uj z9*L6c75w|S`%KAo6OZn9p9%jo!y{30y@G%DyU&zdH}U8VpU``n@J};55+&Cw`1giS zl#(kYSHh#i+-Jfg&G1N+T(98YVeT^}*G)XS&3z{P(+rP9$@L2U-R3@1a^1wEB|fS5 zG~u6ScqB@$SMYC%PnMD^C0D|uz1(NQKh5w+lw7ai-(K!BCD%7NM9K9E z{=LtArsTSbN0++Kgh!g;ktn%d!M{u0XG*S{c=Uq%O!%i69*L6c75sa_eWv8PiAQfg zOW)6gf12TuD7jw2zqg-NO0JY#36DPIJ`)~khDW00dIkSJS8K2vht#H0B>qxUr7pJsR@O0HM% zZ@$lzk}D-w!lQS&&xC)P;gKl0UctY2xzChbH}U8K_nGiWGdvO{*DLsUf%{C!brX-C za-RwRG{YlNa=n6oPr1*OTsQG(!_Vsbneb0DJQ5|>EBLqJXG_VIk}KiSDeg1jk!E-# zO0HM%?-ci$lItcO-RnLR{%M9sqU3r7|L%34DYEBN<)_nDIGCLS$%j^5LR zf12TuD7jw2zeUd}C09zWgh#u(&xC)P;gKl0UctZJ-DgU!n|O4&`%HMG86Js}>lOUF z+5a;4--cyxpN zOn9Uj9*L6c75uxweWv8PiAM{bulF?JpJsR@O0HM%Z^83R$(52T;n92CXTm?t@JN(g zui)Q%+-FLzn|Sm^_nGiWGdvO{*DLtn0vO=ROnuX@*ClEBLp`1*POl$(8Wv6YewNk!E-#O0HM%?-TAbCD%Jkku0 zM9K9E{(aParsTSbN562N3I8<1BT;g_f`7kopDDR+;?Z(n)O(unPcu9cCD$wXx7-&? z$(52T;n8gOnea$6JQ5|>EBH6teWv8PiAOiN&xC)P;gKl0UctYc+-FLzn|L(-OL|We z{%M9sqU3r7|Hgl*lw2vf5+3d9J`?_FhDW00dIkS>b)PA@ZsO5b+-Jfg&G1N+T(98Y zSKMbxuA6xDclVj_Pcu9cCD$wX_jmW1lItcOZE=adp9%jo!y{30y@G#RTvAG|lw1jq z&UBv%k2J$0QF6V4e`mVSlw3FQ==bh3;h$!BBucJV@bCBTGbPteJX+(+dQTJnX@*Cl zlOUF#eJsax`{`NU9R^u;h$!BBucJV@NcopOUadzE8)?e?la+^ zW_Tn@u2=AHPxqOU>n0vu={^%4X@*ClgK2vht#G}_;rS~-9pJsR@O0HM%?{!y|k}D-w z!lMtk&xA*s;gKl0UctW)xX+YaH}UA(?la+^W_Tn@u2=By+wLEBNn0v8 zd9B{lgnydhktn%d!M`Q1EhSen0vu>pl}6X@*Cl z9nbHC@RPcu9cCD$wXx8=7=$(52T;nAnvXTl@R@JN(gui)RO-DgU!n|SmG_nGid zGdvO{*DLt<2ltth>n0wpdA;7#gnydhktn%d!M`=HFC|w>u7pR&y3d41n&FWsxn9A) zW8G&;uA6vtr~6F!rx_lJlIs=xyVHH9EBN<@ z@05}&C0D|u!`x@WBhBzglw7ai-(l`ECD%EBJSg`%KAo6OSHqp9%jo!y{30y@G#_xzChbH}Pnl@9F!Q@J};5 z5+&Cw__xmYO39UyE8)>c+-Jfg&G1N+T(98YN8D#huA6vtkNZscrx_lJlIs=xyT^T| z0WO0JuDH1_*?PZR!WhDW00dIkT+e!rAlDY+6Jz0Z9n{L>7NM9K9E z{=LtArsTSbM_+cI36C_xBT;g_f`4CjpDDR+;?dvSXTm?t@JN(gui)R`+-FLzn|QSO zE&6^Y{L>7NM9K9E{%w9sDY;T|B|JLAeI`88439*~^$Px-;XYGx-Nd7Z+-Jf+&G1N+ zT(98YL+&#r*G)WH?N+^~3I8<1BT;g_f`6;sT1u{zTnUeka-Rv0G{YlNa=n6oN4d|G zTsQIP$L=%XpJsR@O0HM%@5k;lCD%7NM9K9E{%!cfQgWr_N_g}!_nGiWGdvO{ z*DLtn0xk+I=Sc(+rP9$@L2U{n~w|{$(8WvIQNOD>Prx_lJlIs=xd&QllEBLpw z`%KAobD_ceFTV5CeFmP~YwYNO30QTp+f7)d8CHps>rHIr<|Pj^M9K9EHa_b|c4eI~5Z468)R^$IqA$bF{dx`|b{xzB`+nqieFxn9A>+uUbLuA6wY#9ew% z6CP=XN226<1^EBJS!`%KAo6OVrGJ`?_FhDW00dIkS}?mkm; z-Nd8iey;a4;h$!BBucJV@Nc=Fmy#pm0y zX@*ClEBH6=7p3G%$(8Wv{q8g2pJsR@O0HM% z@BQvGCD%t z_5Dovrx_lJlIs=xd;2d-$(52T;nA7yGvSeDcqB@$SMcvl_nDIGCLaCXeJ1?V439*~ z^$Py|-hHOzx`{_``jy_(gnydhktn%d!M`{Cs+3$Qxe^{7?LHG8X@*ClEBN;X_nDIGCLTTQ zJ`?_FhDW00dIkTUcAqJ^ZsO5KztQ(I;h$!BBucJV@Nc8vl#(kYSHh!H+-Jfg&G1N+ zT(98YDef~R*G)XS*L^1Z(+rP9$@L2U-RnM6a^1wEmG0Afn($9EJQ5|>EBLq4eWm0| z$(8Wv5ciqzNHaVVCD$wXcZmB;$#oNtZgHOp|1`rRQF6V4f48{Llw3FQ=#}^DJx%zh z86Js}>lOTa<^84PO39V*Xb<<9@J};55+&Cw__v4qOv!ZEBN=K`%KAo6OXojK;O@Vf12TuD7jw2zpWoAC09zWgh!uo zp9zmN!y{30y@G$Aai1xn0w(EBN;%_nDIGCLaCOeJ1?V439*~^$Py|)qSSqx`{`d{!ZV| zgnydhktn%d!M{y^S4yswTnUdp={^%4X@*Cl;b$3I8<1BT;g_f`9M$Qz^MpawR-E%Y7z1 z(hQG8$@L2Uo#j4La^1wEhuvqwKh5w+lw7ai-^1=RCD%S3K2vht#H0EDtoJnGpJsR@O0HM%Z~i}*k}D-w!lRwsXTm?t@JN(gui)QK?lUFV zO+32LeI`88439*~^$Px7=sr_&-Nd73+-Jf+&G1N+T(98YGww4b*G)Wn>l6BZCj8S3 zk3`A!3jV$IiBfW<^vc`os2Ns? zlIs<09QAZ5xl(c^tlHLnCT!FUt3=853N~)*K2vht#H!D^&xBQ)VU;MkUctuCxzChb zH?itZ?lWPdW>_Uku2-<}Pwq1%*G;Ti`x$*d6E*lItcOz3wl1PZR!WhDW00dIkSp z_m@&~rQ}L@G}C=1Jkku0M9K9E{>^lsDY7NM9K9E{%!VLDY;T| zB|JLaeI`88439*~^$Px-?mkm;-Nd7Z+-Jf+&G1N+T(98YL+&#r*G)WH?XP-I6aHz2 zN226<1^-t2Ybm)>awR-E(tRd8(hQG8$@L2U9qB$(a^1wEAGyzjf12TuD7jw2zaP2J zlw3FQXsPG*o+kX$439*~^$Pwi^?WJ0QgS6cn(96i{%M9sqU3r7|E9Xnlw3FQ=vw!g z@JKT}5+&Cw_;;=QOv!Z}N1Zfy)(i8E z%#M8bwo86-aD-lb?DPfKn}3A(r_GPLtu*2BWmh&J``|2tcMy|Tn# z7ff1vY-VpZSA?SAw9QCB@%TxQLM<{E$8_>t)YLp#iM z{UQsG;K0#m&vorZ#ocbXf3B6^yvWFOqs^9^`_AtdA6Q}kxmVe7(UIwm7hgH|=-(B$ z+vEAUw?5#NBhzuGZ!phkFBFd&b=*9cAHUehbl_7znCH)zmMMi%;LHK+GxJ>Ui<1!AN=rq*L<~j?((G_aqWZ zg^EZbq3lA7|NVP9^K?(=e_iJ|b6u|7=Y5~|8K3XmbPk=i*igR{f$4wgCYVX1E6L+0Be$#*bgx*-k(9wa72L|54zr6E5j<37#;N(eTDj z4xJjEn0bY@bQH3{D*yPnA=9y&uBh= z^IL2=xc6S;_WwT|pUuZN8W?yhhyU{}@)IA+CwxOc?YH{!76SuA_^$u>X>T}f%fSh2 z?X}m$ag)an4pr|mWy)TI<0k)ao@Ll9L%$q18yJ}V|8sb&>EotPoHF_U@F24f%P%%= z@?-OXfjRh>`T5PS&9BUlPMJJ?>Xg0K88?008+w&Fh7BytwSWEi#K6GY_`Ub?+tB$< zChj}9&BVzQrtCLnnQ8n1>@#i5Cfh8$?&NX1>@_&y4ZhCXA}zlX=j8a%Z<~$fr#yLZ z{Lr%u{VLsK+LXy3;9n0MJ#GB%gOkQZynn9@c{}<)rQ|0+n#joZ#LI8 z8@%VO-yXl(zdA*sEC}s_#ppdekINLQ}~b;n@rhl({YpetHe}(9ql@0>ZEbg`8773)K1fP+GpD6 zgBPA-*iDPgKJ?Qa`mylvVZYmFNag%kc*IOJawZxz6D2p}m(0I%rQ}M@N0MOIZDZuk}C}#w&u?6 zGmV^SHfknHu2)73%}U9Yk}C}#w(}+KGmV^SHfknHu2)73%}U9Yk}C}#cG%zDXBs)v zY}8DYT(686nw63(C080g?Cgz({%k$t`hGwPYO39Um54-7f_nAh{ zG#fP&CD$t>hGwPYO39V*=yCU%@JKT}5+&Cwc>B2fOv!ZcO39UyE8)>L?la+^W_Tn@ zu2=AH8~2%#>n0wZ?>-YAX@*Clt=jE6aHz2N226<1^?C_SxT;yTnUd(a-Rv0G{YlNa=n6oC%Mm*TsQIPA@`Z^ zPcu9cCD$wX_mKNc$#oNtmKyr^uruD%gnydhktn%d!M~+Om69tZSHhzM+-Jfg&G1N+ zT(98Y0q!#;*G)XS-F+tf(+rP9$@L2U-R?e9a^1wEd58Y}`i%E9;h$!BBucJV@NeF^ zO39UyE8)?g`%L(!86Js}>lOSPbe}1?ZsO5p?la+$W_Tn@u2=ByGWVI1>n0w}GW5Tp z%y>@|{%M9sqU3r7|7MxHlw2vf5*}^tJ`?_FhDW00dIkSBcb_S_ZsO4y?la+$W_Tn@ zu2=By4ELFm>n0vO7NM9K9E{@v$3Q*zzJqeb4K_cY<3 zW_Tn@u2=AHk$04mDKh5w+lw7ai-}dh;C09zW zghv;-&xA*s;gKl0UctYM+-FLzn|So9`%L(!86Js}>lOTa)qSSqx`{{Y&!g{W!avRM zNR(W!;NSZ5l#(kYSHh!H-Dkoh&G1N+T(98YsqQl+*G)Wn)O{xW(+rP9$@L2UJ?cJF za^1wEWrzM2M}FXg3I8<1BT;g_l3XddQgS6cI@En8Jkku0M9K9E{vA4RDY;T|B|N&z zeI`88439*~^$PyohW@tBjQ2F*pJsR@O0HM%Z-Mzr$(52T;n5!MGvS|R zcqB@$SMYBS_nDIGCLUeoJ`)~khDW00dIkTka-S)=ZsO5wLw~<%#(SFZPcu9cCD$wX zH`}{P$(52T;n7(4neb0DJQ5|>EBH6oeWv8PiAU$S&xA*s;gKl0UctX}+-FLzn|SoB z`%HMG86Js}>lOTa)_tbrx`{`t5B-g|8Q;%@f12TuD7jw2zt!h2C09zWghwa1&xA*s z;gKl0UctW;+-FLzn|Snq`%L(!86Js}>lOTazn0xE>OK?xX@*ClR{K2vht#G@zNXTm?t@JN(gui)Pk?lUFVO*~rhJ$g?Q z{%M9sqU3r7|5kiYDY;T|B|JLHeI`88439*~^$PwSrhxl(c^Jjy?JGo#Ohf12TuD7jw2zsU=g zk}D-w!lUc`$7jML&G1N+T(98Y_3kqz*G)Wn+rs~PP5x;EJQCrb2#-X`^-6N3EBLqV!lmR&$(8Wv0{5BlNHaVVCD$wXcY*s%$#oNtUUHua|1`rR zQF6V4e=oVulw3FQ=%Yjbq*Q+J{Y?0$86Js}>y_k6$(52T;n5e|XTl@R@JN(gui)Po z-djqplw1jq9(JDzk2J$0QF6V4e-FFQlw3FQXqg%RG}n+1Cj8S3k3`A!N^+&-O39V* z=pgr*@JKT}5+&Cw_;=9gQgWr_N_cdK`%HMG86Js}>lOUF!+oaYx`{{g&G?6zhThYJ zf12TuD7juqu9RFUxe^}j>OK?xX@*Cl)-On9Uj9*L6c75w|Y z`%KAo6OZ1sh zEBLqS`%B4{k}KiSaqctWk!E-#O0HM%?>P6FlItcO-S0jV9%+V0qU3r7|L%96DYo6Agnydh zktn%d!M~f`XG*S{cr7NM9K9E{%x>$DY;T|B|Q3)`%HMG86Js}>lOU_lKV`_brX;N;64-nX@*ClEBLqE5~buy$(8WvaQB(;NHaVVCD$wXcewjZ$#oNt ze(F9G{%M9sqU3r7|9n0vO z?>-a$X@*ClEBLqOQl;cd$(8WvME9BSNHaVV zCD$wXccS}D$#oNt9(11x|1`rRQF6V4e-FCPlw3FQXvwAZo+kX$439*~^$PwixpXPH zQgS6c`n3B@c%&H~iIVFT{QI=~Ov!Zs z-Dkoh&G1N+T(98YSKVhyuA6xDC-<4~Pcu9cCD$wX_b2z6lItcOeR$|U)|Vgr_11)c zn&FWsxn4=Glw2vf5*{7xJ`)~khDW00dIkTE{$MG&QgS6c`lb6!_@@~jiIVFT{QIT* zOv!ZjO!%i69*L6c z75w|8`%KAo6OX>>J`)~khDW00dIkT!={{3(-Nd7p-Dkoh&G1N+T(98Y%kDEJ*G)WH z_e1)ACj8S3k3`A!3jVG8p;B_CO0JuDG-i3drwRWw!y{30y@G#ZmMahTq(H{9_{8n z6aHz2N226<1^;$)pDDR+;?WiEGvSeDcqB@$SMcu&_nDIGCLXn0xk z#eF9H(+rP9$@L2U{l$HzEBLpM`%KAo6OV3jp9%jo!y{30y@G$YxX+YaH}Pn$mGzz` z{L>7NM9K9E{>`;=DY;T|B|O^6eJ1?V439*~^$Py&EBN;__nDIG zCLX=#BYIC0{%M9sqU3r7|K9VFQgWr_N_e!F`%L(!86Js}>lOUl%YCNgx`{{ExX*+~ zn&FWsxn9A)Yusl_uA6u?`)Ybm6aHz2N226<1^;GWt(06Txe^|2<31DqX@*ClzUJlItcOz2H6*{%M9sqU3r7|6XvPDYHsO!%i69*L6c z75w|H`%KAobNH}_Kd^@0(=>9X*{GQ)xn3DDG%F=nO0G0~*z5bd&opwT*{GQ)xn3DD zG%F=nO0G0~*h06v&opwT*{GQ)xn3DDG%F=nO0G0~*c$V!srNLEoM|>{CQ7bXMhwkL z$(52T4Ij4C1oxRn&NLe}6D8LxBZg+B{CQ7bXMhwkL$(52T z4Ig&az*>4w)5w`-qh_MydS%4Wtdv|Sxzg}qH*V%W)5w`-qh_MydS%4Wtdv|Sxe^|I z&3z_3(hQG8$@L2Uea(HQ9N1EZ0D7jw2zbD;iO0JuDwDQ{eekMH9439*~ z^$Py2yml$MQgS6cI>vn_Jkku0M9K9E{vG2!Q*zzJqkG+F!XwS_NR(W!;NQLOGbPte zJbK?p^`0g?(hQG8$@L2Uz3-!?*zgA_@@~jiIVFT{2RVbDY;T|B|O^BeJ1?V439*~^$PxN z=RQ+%-Nd7d+-Jfg&G1N+T(98YMeZ{t*G)Wn#eF9H(+rP9$@L2Uz2ZJoa^1wE_14w* zGvS|RcqB@$SMYDWbxX;Wk}KiSDeg1jk!E-#O0HM%?-ci$lI!N&!_NBsy5oNT#0$HQ zoNeg;TP>tNS`#*EhE<~EdJ`Le?{-si-NdR7uBSc|Hfn}dqU3r78$Y;SDY;T|C9FEc zeI~5Z468)R^$Io~;yzPy-NdRp-Dko^&9F+8T(4l`o$fOw*G;TiV12!(2^%%TDp7L1 zf{hETUrMf&TnVcty3d4-nqieFxn9A>iS9Ec*G;Ut(tReZ(hRFa$@L00Ug^>9zX@*Cl7NM9K9E{{6#!rsTSbM;mXV?`Oh4&G1N+ zT(98Y#+#IqDTsQG(j?ML+Cj8S3k3`A!3jWQpc`3P4awR)k2J$0QF6V4f9Jc;lw3FQ=tcLL@J};55+&Cw z`1hjwOv!Zn0w}yQSXKgnydhktn%d!M}O8 zEG1V;u7pQ}?la+^W_Tn@u2=AH(0!)lx`{`ZxzB`0n&FWsxn9A)%iL#5uA6u?%T{_% z6aHz2N226<1^;H*s+3$Qxe^|2;XV`oX@*Cl;k!E-#O0HM% z?@afZlItcOJ?%ad{%M9sqU3r7|DJZADYlOT4Wo#+AQgS6c zI@WzAJkku0M9K9E{vGQ+Q*zzJqx;-v!avRMNR(W!;NN}jGbPteJX+-AdQTJnX@*Cl zEBJS_`%KAo z6OTr2t@kwHpJsR@O0HM%Z{*gcEBLpA`%KAo6OX>-J`)~k zhDW00dIkT!OK=5X@*CllOSv*L|ksx`{{6xzB`un&FWsxn9A)=iFyX zuA6wY#&-ICCj8S3k3`A!3jVFJT`9RzawRCho+kX$439*~^$Pwiv3)7IQgS6c+RuF^Jkku0M9K9E z{_W>JQ*zzJqg&l)!avRMNR(W!;NPw8GbPteJbK3tdQTJnX@*CllOUFzlOT4W_&5RQgS6cI>>z{Jkku0M9K9E{vG5#Q*zzJqdVMZ!avRMNR(W!;NKnY zGbPteJeqHU-qVDCn&FWsxn9A)`6iT-DOD>Prx_lJlIs=xd-Gr^xl(c^Jo=dXO!%i69*L6c z75w{{`%KAo6OYbvp9zmN!y{30y@G#dxzChbH}UAt?la+^W_Tn@u2=By&+aoN*G)WH zbys~q6aHz2N226<1^-svwUk^bxe^{7=ROl2X@*Cl=+fO0JuDwCHYnPZR!WhDW00dIkR$-K~^dDY+6JO?RIOk2J$0QF6V4f79J( zO0JuD^aJ;q@J};55+&Cw`1b?%nUd=!9*x>v?`gt6&G1N+T(98YsNGA+m69vr(T?sj z;h$!BBucJV@NY-=nUd=!9$oA{6CP=XN226<1^+H~pDDR+;?ZmFGvS|RcqB@$SMcvO z_nDIGCLV1#QQyymf12TuD7jw2zYQmrk}D-w!lTpNXTl@R@JN(gui)Ql?lUFVO+0$c zeJ1?V439*~^$Pwy<~~z$-Nd60?VFd($#oNt7Ti6%*Kh5w+lw7ai-=6L>CD%lOSPwpS^+QgS6c+S+|4{L>7NM9K9E{%!3(Q*zzJqi?v+gh!g;ktn%d!M|^~ z&y-v@@#uN?neb0DJQ5|>EBN=k`%KAo6OY!Mr0-|KKh5w+lw7ai-lOUl-+iX!x`{`(xzB`un&FWsxn9A)+uUbLuA6xD z&MA6N6aHz2N226<1^?bTrIcJLxe^|Ycb^IWG{YlNa=n6olOU_ zrTa|DbrX+9Pt|*x@J};55+&Cw_&0iLDY;T|B|O^OeJ1?V439*~^$Py&?LJd--Nd6C z+-Jfg&G1N+T(98Y4em1~*G)VcK27gw!avRMNR(W!;NS3RrQ}MA5brX*+be{>2G{YlNa=n6o7rM`sTsQIPukJJ9pJsR@O0HM%@2~DNCD%p9%jo!y{30y@G%1PA?@_O0I-QC%eytN1EZ0D7jw2zmwf(O0JuD^oaXR_@@~j ziIVFT{CmWGrsTSbM`QNUdz$c1GdvO{*DLrpW}i}WrQ}L@bcp**c%&H~iIVFT{5!;b zrsTSbM|Zln0vO z<31DqX@*ClVK2vht#G|EBLpw`%KAo6OS%&p9zmN!y{30y@G$2 zxX+YaH}UB2?la+^W_Tn@u2=By@9r}t*G)Xy=sS(}aJT;gKl0 zUctZR4=N>BO0I-QN4U>~N1EZ0D7jw2za!jdO0JuDbdUQ?_@@~jiIVFT{JY0}rsTSb zM++US_cY<3W_Tn@u2=AHp@U1wm69vr(Ioeo@J};55+&Cw_&3RYrsTSbN7uT~gh!g; zktn%d!M|(WXG*S{cr^PVdQTJnX@*Cl2j zpDDR+;?a5TGvSeDcqB@$SMcvV_nDIGCLX=uJ`?_FhDW00dIkSpaGxo;ZsO6}hwA&8 z@J};55+&Cw__y|(hQG8$@L2U9pFAwa^1wE z+udivKh5w+lw7ai-|g-*CD%EBN=6`%KAo6OUFtQs2*nf12TuD7jw2zm<lOUF*L|ksx`{`N9HsX(;h$!BBucJV@Nbc$O39UyE8)>p z_nGiWGdvO{*DLrp)qSSqx`{_OxzB`un&FWsxn9A)o7`tguA6u?;%L363I8<1BT;g_ zf`229E+tn=u7pS1yU&Dwn&FWsxn9A)?cHZeuA6vtk^4+|q!}KGlIs=xyU2Z}lOT4?=z+3O39V*=v4Qa@JKT} z5+&Cw_;;%NOv!ZlOSP>poL*-Nd7F z+-Jfg&G1N+T(98YIqowh*G)Wn)_o@Y(+rP9$@L2UJ?lPGa^1wE)jzB6XTm?t@JN(g zui)S6pDiUNO_38myp$(68b zocm1Js2Ns?lIs<09Oph$a^1wL@3_x|RhnUyD7jw2#_zb#lw3El>L2biVWVbPB}%SW zu<;-6GbPtetlIc<`hF&C)C{Xc$@L00Zv44Ya;4--c=ToWnea$6JQ5|>EBNlOSv%6+Efx`{_Wcb^IWG{YlNa=n6oKX;!gxo+ao!k^cBn($9EJQ5|>EBLqY z=S#_zk}KiSWcQiyPcu9cCD$wXH`#rr`?la+$W_Tn@u2=ByIQN;7>n0xE?>-a$X@*Cl z-(ATPcu9cCD$wXx54S9lOU_gZoU$brX-4`?B8Cgnydhktn%d!N29cTuQE#TnUd3bDs&1G{YlN za=n6ohq=#`TsQIPZugn+Pcu9cCD$wXcendY$#oNt7W|6d(}aJT;gKl0UctWwzfwxB zlw1jq_H>^K|1`rRQF6V4e|x&mlw3FQ=xXo%O!%i69*L6c75w|S`%KAo6OYbyp9zmN!y{30y@G$| zy3dqcH}U8>_nGidGdvO{*DLtn0v;dX~PQ3I8<1BT;g_f`6Ny zRZ6auTnUf9>OK=5X@*CllOU_@Y$v0O39V*=xFzu@JKT}5+&Cw_;TK2vht#G^US(R-TkPcu9cCD$wXH|IH}lOU_ru$6EbrX+XcAp9VG{YlNa=n6o zFT2l_TsQG(opbg5O!%i69*L6c75rQ0+){F-otS9%+V0qU3r7{~mFlDYlOSP^Nmt+rQ}L@bg=tOc%&H~ ziIVFT{5#lvrsTSbM?Z0&3I8<1BT;g_f`318pDDR+;?cX#(|elmPcu9cCD$wX_pb9w z$(52T;n8mHGvS|RcqB@$SMYB)_nDIGCLUexJ`)~khDW00dIkS3cb_S_ZsO6K&)0jJ z@J};55+&Cw`1j`XOUadzE8)?W?la+^W_Tn@u2=AHOZSn0wZ?LHG8X@*CllOU_$OWb3O39V* z=(FxK;gM!|BucJV@b9zkGbPteJo=UUO!%i69*L6c75w{^`%KAo6OR`Crry(pf12Tu zD7jw2zeT@UO0JY#36G|`&xA*s;gKl0UctZV?lUFVO+32AeJ1?V439*~^$Pyo;yzPy z-Nd80F4TLP@J};55+&Cw_&3*urQ}MlOUl z@S;+3rQ}L@bej82c%&H~iIVFT{5#EkrsTSbM~}JBgnydhktn%d!N14cXG*S{c(nYt z^qwaC(+rP9$@L2UE&r`ja;4--cyxsOOn9Uj9*L6c75qEGeWv8PiAO(kp9%jo!y{30 zy@G!~bDt@>ZsO5z?la+^W_Tn@u2=AHFZY>} z>n0vu<31A}X@*ClN1EZ0D7jw2zw_K@O0JuD^n&|L_@@~jiIVFT{CmND zrsTSbM{8Z8?`Oh4&G1N+T(98YT9=fPD7NM9K9E{(ay(rQ}M|gKh5w+lw7ai-)8PJCD%HC@RPcu9cCD$wXx6=1Y$(52T;n6Yf zGvSeDcqB@$SMcu`_nDIGCLZ1EJ`?_FhDW00dIkUPb)PA@ZsO7VF4KFO@J};55+&Cw z`1iicO39UyE8)?n+-Jfg&G1N+T(98Yr`%^suA6vtqx($wrx_lJlIs=xyU~57s?+- zu9RE}k4|x)36C_xBT;g_f`6yD&y-v@@#y#NGvS|RcqB@$SMcxm?lUFVO+5PG6?#t- z{%M9sqU3r7|2}v{DY;T|B|JLBeI`88439*~^$PwS;yzPy-Nd6i-Dko-&G1N+T(98Y zo$fOw*G)W{|4O~53I8<1BT;g_f`9X0SxT;yTnUdRy3d4vn&FWsxn9A)iS9Ec*G)XS z(tRd8(hQG8$@L2UUFkkka^1wES+CN2n($9EJQ5|>EBH6-Ri)%g$(8VEEBBf3Pcu9c zCD$wXx0U-$$#oNtzV1E~9%+V0qU3r7|Gw@%Q*zzJqi5V_!avRMNR(W!;NLUuGbPte zJX-B)eLoZaX@*Cl^>9zX@*Clg{L>7NM9K9E z{{7v3rsTSbM;l+K?`Oh4&G1N+T(98Y#@CgSDTsQG(jvMu! zCj8S3k3`A!3jWP;V=1{(awR)k2J$0QF6V4 zf9Jc;lw3FQ=tcLL@J};55+&Cw`1hjwOv!Z;k!E-#O0HM%?@afZlItcOJ?%ad{%M9sqU3r7|DJZADYRpDaK2vht#G^%S)q9%oPcu9cCD$wXx5%xf5SnGvS|RcqB@$SMYEB+e*omk}KiSsqQo3k!E-#O0HM%?^O4hlItcOJ?cIa z{%M9sqU3r7{~mRpDYlOT4_V!Y8rQ}L@beQ{0c%&H~iIVFT z{5#BjrsTSbM|Zo=gnydhktn%d!N0rRXG*S{c=YZc>pe~Qrx_lJlIs=xd-sn^$(52T z;n5!MGvS|RcqB@$SMYBS_nDIGCLUeoJ`)~khDW00dIkTka-S)=ZsO5wcj!G$_@@~j ziIVFT{G08LQgWr_N_aHZeJ1?V439*~^$Pxtb)PA@ZsO57?la+$W_Tn@u2=By9QT=$ z>n0vO=ROnuX@*CldK2vht#G|+0 zrS~-9pJsR@O0HM%@9lS$k}D-w!lPZ>XTm?t@JN(gui)PlOU_uKP^MbrX;N={^(wX@*Clqba^1wEP43qBGvS|RcqB@$SMYC>yGzNH zk}KiSSKMd9BhBzglw7ai-&fpcO0JuD^o09N_@@~jiIVFT{CmQErsTSbM=SnR?`gt6 z&G1N+T(98Yia#wSS4ysgM@PBOgh!g;ktn%d!M~&2XG*S{c=QYRneb0DJQ5|>EBN;d z_nDIGCLX=_XL?T){%M9sqU3r7|K9tvQgWr_N_aHIeJ1?V439*~^$Pw?ai1xn0w(?H;|S3I8<1BT;g_f`4zjr<7bNxzgOjZrt{saWC}0 ztprlL2^%%TDiJnr`|mz%6eZWb_Uku2-<} zi|#We*G;VYo%>AKs2Ns?lIs<0{GIzu$#oN}midL=(}azhVU;MkUcts?eo;!Ulw1j` z4sxFft2DzZQF6V4jR(2Ulw3FQ=nnUp@JKT}5+&Cw_;-ieSu9RE}k9KvR3I8<1BT;g_f`7ZZ&y-v@@#u2*nea$6JQ5|>EBJT0`%KAo z6OZ0}uin#yf12TuD7jw2zc=4oO0JY#36DPJJ`?_FhDW00dIkSJ<~~z$-Nd7_+-Jfg z&G1N+T(98YS?)6>*G)Y7v-?c=rx_lJlIs=x`?LE@$#oNtR=rQ(&xC)P;gKl0UctXr z?<*x&O0I-Q$GOjhN1EZ0D7jw2zvJ9zO0JuDbieyd_@@~jiIVFT{JYlOTa&3&fix`{^{{9517gnydh zktn%d!M_cDT}rN$TnUd(bDs&1G{YlNa=n6or@7CRTsQIPG54A9Pcu9cCD$wX_n7-k z$#oNtKJ**CrwRWw!y{30y@G!q`b{ahQgS6cI^2CGJkku0M9K9E{vGZlOU_hWkv(brX-Ccb^IWG{YlN za=n6o&%4i*TsQG(&EM+#neb0DJQ5|>EBLqOZ%fIQk}KiSiS9Guk!E-#O0HM%??m^R zlItcOJ?K6Y{%M9sqU3r7{~mOoDYlOT4@}W|4rQ}L@w7>gI zc%&H~iIVFT{M+AsrsTSbN4L4pgnydhktn%d!N1$wXG*S{c=XPP^`0jD(+rP9$@L2U zz4PHxa;4--cr@O9Cj8S3k3`A!3jU3EpDDR+;?bqEBJSV z`%KAo6OZP6RPSlRKh5w+lw7ai-<*$@k}D-w!lUinXTm?t@JN(gui)Qy?lUFVO+32L zeI`88439*~^$Px7=sr_&-Nd86y3d4vn&FWsxn9A)zq-$qTsQG(-9PC2neb0DJQ5|> zEBLqWA4otS{%M9sqU3r7{~mFlDYn0xk#eF9H(+rP9$@L2U z{l$HzEBJSd`%KAo6OZP4QtxTPKh5w+lw7ai-&{|Y zk}D-w!lRwsXTm?t@JN(gui)QK?lUFVO+329eI`88439*~^$Px7;yzPy-Nd86yU&Dw zn&FWsxn9A)zq`+rTsQG(qo?%!O!%i69*L6c75v-isZw&KOXG*S{c(l-;^`0jD(+rP9 z$@L2UE%fJ7a;4--cr?jn0w(;64-nX@*ClbFneb0DJQ5|>EBLq8 zGo|E8$(8WvB=?!{NHaVVCD$wXcar-|$#oNt9&(=v|1`rRQF6V4e-F9Olw3FQXsKuQ zo+kX$439*~^$Pwi^=v7*QgS6cI>3D=^W5nFDqI+=Cb->%jhbPV zD7oIm#tCjWCD%=?`kwntSfv?OiIVFTZ2X@4Ov!Z_4KJ`+}HhE<~EdIcMg zai1xn0w(??t_*3I8<1BT;g_f`9LOv6NgXxe^{t zb)N~3G{YlNa=n6oQ{87uuA6vtllx5grx_lJlIs=xyUBf~ zEBH6!rBZUGwO0JuD z^osjT_@@~jiIVFT{CmZHrsTSbN9(<;?`Oh4&G1N+T(98YdM}rfDUeSA+ z@J};55+&Cw__x3-rQ}MlOT)=sr_&-Nd6S-Dkoh&G1N+T(98Y zmF_bo*G)W{^;Ny63I8<1BT;g_f`7BVT1u{zTnUfHy3d4vn&FWsxn9A)vF`3(R7 diff --git a/rio/tests/logs/FRC_20240120_210521.wpilog b/rio/tests/logs/FRC_20240120_210521.wpilog deleted file mode 100644 index 9a8183a3f262a47904b2397dbc0b2a5e119a85e2..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 90721 zcmbWA3E1Fsy}sYEzGN0H63H}KT4W;0k}O%0H3mtRFrmy|?Oq9j5| zNrVa|kwnsJO(voa^*gs&+~cs{^mx}X@{*h-F&M}21X6!$Fc+Y zwLxI|(Cpch_ZphL5WhZq&aA03_IhDulYxQx__f9PWy?uxP26~kZ8x2<=gf&K&pBwn zp?~jkXyA|u`^}oU-_Wc%Q-@|xSnH5OSDG+;=G<9Rh9;~vVarJq4jq_p;er234-9NK zFfc!n;h3*(GI{#Yw)+iDnY!oHDOvy;h&CM4h$^v|2e$voXK;h&Ybaoc#uT~^OKF6 z{Mc$>U{U^MDSq*5^E2~ZGiS`1HFMgAljlr+QLnP-;J|pUz3}6G0|PJN*Ivsn!{;}j zdce>QQ)lck^T3HK&*pbv?(B)1?=XI&8IyOPHnhi!e4Up>T7D)j#_{1_ww%Oo@{FM= z!_PAOvvi-?GiSVse?4Tv>?wN>O`ja`{=G8n?S%i7lHd3VM23Hi8`x(2mII^twO8}Y z@NfOL(!9BjlpVxcf82ok8$&KY|Zx={sZsu zPuST9&7LzfebUtF|5o-b{Ga*OBY%(`{)xN!%)PdlJcB=u&*Dq(Idj(Z$#eKwFo)Ex zb9S9Od%~gP26p7=OG&@2E;4+1_{aFsgTIINMalIFKX+fcXeqf;a;4FO>+R+~)7W`t z`-uBY$#oNt-ZuOvrjhqF;gM!|BucJV@b7Jl zm69tZSHh#C-Dkoh&G1N+T(98Y(e5)P*G)XS(|sm9(hQG8$@L2U-RVA4a^1wEWrzQK zJo26lOUF#(k#bx`{`N4F9#q$a|XbNHaVVCD$wXx5$`Ma;4--c(jB2O!%i69*L6c75v-5 zeWv8PiASGzp9zmN!y{30y@G$Acb_S_ZsO5X?la+^W_Tn@u2=ByDfgL@>n0v;F#Okb zBj3-2f12TuD7jw2zYWHgk}D-w!lRSjXTl@R@JN(gui)Rw?lUFVO+32aeJ1?V439*~ z^$Pyo?>7NM9K9E{%z$xQ*zzJqjTJ6!XwS_NR(W!;NLm! zGbPteJo>%+O!%i69*L6c75w|X`%KAo6OYz>nZBP1|1`rRQF6V4e`~(1lw2vf5*{7z zJ`)~khDW00dIkTEcb_S_ZsO71?la+^W_Tn@u2=ByZugmz>n0v8_j0|b3I8<1BT;g_ zf`7}syp&uixe^}Da-Rv0G{YlNa=n6ov)pG&uA6vtgZoVQrx_lJlIs=xyTN^?QgS6cy2O1Z zJkku0M9K9E{$1ieQ*zzJqrbV&gnydhktn%d!N0$`&y-v@@o1AJ^!-furx_lJlIs=x z+hmDSa;4--cyy}!On9Uj9*L6c75qEZeWv8PiAN8)&xC)P;gKl0UctYI+-FLzn|QR! z@ZaLd4}37;pJsR@O0HLuD7NM9K9E z{_W#FQ*zzJqi?v+gh!g;ktn%d!M|^~&y-v@@o1spzuz?So+kX$439*~^$Pwi^r}*F zrQ}L@G|7D?{L>7NM9K9E{!Mb9DY>*u9RE}k3Q-?6CP=XN226< z1^+(kK2vht#G_xh&xC)P;gKl0UctX#xX+YaH}U8VOY1#N_@@~jiIVFT{CmUFrQ}M< zmGJ05_nGiWGdvO{*DLsUp!-b8brX+na-RwRG{YlNa=n6oH@VN0TsQIPWy62(Ge3s^ z{3pUc5gv(>>y_k6$(52T;n8IGneb0DJQ5|>EBH70)urT0$(8Wv%kDGbk!E-#O0HM% z@5}BpCD%n0wJAO0sM@`FFVnD9?CJQ5|>E6J6TDBYh_P(+rP9$@L2U%~-aSTq(H{ z9(~Kde(hQG8$@L2UJ>Wi5a^1wEl}G+*u3;Zc_@@~jiIVG;lOUl^L3@n0v8zk=S= zgnydhktn%d!N28KC?!`)u7pRk-Dkoh&G1N+T(98YZ1n0w3+kGbd(+rP9$@L2U zecOGeEBH5V#Zq#mczOn9Uj9*L6c75w|O`%KAo6OVr5J`?_FhDW00dIkS}<33Yz z-Nd6+-=Oz2;h$!BBucJV@Nd;Ol#(kYSHhzs-Dkoh&G1N+T(98Yk?u1k*G)XS-F+tf z(+rP9$@L2U-R?e9a^1wEW!|XwG~u6ScqB@$SMYC{HEBNEBN=uHGbPteJbL-cdQTJnX@*Cl zEBJT0`%KAo z6OW#Ep9%jo!y{30y@G$wyU&zdH}PnTiTZvf{L>7NM9K9E{%tX_lw2vf5+0r9J`)~k zhDW00dIkT^a-S)=ZsO7J+-Jf+&G1N+T(98Y@7!lfuA6xD_Tm51V1DrDTND0ihDW00 zdL_A1a;4--cyz4$On9Uj9*L6c75qE)Ev4j2$(8Wvr|vW1pJsR@O0HM%@2BoFCD%6_9;h$!BBucJV@bCTZ zGbPteJoEBH5YwNi4WEBNlOSv-+iX!x`{`Ba-RwR zG{YlNa=n6oe{!EGxo+aoJKnDEXTm?t@JN(gui)Q1-d;+slw1jqKH@$T9%+V0qU3r7 z|32bAQ*zzJqo29Ygnydhktn%d!M~rm&y-v@@o2?0^qwaC(+rP9$@L2Ut++-hxl(c^ zJeuo16CP=XN226<1^?!{&y-v@@#sePneb0DJQ5|>EBJS#`%KAo6OR^OQ}1cQKh5w+ zlw7ai-{NbQk}D-w!lPZ?XTm?t@JN(gui)RV?lUFVO+5ON`%HMG86Js}>lOU_lKV`_ zbrX;N?miR#X@*ClFzV( zk!E-#O0HM%?{xQ>lItcOJ>otS{%M9sqU3r7{~mFlDYlOT4 zZS7KWrQ}L@bd>u{c%&H~iIVFT{5#5hrsTSbM|Zf-gnydhktn%d!M{7)XG*S{c=Vcg z=sivNrx_lJlIs=xd(Asa$(52T;n6hrneb0DJQ5|>EBH6feWv8PiAUdbp9zmN!y{30 zy@G$=be}1?ZsO4*>*zgA_@@~jiIVFT{99z5QgWr_N_ez``%L(!86Js}>lOUl!F{IW zx`{^@y3d41n&FWsxn9A)3*Bc*uA6xDr29%UXq&xC)P z;gKl0UctZh-&snolw1jqKJGpf9%+V0qU3r7|32?^~#vxSt+?va;4FO&mZJI)7W`t2XEtsgO0HMN49`l*m69us z9^7>g_nF4dGaEM#CD$ushG(VZO39T*4<7zi_nF4dGaEM#CD$ushG(VZO39T*51u!$ zzTVR`cAnX|c__JF88bX9C09zWG`?la+$W_Tn@u2=ByIQN;7>n0xE)xgJ zG~tnEcqB@$SMcw3?M z((kPa8#Ti!QF6VBjlXugDY9p*k0R%wP+ zqU3r78xM1zDYr8g-h zS4ysgRa4z(!bZ)oN|ao$VB=KxnUd=!R$b*j6IN-4Rifm21skt&pDDR+V%380)_a<; zQ8TO(CD$w1xZt}>$(52T;nBA4GvS|RcqB@$SMYCJ_nDIGCLVppeI`88439*~^$Pxd z#(k#bx`{`BcAp9VG{YlNa=n6oe|DcKxo+aoI-BbIneb0DJQ5|>EBLq0rlsUc$(8Wv zqwX`|k!E-#O0HM%@1yQBCD%7NM9K9E{@v?7Q*zzJqu0Mj?`gt6&G1N+ zT(98Y>)%sKu9RE}j}CC336C_xBT;g_f`13N&y-v@@#wqmGvS|RcqB@$SMcw9C09zWghw;nXTm?t@JN(gui)Pd_nDIG zCLUeuJ`)~khDW00dIkTkb)PA@ZsO6RTj@Pb_@@~jiIVFT{9AOZQgWr_N_g}>_nGid zGdvO{*DLtn0w3-hC!K(hQG8$@L2UecpYh zEBJSa`%KAo6OVr2J`?_FhDW00dIkS};677w-Nd6;Zlm`!;h$!BBucJV@b8t|l#(kY zSHhzq_nGidGdvO{*DLrp5ekT0W439*~^$Py2HK~+bDY+6J9q&F9 z9%+V0qU3r7|BiQ`DY-qVDCn&FWsxn9A) z<+dv&S4ysgN3+~#!XwS_NR(W!;NL9wnUd=!9(~(=Cj8S3k3`A!3jTfDeWv8PiAQ6% z*L#}qPcu9cCD$wXH+K6{a;4--c(k+oO!%i69*L6c75v-TeWv8PiAP^_p9zmN!y{30 zy@G#Vbe}1?ZsO73+-Jf+&G1N+T(98Y-`rLP>Jkku0M9K9E{vF{yQ*zzJqaV4?gnydhktn%d!M`85 z&y-v@@#xh%>OD>Prx_lJlIs=xd-aZ`EBLpM`%KAo6OX>( zJ`)~khDW00dIkT!;XYGx-Nd7X-mmvG;h$!BBucJV@Nc2_my#pl}6X@*CllOUF$$h5ex`{_G-&yZz!avRMNR(W!;NQ!4E+tn=u7pRs zyU&Dwn&FWsxn9A)-Q8zOuA6xD75AC&NHaVVCD$wX_Z9b7NM9K9E z{{72+rsTSbN1N}W?`Oh4&G1N+T(98Y=DU=VDEBJS;`%KAo6OVr4J`?_FhDW00dIkS};yzPy-Nd8U?xy!N;h$!BBucJV@b9&| zm69tZSHh#2?la+^W_Tn@u2=AHru$6EbrX-iEBN=4$))5<$(8VENB5cVPcu9cCD$wXx1;+^$#oNtE^?mn0v8xrg4xzB`un&FWsxn9A)z1(L?uA6xDHTRkD zNHaVVCD$wX_ciyKlItcO%|E2~G~u6ScqB@$SMYECp;B_CsPcu9cCD$wX z_g?pzlItcOo##Fi9%+V0qU3r7|ITxtDY7NM9K9E{;jZADY;T|B|MtrJ`)~khDW00dIkUHxX+Ya zH}U8@?la+^W_Tn@u2=ByJMJ?j*G)Vcx3}KYgnydhktn%d!M|~Pmy#7N zM9K9E{tZqmC09zWgh$)E&xC)P;gKl0UctZZ-DgU!n|Sm&_nGiWGdvO{*DLtn0vO;XV`oX@*Cln0wpvA^Eagnydh zktn%d!M`>3FC|w>u7pP)cAp84G{YlNa=n6oA9kN9xo+aoPu*w2Kh5w+lw7ai-%s6V zO0JuDG+~zB(}aJT;gKl0UctW!vr5U8k}KiSe(p2jpJsR@O0HM%Z$I~$lItcOUFSX% z9%+V0qU3r7|E_bNDYONC)-Nd7%4$ym=@J};55+&Cw__x#nrQ}MEBLp- zfu-b1$(8VE8~2&;Pcu9cCD$wXw~hNu$#oNt&Uc>)k2J$0QF6V4f9Jc;lw3FQ=+Evm z;h$!BBucJV@bAy=GbPteJX+_2`hF(-(+rP9$@L2Ut@FWBa;4--cyyxsOn9Uj9*L6c z75qEVeWv8PiAO(op9%jo!y{30y@G!~cb_S_ZsO632kAXc_@@~jiIVFT{9Ey$QgWr_ zN_aHaeI`88439*~^$PyYb)PA@ZsO66?la+^W_Tn@u2=ByM)#SL>n0v8ez4xtgnydh zktn%d!N0{1E+tn=u7pRsxzB`un&FWsxn9A)-P~tNuA6vtnfpw5q!}KGlIs=xyUcy2 zlOT)?mkm;-Nd77+-Jfg&G1N+ zT(98YHSRMd*G)WHlOUl;P6s%rQ}L@bh7(Qc%&H~iIVFT{5#owrsTSbNB6tWgnydhktn%d!N2?6 zXG*S{c(l?HdQTJnX@*CllOUF!hNRXx`{`lj?#OY@J};55+&Cw_&4gPQgWr_ zN_e!D`%L(!86Js}>lOUl%6+Efx`{{UxX*+~n&FWsxn9A)bKGZ2uA6xDd-s{}Pcu9c zCD$wX_j~u5lItcOt$DP*p9%jo!y{30y@G#h9$iYVlw1jqj&q+0k2J$0QF6V4f5*Ac zlw3FQ=q~q}@J};55+&Cw_;;84Ov!ZtaCDpoL*-Nd7fKdkR(!avRMNR(W!;NQj{E+tn=u7pRYy3d41n&FWs zxn9A)Q{87uuA6xDko!#drx_lJlIs=xd&qsJlOT4`uI|ErQ}L@G}V13{L>7NM9K9E{!MkCDYsPcu9cCD$wXcdz?Q$#oNt zUVozA(}aJT;gKl0UctZDpIAz+lw4`?!DSCT@xZt4GV;IX0jb@DRhnUy2pbRlcON#2 zl51abC9Jy1eI{(w468)R^$IrL^zZiMI!msERWJLfe*a9^s2Ns?lIs<0eA!1!$(52T zVbx^!nXpkatP&;HE7&;MeWv8PiB(^Ap9!lp!zxj7y@HKjcAqJ^ZerCx-Dko^&9F+8 zT(4l`Kiy|auA5l3*~j$#OxUOyR*9196>Qw>W2NLu$(69`4ELF^N;9kyCD$w1c!v8- z$#oNt9(A7yk2J$0QF6V4e~-G)lw3FQX!Voyo+dof439*~^$Py2eo`s9QgS6cI>vn_ zJkku0M9K9E{vG2!Q*zzJqaVA^gnydhktn%d!M`87&y-v@@o4EBJS<`%KAo6OR@> zS?_7WKh5w+lw7ai-=Zg%k}D-w!lNDCXTm?t@JN(gui)Q~?lUFVO+32DeI`88439*~ z^$Px7O0JuD^nm+J_@@~jiIVFT{CmKCrsTSbM{oY5-qVDC zn&FWsxn9A)H-EB}Tq(H{9v$L76CP=XN226<1^*6lpDDR+;?XVcGvS|RcqB@$SMcu^ z_nDIGCLS&MDZQr&|1`rRQF6V4e@lLn0w}cZ%NAgnydhktn%d!N2)VDJ54*u7pQhyU&Dwn&FWs zxn9A)t=(rzuA6vtuKP@Qq!}KGlIs=xJJ)@tpe~Qrx_lJlIs=xTmI9fEBN>B(@V*fk}KiSr`>15BhBzglw7ai->2PYO0JuD z^c(k?@J};55+&Cw`1c$4nUd=!9<6$Y-qVDCn&FWsxn9A)RnI6TS4ysgM@P8Ngh!g; zktn%d!M`KiXG*S{c=RLpneb0DJQ5|>EBN;#_nDIGCLS$wrry(pf12TuD7jw2zh%xW zC09zWgh%_j&xC)P;gKl0UctY8-DgU!n|O4!`%HMG86Js}>lOUF+I^O39UyE8)?0?la+^W_Tn@u2=AHJNKEA>n0w3)_o>C(hQG8$@L2U zeb#-ZsZekT0W439*~^$Py2dv+~(|Jkku0M9K9E{+;ALQ*zzJqx;-v!avRMNR(W!;NN}jGbPteJbL3fdQTJnX@*Cl z zq!}KGlIs=x`-=Na$#oNto_C)K|1`rRQF6V4f6u$mlw3FQXp8go{Y?0$86Js}>lOUl z;=EFFrQ}L@be8)}c%&H~iIVFT{5#8irsTSbN56BQ3I8<1BT;g_f`7krpDDR+;?djB z*L#}qPcu9cCD$wX_xAHk$(52T;nA_~GvSeDcqB@$SMcvx_nDIGCLaC7eJ1?V439*~ z^$Py|#C@jZx`{`x{mj6B{TPo&BK#BKktn%dNv@P!DY+6J?dLud{%M9sqU3r7|MvS# zDY;T|B|N&$eI`88439*~^$Px7=RQ+%-Nd8CF3@|L@J};55+&Cw__x>vrQ}MxzB`un&FWsxn9A)z1(L?uA6xDHTRkDNHaVVCD$wX_ciyKlItcO&Hs75 zrwRWw!y{30y@G%9f4-DlDY+6JZR0)@{%M9sqU3r7|F&_TDYUtCJAlw1jqc6FZ#|1`rRQF6V4f4jQRlw3FQ=u7T1 z;gM!|BucJV@b63RGbPteJo>x)O!%i69*L6c75w|V`%KAo6OT5%MBmSZf12TuD7jw2 zzfCVGC09zWgh!{j&xA*s;gKl0UctZ9+-FLzn|SoF`%L(!86Js}>lOTa*nOttx`{`t zeNpde!avRMNR(W!;NNOrEG1V;u7pQNxzB`0n&FWsxn9A)quggouA6vthx<(Urx_lJ zlIs=xyTg5^ErQ}L@w7vUG z_@@~jiIVFT{M+7rrsTSbM;E%!gh!g;ktn%d!M_XLXG*S{c=V+EO!%i69*L6c75sbB zeWv8PiAU>SrtfFMKh5w+lw7ai-};x8k}D-w!lRG7&xA*s;gKl0UctYQyU&zdH}U9~ z?la+^W_Tn@u2=Bym+mtq*G)Wn)0g$0Cj8S3k3`A!3jV$6%cbN>$(8WvAorQ@NHaVV zCD$wXcaZx`$#oNtzVAL0{%M9sqU3r7|Gw`&Q*zzJqb0tg_cY<3W_Tn@u2=AHiLaEB zD8pOn9Uj9*L6c75w|E`%KAo6ORTi*L#}q zPcu9cCD$wXH*k3=xl(c^JlfKICj8S3k3`A!3jS^BK2vht#G|v_XTl@R@JN(gui)R= z?lUFVO+0$ceJ1?V439*~^$Pwy<~~z$-Nd6czN+tM!avRMNR(W!;NKcwEhSe!svM$(8WvF!!18NHaVVCD$wXcbNN3$#oNtZgrmt|1`rRQF6V4 zf492Nlw3FQXsN68o+kX$439*~^$PwibyX?3QgS6cn(96i{%M9sqU3r7|E9Xnlw3FQ z=qmS_@JKT}5+&Cw_;;22Ov!ZlOU_v-?cR zbrX-)xmw@Pgnydhktn%d!M}B`E+tn=u7pP?y3d41n&FWsxn9A)6WwP@uA6vtulr2+ zrx_lJlIs=xyVrfDn0v;cCEgj3I8<1BT;g_f`6M`TS~5!TnUfPaGwc}G{YlNa=n6oXSmOl zTsQIPx9&6HpJsR@O0HM%@3-zVCD%EBJS<`%KAo6OR_X zUhiqbKh5w+lw7ai-=f!-k}D-w!lU=O&xC)P;gKl0UctZjxzChbH}UB6?la+$W_Tn@ zu2=By^X@Yx*G)Wn%6%sM(+rP9$@L2UJ>@=Aa^1wE4Q|l)GvS|RcqB@$SMYCx8%oKQ zk}KiS$?h}Zk!E-#O0HM%?_~FxlItcO-S0jV{%M9sqU3r7|L%96DYEBN;V z_nDIGCLX==J9QgWr_N_aHnJ`?_FhDW00dIkT6+-FLzn|O4E z`%HMG86Js}>lOUF!hNRXx`{`lZq$34@J};55+&Cw_&4gtQgWr_N_e!D`%L(!86Js} z>lOUl%6+Efx`{{Uy3d41n&FWsxn9A)bKPf3uA6xD2ltuqPcu9cCD$wX_XqcxlItcO zt@T}fKNJ3GhDW00dIkU1`fe$?QgS6cI^KOIJkku0M9K9E{vGc=Q*zzJqr2T_!avRM zNR(W!;NRWuGbPteJX-E1y{8HPG{YlNa=n6o%iUB;u9RE}k7l{ggh!g;ktn%d!M|DV zGbPteJi5VsCj8S3k3`A!3jW>TK2vht#G|p_(|elmPcu9cCD$wXH}-p_EBLpw`%KAo6OX>=J`)~khDW00dIkT!=sr_&-Nd86xzB`un&FWsxn9A) zzq!wpTsQG(lke;Mneb0DJQ5|>EBLp`_e;r@k}KiSsqQo3k!E-#O0HM%?^O4hlItcO zJ>)(U{%M9sqU3r7{~mImDYlOT4<>pdyrQ}L@bcFj%c%&H~ ziIVFT{5!&ZrsTSbM?Z3(3I8<1BT;g_f`319pDDR+;?b*rp!YQ4pJsR@O0HM%@6|sj zC09zWgh%_h&xC)P;gKl0UctY8+-FLzn|SmM_nGiWGdvO{*DLt<4fmOn>n0v8bc^28 zgnydhktn%d!M}xWDJ54*u7pRE+-Jf+&G1N+T(98YB=?z;>n0vu;64)`X@*CluOv!Zk}D-w!lMJ-XTl@R@JN(gui)Q-?lUFVO+32EeJ1?V439*~^$Pyo zq!}KGlIs=x`-=Na$#oNt{^dRs{%M9sqU3r7|NiAZQ*zzJqs@P$?`Oh4&G1N+T(98Y z=07SWS4ysgM`yavgh!g;ktn%d!M`)zXG*S{c=V|IO!%i69*L6c75sbDeWv8PiASs7 zuJ<(IpJsR@O0HM%Z}rn0w3%Y7z1(hQG8$@L2Uean5Ozc4pGprIN*DKh#@=r?1 zm69uA)rZ_?!Ya+MN|ao$VB?3}XG*S{Sapm0OxUOyR*9196>Pl4eWv8PiB(JfRPSlR zM$NEFlw7Z1n2vsf0y3VgpHbEl_zUJlItcO{n33U{L>7NM9K9E{{7K?rsTSbM{D1$?`Oh4&G1N+ zT(98Y+IN?dDWK2vht z#G@7N(R-TkPcu9cCD$wXx57Q87NM9K9E{*C)tDY;T|B|O^2eJ1?V439*~^$Py& z;yzPy-Nd6y-Dkoh&G1N+T(98YrS3B&*G)Wn&V45Q(+rP9$@L2UJ?B1Ca^1wEcmG`9 z&xC)P;gKl0UctY2|Gbo3DY+6Jo#s9h9%+V0qU3r7|4wtCDYn0uz-lz98;h$!BBucJV@Ne+G zQgWr_N_e!r`%L(!86Js}>lOUl-hHOzx`{`hbDs&1G{YlNa=n6opL3rnxo+ao6YewN zpJsR@O0HM%?+N#rlItcOt@lfPKNJ3GhDW00dIkU1`(-J)QgS6cI>~(|Jkku0M9K9E z{+;ALQ*zzJqx;-v!avRMNR(W!;NN}jGbPteJbL5(dQTJnX@*CllOT);yzPy-Nd8I-Dkoh&G1N+T(98YEBNlOUF&V8ojx`{`NJ*4+E;h$!BBucJV@Ncn)O39UyE8)>j?la+^W_Tn@u2=AHC-<3> z>n0vu>^>77X@*CllOUl=r^V0O39V*=u_@9;gM!|BucJV@b6RZGbPteJbKW5Cj8S3k3`A!3jRIl zK2vht#G{E1>pe~Qrx_lJlIs=xoA_`kxl(c^JUY~UCOpy%k3`A!3jQ7HK2vht#G_l? zXTm?t@JN(gui)RU?lUFVO*~ra5xu7g|1`rRQF6V4e@i`5O0JY#36J)6p9%jo!y{30 zy@G#xyU&zdH}UA}?la+$W_Tn@u2=By>+Ulp*G)WH;J12D6aHz2N226<1^*WKZ7I1@ zawR<4#(gII(+rP9$@L2UZR0*ua^1wE^WA5{BhBzglw7ai-}&w{CD%EBNlOUF(S4@mx`{`NKc@FI;h$!BBucJV z@Nen0vu<~|c1X@*Cl}Dzp!YQ4pJsR@O0HM%Z?!*^ zk}D-w!lR?yXTl@R@JN(gui)QN?lUFVO+328eJ1?V439*~^$Pyo;XYGx-Nd71|ETvg z;h$!BBucJV@Nd~amXa$aSHh#|?la+^W_Tn@u2=AHy8BGYbrX-Sai0l~G{YlNa=n6o z*SOD=TsQG(kw57@P57r79*L6c75rP|Po?Ba$(8VE2ltuqPcu9cCD$wXw}bmk$#oNt zE_9y>k2J$0QF6V4e;2yXlw3FQ=t=jP@J};55+&Cw`1hpyOv!Z;JiwTq(H{9-Zty6CP=XN226<1^-TVpDDR+;?e!?GvS|RcqB@$SMcwC_nDIG zCLXQy7rmzm|1`rRQF6V4e=Gf^lw2vf5*{7wJ`)~khDW00dIkRucAqJ^ZsO6+?la+^ zW_Tn@u2=ByX7`zr>n0v8@wnd8gnydhktn%d!M`OQFC|w>uC(aj4NE*e;lB#UW7QsR zH({e@SS3oXH?eUKx0{mdCRTmbeI~5Z468)R^$IqA)qSSqx`|bzp3v)>uu(It5+&Cw z*f{EmQgWr_N?5g(`%KuV8CHps>lJL=%6+Efx`|cixX*-DnqieFxn9A>bKGZ2uA5l( zd-s{JQ8TO(CD$w1_TK2vht#G^4!>pe~Q zrx_lJlIs=x8}oE2xl(c^Jo&N1EZ0D7jw2zr)>UO0JuDbesE3_@@~jiIVFT{JYJ4rsTSb zM@v7e_cY<3W_Tn@u2=AH>1RvHm69vr(Ny=D@J};55+&Cw_&3#krsTSbM_0Megh!g; zktn%d!N05AXG*S{c(mZ(^qwaC(+rP9$@L2UE%>)ma;4--cr?joYeYR|C?4UAuW@PS*VY;$dC;Jb_5_|`o~ zW{1CTs|T+rJ#g+R8?Cn2$c%s5e1$7Z>#e=~zC-(t%&xzEpZy;!@xR)13oXCofg{rgK7RQ^7o1sK_wHvF zTH#e69GPCf&pQ@A?6l&tmmj(CDoY(SGClmS-&=V8bBhnGv)JI=*BqSb_<;on_E~b& IV*DKZf2NRtR{#J2 diff --git a/rio/tests/logs/FRC_20240120_211051.wpilog b/rio/tests/logs/FRC_20240120_211051.wpilog deleted file mode 100644 index ede5524b5cbdf64bb9310d39fcd4e2a40f4f7bcb..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 90721 zcmbWA3Hac1y|({j{gc@wYs?rzB@-b_)-2h@lq50CU@$Y986?SCh$IqHNJ&Xj5+X}d z60$^_EGdapmW1kf@BiccpYG{;ulF49T$d~N`Fx(wGrqsMwa{so9k<$chb;$24CKeS zf&AVeFn#Lm*^~F3I(q?rfA*YN(`M}Z^2(M21M~2EWBG0SN$XAAV!NHUnz7H!iL1>y z?4YUt-sRB1;S&y;HS?gUv*t{jI(x!;haa)(gxNC>p0)SX3F}SRe$s>^2Ig6C;J?!Y z1MeCbn3u><%vZOZJbmiU2Tk33+CJ0v-tOg}&@1a0Iy$h;z`%U`!)yNI_~wU9oiS&7 z8eZASp;H6f4h+oCKP>SdC%2nCXYc*CKY03HQ)dm0r_GqN_L~2jrvSpuGKoXK;h&7AT7@gNHg z<`)|``LV;mz{32)(){Mv=2zz1XU>>2YvzHQO`bFP6}`&Bg9GEa_VSPS3=F)A-&>yF zhR$z0?U1RvOq(%f=Ajc;o6R4k2&pEPayzkjK}ng26y zH2f#0p%1?8%zd|;JcB>~&EkdjnK^6v0s++W+?l*y$BM&e*v+cBNxcI#%GfeB}KO-e;c!r@s7&SDQX{&g3aP$i$te z9>}k(sZ(CjpDf2OJb1&(3k_W!`Z0dw;6n!wshl6__PvI~OI_E2D;HrQ}M_PvI~OI_E2D;HrQ}MEBJSf`%KAo6OR@e`qv)A?`gs#&G1N+T(98YLZeE_m69vr(Jt;Y z;h$!BBucJV@NXCQnUd=!9(~z;COpy%k3`A!3jTfBeWv8PiAT@6&xC)P;gKl0UctX- z-DgU!n|QS8(7&!5{(dI>(+rP9$@L2UZ92M?Tq(H{9-Znw6CP=XN226<1^-TUpDDR+ z;?V={GvS|RcqB@$SMcuv_nDIGCLXOi^e<_L-_wMDn&FWsxn9A)RmYT)DEBJSd`%KAo6OWb{`j_v+?`gt6&G1N+ zT(98Y5@Sorm69vr(Ny=D@J};55+&Cw_&3#krsTSbN0+MA1rwRWw!y{30y@G!u7A+-LO0I-QJGjq;f12TuD7jw2za89XO0JuD^m+H0 z@JKT}5+&Cw`1g7DnUd=!9zEec6aHz2N226<1^=FKpDDR+;?cUV*7q~vpJsR@O0HM% zZ{1gyk}D-w!lM)1XTl@R@JN(gui)Pa?lUFVO+32KeJ1?V439*~^$Pyo=RQ+%-Nd66 z7t?#1@J};55+&Cw__yL>rQ}MlOT)rhxl(c^JbIt|O!%i69*L6c75sbO zYf8zLk}KiS#qKlVk!E-#O0HM%?_&3vlItcOz2H6*{%M9sqU3r7|6XvPDYIB}O39V*=nVIn@JKT}5+&Cw_;-f;Ov!ZLr_+Y|6&G1N+T(2ZoO0JY#36G9+p9zmN!y{30y@G#7E>TLZ zlw1jqZg-yvk2J$0QF6V4f495Olw3FQXqlnEtTX(cCj8S3k3`A!3jQs#WGT5)awR<4 z-+d(hQG8$@L2UUEn@b za^1wEzq-$aN1EZ0D7jw2zrVWAlw3FQXv3ku@HYJYO!%i69*L6c75v+9=~8l~(QQBucJVk}D-wO0I-Qlig>+Kh5w+lw7ai-{jYpk}D-w!lQ4v z&xA*s;gKl0UctX_xX+YaH}U9S?la+^W_Tn@u2=ByU+yy{*G)Xyc3FKt6aHz2N226< z1^>2Pwv=2cxe^|I)_o>C(hQG8$@L2Ueb#-ZCj8S3k3`A! z3jWO)S4yswTnUf9=RZCZ9%+V0qU3r7|GwuwQ*zzJqgRc8`8D~w4e&^Wey_k6 z$(52T;n9cPXTl@R@JN(gui)Q@CzO&aC0D|uTis{EBhBzglw7ai->vR5CD%n0w}v!Z-A;gM!|BucJV@Nb?KOUadzE8)?O?la+^W_Tn@u2=AHNB5bM z>n0wZ=ROl2X@*ClEBN=0RZ7W~k}KiSXWVDPBhBzglw7ai-)G!sO0JuD^gH*N@J};5 z5+&Cw`1d>anUd=!97NM9K9E{w@24QgWr_N_cdD z`%L(!86Js}>lOSvzn0vO>^>9z zX@*Cl?DY+6J z?dv`h{%M9sqU3r7|MqpCDYm3I8<1BT;g_f`4y&Ybm)>awRlOU_hx<&)brX-aT2J54gnydhktn%d!N0B6DNG~u6S zcqB@$SMYDm^-IZ>k}KiSG43n0v; zvZ2183I8<1BT;g_f`6NASW2#xTnUdp={^%4X@*Cl}qv&PB=f%BZ1PDY;T|rICX#9p*mMn7L+S=c43# zWz^8Dlw2vf(#XMax46$VX0F-TxhT0_88tL3C09zWG;(mG#W&V_n#RmE8#@;z*DIrj zW~Jmx$(2S9?lHxErZIEP#?D2_^~$KBSt+?va;1@jM}EtFrZIEP#?D2_^~$KBSt+?v za;1@j=M8M4_cV=}Yc_T+O0HK%4b4i)m69us9K3#e_nF4bH5)q@CD$vXhGwPYO39V* z=yUEf;gM!|BucJV@b7c(GbPteJbK)HCOpy%k3`A!3jRIrK2vht#G`dL)%P>uk!E-# zO0HM%Z{1Bx$(52T;nDH#GvSeDcqB@$SMcw6_nDIGCLZ1EJ`)~khDW00dIkUPb)PA@ zZsO4jZ`XU8@JKT}5+&Cw__xB_OUadzE8)=x+-Jfg&G1N+T(98Y2i#{$uA6vtz57gf zq!}KGlIs=xyWV}KEBH5Zvr=-U4O>>_K8#Ti!QF6V4jnmv`O0Jt&b*1}ESfv?OiIVFTY`oHarsTSb zRr9|??`gtD&9F+8T(4l`{O>3wS4ysgRXe-SgpHbEl_nXpPT ztP&;HE7S=BhBzglw7ai-y!ZZCD%lOSv+kK|wx`{`BaGwePG{YlNa=n6oe{i2E zxo+aoTHEP8P57r79*L6c75rOkyHawclOT4c!yGQrQ}L@ z^d9$_@J};55+&Cw`1c<7nUd=!9(~z;COpy%k3`A!3jTfBeWv8PiAT@6&xC)P;gKl0 zUctX--DgU!n|QS8j{1Hk{L>7NM9K9E{%yKrDY;T|B|JLSeI`88439*~^$Px->ONC) z-Nd5@-Dko-&G1N+T(98YgYGjW*G)Wn(>wK^Cj8S3k3`A!3jV$6ou%YT$(8WvaQB(; zNHaVVCD$wXcewjZ$#oNte(XLI{%M9sqU3r7|9!{L>7NM9K9E{;fBulw2vf z5+0r4J`)~khDW00dIkSZaGxo;ZsO5>?la+^W_Tn@u2=ByKKGfD>n0wp_%6Mt3I8<1 zBT;g_f`2Q%tCUawR<4-+dBx$(52T;n64DXTl@R@JN(gui)P&+-FLzn|SnV z_nGidGdvO{*DLtn0w({(X8+6aHz2N226<1^-_EzEX0fO0JuDwAk)?PZR!WhDW00dIkR$+r5-r zDY+6J?d3ib{%M9sqU3r7|Mqg9DYlOU_ ztouyKbrX*sbDs(SG{YlNa=n6okGap3TsQG(?LG9KCj8S3k3`A!3jVFVM=7~dawR6X&$#oNt?s1<9|1`rRQF6V4fA_f0lw3FQX!$+$o+kX$439*~ z^$Pwizh^1AQgS6cn(00h{%M9sqU3r7|7N<+lw3FQ=zH!n;gM!|BucJV@b7!>GbPte zJbKk+y{8HPG{YlNa=n6oubNy+u9RE}k9KvR3I8<1BT;g_f`7ZZ&y-v@@#rG=nea$6 zJQ5|>EBJSj`%KAo6OW#Ap9%jo!y{30y@G$wxzChbH}Pn*z4ZM|_@@~jiIVFT{M&4= zQgWr_N_cdd`%HMG86Js}>lOSv&3&fix`{^*xzB`un&FWsxn9A)humjMuA6wY+TMCk z6aHz2N226<1^-styOdlhxe^|I*nK8E(hQG8$@L2Ueb{}b=-pDDR+;?a}tGvS|RcqB@$SMcvi_nDIG zCLXQ7kG`J?|1`rRQF6V4f9vm4O0JY#36D;6p9zmN!y{30y@G!yy3dqcH}U8f?la+^ zW_Tn@u2=By7w$79*G)WHX9}$(8Wv z{q8g2pJsR@O0HM%@BQvGCD%EBN<&_nDIGCLX;ZaD6aHz2N226<1^<>k zpp;xGxe^{7;64-nX@*ClSxT;yTnUf%cAp9VG{YlNa=n6od%Mq+TsQIPo9;8= zk!E-#O0HM%@0;#3CD%7NM9K9E{=MWrQ*zzJqwNmT_cP(2W_Tn@u2=AH zyMs!}m69vr(dXP}!XwS_NR(W!;NR!mXG*S{c=WjYO!%i69*L6c75sbLeWv8PiAU>v zK<{b7Kh5w+lw7ai-#Q;CC09zWghwB9p9zmN!y{30y@G!qbDt@>ZsO6;+-Jf+&G1N+ zT(98Y&)jE9uA6u?VV2(0gnydhktn%d!M_Q!O39UyE8)>W?la+^W_Tn@u2=ByAorP) z>n0vu=ROl2X@*ClN6aHz2N226<1^-6QE+tn=u7pRsxzB`u zn&FWsxn9A)-P~tNuA6xDRri_jNHaVVCD$wX_f_|qlItcO{oQ>g{L>7NM9K9E{{7v3 zrsTSbN1M;l_cP(2W_Tn@u2=AH^EsvDO39V*=ydm)@JKT}5+&Cw_;EBNlOUF$$h5ex`{_) zKcx3G;h$!BBucJV@NevgO39UyE8)?e?la+^W_Tn@u2=AHPxqOU>n0vu>OK=5X@*Cl z2G{YlNa=n6o7rM`sTsQIP8TXm+Pcu9cCD$wX_l)~Y$#oNt zHa$|`&xC)P;gKl0UctXjk1QouO0I-Qr@GICN1EZ0D7jw2zf;|3O0JuD^nm+J_@@~j ziIVFT{CmKCrsTSbN2?yC_cY<3W_Tn@u2=AH)uT$um69vr(TCh;!XwS_NR(W!;NOSb zXG*S{cyx>VO!%i69*L6c75ux!eWv8PiARect@kwHpJsR@O0HM%Z}Fo`$(52T;n5WL zneb0DJQ5|>EBH6XeWv8PiAR^a&xA*s;gKl0UctZ1-DgU!n|L(h7`>+n|1`rRQF6V4 zen0vO z;XV`oX@*ClEBLqWv8Cin$(8Wvc=ws`NHaVV zCD$wXcf9*d$#oNt?scCD|1`rRQF6V4fA_l2lw3FQXvO37o+kX$439*~^$Py2cw8yD zQgS6cn&mzd9%+V0qU3r7|7N+*lw3FQ=mz(h@J};55+&Cw_;-W*Ov!ZEBJS@ z`%KAo6OW#Ep9%jo!y{30y@G$wyU&zdH}PnTkLmlF@J};55+&Cw__xK!O39UyE8)=@ z?la+$W_Tn@u2=By4ELFm>n0vO;yx4pX@*Clc{InP57r79*L6c z75rQM_)>DE>_K|1`rRQF6V4f79G&O0JuDbfx=Dc%&H~ ziIVFT{JYY9rsTSbM+=;&_cY<3W_Tn@u2=AHffGx~m69vr(Ioeo@J};55+&Cw_&3RY zrsTSbM;Exygh!g;ktn%d!M_XKXG*S{c=T8Ineb0DJQ5|>EBN^@U+-Nd6`xzB`un&FWsxn9A) zU%AheTsQG(m6PfBm~Xxz3U+VbwOD(AP9!qh?qoO0HM1ahp$+k}D-w!m6{~XTmDYuu7C% zuVCZZ?lUFVO{{v%eI{(w468)R^$IpV<~~z$-NdT3Ptkjtuu(It5+&Cw*tqs7rQ}M< zm9XkK_nELtGprIN*DKg~ocm14brY-ZcAp6wHNz@Va=n6$ce~G&TsQG({3rFECOpy% zk3`A!3jU4%WGT5)awR;P;XV@{X@*Cl&X6CD%n0w( z=`_8k3I8<1BT;g_f`4y1t(06Txe^{7?miP9X@*Clo}$#oNtUiTTjrwRWw!y{30y@G$Q`%EdhQgS6cn(aOl9%+V0 zqU3r7|7N?-lw3FQ=tlRM@J};55+&Cw_;;iGOv!ZEBN=K`%KAo6OZ0;mcE|}|1`rRQF6V4fA2V}lw2vf5*~fVeI`88439*~^$Pxd z#(k#bx`{`>bDs(SG{YlNa=n6ozjL1{xo+ao8fWW0P57r79*L6c75rP{>{4>21D0q!}KGlIs=xJIZ~gn0v;^f`S$6aHz2N226<1^+hs zTq(IyawR-E#eF6`(hQG8$@L2Uo#H-Ia^1wE``u^4Kh5w+lw7ai-~H}0CD%1?uasOVxe^|o<31A}X@*Cl2(S4ysgN7uQ}gh!g;ktn%d!N2R=XG*S{c(lj`dQTJnX@*Cln0v;_Cu9RE}kM?z+3I8<1BT;g_f`9wE&y-v@@#qTonea$6JQ5|>EBJSX`%KAo z6OZQovfk5#f12TuD7jw2zj?o0O0JY#36FMip9%jo!y{30y@G!`xzChbH}U9v_nGiW zGdvO{*DLsUzWYqcbrX;N>^>9zX@*Clra;4--cr?d-COpy%k3`A!3jWP;pDDR+;?Yg+GvS|RcqB@$ zSMcv9_nDIGCLWFbs@~Itf12TuD7jw2zp-B}C09zWghzY0&xC)P;gKl0UctXT+-FLz zn|Snf_nGiWGdvO{*DLtn0xk!+j?F(+rP9$@L2U{lk5xyxBhBzglw7ai-EBJS(`%KAo6OWd=MDJlOU_uKP^MbrX*UzpnQ*;h$!BBucJV@Ne+zrQ}M< zmGJ1@?la+^W_Tn@u2=By-R?6b*G)XS(0wL6(hQG8$@L2UUFbeja^1wEXWVDPKh5w+ zlw7ai-!twrCD%DwKNJ3GhDW00dIkSBxwMpADY+6JebRjn0v8ewp6Wgnydhktn%d z!N0{XDn0w3%Y7z1(hQG8$@L2Uean5OEBH6?%~Eou7NM9K9E{ypzLQ*zzJqb;t`_cP(2W_Tn@ zu2=AHiz`aWm69vr(Wl*K!XwS_NR(W!;NPd+XG*S{c=TKMneb0DJQ5|>EBNn0xE<~|et zX@*Cl>_qxo+aomF_d)k!E-#O0HM%?@ITXlItcO&3~2N(}aJT;gKl0UctZluPP;1O0I-Q zJG;+>f12TuD7jw2zn$G@O0JuD^ab~s@JKT}5+&Cw`1b|(nUd=!9{t6ACj8S3k3`A! z3jY1YeWv8PiANh;t?y^TKh5w+lw7ai-v(Egk}D-w!lRSiXTl@R@JN(gui)QF?lUFV zO+5OQ`%L(!86Js}>lOU_mHSM|brX+P`L5p6gnydhktn%d!M|0$TS~5!TnUd3ai0l~ zG{YlNa=n6ohq%v_TsQIP2ktZBpJsR@O0HM%?+5NPCD% zUQ^K|1`rRQF6V4e|x&mlw3FQ=u-EY@JKT}5+&Cw_;;!MOv!ZupJsR@O0HM%Z<}jN$(52T;nCUdGvSeDcqB@$ zSMcv__nDIGCLaC4eJ1?V439*~^$Py|!F{IWx`{_?eNXRc!avRMNR(W!;NM!`DEBH6=x>9nbpzmkGKh5w+lw7ai z-=;T|k}D-w!lP5&XTl@R@JN(gui)RQ?lUFVO+0$QeJ1?V439*~^$Pwy;677w-Nd6e z-Kh68;h$!BBucJV@b67GmXa$aSHh#i-Dkoh&G1N+T(98Y;qEgf*G)Y7vHMK;rx_lJ zlIs=x`?32>$#oNtmiWHj(}aJT;gKl0UctX5zF$hNlw1jqrn=9Bf12TuD7jw2zp3ss zCD%lOSPaZ@R|QgS6c+QEG$ z{L>7NM9K9E{_WsCQ*zzJqjTM7!XwS_NR(W!;NQ9KGbPteJo=OSO!%i69*L6c75w{? z`%KAo6OY#Wfxe#!|1`rRQF6V4f9w6Alw2vf5+0r4J`)~khDW00dIkSZaGxo;ZsO5> z?la+^W_Tn@u2=ByKKGfD>n0wpc(dNqgnydhktn%d!M_!6E+tn=u7pRk+-Jfg&G1N+ zT(98YEccm`>n0xE;64-nX@*Cl?w!avRMNR(W!;NMg3GbPteJlg0d`hF(-(+rP9$@L2UZS<2; za;4--c=QSPnea$6JQ5|>EBN;b_nDIGCLaCTeJ1?V439*~^$Py|+I^tHKh5w+ zlw7ai-?n#@k}D-w!lTc+&xA*s;gKl0UctZ5y3dqcH}U8(_nGidGdvO{*DLt zPcu9cCD$wXcaQr_$#oNtmcLW)X~I9v@JN(gui)SEcb1YXC0D|uneH>;pJsR@O0HM% zZ>IZ9$#oNtzUMv@9%+V0qU3r7|GwuwQ*zzJqgUOf_cY<3W_Tn@u2=ByRdK2vht#G}8t&xC)P;gKl0 zUctY=xzChbH}Pn*yY>A{_@@~jiIVFT{M+pAQgWr_N_cdd`%HMG86Js}>lOSv&3&fi zx;c7qkB9Dlda+jx?mK#cf$>=NklRhzs2Ns?lIu-ue8}ylSS3oX zSFmxldrHZbk}F}=huvqwD$TG;lw7Z1d6O0Jt&b*uYK*r*v+iIVFTY`oQdrsTSb zRZIR%?`gtD&9F+8T(4l`l0PdYS4ysgRr|QlgpHbEl_Pl1eWv8PiBlOSv&wZxkx`{_my3d4vn&FWsxn9A)C*5aCuA6wY{(bs> zCj8S3k3`A!3jVEsUn#j#awR-E(S0U7(hQG8$@L2Uo#;MOa^1wEpS#b5f12TuD7jw2 zzn{C$lw3FQXr-U)Jx%zh86Js}>lOT4>F1^7O39V*XpZ|#c%&H~iIVFT{F~!GQ*zzJ zqwl-Vgnydhktn%d!N2dj&y-v@@o3C1^qwaC(+rP9$@L2Ujrm0>xl(c^JbJ(TO!%i6 z9*L6c75sa@`%KAo6OS%&p9zmN!y{30y@G$2xX+YaH}U92_nGidGdvO{*DLtEBNn0uz-mmvG;h$!B zBucJV@Ne+`QgWr_N_g~c_nGidGdvO{*DLtn0w3$$cg~(hQG8$@L2UeaU^M zlOUl_&25GO39V*=oI&v z@JKT}5+&Cw_;-r?Ov!ZEBJSq`%KAo6OVr6J`?_FhDW00dIkS}{Y?0$86Js}>lOUl?x9k0 zrQ}L@bdLK>c%&H~iIVFT{5!{ersTSbM~}PDgnydhktn%d!N14dXG*S{c(l&LdQTJn zX@*ClZsO5|-|9V0_@@~jiIVFT{G0IGQgWr_N_cdT`%L(!86Js}>lOSv$bF{dx`{{E zxzB`0n&FWsxn9A)>)dBbuA6wY$Rm1B6aHz2N226<1^*U#q?BAKxe^}j<~|etX@*Cl z=I)lw3FQ=zRB?@JKT}5+&Cw_;EBH6|@ltZ7EBN=Q`%KAo6OY#Xlit&Wf12Tu zD7jw2zcv3lOT)?mkm;-Nd77+-Jfg z&G1N+T(98YHSRMd*G)WH=+Amj6aHz2N226<1^*WMb1At}awR<4#eF9H(+rP9$@L2U z?czRDa^1wE3*BeJBhBzglw7ai--YfoCD%7NM9K9E{ypP9Q*zzJqfP#z z?`Oh4&G1N+T(98YCVwdQEtC09zWghwB8p9zmN!y{30y@G!qa-S)= zZsO4`?la+^W_Tn@u2=By7WbKw>n0v8{*>O+gnydhktn%d!N0|yDkWD+uC(yr4U0cD z;lB&{yLlg+;&u}@YKB#!y0Sfv?OiIVFT zZ2Y|YOv!ZL1-eWv8PiAVRk&xC)P;gKl0UctY6-DgU!n|QRsvwBYx{%M9s zqU3r7|5kXmlw2vf5+2QRp9zmN!y{30y@G$U+-FLzn|O4C`%L(!86Js}>lOUF!F{IW zx`{`l{-*ad;h$!BBucJV@Nd-LO39UyE8)@m+-Jf+&G1N+T(98Y``l+ruA6vtvHMJT zq!}KGlIs=xyV!lEpe~Qrx_lJlIs=xTmA2)EBJS$`%KAo6OV3pp9%jo!y{30 zy@G$YyU&zdH}Pnh=k=Z@{L>7NM9K9E{w?!-DY;T|B|MtuJ`?_FhDW00dIkTcxzChb zH}U97_nGiWGdvO{*DLsUrTa|DbrX-~e?jkQ!avRMNR(W!;NSc&l#(kYSHh!7?la+^ zW_Tn@u2=AHlKV`_brX*+aGwc}G{YlNa=n6o7r4)qTsQIPukJJ9pJsR@O0HM%@2~DN zCD%R(|27C5CC-8)xi)%#y>z z|IWyb^Db37C=Uv_wU)LN^}`?{|e2X37`@5O5_H$1)J z#jEFC=KA8spL}WFW49hRJRLu3i}@bAyLkQuC(XCdF5`!%=g+)*zS|!DH@$Db{ZJNLr*pFe8C@bvnlo|u2n1y&fwjb5|*0?VFIJa4x{7I=346+7MV z+69(Axp>}jBNjY&)a!<)H#JaG4d51d_maMqH8i?6tH SrsD_ZAJ~7%5sUC^@c#gHe{Q4z diff --git a/rio/tests/pyfrc_test.py b/rio/tests/pyfrc_test.py index 982b0272..90ae6731 100644 --- a/rio/tests/pyfrc_test.py +++ b/rio/tests/pyfrc_test.py @@ -1,13 +1,6 @@ -# -# 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. -# - -""" +''' This test module imports tests that come with pyfrc, and can be used to test basic functionality of just about any robot. -""" +''' from pyfrc.tests import * -from magicbot.magicbot_tests import * From 6f3cd8a09c131c20942a1f5908bebc9b1008e651 Mon Sep 17 00:00:00 2001 From: KenwoodFox Date: Sun, 21 Jan 2024 14:22:37 -0500 Subject: [PATCH 39/61] Rename swerve module --- rio/subsystems/drivesubsystem.py | 10 +++++----- .../{maxswervemodule.py => mikeswervemodule.py} | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) rename rio/subsystems/{maxswervemodule.py => mikeswervemodule.py} (99%) diff --git a/rio/subsystems/drivesubsystem.py b/rio/subsystems/drivesubsystem.py index 732d1ca6..fac7310a 100644 --- a/rio/subsystems/drivesubsystem.py +++ b/rio/subsystems/drivesubsystem.py @@ -15,7 +15,7 @@ from constants import DriveConstants import swerveutils -from .maxswervemodule import MAXSwerveModule +from .mikeswervemodule import MikeSwerveModule class DriveSubsystem(Subsystem): @@ -23,25 +23,25 @@ def __init__(self) -> None: super().__init__() # Create MAXSwerveModules - self.frontLeft = MAXSwerveModule( + self.frontLeft = MikeSwerveModule( DriveConstants.kFrontLeftDrivingCanId, DriveConstants.kFrontLeftTurningCanId, DriveConstants.kFrontLeftChassisAngularOffset, ) - self.frontRight = MAXSwerveModule( + self.frontRight = MikeSwerveModule( DriveConstants.kFrontRightDrivingCanId, DriveConstants.kFrontRightTurningCanId, DriveConstants.kFrontRightChassisAngularOffset, ) - self.rearLeft = MAXSwerveModule( + self.rearLeft = MikeSwerveModule( DriveConstants.kRearLeftDrivingCanId, DriveConstants.kRearLeftTurningCanId, DriveConstants.kBackLeftChassisAngularOffset, ) - self.rearRight = MAXSwerveModule( + self.rearRight = MikeSwerveModule( DriveConstants.kRearRightDrivingCanId, DriveConstants.kRearRightTurningCanId, DriveConstants.kBackRightChassisAngularOffset, diff --git a/rio/subsystems/maxswervemodule.py b/rio/subsystems/mikeswervemodule.py similarity index 99% rename from rio/subsystems/maxswervemodule.py rename to rio/subsystems/mikeswervemodule.py index 3579caf2..e28fe247 100644 --- a/rio/subsystems/maxswervemodule.py +++ b/rio/subsystems/mikeswervemodule.py @@ -5,7 +5,7 @@ from constants import ModuleConstants -class MAXSwerveModule: +class MikeSwerveModule: def __init__( self, drivingCANId: int, turningCANId: int, chassisAngularOffset: float ) -> None: From d15c6ca96b7629428cd3c4cf691a7e68f9bea172 Mon Sep 17 00:00:00 2001 From: KenwoodFox Date: Sun, 21 Jan 2024 14:39:59 -0500 Subject: [PATCH 40/61] Add better input controls --- rio/robotcontainer.py | 11 ++++-- rio/simgui-ds.json | 60 +++++++++--------------------- rio/simgui-window.json | 10 ++--- rio/simgui.json | 19 ++++++++++ rio/subsystems/drivesubsystem.py | 34 +++++++++++++++++ rio/subsystems/mikeswervemodule.py | 9 +++-- 6 files changed, 88 insertions(+), 55 deletions(-) diff --git a/rio/robotcontainer.py b/rio/robotcontainer.py index cd8a1ea4..41f5778a 100644 --- a/rio/robotcontainer.py +++ b/rio/robotcontainer.py @@ -26,7 +26,7 @@ def __init__(self) -> None: self.robotDrive = DriveSubsystem() # The driver's controller - self.driverController = wpilib.XboxController(OIConstants.kDriverControllerPort) + self.driverController = wpilib.Joystick(0) # Configure the button bindings self.configureButtonBindings() @@ -38,13 +38,16 @@ def __init__(self) -> None: commands2.RunCommand( lambda: self.robotDrive.drive( -wpimath.applyDeadband( - self.driverController.getLeftY(), OIConstants.kDriveDeadband + self.driverController.getRawAxis(0), + OIConstants.kDriveDeadband, # TODO: Use constants to set these controls ), -wpimath.applyDeadband( - self.driverController.getLeftX(), OIConstants.kDriveDeadband + self.driverController.getRawAxis(1), + OIConstants.kDriveDeadband, # TODO: Use constants to set these controls ), -wpimath.applyDeadband( - self.driverController.getRightX(), OIConstants.kDriveDeadband + self.driverController.getRawAxis(2), + OIConstants.kDriveDeadband, # TODO: Use constants to set these controls ), True, True, diff --git a/rio/simgui-ds.json b/rio/simgui-ds.json index 73cc713c..6b37128a 100644 --- a/rio/simgui-ds.json +++ b/rio/simgui-ds.json @@ -3,61 +3,26 @@ { "axisConfig": [ { - "decKey": 65, - "incKey": 68 + "decKey": 83, + "incKey": 87 }, { - "decKey": 87, - "incKey": 83 + "decKey": 68, + "incKey": 65 }, { - "decKey": 69, - "decayRate": 0.0, - "incKey": 82, - "keyRate": 0.009999999776482582 + "decKey": 81, + "incKey": 69 } ], "axisCount": 3, - "buttonCount": 4, + "buttonCount": 10, "buttonKeys": [ 90, 88, 67, 86 ], - "povConfig": [ - { - "key0": 328, - "key135": 323, - "key180": 322, - "key225": 321, - "key270": 324, - "key315": 327, - "key45": 329, - "key90": 326 - } - ], - "povCount": 1 - }, - { - "axisConfig": [ - { - "decKey": 74, - "incKey": 76 - }, - { - "decKey": 73, - "incKey": 75 - } - ], - "axisCount": 2, - "buttonCount": 4, - "buttonKeys": [ - 77, - 44, - 46, - 47 - ], "povCount": 0 }, { @@ -87,6 +52,17 @@ "axisCount": 0, "buttonCount": 0, "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0", + "name": "Driver Joystick" } ] } diff --git a/rio/simgui-window.json b/rio/simgui-window.json index 667bac5b..22ae3678 100644 --- a/rio/simgui-window.json +++ b/rio/simgui-window.json @@ -22,17 +22,17 @@ }, "###Joysticks": { "Collapsed": "0", - "Pos": "250,465", - "Size": "796,73" + "Pos": "291,574", + "Size": "796,138" }, "###NetworkTables": { "Collapsed": "0", - "Pos": "250,277", - "Size": "750,185" + "Pos": "269,170", + "Size": "750,395" }, "###NetworkTables Info": { "Collapsed": "0", - "Pos": "250,130", + "Pos": "269,27", "Size": "750,145" }, "###Other Devices": { diff --git a/rio/simgui.json b/rio/simgui.json index 5f9d2754..4f4a6cec 100644 --- a/rio/simgui.json +++ b/rio/simgui.json @@ -1,9 +1,28 @@ { + "HALProvider": { + "Other Devices": { + "ADIS16448[4]": { + "header": { + "open": true + } + } + } + }, "NTProvider": { "types": { "/FMSInfo": "FMSInfo" } }, + "NetworkTables": { + "transitory": { + "SmartDashboard": { + "Swerve": { + "open": true + }, + "open": true + } + } + }, "NetworkTables Info": { "visible": true } diff --git a/rio/subsystems/drivesubsystem.py b/rio/subsystems/drivesubsystem.py index fac7310a..23380967 100644 --- a/rio/subsystems/drivesubsystem.py +++ b/rio/subsystems/drivesubsystem.py @@ -13,6 +13,8 @@ SwerveDrive4Odometry, ) +from ntcore import NetworkTableInstance + from constants import DriveConstants import swerveutils from .mikeswervemodule import MikeSwerveModule @@ -47,6 +49,10 @@ def __init__(self) -> None: DriveConstants.kBackRightChassisAngularOffset, ) + # Configure networktables + self.nt = NetworkTableInstance.getDefault() + self.sd = self.nt.getTable("SmartDashboard") + # The gyro sensor self.gyro = wpilib.ADIS16448_IMU() @@ -83,6 +89,34 @@ def periodic(self) -> None: ), ) + # desired + self.sd.putNumber( + "Swerve/FL desired", self.frontLeft.desiredState.angle.degrees() + ) + self.sd.putNumber( + "Swerve/FR desired", self.frontRight.desiredState.angle.degrees() + ) + self.sd.putNumber( + "Swerve/RL desired", self.rearLeft.desiredState.angle.degrees() + ) + self.sd.putNumber( + "Swerve/RR desired", self.rearRight.desiredState.angle.degrees() + ) + + # actual + self.sd.putNumber( + "Swerve/FL", self.frontLeft.getState().angle.degrees() * (360 / 60) + ) + self.sd.putNumber( + "Swerve/FR", self.frontRight.getState().angle.degrees() * (360 / 60) + ) + self.sd.putNumber( + "Swerve/RL", self.rearLeft.getState().angle.degrees() * (360 / 60) + ) + self.sd.putNumber( + "Swerve/RR", self.rearRight.getState().angle.degrees() * (360 / 60) + ) + def getPose(self) -> Pose2d: """Returns the currently-estimated pose of the robot. diff --git a/rio/subsystems/mikeswervemodule.py b/rio/subsystems/mikeswervemodule.py index e28fe247..d95b48a3 100644 --- a/rio/subsystems/mikeswervemodule.py +++ b/rio/subsystems/mikeswervemodule.py @@ -9,10 +9,11 @@ class MikeSwerveModule: def __init__( self, drivingCANId: int, turningCANId: int, chassisAngularOffset: float ) -> None: - """Constructs a MAXSwerveModule and configures the driving and turning motor, - encoder, and PID controller. This configuration is specific to the REV - MAXSwerve Module built with NEOs, SPARKS MAX, and a Through Bore - Encoder. + """Constructs a Mike Swerve Module, a custom module designed in house. + + This code is based on Rev's swerve module the Max Swerve module, and + the original code for that can be found here + https://github.com/robotpy/robotpy-rev/blob/main/examples/maxswerve/subsystems/maxswervemodule.py """ self.chassisAngularOffset = 0 From c4ea720bac96d69d088e9691a1bfb9a73702e454 Mon Sep 17 00:00:00 2001 From: KenwoodFox Date: Sun, 21 Jan 2024 15:18:09 -0500 Subject: [PATCH 41/61] Stub, need to address #8 --- rio/Makefile | 11 +++++------ rio/Pipfile | 2 +- rio/Pipfile.lock | 14 +++++++------- rio/pyproject.toml | 2 +- rio/subsystems/drivesubsystem.py | 4 +++- 5 files changed, 17 insertions(+), 16 deletions(-) diff --git a/rio/Makefile b/rio/Makefile index 91240178..71faa9a4 100644 --- a/rio/Makefile +++ b/rio/Makefile @@ -13,22 +13,21 @@ help: ## Prints this help message sim: ## Run the simulator python -m robotpy sim - -deploy: ## Deploy to the robot (with console) +deploy: ## Deploy to the robot (with netconsole) python -m robotpy deploy --nc - -push: ## Deploy to the robot (push only, no console) +push: ## Deploy to the robot (push only, no netconsole) python -m robotpy deploy - download: ## Download robot requirements locally python -m robotpy installer download -r robot_requirements.txt - + python -m robotpy installer download-python install: ## Install requirements, run download first! python -m robotpy installer install -r robot_requirements.txt +sync: ## Sync robotpy requirements + python -m robotpy sync info: ## Shortcut to get information about the code already on the bot python -m robotpy deploy-info diff --git a/rio/Pipfile b/rio/Pipfile index a30896fe..4eeddd5e 100644 --- a/rio/Pipfile +++ b/rio/Pipfile @@ -5,7 +5,7 @@ name = "pypi" [packages] wpilib = {extras = ["all"]} -robotpy = "2024.1.1.1" +robotpy = "2024.1.1.3" robotpy-rev = "2024.2.0" robotpy-ctre = "*" robotpy-navx = "*" diff --git a/rio/Pipfile.lock b/rio/Pipfile.lock index c8a6223d..682f153d 100644 --- a/rio/Pipfile.lock +++ b/rio/Pipfile.lock @@ -1,7 +1,7 @@ { "_meta": { "hash": { - "sha256": "0bfc5b6776b92c7072fc224883ce8d79eeb0ad0b6170f8a5a663f3241f093b62" + "sha256": "c4565a09bd5f114ab70b68febd001751ecf444234f7b2e3c41d9027b065bea41" }, "pipfile-spec": 6, "requires": { @@ -265,12 +265,12 @@ }, "robotpy": { "hashes": [ - "sha256:b2589cea10a947ea26eb099d9345fe0e5bd400f678fffc2b68d1c7c4f63509e3", - "sha256:cacd0912b6622198a9e5864a1accfd2c2264ddb09da8223d2c93054d65925486" + "sha256:a9f9f7de9aac362ec7f027194d11644ad555f4da1951c9ebdf9c6ba8ec15cb36", + "sha256:cc9a1dfa216a8208efa848683ce0df45c98f7905c3450f8d61ea32fed4adab9b" ], "index": "pypi", "markers": "python_version < '3.13' and python_version >= '3.8'", - "version": "==2024.1.1.1" + "version": "==2024.1.1.3" }, "robotpy-cli": { "hashes": [ @@ -353,11 +353,11 @@ }, "robotpy-installer": { "hashes": [ - "sha256:2e7692bd6c0d305531bcede806e37e6df15fd446e856dee2670af2283337ed35", - "sha256:dde4f529c84b2fcc7b97193419d72c9184531578cdbadf310cdab72f49146110" + "sha256:0ab8a074fa4c342e300f3f942fd738b8a6a45df888f5e369868d306d8fe9a541", + "sha256:917d42d1e2a098caa61d00e8f1801378c354edf605b4d0c95373272096f91195" ], "markers": "platform_machine != 'roborio' and platform_machine != 'armv7l' and platform_machine != 'aarch64'", - "version": "==2024.1.2" + "version": "==2024.1.3" }, "robotpy-navx": { "hashes": [ diff --git a/rio/pyproject.toml b/rio/pyproject.toml index 1216941e..ad57d475 100644 --- a/rio/pyproject.toml +++ b/rio/pyproject.toml @@ -6,7 +6,7 @@ [tool.robotpy] # Version of robotpy this project depends on -robotpy_version = "2024.1.1.1" +robotpy_version = "2024.1.1.3" # Which extra RobotPy components should be installed # -> equivalent to `pip install robotpy[extra1, ...] diff --git a/rio/subsystems/drivesubsystem.py b/rio/subsystems/drivesubsystem.py index 23380967..0a03b33e 100644 --- a/rio/subsystems/drivesubsystem.py +++ b/rio/subsystems/drivesubsystem.py @@ -13,6 +13,8 @@ SwerveDrive4Odometry, ) +from navx import AHRS + from ntcore import NetworkTableInstance from constants import DriveConstants @@ -54,7 +56,7 @@ def __init__(self) -> None: self.sd = self.nt.getTable("SmartDashboard") # The gyro sensor - self.gyro = wpilib.ADIS16448_IMU() + self.gyro = AHRS.create_spi() # Slew rate filter variables for controlling lateral acceleration self.currentRotation = 0.0 From 200d9a3a210c4a9e69dcd243dde0673a90a85528 Mon Sep 17 00:00:00 2001 From: KenwoodFox Date: Sun, 21 Jan 2024 17:11:58 -0500 Subject: [PATCH 42/61] This is just a patch/halfway fix for #8 A real fix will come from upstream (eventually, no timeframe from robotpy) --- rio/subsystems/drivesubsystem.py | 12 ++++++++++-- rio/utils/dummygyro.py | 7 +++++++ rio/{ => utils}/swerveutils.py | 0 3 files changed, 17 insertions(+), 2 deletions(-) create mode 100644 rio/utils/dummygyro.py rename rio/{ => utils}/swerveutils.py (100%) diff --git a/rio/subsystems/drivesubsystem.py b/rio/subsystems/drivesubsystem.py index 0a03b33e..e0296044 100644 --- a/rio/subsystems/drivesubsystem.py +++ b/rio/subsystems/drivesubsystem.py @@ -18,7 +18,11 @@ from ntcore import NetworkTableInstance from constants import DriveConstants -import swerveutils + +import utils.swerveutils as swerveutils + +from utils.dummygyro import DummyGyro + from .mikeswervemodule import MikeSwerveModule @@ -56,7 +60,11 @@ def __init__(self) -> None: self.sd = self.nt.getTable("SmartDashboard") # The gyro sensor - self.gyro = AHRS.create_spi() + if wpilib.RobotBase.isReal(): + self.gyro = AHRS.create_spi() + else: + # Bug with navx init! For sim/unit testing just use the ADIS + self.gyro = DummyGyro() # Slew rate filter variables for controlling lateral acceleration self.currentRotation = 0.0 diff --git a/rio/utils/dummygyro.py b/rio/utils/dummygyro.py new file mode 100644 index 00000000..7329053c --- /dev/null +++ b/rio/utils/dummygyro.py @@ -0,0 +1,7 @@ +# Used to bypass bugs in the gyro init during sim +class DummyGyro: + def __init__(self): + pass + + def getAngle(self) -> float: + return 0.0 diff --git a/rio/swerveutils.py b/rio/utils/swerveutils.py similarity index 100% rename from rio/swerveutils.py rename to rio/utils/swerveutils.py From 332f65b72e1c16baf9b30db70eb42ca6b923e7a0 Mon Sep 17 00:00:00 2001 From: KenwoodFox Date: Sun, 21 Jan 2024 17:12:55 -0500 Subject: [PATCH 43/61] Do unit tests again --- .github/workflows/robot-workflow.yml | 70 ++++++++++++++-------------- 1 file changed, 35 insertions(+), 35 deletions(-) diff --git a/.github/workflows/robot-workflow.yml b/.github/workflows/robot-workflow.yml index 019ba762..51dd6bff 100644 --- a/.github/workflows/robot-workflow.yml +++ b/.github/workflows/robot-workflow.yml @@ -74,38 +74,38 @@ jobs: with: args: "<@614313406345904148> Simulator failed in {{ EVENT_PAYLOAD.pull_request.html_url }} !" - # unit_test: - # name: pyfrc Unit tests - # runs-on: ubuntu-latest - # steps: - # - uses: actions/checkout@v2 - # - uses: actions/setup-python@v2 - # with: - # python-version: "3.11" - - # - name: Install pipenv - # run: | - # python -m pip install --upgrade pipenv wheel - - # - id: cache-pipenv - # uses: actions/cache@v3 - # with: - # path: ~/.local/share/virtualenvs - # key: ${{ runner.os }}-pipenv-${{ hashFiles('rio/Pipfile.lock') }} - - # - name: Install dependencies - # if: steps.cache-pipenv.outputs.cache-hit != 'true' - # run: | - # cd rio && pipenv install --deploy --dev - - # - name: Run unit tests - # run: | - # cd rio && pipenv run python -m robotpy test -- -vs || code=$?; if [[ $code -ne 124 && $code -ne 0 ]]; then exit $code; fi - - # - name: Discord notification - # if: ${{ failure() }} - # env: - # DISCORD_WEBHOOK: ${{ secrets.DISCORD_WEBHOOK }} - # uses: Ilshidur/action-discord@master - # with: - # args: "<@614313406345904148> Unit tests failed in {{ EVENT_PAYLOAD.pull_request.html_url }} !" + unit_test: + name: pyfrc Unit tests + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + - uses: actions/setup-python@v2 + with: + python-version: "3.11" + + - name: Install pipenv + run: | + python -m pip install --upgrade pipenv wheel + + - id: cache-pipenv + uses: actions/cache@v3 + with: + path: ~/.local/share/virtualenvs + key: ${{ runner.os }}-pipenv-${{ hashFiles('rio/Pipfile.lock') }} + + - name: Install dependencies + if: steps.cache-pipenv.outputs.cache-hit != 'true' + run: | + cd rio && pipenv install --deploy --dev + + - name: Run unit tests + run: | + cd rio && pipenv run python -m robotpy test -- -vs || code=$?; if [[ $code -ne 124 && $code -ne 0 ]]; then exit $code; fi + + - name: Discord notification + if: ${{ failure() }} + env: + DISCORD_WEBHOOK: ${{ secrets.DISCORD_WEBHOOK }} + uses: Ilshidur/action-discord@master + with: + args: "<@614313406345904148> Unit tests failed in {{ EVENT_PAYLOAD.pull_request.html_url }} !" From 872126053946f827c080870e426ade1e3ce29d5b Mon Sep 17 00:00:00 2001 From: KenwoodFox Date: Sun, 21 Jan 2024 17:13:20 -0500 Subject: [PATCH 44/61] Fix black --- rio/tests/pyfrc_test.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rio/tests/pyfrc_test.py b/rio/tests/pyfrc_test.py index 90ae6731..bbe80a0c 100644 --- a/rio/tests/pyfrc_test.py +++ b/rio/tests/pyfrc_test.py @@ -1,6 +1,6 @@ -''' - This test module imports tests that come with pyfrc, and can be used - to test basic functionality of just about any robot. -''' +""" +This test module imports tests that come with pyfrc, and can be used +to test basic functionality of just about any robot. +""" from pyfrc.tests import * From 9ba7caca995f9ff3899560596453de891186ef9b Mon Sep 17 00:00:00 2001 From: Jack Jewett Date: Sun, 21 Jan 2024 17:52:23 -0500 Subject: [PATCH 45/61] fixed some stuff in the constants folder, apparently the rev suggestions for the angular offsets were what was making the wheels wonky. So I set them all to 0 and now it works. Also updated the distance between modules and reassigned Spark IDs because they were wrong and dumb --- Pipfile | 11 +++++++++++ Pipfile.lock | 20 ++++++++++++++++++++ rio/constants.py | 28 ++++++++++++++-------------- 3 files changed, 45 insertions(+), 14 deletions(-) create mode 100644 Pipfile create mode 100644 Pipfile.lock diff --git a/Pipfile b/Pipfile new file mode 100644 index 00000000..0757494b --- /dev/null +++ b/Pipfile @@ -0,0 +1,11 @@ +[[source]] +url = "https://pypi.org/simple" +verify_ssl = true +name = "pypi" + +[packages] + +[dev-packages] + +[requires] +python_version = "3.11" diff --git a/Pipfile.lock b/Pipfile.lock new file mode 100644 index 00000000..54a70783 --- /dev/null +++ b/Pipfile.lock @@ -0,0 +1,20 @@ +{ + "_meta": { + "hash": { + "sha256": "ed6d5d614626ae28e274e453164affb26694755170ccab3aa5866f093d51d3e4" + }, + "pipfile-spec": 6, + "requires": { + "python_version": "3.11" + }, + "sources": [ + { + "name": "pypi", + "url": "https://pypi.org/simple", + "verify_ssl": true + } + ] + }, + "default": {}, + "develop": {} +} diff --git a/rio/constants.py b/rio/constants.py index a7be1508..dc2dbfcf 100644 --- a/rio/constants.py +++ b/rio/constants.py @@ -33,9 +33,9 @@ class DriveConstants: kRotationalSlewRate = 2.0 # percent per second (1 = 100%) # Chassis configuration - kTrackWidth = units.inchesToMeters(26.5) + kTrackWidth = units.inchesToMeters(20.3937) # Distance between centers of right and left wheels on robot - kWheelBase = units.inchesToMeters(26.5) + kWheelBase = units.inchesToMeters(24) # Distance between front and back wheels on robot kModulePositions = [ @@ -47,21 +47,21 @@ class DriveConstants: kDriveKinematics = SwerveDrive4Kinematics(*kModulePositions) # Angular offsets of the modules relative to the chassis in radians - kFrontLeftChassisAngularOffset = -math.pi / 2 + kFrontLeftChassisAngularOffset = 0 kFrontRightChassisAngularOffset = 0 - kBackLeftChassisAngularOffset = math.pi - kBackRightChassisAngularOffset = math.pi / 2 + kBackLeftChassisAngularOffset = 0 + kBackRightChassisAngularOffset = 0 # SPARK MAX CAN IDs - kFrontLeftDrivingCanId = 1 - kRearLeftDrivingCanId = 3 - kFrontRightDrivingCanId = 5 - kRearRightDrivingCanId = 7 - - kFrontLeftTurningCanId = 2 - kRearLeftTurningCanId = 4 - kFrontRightTurningCanId = 6 - kRearRightTurningCanId = 8 + kFrontLeftDrivingCanId = 5 + kRearLeftDrivingCanId = 7 + kFrontRightDrivingCanId = 1 + kRearRightDrivingCanId = 3 + + kFrontLeftTurningCanId = 6 + kRearLeftTurningCanId = 8 + kFrontRightTurningCanId = 2 + kRearRightTurningCanId = 4 kGyroReversed = False From a88ec714cb85cd7ff78f1813f5ed9b30b07b4a70 Mon Sep 17 00:00:00 2001 From: dublUayaychtee Date: Mon, 22 Jan 2024 13:11:44 -0500 Subject: [PATCH 46/61] fix degrees back --- rio/subsystems/drivesubsystem.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rio/subsystems/drivesubsystem.py b/rio/subsystems/drivesubsystem.py index e0296044..7c416f0c 100644 --- a/rio/subsystems/drivesubsystem.py +++ b/rio/subsystems/drivesubsystem.py @@ -115,16 +115,16 @@ def periodic(self) -> None: # actual self.sd.putNumber( - "Swerve/FL", self.frontLeft.getState().angle.degrees() * (360 / 60) + "Swerve/FL", self.frontLeft.getState().angle.degrees() ) self.sd.putNumber( - "Swerve/FR", self.frontRight.getState().angle.degrees() * (360 / 60) + "Swerve/FR", self.frontRight.getState().angle.degrees() ) self.sd.putNumber( - "Swerve/RL", self.rearLeft.getState().angle.degrees() * (360 / 60) + "Swerve/RL", self.rearLeft.getState().angle.degrees() ) self.sd.putNumber( - "Swerve/RR", self.rearRight.getState().angle.degrees() * (360 / 60) + "Swerve/RR", self.rearRight.getState().angle.degrees() ) def getPose(self) -> Pose2d: From b0df3e025654d7a2f9eeb7dc9c0deb114be99bdb Mon Sep 17 00:00:00 2001 From: dublUayaychtee <62506798+dublUayaychtee@users.noreply.github.com> Date: Mon, 22 Jan 2024 18:16:12 +0000 Subject: [PATCH 47/61] formatting !!! Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> --- rio/subsystems/drivesubsystem.py | 16 ++++------------ 1 file changed, 4 insertions(+), 12 deletions(-) diff --git a/rio/subsystems/drivesubsystem.py b/rio/subsystems/drivesubsystem.py index 7c416f0c..9d7f7940 100644 --- a/rio/subsystems/drivesubsystem.py +++ b/rio/subsystems/drivesubsystem.py @@ -114,18 +114,10 @@ def periodic(self) -> None: ) # actual - self.sd.putNumber( - "Swerve/FL", self.frontLeft.getState().angle.degrees() - ) - self.sd.putNumber( - "Swerve/FR", self.frontRight.getState().angle.degrees() - ) - self.sd.putNumber( - "Swerve/RL", self.rearLeft.getState().angle.degrees() - ) - self.sd.putNumber( - "Swerve/RR", self.rearRight.getState().angle.degrees() - ) + self.sd.putNumber("Swerve/FL", self.frontLeft.getState().angle.degrees()) + self.sd.putNumber("Swerve/FR", self.frontRight.getState().angle.degrees()) + self.sd.putNumber("Swerve/RL", self.rearLeft.getState().angle.degrees()) + self.sd.putNumber("Swerve/RR", self.rearRight.getState().angle.degrees()) def getPose(self) -> Pose2d: """Returns the currently-estimated pose of the robot. From 4edb5e0e1205baa4b67ae664acee03271c1812d1 Mon Sep 17 00:00:00 2001 From: dublUayaychtee Date: Mon, 22 Jan 2024 13:22:02 -0500 Subject: [PATCH 48/61] smh my head jack >:( no pipfile 4u --- Pipfile | 11 ----------- Pipfile.lock | 20 -------------------- 2 files changed, 31 deletions(-) delete mode 100644 Pipfile delete mode 100644 Pipfile.lock diff --git a/Pipfile b/Pipfile deleted file mode 100644 index 0757494b..00000000 --- a/Pipfile +++ /dev/null @@ -1,11 +0,0 @@ -[[source]] -url = "https://pypi.org/simple" -verify_ssl = true -name = "pypi" - -[packages] - -[dev-packages] - -[requires] -python_version = "3.11" diff --git a/Pipfile.lock b/Pipfile.lock deleted file mode 100644 index 54a70783..00000000 --- a/Pipfile.lock +++ /dev/null @@ -1,20 +0,0 @@ -{ - "_meta": { - "hash": { - "sha256": "ed6d5d614626ae28e274e453164affb26694755170ccab3aa5866f093d51d3e4" - }, - "pipfile-spec": 6, - "requires": { - "python_version": "3.11" - }, - "sources": [ - { - "name": "pypi", - "url": "https://pypi.org/simple", - "verify_ssl": true - } - ] - }, - "default": {}, - "develop": {} -} From abdf99d7adb7b5a6e39bae0913a68b46eef960f0 Mon Sep 17 00:00:00 2001 From: kredcool Date: Mon, 22 Jan 2024 13:22:07 -0500 Subject: [PATCH 49/61] updating CANSparkLowLevels --- rio/subsystems/mikeswervemodule.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rio/subsystems/mikeswervemodule.py b/rio/subsystems/mikeswervemodule.py index d95b48a3..4c3b3055 100644 --- a/rio/subsystems/mikeswervemodule.py +++ b/rio/subsystems/mikeswervemodule.py @@ -1,4 +1,4 @@ -from rev import CANSparkMax, SparkMaxAbsoluteEncoder +from rev import CANSparkMax, SparkMaxAbsoluteEncoder, CANSparkLowLevel from wpimath.geometry import Rotation2d from wpimath.kinematics import SwerveModuleState, SwerveModulePosition @@ -20,10 +20,10 @@ def __init__( self.desiredState = SwerveModuleState(0.0, Rotation2d()) self.drivingSparkMax = CANSparkMax( - drivingCANId, CANSparkMax.MotorType.kBrushless + drivingCANId, CANSparkLowLevel.MotorType.kBrushless ) self.turningSparkMax = CANSparkMax( - turningCANId, CANSparkMax.MotorType.kBrushless + turningCANId, CANSparkLowLevel.MotorType.kBrushless ) # Factory reset, so we get the SPARKS MAX to a known state before configuring From 5f9bd6dbe12003fd90b2a126366d74c8a18e189b Mon Sep 17 00:00:00 2001 From: kredcool Date: Mon, 22 Jan 2024 13:35:29 -0500 Subject: [PATCH 50/61] updating robotpy --- rio/Pipfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rio/Pipfile b/rio/Pipfile index 4eeddd5e..5e5d9bd4 100644 --- a/rio/Pipfile +++ b/rio/Pipfile @@ -5,7 +5,7 @@ name = "pypi" [packages] wpilib = {extras = ["all"]} -robotpy = "2024.1.1.3" +robotpy = "2024.2.1.0" robotpy-rev = "2024.2.0" robotpy-ctre = "*" robotpy-navx = "*" From e032d10b82f52540b13284288cd79c6cb8aa034c Mon Sep 17 00:00:00 2001 From: kredcool Date: Mon, 22 Jan 2024 15:27:26 -0500 Subject: [PATCH 51/61] updating values for our swerve --- rio/constants.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/rio/constants.py b/rio/constants.py index dc2dbfcf..b517bd9a 100644 --- a/rio/constants.py +++ b/rio/constants.py @@ -33,9 +33,9 @@ class DriveConstants: kRotationalSlewRate = 2.0 # percent per second (1 = 100%) # Chassis configuration - kTrackWidth = units.inchesToMeters(20.3937) + kTrackWidth = units.inchesToMeters(1) # Distance between centers of right and left wheels on robot - kWheelBase = units.inchesToMeters(24) + kWheelBase = units.inchesToMeters(20.25) # Distance between front and back wheels on robot kModulePositions = [ @@ -70,7 +70,7 @@ class ModuleConstants: # The MAXSwerve module can be configured with one of three pinion gears: 12T, 13T, or 14T. # This changes the drive speed of the module (a pinion gear with more teeth will result in a # robot that drives faster). - kDrivingMotorPinionTeeth = 14 + kDrivingMotorPinionTeeth = 17 # Invert the turning encoder, since the output shaft rotates in the opposite direction of # the steering motor in the MAXSwerve Module. @@ -78,10 +78,10 @@ class ModuleConstants: # Calculations required for driving motor conversion factors and feed forward kDrivingMotorFreeSpeedRps = NeoMotorConstants.kFreeSpeedRpm / 60 - kWheelDiameterMeters = 0.0762 + kWheelDiameterMeters = 0.09525 kWheelCircumferenceMeters = kWheelDiameterMeters * math.pi # 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15 teeth on the bevel pinion - kDrivingMotorReduction = (45.0 * 22) / (kDrivingMotorPinionTeeth * 15) + kDrivingMotorReduction = (60 * 34) / (kDrivingMotorPinionTeeth * 15) kDriveWheelFreeSpeedRps = ( kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters ) / kDrivingMotorReduction From f68e250641447e1b1dd9a5247ffeb634547b0a47 Mon Sep 17 00:00:00 2001 From: Jack Jewett Date: Mon, 22 Jan 2024 15:51:57 -0500 Subject: [PATCH 52/61] if I'm understanding angular offsets correctly then this should work. --- rio/constants.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/rio/constants.py b/rio/constants.py index b517bd9a..1a114840 100644 --- a/rio/constants.py +++ b/rio/constants.py @@ -33,7 +33,7 @@ class DriveConstants: kRotationalSlewRate = 2.0 # percent per second (1 = 100%) # Chassis configuration - kTrackWidth = units.inchesToMeters(1) + kTrackWidth = units.inchesToMeters(20.3937) # Distance between centers of right and left wheels on robot kWheelBase = units.inchesToMeters(20.25) @@ -47,10 +47,10 @@ class DriveConstants: kDriveKinematics = SwerveDrive4Kinematics(*kModulePositions) # Angular offsets of the modules relative to the chassis in radians - kFrontLeftChassisAngularOffset = 0 - kFrontRightChassisAngularOffset = 0 - kBackLeftChassisAngularOffset = 0 - kBackRightChassisAngularOffset = 0 + kFrontLeftChassisAngularOffset = math.pi * 0.25 + kFrontRightChassisAngularOffset = math.pi * 1.75 + kBackLeftChassisAngularOffset = math.pi * 0.75 + kBackRightChassisAngularOffset = math.pi * 1.25 # SPARK MAX CAN IDs kFrontLeftDrivingCanId = 5 From cb7bb62a4bd501dac2c6061ed03da894c0dc0a7e Mon Sep 17 00:00:00 2001 From: kredcool Date: Tue, 23 Jan 2024 14:14:12 -0500 Subject: [PATCH 53/61] This for some reason zeros all the modules, its dumb --- rio/constants.py | 8 ++++---- rio/pyproject.toml | 30 ------------------------------ rio/robot.py | 3 +++ 3 files changed, 7 insertions(+), 34 deletions(-) delete mode 100644 rio/pyproject.toml diff --git a/rio/constants.py b/rio/constants.py index 1a114840..a1219a9f 100644 --- a/rio/constants.py +++ b/rio/constants.py @@ -47,10 +47,10 @@ class DriveConstants: kDriveKinematics = SwerveDrive4Kinematics(*kModulePositions) # Angular offsets of the modules relative to the chassis in radians - kFrontLeftChassisAngularOffset = math.pi * 0.25 - kFrontRightChassisAngularOffset = math.pi * 1.75 - kBackLeftChassisAngularOffset = math.pi * 0.75 - kBackRightChassisAngularOffset = math.pi * 1.25 + kFrontLeftChassisAngularOffset = 0 + kFrontRightChassisAngularOffset = 0 + kBackLeftChassisAngularOffset = 0 + kBackRightChassisAngularOffset = 0 # SPARK MAX CAN IDs kFrontLeftDrivingCanId = 5 diff --git a/rio/pyproject.toml b/rio/pyproject.toml deleted file mode 100644 index ad57d475..00000000 --- a/rio/pyproject.toml +++ /dev/null @@ -1,30 +0,0 @@ -# -# Use this configuration file to control what RobotPy packages are installed -# on your RoboRIO -# - -[tool.robotpy] - -# Version of robotpy this project depends on -robotpy_version = "2024.1.1.3" - -# Which extra RobotPy components should be installed -# -> equivalent to `pip install robotpy[extra1, ...] -robotpy_extras = [ - # "all" - # "apriltag" - # "commands2" - # "cscore" - # "navx" - # "pathplannerlib" - # "phoenix5" - # "phoenix6" - # "playingwithfusion" - # "rev" - # "romi" - # "sim" - # "xrp" -] - -# Other pip packages to install -requires = [] diff --git a/rio/robot.py b/rio/robot.py index 6b4fedb7..bbce6218 100644 --- a/rio/robot.py +++ b/rio/robot.py @@ -28,6 +28,9 @@ def teleopInit(self) -> None: if self.autonomousCommand: self.autonomousCommand.cancel() + # def teleopPeriodic(self) -> None: + # self.container.robotDrive.drive(0.1, 0.1, 0, False, False) + def testInit(self) -> None: commands2.CommandScheduler.getInstance().cancelAll() From a4d5067bf2dfa82548c643b4c8e541c93f2268e5 Mon Sep 17 00:00:00 2001 From: kredcool Date: Tue, 23 Jan 2024 14:14:59 -0500 Subject: [PATCH 54/61] This for some reason zeros all the modules, its dumb --- rio/robotcontainer.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rio/robotcontainer.py b/rio/robotcontainer.py index 41f5778a..f4573c0e 100644 --- a/rio/robotcontainer.py +++ b/rio/robotcontainer.py @@ -49,8 +49,8 @@ def __init__(self) -> None: self.driverController.getRawAxis(2), OIConstants.kDriveDeadband, # TODO: Use constants to set these controls ), - True, - True, + False, + False, ), self.robotDrive, ) From 29f0b51e3275f5bc18385d0d8cc8a5605c89f693 Mon Sep 17 00:00:00 2001 From: kredcool Date: Tue, 23 Jan 2024 14:37:39 -0500 Subject: [PATCH 55/61] funky town: this works --- rio/robotcontainer.py | 4 ++-- rio/subsystems/mikeswervemodule.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/rio/robotcontainer.py b/rio/robotcontainer.py index f4573c0e..331ccfda 100644 --- a/rio/robotcontainer.py +++ b/rio/robotcontainer.py @@ -38,11 +38,11 @@ def __init__(self) -> None: commands2.RunCommand( lambda: self.robotDrive.drive( -wpimath.applyDeadband( - self.driverController.getRawAxis(0), + self.driverController.getRawAxis(1), OIConstants.kDriveDeadband, # TODO: Use constants to set these controls ), -wpimath.applyDeadband( - self.driverController.getRawAxis(1), + self.driverController.getRawAxis(0), OIConstants.kDriveDeadband, # TODO: Use constants to set these controls ), -wpimath.applyDeadband( diff --git a/rio/subsystems/mikeswervemodule.py b/rio/subsystems/mikeswervemodule.py index 4c3b3055..15dd4740 100644 --- a/rio/subsystems/mikeswervemodule.py +++ b/rio/subsystems/mikeswervemodule.py @@ -148,7 +148,7 @@ def setDesiredState(self, desiredState: SwerveModuleState) -> None: # Apply chassis angular offset to the desired state. correctedDesiredState = SwerveModuleState() correctedDesiredState.speed = desiredState.speed - correctedDesiredState.angle = desiredState.angle + Rotation2d( + correctedDesiredState.angle = -desiredState.angle + Rotation2d( self.chassisAngularOffset ) From 340db7b3bab98a12c0d1bc066d3a006933fc2c46 Mon Sep 17 00:00:00 2001 From: kredcool Date: Tue, 23 Jan 2024 15:19:25 -0500 Subject: [PATCH 56/61] it drives --- rio/constants.py | 4 ++-- rio/robotcontainer.py | 9 ++++++--- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/rio/constants.py b/rio/constants.py index a1219a9f..b33c0d1c 100644 --- a/rio/constants.py +++ b/rio/constants.py @@ -108,7 +108,7 @@ class ModuleConstants: kTurningP = 1 kTurningI = 0 - kTurningD = 0 + kTurningD = 0.1 kTurningFF = 0 kTurningMinOutput = -1 kTurningMaxOutput = 1 @@ -122,7 +122,7 @@ class ModuleConstants: class OIConstants: kDriverControllerPort = 0 - kDriveDeadband = 0.05 + kDriveDeadband = 0.075 class AutoConstants: diff --git a/rio/robotcontainer.py b/rio/robotcontainer.py index 331ccfda..e4e5d6ec 100644 --- a/rio/robotcontainer.py +++ b/rio/robotcontainer.py @@ -40,15 +40,18 @@ def __init__(self) -> None: -wpimath.applyDeadband( self.driverController.getRawAxis(1), OIConstants.kDriveDeadband, # TODO: Use constants to set these controls - ), + ) + * 0.62, -wpimath.applyDeadband( self.driverController.getRawAxis(0), OIConstants.kDriveDeadband, # TODO: Use constants to set these controls - ), + ) + * 0.62, -wpimath.applyDeadband( self.driverController.getRawAxis(2), OIConstants.kDriveDeadband, # TODO: Use constants to set these controls - ), + ) + * 0.62, False, False, ), From 160b76028b23e413c9656aa940beba08d946c656 Mon Sep 17 00:00:00 2001 From: KenwoodFox Date: Wed, 24 Jan 2024 15:19:02 -0500 Subject: [PATCH 57/61] Bump robotpy, fix unit tests, modify swerve command --- rio/Pipfile | 8 +- rio/Pipfile.lock | 303 ++++++++++++++++++++++-------------------- rio/robotcontainer.py | 18 ++- 3 files changed, 173 insertions(+), 156 deletions(-) diff --git a/rio/Pipfile b/rio/Pipfile index 5e5d9bd4..ef016fa0 100644 --- a/rio/Pipfile +++ b/rio/Pipfile @@ -5,11 +5,11 @@ name = "pypi" [packages] wpilib = {extras = ["all"]} -robotpy = "2024.2.1.0" +robotpy = "2024.2.1.1" robotpy-rev = "2024.2.0" -robotpy-ctre = "*" -robotpy-navx = "*" -robotpy-commands-v2 = "2024.0.0b4" +robotpy-ctre = "2024.1.1" +robotpy-navx = "2024.1.0" +robotpy-commands-v2 = "2024.2.1" [dev-packages] diff --git a/rio/Pipfile.lock b/rio/Pipfile.lock index 682f153d..b33e3c8f 100644 --- a/rio/Pipfile.lock +++ b/rio/Pipfile.lock @@ -1,7 +1,7 @@ { "_meta": { "hash": { - "sha256": "c4565a09bd5f114ab70b68febd001751ecf444234f7b2e3c41d9027b065bea41" + "sha256": "df41db945548afa457446f51edf3e125beddfe215a35c68d3f70046811f18990" }, "pipfile-spec": 6, "requires": { @@ -104,37 +104,46 @@ "sha256:fa3a0128b152627161ce47201262d3140edb5a5c3da88d73a1b790a959126956", "sha256:fcc8eb6d5902bb1cf6dc4f187ee3ea80a1eba0a89aba40a5cb20a5087d961357" ], - "markers": "python_version >= '3.8'", + "markers": "platform_python_implementation != 'PyPy'", "version": "==1.16.0" }, "cryptography": { "hashes": [ - "sha256:079b85658ea2f59c4f43b70f8119a52414cdb7be34da5d019a77bf96d473b960", - "sha256:09616eeaef406f99046553b8a40fbf8b1e70795a91885ba4c96a70793de5504a", - "sha256:13f93ce9bea8016c253b34afc6bd6a75993e5c40672ed5405a9c832f0d4a00bc", - "sha256:37a138589b12069efb424220bf78eac59ca68b95696fc622b6ccc1c0a197204a", - "sha256:3c78451b78313fa81607fa1b3f1ae0a5ddd8014c38a02d9db0616133987b9cdf", - "sha256:43f2552a2378b44869fe8827aa19e69512e3245a219104438692385b0ee119d1", - "sha256:48a0476626da912a44cc078f9893f292f0b3e4c739caf289268168d8f4702a39", - "sha256:49f0805fc0b2ac8d4882dd52f4a3b935b210935d500b6b805f321addc8177406", - "sha256:5429ec739a29df2e29e15d082f1d9ad683701f0ec7709ca479b3ff2708dae65a", - "sha256:5a1b41bc97f1ad230a41657d9155113c7521953869ae57ac39ac7f1bb471469a", - "sha256:68a2dec79deebc5d26d617bfdf6e8aab065a4f34934b22d3b5010df3ba36612c", - "sha256:7a698cb1dac82c35fcf8fe3417a3aaba97de16a01ac914b89a0889d364d2f6be", - "sha256:841df4caa01008bad253bce2a6f7b47f86dc9f08df4b433c404def869f590a15", - "sha256:90452ba79b8788fa380dfb587cca692976ef4e757b194b093d845e8d99f612f2", - "sha256:928258ba5d6f8ae644e764d0f996d61a8777559f72dfeb2eea7e2fe0ad6e782d", - "sha256:af03b32695b24d85a75d40e1ba39ffe7db7ffcb099fe507b39fd41a565f1b157", - "sha256:b640981bf64a3e978a56167594a0e97db71c89a479da8e175d8bb5be5178c003", - "sha256:c5ca78485a255e03c32b513f8c2bc39fedb7f5c5f8535545bdc223a03b24f248", - "sha256:c7f3201ec47d5207841402594f1d7950879ef890c0c495052fa62f58283fde1a", - "sha256:d5ec85080cce7b0513cfd233914eb8b7bbd0633f1d1703aa28d1dd5a72f678ec", - "sha256:d6c391c021ab1f7a82da5d8d0b3cee2f4b2c455ec86c8aebbc84837a631ff309", - "sha256:e3114da6d7f95d2dee7d3f4eec16dacff819740bbab931aff8648cb13c5ff5e7", - "sha256:f983596065a18a2183e7f79ab3fd4c475205b839e02cbc0efbbf9666c4b3083d" + "sha256:0a68bfcf57a6887818307600c3c0ebc3f62fbb6ccad2240aa21887cda1f8df1b", + "sha256:146e971e92a6dd042214b537a726c9750496128453146ab0ee8971a0299dc9bd", + "sha256:14e4b909373bc5bf1095311fa0f7fcabf2d1a160ca13f1e9e467be1ac4cbdf94", + "sha256:206aaf42e031b93f86ad60f9f5d9da1b09164f25488238ac1dc488334eb5e221", + "sha256:3005166a39b70c8b94455fdbe78d87a444da31ff70de3331cdec2c568cf25b7e", + "sha256:324721d93b998cb7367f1e6897370644751e5580ff9b370c0a50dc60a2003513", + "sha256:33588310b5c886dfb87dba5f013b8d27df7ffd31dc753775342a1e5ab139e59d", + "sha256:35cf6ed4c38f054478a9df14f03c1169bb14bd98f0b1705751079b25e1cb58bc", + "sha256:3ca482ea80626048975360c8e62be3ceb0f11803180b73163acd24bf014133a0", + "sha256:56ce0c106d5c3fec1038c3cca3d55ac320a5be1b44bf15116732d0bc716979a2", + "sha256:5a217bca51f3b91971400890905a9323ad805838ca3fa1e202a01844f485ee87", + "sha256:678cfa0d1e72ef41d48993a7be75a76b0725d29b820ff3cfd606a5b2b33fda01", + "sha256:69fd009a325cad6fbfd5b04c711a4da563c6c4854fc4c9544bff3088387c77c0", + "sha256:6cf9b76d6e93c62114bd19485e5cb003115c134cf9ce91f8ac924c44f8c8c3f4", + "sha256:74f18a4c8ca04134d2052a140322002fef535c99cdbc2a6afc18a8024d5c9d5b", + "sha256:85f759ed59ffd1d0baad296e72780aa62ff8a71f94dc1ab340386a1207d0ea81", + "sha256:87086eae86a700307b544625e3ba11cc600c3c0ef8ab97b0fda0705d6db3d4e3", + "sha256:8814722cffcfd1fbd91edd9f3451b88a8f26a5fd41b28c1c9193949d1c689dc4", + "sha256:8fedec73d590fd30c4e3f0d0f4bc961aeca8390c72f3eaa1a0874d180e868ddf", + "sha256:9515ea7f596c8092fdc9902627e51b23a75daa2c7815ed5aa8cf4f07469212ec", + "sha256:988b738f56c665366b1e4bfd9045c3efae89ee366ca3839cd5af53eaa1401bce", + "sha256:a2a8d873667e4fd2f34aedab02ba500b824692c6542e017075a2efc38f60a4c0", + "sha256:bd7cf7a8d9f34cc67220f1195884151426ce616fdc8285df9054bfa10135925f", + "sha256:bdce70e562c69bb089523e75ef1d9625b7417c6297a76ac27b1b8b1eb51b7d0f", + "sha256:be14b31eb3a293fc6e6aa2807c8a3224c71426f7c4e3639ccf1a2f3ffd6df8c3", + "sha256:be41b0c7366e5549265adf2145135dca107718fa44b6e418dc7499cfff6b4689", + "sha256:c310767268d88803b653fffe6d6f2f17bb9d49ffceb8d70aed50ad45ea49ab08", + "sha256:c58115384bdcfe9c7f644c72f10f6f42bed7cf59f7b52fe1bf7ae0a622b3a139", + "sha256:c640b0ef54138fde761ec99a6c7dc4ce05e80420262c20fa239e694ca371d434", + "sha256:ca20550bb590db16223eb9ccc5852335b48b8f597e2f6f0878bbfd9e7314eb17", + "sha256:d97aae66b7de41cdf5b12087b5509e4e9805ed6f562406dfcf60e8481a9a28f8", + "sha256:e9326ca78111e4c645f7e49cbce4ed2f3f85e17b61a563328c85a5208cf34440" ], "markers": "python_version >= '3.7'", - "version": "==41.0.7" + "version": "==42.0.0" }, "iniconfig": { "hashes": [ @@ -181,11 +190,11 @@ }, "pluggy": { "hashes": [ - "sha256:cf61ae8f126ac6f7c451172cf30e3e43d3ca77615509771b3a984a0730651e12", - "sha256:d89c696a773f8bd377d18e5ecda92b7a3793cbe66c87060a6fb58c7b6e1061f7" + "sha256:7db9f7b503d67d1c5b95f59773ebb58a8c1c288129a88665838012cfb07b8981", + "sha256:8c85c2876142a764e5b7548e7d9a0e0ddb46f5185161049a79b7e974454223be" ], "markers": "python_version >= '3.8'", - "version": "==1.3.0" + "version": "==1.4.0" }, "pycparser": { "hashes": [ @@ -228,24 +237,24 @@ }, "pyntcore": { "hashes": [ - "sha256:089c9721b1aeba6f31f211a9033af9600cabb9148996d1b019cbc03eb4bd3d0b", - "sha256:2fd033786a948b0d63bd6596e9cf5ad0386f472abefe7cc1271edec85bb4c4e8", - "sha256:3422f6b07d0c5025e8c87b37964f7a9bc512d3c8087c5a1f88eaffdbbd220781", - "sha256:3f6961b8576e8a8a80d8ef28fae1129c53c1e5a91b231a12a9328ab4c6d1f9b1", - "sha256:479b8ca8d3611f85ab2c5dcca39af299f40db3d7ba4efe9256842f067a4443df", - "sha256:57080b81dbca5e0b28715f19867b848aa5534e09088d5b66d76b64f7cee998ee", - "sha256:6463a5640e52ac0ddda10649babf10616c6278858d5122363d09ff6784dca99e", - "sha256:6f0f5a89ae8a70c3994853f2ba1c29a73f3f6acf8becce71c60c232158f9db3f", - "sha256:803a3ce467527928e22e57c99525a923b79ba716dde9c94d1807b11caba09603", - "sha256:9d4afd35d0d38c30f3ad1de5e9d1bdf53d64e560fe3791536d7ea1b36f58ebbb", - "sha256:a30e97a6971fadaceacb7e357670d22af55d810d081a8b7cf808db7c62425096", - "sha256:a368189aa878be535ba2dd8c6e21173a45e2ccf446b6ef5904da4d07b2ec3acc", - "sha256:c6bd974f2e0eaa3f9f9705fe641793bc759268d75cb9cc424e20a97d587971e1", - "sha256:e68a04a9723d8c2300c1057a69cab1544ac60133b2506c45ecb16838f563d6a0", - "sha256:e93aa0c9951722e0b69288c2792f2bc2a7a65d02b26f8e8230de1edc371bf250" + "sha256:10b0d9a39b6a0dc92f25ae06798a5dcb1eb36ed5fc07083711cce39056ac62ab", + "sha256:13bfacc40106929e9225acc4fb4638e60e4b119435eb65e100e645af50e3b25e", + "sha256:171015482d904817cac66d4a8a2a9efaa9bf3af52f23a6729f687c7a9a22fcd9", + "sha256:45fb06fc5ffb1a995bd5c583866dd029a9ef95819dc33f91766ac235203bc103", + "sha256:56cfb02826e266da6c1ed563d8291d8e4c20c2eb928adc52cca1b43f27d55261", + "sha256:5c8dfb93756c7d945e88d34a2bda9c51f83478cf89e7bd8049d13a3dc9882373", + "sha256:6b8a093cc8ff8f436ddbd83a6b2c2cf0184001d137df2c1e708f19814aa1f65b", + "sha256:701e0b4aece667e286ca4e772fc75fb5cb69fa0458db3b53687cc9aeb0b180f8", + "sha256:8e2d05b55b8c5f01f11016d4e400f2242b4becf7ca26783821c7682a98aae8ea", + "sha256:9abf53adb8a412f9584b14f3a9dabd37940af5f57ff3c60ff5f2b150bf747e3a", + "sha256:b137734c8f4083827079665cd34460863c57c9ddd6b397726a6eb9734cb255ca", + "sha256:dfdcde81b621bd6fee02185c75bcdd4d180de1d3b281df70213b8f8784580dca", + "sha256:e30dd12eafd0d28e306c3076cf99b067347a9c0fbe299f82553a7b0a94c056ab", + "sha256:f06762dd96eed6a5b79ff07c46fab92bab08edfe0788b0265e0720e747f6afa4", + "sha256:fc174b121f984b876cc3322129de7a475155027304f43867ca11e6441d101149" ], "markers": "python_version >= '3.8'", - "version": "==2024.1.1.1" + "version": "==2024.2.1.2" }, "pytest": { "hashes": [ @@ -265,12 +274,12 @@ }, "robotpy": { "hashes": [ - "sha256:a9f9f7de9aac362ec7f027194d11644ad555f4da1951c9ebdf9c6ba8ec15cb36", - "sha256:cc9a1dfa216a8208efa848683ce0df45c98f7905c3450f8d61ea32fed4adab9b" + "sha256:c21f49f5af79d320abcb61ee08524f27376df2b297d2e60ef55d9e00edd135bc", + "sha256:d7e1fceae2b1f9b214b81fdceea8d05f56f9add0f2518381cef7559bc4d5aa3f" ], "index": "pypi", "markers": "python_version < '3.13' and python_version >= '3.8'", - "version": "==2024.1.1.3" + "version": "==2024.2.1.1" }, "robotpy-cli": { "hashes": [ @@ -282,12 +291,12 @@ }, "robotpy-commands-v2": { "hashes": [ - "sha256:0161feea5288b6af6fe3eddc412ad30c920acb929bcb5fea6899ba47e2e5763b", - "sha256:e0f808645f3f01c1a49d54cbdca6a2f528c2797913bb6382bec067c69b646065" + "sha256:c14fad8986c94e4f1cd5eda8bbadca60b733e89d44ea1388aaa030f0de734cd4", + "sha256:f80c9caf2cce2c05f26cbe8024fc587834a07485573971bd116e3fbd6678e5ef" ], "index": "pypi", "markers": "python_version >= '3.8'", - "version": "==2024.0.0b4" + "version": "==2024.2.1" }, "robotpy-ctre": { "hashes": [ @@ -311,45 +320,45 @@ }, "robotpy-hal": { "hashes": [ - "sha256:0cd34df381940f89626e1e2b206d2def4879c1cf66b617745447ab5323c24141", - "sha256:164ce03dda139902ecc2a1e7c1d18600fbd0f5f33386e9eff1309ec32c217cd5", - "sha256:19e445a2bd24e178e5a6ed583a21f8622da1ec214c0953c2cb028ff67b973b28", - "sha256:2254a91d6b3c1a73a4c786338e677eff7d50620eb6b24f831945a53901e98501", - "sha256:281f6bea3793106691568139df7c7722dcc40acbee95076c54070e4fe471aeac", - "sha256:547452b6a2c89ed1ca74eb4523da183bb5c0c0b7281823ce197fe99a738dee29", - "sha256:79d0810b4aee3769ee573e9d6a74f448940efea8ffaf70d86840db40122ce652", - "sha256:7b884b7d756034a42702fdde8e77e8a8b53c379872a47673a8b1b6b898d14352", - "sha256:93626c3a9fbb2ffa9a9dde9680675c845c173811a8b73bbc4790b50cf0d307ad", - "sha256:b5d3a20a1fb67eaf2bb6efc1303f3ed1db8bdea08a9fde284ee63681fffc7aa8", - "sha256:b9939b7076268c6f54f633879005685089a68d60b2df0fa52830b20c58af02cf", - "sha256:b9945c69fb063f49db01ae4b335a92689d42cc1068efadff37403390a19169f9", - "sha256:d201bbd0529701f150806abdf1c77c5203ee3fc82656fb7c9863d43b9ac3a364", - "sha256:df21128092e932490923b49f6aa9741dbdb3a225805f60900b13d1ededeee38e", - "sha256:fb8e58b726ed94c0a5f636f45b65580e5eefdeeb4c2cd2c898ba9ca0de7b1eec" + "sha256:03545dab1d00e8ff196934bf2f7b2f916124df75fb644690a0d01ae1fc5e54aa", + "sha256:26294b8d42259250bfdf1c093b2294aec9ec74934cf50fb94fd5f9bd77f19ac4", + "sha256:26698823ed4bbcffbb5de71e26210dfedacdcfe1a6a215cc4417b0955e006d9a", + "sha256:3964419d68b7ea80d7d4f0fce036339a2e804edc7c0b5d40a0667df85a93f73a", + "sha256:582b14e26d4f181c8481d1d9577d540a72ceacabebecbd21e7d081084cd93268", + "sha256:73fe0bd80a9651daa80351b664153b4f1322a3cb0ebf28e8b1eae32d5fb8635f", + "sha256:77c315235698fcd73f31d34e7c445fdee4b32364ac804b3cddbe6f841fcac9dc", + "sha256:9192db927dda54d383a9b9337591abb3f2c9d68a78a9da3e35bd3a8439e7d935", + "sha256:97d1ac1e640716a76ed59501631ceab3d691f25a6e2804b7b84c746052408c59", + "sha256:aa5f84f00078b91f0f9eb378e494c6e40467f262e820c2d801bc200a355a67ca", + "sha256:c5d35feadbc4ae2d94f9611ce090876bc639ef1ae0a7d8caea6f6dc569b9d2fc", + "sha256:cb4a5b7a960b32cd07dd5de8c3b59607188d467eeb8e041d29869a42cca6bb9e", + "sha256:d92d7429e41b8eb87afc9940ac8d4b45bdab327b4d364b42ec05357dd4785f36", + "sha256:da75e117675c4b59d4dc50af3147d31525e2f80fefabe52b794e7897062897d3", + "sha256:fbdff436dac6034469ba8f402ae21ace28bcf21c3bc67a571a04c131ddb0ae0e" ], "markers": "python_version >= '3.8'", - "version": "==2024.1.1.1" + "version": "==2024.2.1.2" }, "robotpy-halsim-gui": { "hashes": [ - "sha256:14a3084210ded08210328607b788a0202fc14ffe2cf25a1de402045ccf331c9e", - "sha256:1a396f5bb485b0ee2ed378b037fce589eba475525347cc5787efb72c41d2ad0e", - "sha256:37b769bf1b8de7aa6d0d5c9e0f15f6c17694427778d85430ddfa4326464c84d5", - "sha256:3f7faa62735b4b3f2ef8e455929dfb02749269d66924cbe9ec8d673d82baa534", - "sha256:5e0eb35d0da4879a020291231f2298b2d37c0593948f8be286c6921b2b9c975f", - "sha256:6b48f2e3af194fc3184f551ebe9127c119563cd493272d8a917818d836b2f7dc", - "sha256:814473b3714be8f306a9a111b09d8574da36f6b62ffa9a356ec76310d822e3d9", - "sha256:830d9669e31b3270654d9b8f2326cd587a1666dc174394da3b612165949accc7", - "sha256:a351177fecd20bf27ae38305c85388b8cd5b832965725855d3bd27b668ed5016", - "sha256:a7af21ddb6ebdbea381c1de56e4c82fefa78738ef81cff17424cfcd32e37b48a", - "sha256:bce89ffd57caab38b77e90b406d9ca6e02f9f6c05c48a10e451f2648184690a9", - "sha256:bf0ed83c70ecda0fd0bd02e99e296a7563c919fe7850d1cd6c32521020f43b13", - "sha256:cc6353ad02775be38ef9ff996462c44292505b422a5279da78397370138f28bd", - "sha256:da93b6d396a6eab81ee81adaf8ea0cd69cc579c510c1e3a5663615b98393f1b9", - "sha256:f3ea7483b2dbd4c09fe7c9cfca3887797f979688ef4e21eca8eb655d3ab6b29a" + "sha256:0a276bbaaf523604397cbab492d347a2e9733675a773bfec1faf93e8629369ed", + "sha256:0fe53bee95335b73ff25a2dff5f25ff324111e7023118dfe2b2aee38017f6c9b", + "sha256:12b7c2803cd7f9f746dc1a383c3ec8a675cd61a92fa3d320cde5467115871d39", + "sha256:18c9c03cf4c84a1e0a6e18dcd01a5ddaa684b3a65c614a930ed85185ef1c113f", + "sha256:1a4e9d3f428a1b22c3d7a146703fee844c2fe9dd3a6eabba345a26d64bd7e1d9", + "sha256:27f024c5a035a1f4929a427a5554c1dce45facc44a9180e0b76203de0a360511", + "sha256:3756d5546f5daf8712bd5ae4cc080ae807e71b599fd7de5c3e2d13285b3031b2", + "sha256:4563d2b0d58c7c2d179647c496e8f6b5665308fddd3b584e26c7005000707169", + "sha256:473ad062590d5969c07a79d1449ceddeec14635c9055330a698b7944bc4565eb", + "sha256:64940e45d0b79b0a08ecc878245d2a676b48b8b34e39801773e9622abefac03f", + "sha256:7afc42055722b40764f16838d981ec92ac37de5c719ab12cf6c5c9278e7c2a2d", + "sha256:902877dc1963be58399a586916cfad76ca8439cb44cbf94e6d50b0bf9133a55d", + "sha256:91ab958a9e4310a6d900d4dc55dd9681939631d8585a1071e5bdedc22b9a66f0", + "sha256:b3b05bfa38c65fa072043abbf7135851c0167ab1e25abedecf61160bf9a0d598", + "sha256:fae0127e462215e1de12c252561c56641f6dadbafccf7b21f1cc29a67e53574e" ], "markers": "platform_machine != 'roborio' and platform_machine != 'armv7l' and platform_machine != 'aarch64'", - "version": "==2024.1.1.1" + "version": "==2024.2.1.2" }, "robotpy-installer": { "hashes": [ @@ -415,66 +424,66 @@ }, "robotpy-wpimath": { "hashes": [ - "sha256:35eb656efcd6cf1d50b8a0ebe5b802c5f0581b6d20df934f8dd1fcb77d120192", - "sha256:4340882ab0f5c46487c4d5d9862430b58abc1762ef6d96753d9635c895987c6c", - "sha256:59466913060a697fee4cd1afc63b5b72397281f9e0019f3a8cc6a1a6dbfa252e", - "sha256:649ce400c5f3b45deaf73dbd8223be71fcb44f61d41d665bbf21ad947a7d5df3", - "sha256:7167ea5181c815fe10f3e46df495ffc08ae2f90dbf399d455fe3c72643602394", - "sha256:7b27bf280c9ae55e874b2e46f9174a3b07896ee9e4d5b90c4e16dab5173ad72e", - "sha256:81e850df98fd62e8497a49b2bceec4014b9ed3cea155642fa4e034f77d870e18", - "sha256:951ed4f556f9d2bc2addf70be331789742420147aca23e008ec5830488ec97e4", - "sha256:9d570d4dbe27bbee560a14a212dc36d7149c3714c0e9ff77528615817c295a08", - "sha256:bba421856198139d5c2384a66e04a18d47562ba1c7aaa78e4374a586bbafe427", - "sha256:cdbe1585543d5f081c6a86f199d26b2355cb0713c3ff115e065f3697c064611d", - "sha256:e3aef1a9c03d76236ed6049efdb28d4d021921d7a2dac2df7d32fb0137834704", - "sha256:f987609fec0adbf253e47388812f61969c2ffcec1958f63a097bb5a3515c1e24", - "sha256:fd6657f4a41b048f09791c1a2e556f7df12dc6e94631ae84deb1496ed246ca99", - "sha256:fd7f3e247e248c2442ed8dab5b7b807a702576c3eab9d6725796af172d4102fc" + "sha256:00a4b8dc79656050f81748efaa462dff04948bb71916c185d3fa188487113fdc", + "sha256:07b2172efc025c52f958c80115a4043892d10bc0db22dbaa871b32dff5a2f745", + "sha256:1a1c7c7fe30f1af433df3b47d093a5d2d19f57ec1fcd343ff1762d28d2416857", + "sha256:32a0ab62c5e1eca977b9c7b894faec18feaeca791f4c2f3bd6815ff6663a95a0", + "sha256:4a0e00af2a5a25ec641b036389ba1a8ce3ee6d023fcec3f994ee5788478f74f5", + "sha256:4acca2fb7122f23829458a08de31d60dd68de9cf4c18acb64b94f97adb1332f4", + "sha256:4bc7205fa4d41ccb2b53ff33dac013370fc040fe7d6a1253fd7f417c60e27409", + "sha256:562da0476906b745585d34f036edfc49ec95f0d08266520e592051c34cdb1e77", + "sha256:6cb06f8af879ad7959cb4a53ed4004688a7a0845729ddad1457781aee8e827ca", + "sha256:8f9ce5c6507c31ba5091e74c17f8edd48113428c7b08f67a3a8f355ce6fde488", + "sha256:ab0b61d25c97fe277c29ffd8e41b9b377ed691e5b1079bc5bcadd75b003d7c98", + "sha256:bb7898148442690572e5a94dcceed085048d8d0b18d1f67b91cb9f54615e8aef", + "sha256:c5c431c9fdd9f354f20a3a97711ca9d1f3cf75561a3baa6d102c7c68a6d4873a", + "sha256:d7212560be85935b06fcab798fe9d27f6950f8064f70999f1b4e056a17328f8f", + "sha256:f8e2d84acd255431c4346d7f07fcfd638da46aadd32872483de15aa11d34f520" ], "markers": "python_version >= '3.8'", - "version": "==2024.1.1.1" + "version": "==2024.2.1.2" }, "robotpy-wpinet": { "hashes": [ - "sha256:04084d2289de0b1982ec71f9690f20bb62ac0606ba13bafaf087397979f5b168", - "sha256:120e2a371f9a53a3571ebf3e3e933e95c451de80f61e7e14f8c3b69021d101cd", - "sha256:1faf0a5cb40b27538ff20ba1e5a098490f4106a3704c89403a4851a844c25a1e", - "sha256:2816bf1c4dd18412b63c9526e413852f784d680e7b1def0eb9426496ee543bc8", - "sha256:2caf345812138b0b76695f7c858c3ba364403f4de08a9bb21074d1724ad9f326", - "sha256:3a07601b53193179310e8d433d1100ff554f62e87ffd62daf8c9470847b0cce8", - "sha256:52d0fbb187120416846261d3a1150282a33542a672495fdd5189d0f0d7f211a4", - "sha256:63ee412582a42a40cba5c851a3723d84a69f9a8787e66446eea6c2426ea2859e", - "sha256:6a58178cab524f6943027dd93a8e2ace92b2a23cdefac596297aabf15830f28e", - "sha256:79cad99f417c0c3d08d40755fe1d303a7841726befe878bc7f5221b2c8b0271b", - "sha256:8729c00e2b6e8993e3bb7ea9f3d0f70b7028dd91c172b3d8e8e62a5cd04627c5", - "sha256:8ed88d6aa152be5899ff5ac441aebf53dffb529cda552e82031687d1bb13e4a7", - "sha256:94bf23400e7891f9ee4665570c1f5fc638bf8c89049d508af2b99a31c81d2c49", - "sha256:a9dc643a8a00055cbc7d6998ae02fbfa92da81498da4a105ee2edb76d7ef8319", - "sha256:f3ce06ddcce3f458cabb126bffbb78242d541c0500c1b703f0afd212ae64e7cf" + "sha256:02b279d069ba20e0e4eda90bc6e116eb10d89df0370067ca7d9649ef9b2efeb9", + "sha256:02cebe13d76064044c106cd1e8ee9b18e63e6b8371c6041326e5730dcd9f5b03", + "sha256:1a8ed49cdbd748c0dbe15e993ebf26f18d49f400d8efb823b5272c61588988c8", + "sha256:4aa2c2964654262c944226a92a5095d260677d4da287beca2e5353daba225ca0", + "sha256:4fddc51d455591f4657205740df068a42040885e576c719b7a19b9c4fb5e5b55", + "sha256:5cc7838ea6b3b7c7d021c080c19e3bfebb23f52e00928916e4fb46fcf8c3ec64", + "sha256:96463e262e7498a42dc51add5d7fde71da875f2aa314a3033bf956eecb5ad289", + "sha256:ba0276cede651001e963ac98ae57731e1cd2f915e7fd77e04ddc623a55747d5a", + "sha256:cbd64df8095864b933c73344180d8dc2079fa290f93e10e6c2e894ae1eafcd5d", + "sha256:cd136c56e72a2a652f5ec904bd04634c53ac7946d52bb7412cb2f1f0a89243b2", + "sha256:dd267e2415201e10b2b5c5c0de7b2277b111794d8c2c94420af66c5dbe36b958", + "sha256:e14ad187c3ddcd68e0dedae6f5f7ad1831e98c68b48e6745cff2620f018a656c", + "sha256:ee716c89d34df41d3e6532bd92975ff68fddbaf91a7f4feb9e2f9ce9dcf3997c", + "sha256:eebe52fb5cf4bd63210c9cacae0b2b532d505e993df1666cf1283c0e736594f8", + "sha256:f956c46b4d0917ce881e620952b125676537e3347fc486b05882731369b38b7d" ], "markers": "python_version >= '3.8'", - "version": "==2024.1.1.1" + "version": "==2024.2.1.2" }, "robotpy-wpiutil": { "hashes": [ - "sha256:0084575cd93088ab71ee3140506ef497ae3292cab4c786dbf3f8f7b19333b6ee", - "sha256:0b552be4e0d5f411237e70a4c9fed959906917f9e5681e5db8b06b17ac857a56", - "sha256:0d8579546617a5e2967b1e3325a6778ca6e56d87585b09f766258077722fcc47", - "sha256:1aa7bc8abd9635517119ccd6a322e28a5da5ab92bf1c66c0b90ffbb865549710", - "sha256:3008cb3a9e94ae9a0b2c15aacd4e18886a621723a196a20a459d7419ba504ed3", - "sha256:3c72957302d7caa65f2a71274889e481a8b3490bd7bd425cef0340defe09b179", - "sha256:5096c16e90eef7bec892e3ad7ea648110c4d525e0c79d1aa07d9d380a0c0c66c", - "sha256:5f62c29d06f9adf5159c82b2b7bdab98d2d247f6cc84df0aebee6570c916f735", - "sha256:90d600535afbd61892a056e8c292b4de9fa4cd593b0d0a7e1b3585d5801f43d1", - "sha256:ab5f7e193f1f044feb74dea87867811132ce71b19d1cb55b06b03db7fdec2489", - "sha256:ae84c668de63383d633a56d8c089b76ea3e2835f36ab49374ed498c118fd5c47", - "sha256:b1aead0364f3069f82c2eb5bbb860906a8a2be89b520afa1da9d7ff58f04b0cf", - "sha256:f3ee870245ee3cec80f1b74a6f674e93b574d156924c83c239970a07a3ab6dec", - "sha256:fd98ab7f66447387cb329ed8451736c616dbf4c4d31e9ad746853ea9046ca506", - "sha256:ff4f4be6e5d9d6ac9a0bcc1e72fa7756810e986939bfa1ad2522b597dab7f85c" + "sha256:5ba7e3df22812e7924cfa2a868dc76fb4a06077d7ea32f02e662db5ec92fd1b8", + "sha256:6b0b4e3f6a0a354d64d600d87d1e06c4ed9aadbb16d69b3eb280723fa781e31c", + "sha256:6e3b410661d69d02d14929e9dde6d4272a334606ada546331a0e5e087cbcc037", + "sha256:885aee82ab121acc87167e795d4e7217ef40521ac3d993dc723faef2b26e0d04", + "sha256:95a1f0eb1dbad058937f4baa2c2942b6344f5a523a0784a907c750c482caf407", + "sha256:9a90190ba1b8b3b7eb76f0f5f0c40b0a18256e7ecf2ebabccae615389ebb9575", + "sha256:b1a005bb75160aa40825b3d436dfa35b606db674f143b61d77af7f7680308f02", + "sha256:be6ead686096ee25dbe096f1ca1bee1e7db9d42739144bc1eb7af327a617e658", + "sha256:c50f7384224753e2eeae188610bbcf58604275b66efbd189f4dd94ed9b5c244a", + "sha256:e621b62bf4320cb68d270fe12b7f78b5dfac56c1d54703d5822e7dac6fd23d70", + "sha256:ebd3c8fc4280172adf34346a2302ec985f0d8730f3f95200a66c3f6da83613c0", + "sha256:f0029684f096d2ba1618aebffbc126dd571aa2a0d3e73b0c2b3f016e6ec996bb", + "sha256:f69a8b2e2c7a626ab3fed8e5ca3dc017e2871226ef03ec045e4cd7da658d873d", + "sha256:f8c44c5fb1300299d65a89bbb7162465a8201a64175ff06f35768b6ed1c4e4f0", + "sha256:fdee662f432ccd717366da9ea3b5e152e84ee74e6a4c2a95d4c251ff82736fcf" ], "markers": "python_version >= '3.8'", - "version": "==2024.1.1.1" + "version": "==2024.2.1.2" }, "setuptools": { "hashes": [ @@ -505,24 +514,24 @@ "all" ], "hashes": [ - "sha256:023f1b6f404a5ab3a5018bf59b681215c8d1455ab114bbe1c70ad6648fd36750", - "sha256:244745f00b9fc6db7b66b2992a1bfc9c7da5c6951bb85aeda0f277de6589eca6", - "sha256:4e277c109c14550dff64f6e3e19202a0517de6776e59484447ca50539c35830b", - "sha256:6b296068ba2901bd4ec8bc0bcee499ef54fd2e18d68a927b174445397d1a9e65", - "sha256:6f04c81531e67a3f06e7a50198a21657095f0fc5bdd823cd2ada4931317b2e41", - "sha256:82a2cee6ba2ff21b545388c3752ccbf5eafb60c73188d30a2161e2fce4ec2f7f", - "sha256:9edad7cd2483e514864c0fa4128abf9aeb929d2888c95fbc37a00579daca98c9", - "sha256:a5ef39347c780cc50d7ef0f079377541298070174654446272b3e392824739ab", - "sha256:b9bbd811fc38689e20714bd584bf9af13f1a1bc885ba0de282ff0ef100ebd528", - "sha256:c2816c5fb4febaeb4c7055e5528780103b0ea705084e7d99c5e219d9873a96fb", - "sha256:c8172b7ccd416a0335aaf44cc54d2ee6d92a404f880026d9934445977c74e9d0", - "sha256:c825d146270ef9c2ceaa9abfde2ea5f27270149cf97c1ccf07f3dab9ab659077", - "sha256:d4120bc2482b3af2eaaaa1562a18e86a96e7830194926dbdd66de679727d7871", - "sha256:e3b4063c4068fadd48e7cfb220476bfd529e59614e0b3f8a825d06115d2a9b44", - "sha256:fcfa81321de93933406e313b7f3f89d6300ab5bc9d757c89b4214858fb4393d3" + "sha256:0829435a16cda9f312962afe7f63dcf5812215048f29cad3ec8cfaaf66ba9d6e", + "sha256:0b9a102c2fe1e7322c6c2ae4c249aa61706bce84f13168ed8d35d0209e62394f", + "sha256:1f280c68acc2a83b587ffa3d3dbbd731f842b318d6a207d5b2a03ce17f781168", + "sha256:24b58eea56909cf6de0ccd8320dff02b0c47c33c2c761c518c918582b5cf8ac8", + "sha256:41f439f2389437547d39124a2a4780877603a220a3eb7b03a521b18dcbe8e637", + "sha256:5690544babc6ccbc5785b53f8d5aed2db6fdbb68f5e0b01d7d85cd112af34fba", + "sha256:57c7d72d9326b6cee51005fa13926b907c4e37851da5727fc086b0e5b8972c5b", + "sha256:5a93a4634babbcc111adc8edd5c309cd4654f6fe4d271cbd67fd250f9a8b6bbb", + "sha256:93acc9a77fc78fd9b6c8a7d048ff60024e930abb6671c3a29c0cdc774198d7b5", + "sha256:95ef8391edcee26f29081705664b7d59c4c51c34c92047d6aa28af5345dd2b11", + "sha256:ac186f81c3c2246cb9ce4d83f5c87cf496e47c4eb0b6096f2a53f0d1d5399ac4", + "sha256:afe472774b69e65b7dc99dbfd42e3adafb7156b332122b1e5eba660b05d7f383", + "sha256:b499bb809b24208f50b83fb79e452dacc28701bce964fae35a41085efff4a39f", + "sha256:c9fdd5d8377fbe990af506b304b7efb24dd1888ac1d18ab85f5ea84f1da2bfae", + "sha256:e644e7f6c0b4d1fc71c1cd8efebf908ab4088447db939ee213286703990ca860" ], "markers": "python_version >= '3.8'", - "version": "==2024.1.1.1" + "version": "==2024.2.1.2" } }, "develop": {} diff --git a/rio/robotcontainer.py b/rio/robotcontainer.py index e4e5d6ec..24bc091f 100644 --- a/rio/robotcontainer.py +++ b/rio/robotcontainer.py @@ -5,7 +5,11 @@ import wpilib from commands2 import cmd -from wpimath.controller import PIDController, ProfiledPIDControllerRadians +from wpimath.controller import ( + PIDController, + ProfiledPIDControllerRadians, + HolonomicDriveController, +) from wpimath.geometry import Pose2d, Rotation2d, Translation2d from wpimath.trajectory import TrajectoryConfig, TrajectoryGenerator @@ -102,14 +106,18 @@ def getAutonomousCommand(self) -> commands2.Command: ) thetaController.enableContinuousInput(-math.pi, math.pi) + holoController = HolonomicDriveController( + PIDController(AutoConstants.kPXController, 0, 0), + PIDController(AutoConstants.kPYController, 0, 0), + thetaController, + ) + swerveControllerCommand = commands2.SwerveControllerCommand( exampleTrajectory, self.robotDrive.getPose, # Functional interface to feed supplier DriveConstants.kDriveKinematics, - # Position controllers - PIDController(AutoConstants.kPXController, 0, 0), - PIDController(AutoConstants.kPYController, 0, 0), - thetaController, + # Position controller + holoController, self.robotDrive.setModuleStates, (self.robotDrive,), ) From d1d2647c7e555173b5a1bf14298903ac75fee185 Mon Sep 17 00:00:00 2001 From: KenwoodFox Date: Wed, 24 Jan 2024 15:20:47 -0500 Subject: [PATCH 58/61] Bump lockfile --- rio/Pipfile.lock | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rio/Pipfile.lock b/rio/Pipfile.lock index b33e3c8f..297c08a6 100644 --- a/rio/Pipfile.lock +++ b/rio/Pipfile.lock @@ -1,7 +1,7 @@ { "_meta": { "hash": { - "sha256": "df41db945548afa457446f51edf3e125beddfe215a35c68d3f70046811f18990" + "sha256": "ff968964e7b21e108cc95483cffcf81867e8703894b999f7be450e352f4ecd64" }, "pipfile-spec": 6, "requires": { From 57168f22bbe41911ab51d7d7a9b78145198abc12 Mon Sep 17 00:00:00 2001 From: kredcool Date: Thu, 25 Jan 2024 14:18:20 -0500 Subject: [PATCH 59/61] fieldrelative works but fells weird on joy --- rio/robotcontainer.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/rio/robotcontainer.py b/rio/robotcontainer.py index 24bc091f..67258dd7 100644 --- a/rio/robotcontainer.py +++ b/rio/robotcontainer.py @@ -41,23 +41,23 @@ def __init__(self) -> None: # Turning is controlled by the X axis of the right stick. commands2.RunCommand( lambda: self.robotDrive.drive( - -wpimath.applyDeadband( - self.driverController.getRawAxis(1), + wpimath.applyDeadband( + self.driverController.getRawAxis(0), OIConstants.kDriveDeadband, # TODO: Use constants to set these controls ) - * 0.62, + * 0.3, -wpimath.applyDeadband( - self.driverController.getRawAxis(0), + self.driverController.getRawAxis(1), OIConstants.kDriveDeadband, # TODO: Use constants to set these controls ) - * 0.62, + * 0.3, -wpimath.applyDeadband( self.driverController.getRawAxis(2), OIConstants.kDriveDeadband, # TODO: Use constants to set these controls ) - * 0.62, - False, - False, + * 0.3, + True, + True, ), self.robotDrive, ) From 24f1af3729349a85e24ff2bfb5b04ccdac9a2175 Mon Sep 17 00:00:00 2001 From: kredcool Date: Thu, 25 Jan 2024 16:58:58 -0500 Subject: [PATCH 60/61] idk --- rio/robotcontainer.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/rio/robotcontainer.py b/rio/robotcontainer.py index 67258dd7..54981581 100644 --- a/rio/robotcontainer.py +++ b/rio/robotcontainer.py @@ -56,7 +56,7 @@ def __init__(self) -> None: OIConstants.kDriveDeadband, # TODO: Use constants to set these controls ) * 0.3, - True, + False, True, ), self.robotDrive, @@ -77,7 +77,8 @@ def disablePIDSubsystems(self) -> None: def getAutonomousCommand(self) -> commands2.Command: """Use this to pass the autonomous command to the main {@link Robot} class. - :returns: the command to run in autonomous + :returns: + command to run in autonomous """ # Create config for trajectory config = TrajectoryConfig( From 23e9f75ba6b918f91a41e24a8b3698660303b9c5 Mon Sep 17 00:00:00 2001 From: kredcool Date: Sat, 27 Jan 2024 17:15:15 -0500 Subject: [PATCH 61/61] preparing for buttons ' --- rio/robotcontainer.py | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/rio/robotcontainer.py b/rio/robotcontainer.py index 54981581..e52e4e32 100644 --- a/rio/robotcontainer.py +++ b/rio/robotcontainer.py @@ -5,6 +5,8 @@ import wpilib from commands2 import cmd +from commands2.button import CommandJoystick + from wpimath.controller import ( PIDController, ProfiledPIDControllerRadians, @@ -30,7 +32,7 @@ def __init__(self) -> None: self.robotDrive = DriveSubsystem() # The driver's controller - self.driverController = wpilib.Joystick(0) + self.driverController = CommandJoystick(0) # Configure the button bindings self.configureButtonBindings() @@ -39,15 +41,18 @@ def __init__(self) -> None: self.robotDrive.setDefaultCommand( # The left stick controls translation of the robot. # Turning is controlled by the X axis of the right stick. - commands2.RunCommand( + commands2.cmd.run( lambda: self.robotDrive.drive( + # -0.1, + # 0, + # 0, wpimath.applyDeadband( - self.driverController.getRawAxis(0), + self.driverController.getRawAxis(1), OIConstants.kDriveDeadband, # TODO: Use constants to set these controls ) * 0.3, - -wpimath.applyDeadband( - self.driverController.getRawAxis(1), + wpimath.applyDeadband( + self.driverController.getRawAxis(0), OIConstants.kDriveDeadband, # TODO: Use constants to set these controls ) * 0.3,