generated from Choate-Robotics/7407-DriveCode-Template-v2
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
15 changed files
with
1,353 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 | ||
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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. | ||
""" | ||
... |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1 @@ | ||
from toolkit.sensors.odometry.vision_estimator import VisionEstimator |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 |
Oops, something went wrong.