Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 0 additions & 4 deletions external_samples/color_range_sensor.py
Original file line number Diff line number Diff line change
Expand Up @@ -51,10 +51,6 @@ def get_url(self) -> str:
def get_version(self) -> tuple[int, int, int]:
return (1, 0, 0)

def stop(self) -> None:
# send stop command to sensor
pass

def reset(self) -> None:
pass

Expand Down
7 changes: 6 additions & 1 deletion external_samples/component.py
Original file line number Diff line number Diff line change
Expand Up @@ -71,8 +71,13 @@ def get_url(self) -> str:
def get_version(self) -> tuple[int, int, int]:
pass

def start(self) -> None:
pass

def update(self) -> None:
pass

# This stops all movement (if any) for the component
@abstractmethod
def stop(self) -> None:
pass

Expand Down
123 changes: 123 additions & 0 deletions external_samples/expansion_hub_motor.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,123 @@
# Copyright 2025 Google LLC
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# https://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

"""This is the expansion_hub_motor module.

This component wraps the expansion_hub.ExpansionHubMotor class, providing
support for a motor connected to a REV Expansion Hub.
"""

__author__ = "lizlooney@google.com (Liz Looney)"

from typing import Self
from component import Component, PortType, InvalidPortException
import expansion_hub
import wpimath

# TODO(lizlooney): Update port types.

class ExpansionHubMotor(Component):
def __init__(self, ports : list[tuple[PortType, int]]):
port_type, hub_number = ports[0]
if port_type != PortType.USB_PORT:
raise InvalidPortException
port_type, motor_number = ports[1]
if port_type != PortType.USB_PORT:
raise InvalidPortException
self.expansion_hub_motor = expansion_hub.ExpansionHubMotor(hub_number, motor_number)

def get_manufacturer(self) -> str:
return "REV Robotics"

def get_name(self) -> str:
return "Expansion Hub Motor"

def get_part_number(self) -> str:
return ""

def get_url(self) -> str:
return ""

def get_version(self) -> tuple[int, int, int]:
return (1, 0, 0)

def start(self) -> None:
self.expansion_hub_motor.setEnabled(True)

def stop(self) -> None:
# TODO: Send stop command to motor.
pass

def reset(self) -> None:
pass

def get_connection_port_type(self) -> list[PortType]:
return [PortType.USB_PORT, PortType.USB_PORT]

def periodic(self) -> None:
pass

# Alternative constructor to create an instance from a hub number and a motor port.
@classmethod
def from_hub_number_and_motor_number(cls: type[Self], hub_number: int, motor_number: int) -> Self:
return cls([(PortType.USB_PORT, hub_number), (PortType.USB_PORT, motor_number)])

# Component specific methods

# Methods from expansion_hub.ExpansionHubMotor

def setPercentagePower(self, power: float):
self.expansion_hub_motor.setPercentagePower(power)

def setVoltage(self, voltage: wpimath.units.volts):
self.expansion_hub_motor.setVoltage(voltage)

def setPositionSetpoint(self, setpoint: float):
self.expansion_hub_motor.setPositionSetpoint(setpoint)

def setVelocitySetpoint(self, setpoint: float):
self.expansion_hub_motor.setVelocitySetpoint(setpoint)

def setEnabled(self, enabled: bool):
self.expansion_hub_motor.setEnabled(enabled)

def setFloatOn0(self, floatOn0: bool):
self.expansion_hub_motor.setFloatOn0(floatOn0)

def getCurrent(self) -> float:
return self.expansion_hub_motor.getCurrent()

def setDistancePerCount(self, perCount: float):
self.expansion_hub_motor.setDistancePerCount(perCount)

def isHubConnected(self) -> bool:
return self.expansion_hub_motor.isHubConnected()

def getEncoder(self) -> float:
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I thought encoder ticks were int instead of float

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It's float in the class from Thad. See

return self.expansion_hub_motor.getEncoder()

def getEncoderVelocity(self) -> float:
return self.expansion_hub_motor.getEncoderVelocity()

def setReversed(self, reversed: bool):
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do we need a getReversed as well?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Probably, but it will need to be added here

self.expansion_hub_motor.setReversed(reversed)

def resetEncoder(self):
self.expansion_hub_motor.resetEncoder()

def getVelocityPidConstants(self) -> expansion_hub.ExpansionHubPidConstants:
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do we need a set as well?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Maybe. It will need to be added here as well. We should talk to Thad.

return self.expansion_hub_motor.getVelocityPidConstants()

def getPositionPidConstants(self) -> expansion_hub.ExpansionHubPidConstants:
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do we need a set as well?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Maybe. It will need to be added here as well. We should talk to Thad.

return self.expansion_hub_motor.getPositionPidConstants()
96 changes: 96 additions & 0 deletions external_samples/expansion_hub_servo.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,96 @@
# Copyright 2025 Google LLC
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# https://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

"""This is the expansion_hub_servo module.

This component wraps the expansion_hub.ExpansionHubServo class, providing
support for a servo connected to a REV Expansion Hub.
"""

__author__ = "lizlooney@google.com (Liz Looney)"

from typing import Self
from component import Component, PortType, InvalidPortException
import expansion_hub

# TODO(lizlooney): Update port types.

class ExpansionHubServo(Component):
def __init__(self, ports : list[tuple[PortType, int]]):
port_type, hub_number = ports[0]
if port_type != PortType.USB_PORT:
raise InvalidPortException
port_type, servo_number = ports[1]
if port_type != PortType.USB_PORT:
raise InvalidPortException
self.expansion_hub_servo = expansion_hub.ExpansionHubServo(hub_number, servo_number)

def get_manufacturer(self) -> str:
return "REV Robotics"

def get_name(self) -> str:
return "Expansion Hub Servo"

def get_part_number(self) -> str:
return ""

def get_url(self) -> str:
return ""

def get_version(self) -> tuple[int, int, int]:
return (1, 0, 0)

def start(self) -> None:
self.expansion_hub_servo.setEnabled(True)
pass

def stop(self) -> None:
# TODO: Send stop command to servo.
pass

def reset(self) -> None:
pass

def get_connection_port_type(self) -> list[PortType]:
return [PortType.USB_PORT, PortType.USB_PORT]

def periodic(self) -> None:
pass

# Alternative constructor to create an instance from a hub number and a servo port.
@classmethod
def from_hub_number_and_servo_number(cls: type[Self], hub_number: int, servo_number: int) -> Self:
return cls([(PortType.USB_PORT, hub_number), (PortType.USB_PORT, servo_number)])

# Component specific methods

# Methods from expansion_hub.ExpansionHubServo

def set(self, value: float):
self.expansion_hub_servo.set(value)

def setAngle(self, degrees: float):
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If this takes degrees, should it be setAngleDegrees?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It's just setAngle here. We can talk about changing it with Thad and others.

self.expansion_hub_servo.setAngle(degrees)

def setEnabled(self, enabled: bool):
self.expansion_hub_servo.setEnabled(enabled)

def isHubConnected(self) -> bool:
return self.expansion_hub_servo.isHubConnected()

def setFramePeriod(self, framePeriod: int):
self.expansion_hub_servo.setFramePeriod(framePeriod)

def setPulseWidth(self, pulseWidth: int):
self.expansion_hub_servo.setPulseWidth(pulseWidth)
3 changes: 0 additions & 3 deletions external_samples/rev_touch_sensor.py
Original file line number Diff line number Diff line change
Expand Up @@ -42,9 +42,6 @@ def get_url(self) -> str:
def get_version(self) -> tuple[int, int, int]:
return (1, 0, 0)

def stop(self) -> None:
pass

def reset(self) -> None:
self.pressed_callback = None
self.released_callback = None
Expand Down
2 changes: 1 addition & 1 deletion external_samples/servo.py
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ def get_version(self) -> tuple[int, int, int]:
return (1, 0, 0)

def stop(self) -> None:
# De-energize servo port
# TODO: De-energize servo port
pass

def reset(self) -> None:
Expand Down
2 changes: 1 addition & 1 deletion external_samples/smart_motor.py
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ def get_version(self) -> tuple[int, int, int]:
return (1, 0, 0)

def stop(self) -> None:
# send stop command to motor
# TODO: send stop command to motor
pass

def reset(self) -> None:
Expand Down
2 changes: 1 addition & 1 deletion external_samples/spark_mini.py
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ def get_version(self) -> tuple[int, int, int]:
return (1, 0, 0)

def stop(self) -> None:
# send stop command to motor
# TODO: send stop command to motor
pass

def reset(self) -> None:
Expand Down
4 changes: 4 additions & 0 deletions python_tools/generate_json.py
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,8 @@
sys.path.append("../external_samples")
import color_range_sensor
import component
import expansion_hub_motor
import expansion_hub_servo
import rev_touch_sensor
import servo
import smart_motor
Expand Down Expand Up @@ -119,6 +121,8 @@ def main(argv):
external_samples_modules = [
color_range_sensor,
component,
expansion_hub_motor,
expansion_hub_servo,
rev_touch_sensor,
servo,
smart_motor,
Expand Down
4 changes: 3 additions & 1 deletion server_python_scripts/run_opmode.py
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
import importlib.util
import inspect
import argparse
import traceback
from pathlib import Path

from blocks_base_classes import OpMode
Expand Down Expand Up @@ -151,6 +152,7 @@ def run_opmode(opmode_file, duration=None, loop_frequency=50):

except Exception as e:
print(f"Error running opmode: {e}")
traceback.print_exc()
sys.exit(1)


Expand Down Expand Up @@ -206,4 +208,4 @@ def main():


if __name__ == '__main__':
main()
main()
Loading