diff --git a/command/arm.py b/command/arm.py new file mode 100644 index 0000000..40bb092 --- /dev/null +++ b/command/arm.py @@ -0,0 +1,85 @@ +import math + +import wpilib # noqa +from commands2 import SequentialCommandGroup # noqa + +import config +import constants # noqa +import utils # noqa +from oi.keymap import Controllers # noqa +from subsystem import Arm +from toolkit.command import SubsystemCommand +from units.SI import radians +from enum import Enum + + +# cmds: set arm (angle), zero arm, specific positions that inherit from set arm + +class SetArm(SubsystemCommand[Arm]): + """ + Sets the wrist to a given angle (radians). + param: angle in radians + """ + + def __init__(self, subsystem: Arm, angle: float): + super().__init__(subsystem) + self.subsystem = subsystem + self.angle = angle + + def initialize(self): + # change to actual name + self.subsystem.set_angle(self.angle) + self.subsystem.is_rotating = True + + def execute(self): + pass + + def isFinished(self): + return self.subsystem.is_at_angle(self.angle) + + def end(self, interrupted: bool): + if interrupted: + arm_angle = self.subsystem.get_angle() + + self.subsystem.is_rotating = False + + +class ZeroArm(SubsystemCommand[Arm]): + """ + Zeros the arm. + """ + + def __init__(self, subsystem: Arm): + super().__init__(subsystem) + self.subsystem = subsystem + + def initialize(self): + # change to actual name + self.subsystem.zero_arm() + + def execute(self): + pass + + def isFinished(self): + return self.subsystem.is_zeroed() + + def end(self, interrupted: bool): + if not interrupted: + self.subsystem.arm_zeroed = True + else: + ... + + +class SetSpeakerPosition(SetArm): + def __init__(self, subsystem: Arm): + super().__init__(subsystem, radians(config.speaker_angle)) + + +class SetAmpPosition(SetArm): + def __init__(self, subsystem: Arm): + super().__init__(subsystem, radians(config.amp_angle)) + + +class SetIntakePosition(SetArm): + def __init__(self, subsystem: Arm): + super().__init__(subsystem, radians(config.intake_angle)) diff --git a/config.py b/config.py index 8f6b2e6..031b3ce 100644 --- a/config.py +++ b/config.py @@ -16,4 +16,9 @@ # 3 = ERROR # 4 = SETUP # anything else will log nothing -# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ \ No newline at end of file +# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +# Arm +speaker_angle: float = 20.0 +amp_angle: float = 39.0 +intake_angle: float = 45.0 \ No newline at end of file