Add tests and fix the data_log class

requirements.txt

@@ -1,3 +1,6 @@
 m2r2==0.3.3
 markdown-it-py==2.1.0
 sphinx-rtd-theme==1.1.1
+
+wpilib~=2023.4.3.0
+pytest~=7.3.1

robotpy_toolkit_7407/sensors/gyro/PigeonIMU.py

@@ -1,4 +1,4 @@
-import ctre
+from ctre.sensors import Pigeon2
 import math
 
 from robotpy_toolkit_7407.utils.units import radians
@@ -15,7 +15,7 @@ def __init__(self, port):
         Args:
             port (int): CAN ID of the Pigeon gyro
         """
-        self._gyro = ctre.Pigeon2(port)
+        self._gyro = Pigeon2(port)
         self._gyro.configMountPose(0, 0, 0)
 
     def init(self, gyro_start_angle=0):

robotpy_toolkit_7407/sensors/gyro/__init__.py

@@ -1,3 +1,4 @@
 from robotpy_toolkit_7407.sensors.gyro.ADIS16448 import GyroADIS16448
 from robotpy_toolkit_7407.sensors.gyro.PigeonIMU import PigeonIMUGyro_Wrapper
-from robotpy_toolkit_7407.sensors.gyro.base_gyro import BaseGyro
+from robotpy_toolkit_7407.sensors.gyro.base_gyro import BaseGyro
+from robotpy_toolkit_7407.sensors.gyro.swerve_gyro import SwerveGyro

robotpy_toolkit_7407/sensors/gyro/swerve_gyro.py

@@ -0,0 +1,8 @@
+from robotpy_toolkit_7407.sensors.gyro.base_gyro import BaseGyro
+class SwerveGyro(BaseGyro):
+    """
+    Extendable class for swerve gyro.
+    """
+
+    def __init__(self):
+        super().__init__()

robotpy_toolkit_7407/subsystem_templates/drivetrain/swerve_drivetrain.py

@@ -116,7 +116,7 @@ def _resolve_angles(target_angle: radians, initial_angle: radians) -> tuple[floa
         :param initial_angle: Initial node sensor angle
         :return: (target_sensor_angle, flipped, flip_sensor_offset)
         """
-
+        # TODO This function only has one client function. Rewrite or integrate into _calculate_swerve_node
         # Actual angle difference in radians
         diff = bounded_angle_diff(initial_angle, target_angle)
 
@@ -131,13 +131,15 @@ def _resolve_angles(target_angle: radians, initial_angle: radians) -> tuple[floa
 
 class SwerveGyro(BaseGyro):
     """
+    LEGACY VERSION OF THIS CLASS. HERE FOR BACKWARDS COMPATIBILITY WITH
+    OLD CODE. PLEASE ONLY UPDATE robotpy_toolkit_7407.sensors.gyro.swerve_gyro
+
     Extendable class for swerve gyro.
     """
 
     def __init__(self):
         super().__init__()
 
-
 class SwerveDrivetrain(Subsystem):
     """
     Swerve Drivetrain Extendable class. Contains driving functions.
@@ -329,6 +331,23 @@ def reset_odometry(self, pose: Pose2d):
     @staticmethod
     def _calculate_swerve_node(node_x: meters, node_y: meters, dx: meters_per_second, dy: meters_per_second,
                                d_theta: radians_per_second) -> (meters_per_second, radians):
+        '''
+        This is a helper method to determine the direction and speed to set the nodes
+        ONLY WORKS WITH A SQUARE ROBOT!!!
+        This should really be a node method.
+        Args:
+            node_x: x Position of a node
+            node_y: y Position of a node
+            dx: change in x desired
+            dy: change in y desired
+            d_theta: change in theta desired
+
+        Returns:
+            a tuple of the magnitude and the direction
+
+        '''
+        # TODO move this function to SwerveNode
+        # TODO Rewrite function so that the robot doesn't need to be square
         tangent_x, tangent_y = -node_y, node_x
         tangent_m = math.sqrt(tangent_x ** 2 + tangent_y ** 2)
         tangent_x /= tangent_m

robotpy_toolkit_7407/tests/double_solenoid_test.py

@@ -0,0 +1,52 @@
+from unittest.mock import MagicMock
+from robotpy_toolkit_7407.pneumatics.pistons.double_solenoid import DoubleSolenoidPiston
+import wpilib
+import pytest
+@pytest.fixture
+def solenoid() -> DoubleSolenoidPiston:
+    # Create a Solenoid, but it has mock
+    # classes for its dependencies
+    solenoid = DoubleSolenoidPiston(1, 2, 3 )
+    solenoid.solenoid = MagicMock()
+
+    return solenoid
+def test_extend(solenoid: DoubleSolenoidPiston ):
+    # setup
+    extend=solenoid.solenoid.set
+
+    # action
+    solenoid.extend()
+
+    # assert
+    extend.assert_called_with(wpilib.DoubleSolenoid.Value.kForward)
+
+def test_retract(solenoid: DoubleSolenoidPiston ):
+    # setup
+    retract=solenoid.solenoid.set
+
+    # action
+    solenoid.retract()
+
+    # assert
+    retract.assert_called_with(wpilib.DoubleSolenoid.Value.kReverse)
+
+def test_toggle(solenoid: DoubleSolenoidPiston ):
+    # setup
+    toggle=solenoid.solenoid.toggle
+
+    # action
+    solenoid.toggle()
+
+    # assert
+    toggle.assert_called_once()
+
+def test_get(solenoid: DoubleSolenoidPiston ):
+    # setup
+    get=solenoid.solenoid.get
+
+    # action
+    value=solenoid.get_value()
+
+    # assert
+    get.assert_called_once()
+

robotpy_toolkit_7407/tests/joystick_test.py

@@ -0,0 +1,37 @@
+import pytest
+import wpilib
+from pytest import MonkeyPatch
+
+from robotpy_toolkit_7407.oi.joysticks import Joysticks, JoystickAxis
+
+
+def test_joysticks() -> None:
+    # setup
+    # action
+    jystks = Joysticks()
+
+    # assert
+    assert jystks.joysticks[0].getPort() == 0
+    assert jystks.joysticks[1].getPort() == 1
+
+
+@pytest.mark.parametrize(("shift", "joystick", "axis"), [
+    (3, 0, 1),
+    (2, 1, 2),
+    (0, 1, 3),
+    (1, 0, 4)
+])
+def test_joystickaxis(shift, joystick, axis, monkeypatch: MonkeyPatch) -> None:
+    # setup
+    def mock_getRawAxis(self, id):
+        return id + shift
+
+    monkeypatch.setattr(wpilib.Joystick, "getRawAxis", mock_getRawAxis)
+    # action
+    jystk = JoystickAxis(controller_id=joystick, axis_id=axis)
+    value = jystk.value
+
+    # assert
+    assert value == axis + shift
+    assert joystick == jystk.controller_id
+    assert axis == jystk.axis_id

robotpy_toolkit_7407/tests/subsystem_test.py

@@ -0,0 +1,10 @@
+from robotpy_toolkit_7407.subsystem import Subsystem
+import commands2
+def subsystem_test()->None:
+    # Setup
+
+    system = Subsystem()
+    system.init()
+    assert type(system) == commands2.SubsystemBase
+
+

robotpy_toolkit_7407/tests/subsystemcommand_test.py

@@ -0,0 +1,20 @@
+from robotpy_toolkit_7407.command import SubsystemCommand
+from robotpy_toolkit_7407.subsystem import Subsystem
+
+
+def test_subsystemcommand()->None:
+    '''
+    A test to make sure that a SubsystemCommand adds the necessary
+    required subsystem.
+
+    Asserts: That the Command has the required subsystem
+
+    '''
+    # Setup
+    my_subsystem=Subsystem()
+
+    # Action
+    my_command = SubsystemCommand(my_subsystem)
+
+    # Assert
+    assert my_subsystem in my_command.getRequirements()

robotpy_toolkit_7407/tests/swerve_drivetrain_test.py

@@ -0,0 +1,87 @@
+import pytest
+from pytest import MonkeyPatch
+
+from robotpy_toolkit_7407.subsystem_templates.drivetrain.swerve_drivetrain import *
+#from robotpy_toolkit_7407.utils.units import s, m, deg, rad, hour, mile, rev, meters, meters_per_second, \
+#    radians_per_second, radians
+
+# def test_swerveNode_set()->None:
+#     #TODO Finish finding the right assert
+#     # Setup
+#     node=SwerveNode()
+#
+#     # Action
+#     node.set(vel=10, angle_radians=1.2)
+#
+#     # Assert
+#     assert True
+@pytest.mark.parametrize("initial_angle, target_angle, answer", [
+    (math.pi/4 , math.pi/3, (math.pi/3, False, 0)),
+    (3, 6, (3-math.pi+3, True, math.pi)),
+     (3, -3, (3.2831853071, False, 0))
+])
+def test_resolve_angles(target_angle, initial_angle, answer)->None:
+    # Setup
+    node=SwerveNode()
+
+    # Action
+
+    result = SwerveNode._resolve_angles(target_angle=target_angle, initial_angle=initial_angle)
+
+    # Assert
+    assert result[2]==answer[2]
+    assert result[1]==answer[1]
+    assert result[0]==pytest.approx(answer[0], abs=.00001)
+
+
+
+def test_get_node_state(monkeypatch: MonkeyPatch)->None:
+    # Setup
+    node=SwerveNode()
+
+    def mock_get_motor_velocity(self):
+        return 8
+    def mock_get_turn_motor_angle(self):
+        return math.pi/4
+    monkeypatch.setattr(SwerveNode, "get_motor_velocity", mock_get_motor_velocity)
+    monkeypatch.setattr(SwerveNode, "get_turn_motor_angle", mock_get_turn_motor_angle)
+
+    # Action
+    state=node.get_node_state()
+    # Assert
+    assert type(state)==SwerveModuleState
+    assert state.speed==8
+    assert state.angle.radians()==pytest.approx(math.pi/4, abs=.0001)
+
+def test_get_node_positions(monkeypatch: MonkeyPatch)->None:
+    # Setup
+    node=SwerveNode()
+
+    def mock_get_drive_motor_traveled_distance(self):
+        return 8
+    def mock_get_turn_motor_angle(self):
+        return math.pi/4
+    monkeypatch.setattr(SwerveNode, "get_drive_motor_traveled_distance", mock_get_drive_motor_traveled_distance)
+    monkeypatch.setattr(SwerveNode, "get_turn_motor_angle", mock_get_turn_motor_angle)
+
+    # Action
+    position=node.get_node_position()
+    # Assert
+    assert type(position)==SwerveModulePosition
+    assert position.distance==8
+    assert position.angle.radians()==pytest.approx(math.pi/4, abs=.0001)
+
+# TODO Parameterize and make more cases
+def test_set_angle(monkeypatch: MonkeyPatch)->None:
+    #setup
+    node=SwerveNode()
+    node.motor_sensor_offset=1
+    def mock_set_motor_angle(self, pos):
+        node.target=pos
+
+    monkeypatch.setattr(SwerveNode, "set_motor_angle", mock_set_motor_angle)
+    #action
+    node._set_angle(-3, 3) # returns (3.2831853071, False, 0)
+
+    #assert
+    assert node.target==pytest.approx(2.2831853071)

robotpy_toolkit_7407/tests/swervegyro_test.py

@@ -0,0 +1,88 @@
+import math
+from unittest.mock import MagicMock
+
+import pytest
+from ctre.sensors import Pigeon2
+from pytest import MonkeyPatch
+
+from robotpy_toolkit_7407.sensors.gyro import SwerveGyro, BaseGyro, PigeonIMUGyro_Wrapper
+
+
+@pytest.fixture
+def pigeon() -> PigeonIMUGyro_Wrapper:
+    gyro = PigeonIMUGyro_Wrapper(3)
+    gyro._gyro = MagicMock()
+    return gyro
+
+
+def test_swerve_gyro() -> None:
+    # setup
+
+    # action
+    gyro = SwerveGyro()
+    # assert
+    assert isinstance(gyro, BaseGyro)
+
+
+def test_pigeon_gyro_wrapper_init(pigeon) -> None:
+    # setup
+    # gyro = PigeonIMUGyro_Wrapper(1)
+    syaw = pigeon._gyro.setYaw
+    # action
+    pigeon.init(10)
+    # assert
+    assert syaw.called_once_with(10 / (2 * math.pi) * 360)
+
+
+def test_pigeon_get_robot_heading(monkeypatch: MonkeyPatch) -> None:
+    # setup
+    gyro = PigeonIMUGyro_Wrapper(3)
+
+    def mock_getYaw(self):
+        return 90
+
+    monkeypatch.setattr(Pigeon2, "getYaw", mock_getYaw)
+
+    # action
+    value = gyro.get_robot_heading()
+    # assert
+    assert value == math.pi / 2
+
+
+def test_pigeon_robot_pitch(monkeypatch: MonkeyPatch) -> None:
+    # setup
+    gyro = PigeonIMUGyro_Wrapper(3)
+
+    def mock_get_Pitch(self):
+        return 90
+
+    monkeypatch.setattr(Pigeon2, "getPitch", mock_get_Pitch)
+
+    # action
+    value = gyro.get_robot_pitch()
+    # assert
+    assert value == math.pi / 2
+
+
+def test_pigeon_robot_roll(monkeypatch: MonkeyPatch) -> None:
+    # setup
+    gyro = PigeonIMUGyro_Wrapper(3)
+
+    def mock_get_Roll(self):
+        return 90
+
+    monkeypatch.setattr(Pigeon2, "getRoll", mock_get_Roll)
+
+    # action
+    value = gyro.get_robot_roll()
+    # assert
+    assert value == math.pi / 2
+
+
+def test_reset_angle(pigeon) -> None:
+    # setup
+    setyaw = pigeon._gyro.setYaw
+    # action
+    pigeon.reset_angle(math.pi / 6)
+    # assert
+    assert setyaw.called_once_with(30)