Skip to content

Commit

Permalink
Fix python PhoenixSysId example
Browse files Browse the repository at this point in the history
  • Loading branch information
bhall-ctre committed Apr 10, 2024
1 parent 8c1a78e commit d53d4a1
Show file tree
Hide file tree
Showing 4 changed files with 19 additions and 21 deletions.
2 changes: 2 additions & 0 deletions python/PhoenixSysId/robot.py
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,8 @@ def robotInit(self) -> None:
"""
self.container = RobotContainer()

self.autonomousCommand = None

def disabledInit(self) -> None:
"""This function is called once each time the robot enters Disabled mode."""
pass
Expand Down
4 changes: 2 additions & 2 deletions python/PhoenixSysId/robotcontainer.py
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@

from subsystems.flywheel import FlywheelMechanism

class SysIdRoutineBot:
class RobotContainer:
def __init__(self) -> None:
self.joystick = CommandXboxController(0)
self.mechanism = FlywheelMechanism()
Expand Down Expand Up @@ -46,4 +46,4 @@ def getAutonomousCommand(self) -> Command:
Scheduled during :meth:`.Robot.autonomousInit`.
"""

return cmd.print("No autonomous command configured")
return cmd.print_("No autonomous command configured")
5 changes: 2 additions & 3 deletions python/PhoenixSysId/subsystems/flywheel.py
Original file line number Diff line number Diff line change
@@ -1,15 +1,14 @@
from commands2 import Command, Subsystem
from commands2.sysid import SysIdRoutine
from wpilib.sysid import SysIdRoutineLog
from wpimath.units import volts

from phoenix6 import configs, controls, hardware, BaseStatusSignal, SignalLogger

from constants import Constants

from typing import Callable

class FlywheelMechanism:
class FlywheelMechanism(Subsystem):
def __init__(self) -> None:
self.motor_to_test = hardware.TalonFX(Constants.kTalonFxId, Constants.kCANbus)
self.joystick_control = controls.DutyCycleOut(0)
Expand All @@ -24,7 +23,7 @@ def __init__(self) -> None:
recordState = lambda state: SignalLogger.write_string("state", SysIdRoutineLog.stateEnumToString(state))
),
SysIdRoutine.Mechanism(
lambda volts: self.motor.set_control(self.voltage_req.with_output(volts)),
lambda volts: self.motor_to_test.set_control(self.sys_id_control.with_output(volts)),
lambda log: None,
self
)
Expand Down
29 changes: 13 additions & 16 deletions python/PhoenixSysId/tests/pyfrc_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,11 +5,6 @@

from pyfrc.tests import *
from wpilib.simulation import XboxControllerSim
import time
from subsystems.flywheel import FlywheelMechanism

FIRST_SET = 0
SECOND_SET = 4.8

def assert_almost_equal(a: float, b: float, range_val: float):
"""
Expand All @@ -18,21 +13,23 @@ def assert_almost_equal(a: float, b: float, range_val: float):
assert a >= (b - range_val) and a <= (b + range_val)

def test_sysid_quasistatic(control, robot):
joysim = XboxControllerSim(robot.container.joystick.getHID())
with control.run_robot():
joysim = XboxControllerSim(robot.container.joystick.getHID())

joysim.setYButton(True)
control.step_timing(seconds=0.1, autonomous=False, enabled=True)
joysim.setYButton(True)
control.step_timing(seconds=0.1, autonomous=False, enabled=True)

assert robot.container.mechanism.sys_id_control.output > 0.1
assert_almost_equal(robot.container.mechanism.sys_id_control.output, 0.16, 0.01)

joysim.setYButton(False)
joysim.setYButton(False)

def test_sysid_dynamic(control, robot):
joysim = XboxControllerSim(robot.container.joystick.getHID())
with control.run_robot():
joysim = XboxControllerSim(robot.container.joystick.getHID())

joysim.setXButton(True)
control.step_timing(seconds=0.1, autonomous=False, enabled=True)
joysim.setBButton(True)
control.step_timing(seconds=0.1, autonomous=False, enabled=True)

assert robot.container.mechanism.sys_id_control.output > 0.1

joysim.setXButton(False)
assert_almost_equal(robot.container.mechanism.sys_id_control.output, 4.0, 0.01)
joysim.setBButton(False)

0 comments on commit d53d4a1

Please sign in to comment.