Skip to content

Commit

Permalink
add some toolkit files
Browse files Browse the repository at this point in the history
  • Loading branch information
e-bauman committed Jul 31, 2024
1 parent 58b2b23 commit 48263e2
Show file tree
Hide file tree
Showing 15 changed files with 1,353 additions and 0 deletions.
24 changes: 24 additions & 0 deletions toolkit/command.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,24 @@
from typing import Generic, TypeVar
import commands2

from toolkit.subsystem import Subsystem

T = TypeVar("T", bound=Subsystem)


class BasicCommand(commands2.Command):
"""
Extendable basic command
"""
...


class SubsystemCommand(BasicCommand, Generic[T]):
"""
Extendable subsystem command
"""

def __init__(self, subsystem: T):
super().__init__()
self.subsystem = subsystem
self.addRequirements(subsystem)
85 changes: 85 additions & 0 deletions toolkit/sensors/gyro/Pigeon2.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,85 @@
import phoenix6
import math

from units.SI import radians, radians_per_second
from toolkit.sensors.gyro.base_gyro import BaseGyro


class Pigeon2(BaseGyro):
"""
Wrapper class for the Pigeon2 IMU gyro.
"""

def __init__(self, port):
"""
Args:
port (int): CAN ID of the Pigeon gyro
"""
self._gyro = phoenix6.hardware.Pigeon2(port)

def init(self, gyro_start_angle=0):
"""
Initialize gyro
"""
self.reset_angle(gyro_start_angle)

def get_robot_heading(self) -> radians:
"""
Returns the angle of the robot's heading in radians (yaw)
:return: Robot heading (radians)
"""
return math.radians(self._gyro.get_yaw().value)

def get_robot_heading_rate(self) -> radians_per_second:
"""
Returns the rate of the robot's heading in radians per second
:return: Robot heading (radians)
"""
return math.radians(self._gyro.get_angular_velocity_z_world().value)

def get_robot_pitch(self) -> radians:
"""
Returns the angle of the robot's pitch in radians
:return: Robot pitch (radians)
"""
return math.radians(self._gyro.get_pitch().value)

def get_robot_pitch_rate(self) -> radians_per_second:
"""
Returns the rate of the robot's pitch in radians per second
:return: Robot pitch rate (radians per second)
"""
return math.radians(self._gyro.get_angular_velocity_y_world().value)

def get_robot_roll(self) -> radians:
"""
Returns the angle of the robot's roll in radians
:return: Robot roll (radians)
"""
return math.radians(self._gyro.get_roll().value)

def get_robot_roll_rate(self) -> radians_per_second:
"""
Returns the rate of the robot's roll in radians per second
:return: Robot roll (radians)
"""
return math.radians(self._gyro.get_angular_velocity_x_world().value)

def reset_angle(self, angle: radians = 0):
"""
Resets the gyro's yaw.
"""
self._gyro.set_yaw(math.degrees(angle))

def get_x_accel(self):

return self._gyro.get_acceleration_x().value

def get_y_accel(self):

return self._gyro.get_acceleration_y().value

def get_z_accel(self):

return self._gyro.get_acceleration_z().value

2 changes: 2 additions & 0 deletions toolkit/sensors/gyro/__init__.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
from toolkit.sensors.gyro.base_gyro import BaseGyro
from toolkit.sensors.gyro.Pigeon2 import Pigeon2
38 changes: 38 additions & 0 deletions toolkit/sensors/gyro/base_gyro.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,38 @@
from units.SI import radians


class BaseGyro:
"""
Extendable class for gyro.
"""

def init(self, gyro_start_angle=0):
"""
Initialize the gyro. Overridden class.
"""
...

def get_robot_heading(self) -> radians:
"""
Get the robot heading in radians. Overridden class. Must return radians.
"""
...

def get_robot_pitch(self) -> radians:
"""
Returns the angle of the robot's pitch in radians. Overridden class. Must return radians.
:return: Robot pitch (radians)
"""
...

def get_robot_roll(self) -> radians:
"""
Returns the angle of the robot's roll in radians. Overridden class. Must return radians.
:return: Robot roll (radians)
"""

def reset_angle(self, angle: radians = 0):
"""
Reset the robot heading. Overridden class.
"""
...
1 change: 1 addition & 0 deletions toolkit/sensors/odometry/__init__.py
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
from toolkit.sensors.odometry.vision_estimator import VisionEstimator
18 changes: 18 additions & 0 deletions toolkit/sensors/odometry/vision_estimator.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,18 @@
from wpimath.geometry import Pose3d


class VisionEstimator:
"""
An estimator (e.g. limelight, photon-vision) that returns a list of robot poses relative to the field.
"""

def __init__(self):
pass

def get_estimated_robot_pose(self) -> list[tuple[Pose3d, float, float, float]] | None:
"""
Returns the robot's pose relative to the field, estimated by the vision system. Override this method.
:return: Vision system estimate of robot pose along with the associated timestamp.
:rtype: list[Pose3d, seconds: float] | None
"""
raise NotImplementedError
2 changes: 2 additions & 0 deletions toolkit/subsystem_templates/drivetrain/__init__.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
from toolkit.subsystem_templates.drivetrain.swerve_drivetrain import SwerveDrivetrain, SwerveNode
from toolkit.subsystem_templates.drivetrain.swerve_drivetrain_commands import DriveSwerve, FollowPath
Loading

0 comments on commit 48263e2

Please sign in to comment.