controller

This is the only node which interacts with the expansion board (robot). It is responsible for controlling the robot’s movement and arm movements.

Subscribed topics

  • vr_drive (VRDrive) - The drive data from the VR headset

  • vr_arm (VRArm) - The hand data from the VR headset to control the robotic arm

  • vr_mode (VRMode) - The mode data from the VR headset

  • vr_screw (VRScrew) - The screw data from the VR headset

Published topics

  • switch_camera (SwitchCamera) - Switches the camera view depending on the mode

  • arm_angles (ArmAngles) - The angles of the arm joints to publish to the digital twin

  • _robot_data (RobotData) - Data from the robot such as voltage, speed, gyroscope, etc.

  • message (Message) - Message to be logged including level.

controller node

class robot.src.controller.controller.controller_node.ControllerNode(*args: Any, **kwargs: Any)

Interacts with the Expansion board.

get_robot_data() → None

Gets robot data such as voltage, speed, gyroscope, etc. and publishes it.

_switch_to_forward_camera() → None

Switches to the forward camera.

_switch_to_reverse_camera() → None

Switches to the reverse camera.

handle_vr_drive(msg) → None

Handles VRDrive messages.

Parameters:

msg – VRDrive message

get_arm_angles() → None

Gets the arm angles and publishes them.

robot.src.controller.controller.controller_node.main(args=None)

controller

class robot.src.controller.controller.controller.Controller(production: bool = True)
_convert_coordinates_to_direction(x: float, y: float) → Direction

Converts x, y coordinates to a direction.

Parameters:

  • x – x coordinate

  • y – y coordinate

Returns:

direction to drive in

static convert_x_to_angle_difference(x: float) → int

Converts x to angle difference.

Parameters:

x – x

Returns:

angle difference

static convert_y_to_angle_difference(y: float) → int

Converts y to angle difference.

Parameters:

y – y

Returns:

angle difference

static convert_pinch_to_angle(pinch: float) → int

Converts pinch to angle.

Parameters:

pinch – pinch

Returns:

angle

_set_last_arm_mode(mode: int) → None

Sets the last arm mode.

Parameters:

mode – mode

_set_mode_defaults(mode: int) → None

Sets the mode defaults.

Parameters:

mode – mode

check_last_message_received() → None

Checks if last message was received more than sleep_mode_after seconds ago.

If so, the robot will stop and sleep. Will long beep twice to indicate this. Function gets called by rclpy timer with the hertz specified in the config file.

static _speed_out_of_range(speed: int) → bool

Checks if the speed is out of range.

Parameters:

speed – speed

Returns:

True if the speed is out of range

static _convert_value_to_angle(x: float) → int

Converts x to angle in degrees.

Parameters:

x – x

Returns:

angle in degrees

static _convert_to_point(angle: int, y: float) → tuple[float, float]

Converts angle and y to x and y coordinates.

Parameters:

  • angle – angle

  • y – y

Returns:

x and y coordinates

static _convert_coordinates(y: float, z: float) → tuple[float]

Converts y and z coordinates from [-1, 1] to cm.

Parameters:

  • y – y

  • z – z

Returns:

y and z coordinates in cm

static _would_collide_with_front(x: float, y: float, z: float) → bool

Checks if the point would collide with the front of the robot.

Parameters:

  • x – x coordinate

  • y – y coordinate

  • z – z coordinate

Returns:

True if the point would collide with the front of the robot

_is_illegal_point(x: float, y: float, z: float) → bool

Checks if the point is illegal.

Parameters:

  • x – x coordinate

  • y – y coordinate

  • z – z coordinate

Returns:

True if the point is illegal

handle_vr_arm(msg) → None

Handles VRArm messages.

Parameters:

msg – VRArm message

handle_vr_screw(msg) → None

Handles VRScrew messages.

Parameters:

msg – VRScrew message

static _get_reverse_direction(direction: Direction) → Direction

Converts the direction to reverse mode direction.

Parameters:

direction – direction

Returns:

reverse mode direction

static _is_within_origin_buffer(x: float, y: float) → bool

Checks if the x, y coordinates are within the origin buffer.

Parameters:

  • x – x coordinate

  • y – y coordinate

Returns:

True if the x,y coordinates are within the origin buffer

static _is_in_first_quadrant(x: float, y: float) → bool

Checks if the x, y coordinates are in the first quadrant.

Parameters:

  • x – x coordinate

  • y – y coordinate

Returns:

True if the x,y coordinates are in the first quadrant

static _is_in_second_quadrant(x: float, y: float) → bool

Checks if the x, y coordinates are in the second quadrant.

Parameters:

  • x – x coordinate

  • y – y coordinate

Returns:

True if the x,y coordinates are in the second quadrant

static _is_in_third_quadrant(x: float, y: float) → bool

Checks if the x, y coordinates are in the third quadrant.

Parameters:

  • x – x coordinate

  • y – y coordinate

Returns:

True if the x,y coordinates are in the third quadrant

static _is_in_fourth_quadrant(x: float, y: float) → bool

Checks if the x, y coordinates are in the fourth quadrant.

Parameters:

  • x – x coordinate

  • y – y coordinate

Returns:

True if the x,y coordinates are in the fourth quadrant

static _is_right(x: float, y: float) → bool

Checks if the x, y coordinates are in the right side.

Parameters:

  • x – x coordinate

  • y – y coordinate

Returns:

True if the x,y coordinates are in the right side

static _is_left(x: float, y: float) → bool

Checks if the x, y coordinates are in the left side.

Parameters:

  • x – x coordinate

  • y – y coordinate

Returns:

True if the x,y coordinates are in the left side

static _is_forward(x: float, y: float) → bool

Checks if the x, y coordinates are in the forward side.

Parameters:

  • x – x coordinate

  • y – y coordinate

Returns:

True if the x,y coordinates are in the forward side

static _is_backward(x: float, y: float) → bool

Checks if the x, y coordinates are in the backward side.

Parameters:

  • x – x coordinate

  • y – y coordinate

Returns:

True if the x,y coordinates are in the backward side

_get_precision_direction(x: float, y: float) → Direction

Converts the direction to precision mode direction.

Parameters:

direction – direction

Returns:

precision mode direction

_set_speed(speed: int) → None

Sets the speed of the robot.

Parameters:

speed – speed

vr_drive(x: float, y: float, speed: int, drive_mode: str) → None

Drives the robot based on ROS message input.

Parameters:

  • x – x VR hand coordinate

  • y – y VR hand coordinate

  • speed – speed (length of the xy vector)

  • drive_mode – drive mode (normal, precision, reverse)

handle_vr_mode(msg) → None

Handles VRMode messages.

Parameters:

msg – VRMode message

robot

class robot.src.controller.controller.robot.Robot(production: bool = True)
get_stored_angles() → dict
_reset_arm() → None

Resets the arm to its default position.

reset() → None

Resets the robot.

get_speed() → int

Returns the speed.

Returns:

speed

get_direction(direction: Direction) → list[int]

Returns the direction vector.

Parameters:

direction – direction

Returns:

direction vector

Example

>>> from controller.robot import Robot, Direction
>>> robot = Robot()
>>> robot.set_speed(50)
>>> robot.get_direction(Direction.FORWARD)
[50, 50, 50, 50]
set_speed(speed: int) → None

Sets the speed.

Parameters:

speed – speed

drive(direction: Direction | tuple[int]) → None

Drives the robot in a direction.

Parameters:

direction – direction

stop() → None

Stops the robot.

forward() → None

Drives the robot forward.

set_pinch(angle: int) → None

Sets the pinch angle.

Parameters:

angle – angle

pinch() → None

Pinches the “fingers”.

unpinch() → None

Unpinches the “fingers”.

get_wrist() → int

Returns the wrist angle.

Returns:

wrist angle

set_wrist(angle: int) → None

Turns the wrist. Angle should be between 0 and 270.

Parameters:

angle – angle

reset_wrist() → None

Resets the wrist to its default position.

_screw(clockwise: bool, timeout: int = 1) → None

Screws the “fingers” in a direction.

Parameters:

  • clockwise – if True, the fingers will move clockwise

  • timeout – timeout

handle_screw(clockwise: bool, rounds: int) → None

Screws the “fingers” in a direction.

Parameters:

  • clockwise – if True, the fingers will move clockwise

  • rounds – number of rounds

get_arm_rotation() → int

Returns the arm rotation angle.

Returns:

arm rotation angle

set_arm_rotation(angle: int) → None

Sets the arm rotation angle.

Parameters:

angle – angle

reset_arm_rotation() → None

Resets the arm rotation to its default position.

get_arm_shoulder() → int

Returns the arm shoulder angle.

Returns:

arm shoulder angle

set_arm_shoulder(angle: int) → None

Sets the arm shoulder angle.

Parameters:

angle – angle

set_arm_rotation_difference(angle: int) → None

Sets the arm rotation angle difference.

Parameters:

angle – angle

reset_arm_shoulder() → None

Resets the arm shoulder to its default position.

get_arm_elbow() → int

Returns the arm elbow angle.

Returns:

arm elbow angle

set_arm_elbow(angle: int) → None

Sets the arm elbow angle.

Parameters:

angle – angle

reset_arm_elbow() → None

Resets the arm elbow to its default position.

get_arm_tilt() → int

Returns the arm tilt angle.

Returns:

arm tilt angle

set_arm_tilt(angle: int) → None

Sets the arm tilt angle.

Parameters:

angle – angle

reset_arm_tilt() → None

Resets the arm tilt to its default position.

beep() → None

Beeps the robot for 100ms, used for indicating mode change.

long_beep() → None

Beeps the robot for 200ms, used for indicating robot is ready.

static _estimate_battery_percentage(voltage: float) → int

Estimates the battery percentage based on the voltage.

Parameters:

voltage – battery voltage

Returns:

battery percentage

static _estimate_cms_speed(speed: int) → float

Estimates the cm/s speed

Parameters:

speed – speed in percentage

Returns:

cm/s speed

get_data() → dict

Returns the robot data.

Returns:

robot data

__del__() → None

Cleans up the robot.

arm kinematics

robot.src.controller.controller.arm_kinematics.calculate_auxilaries(partial_point_y: float, partial_point_z: float) → tuple[float]

Calculates auxiliary angles alpha and beta between triangle delta_squared and the two links.

Parameters:

  • partial_point_y – partial point y

  • partial_point_z – partial point z

Returns:

tuple of alpha and beta in radians with the last element being None or np.nan

robot.src.controller.controller.arm_kinematics.calculate_elbow_up_degrees(partial_point_y: float, partial_point_z: float, phi: float) → tuple[float]

Calculates the up angles in degrees.

Parameters:

  • partial_point_y – partial point y

  • partial_point_z – partial point z

  • phi – angle in radians

Returns:

tuple of theta_1, theta_2, theta_3 in degrees

robot.src.controller.controller.arm_kinematics.calculate_elbow_down_degrees(partial_point_y: float, partial_point_z: float, phi: float) → tuple[float]

Calculates the down angles in degrees.

Parameters:

  • partial_point_y – partial point y

  • partial_point_z – partial point z

  • phi – angle in radians

Returns:

tuple of theta_1, theta_2, theta_3 in degrees

robot.src.controller.controller.arm_kinematics.has_any_nan(thetas: tuple[float]) → bool

Checks if any of the thetas are np.nan.

Parameters:

thetas – tuple of thetas in degrees

Returns:

True if any of the thetas are np.nan, False otherwise

robot.src.controller.controller.arm_kinematics.has_out_of_range(thetas: tuple[float]) → bool

Checks if any of the thetas are out of range. The range is -90 to 90 degrees for all joints except the first one which is 0 to 180 degrees.

Parameters:

thetas – tuple of thetas in degrees

Returns:

True if any of the thetas are out of range, False otherwise

robot.src.controller.controller.arm_kinematics.get_mapped_phi(z: float) → float

Maps the z value to a phi value in degrees.

Parameters:

z – z value in cm

Returns:

phi value in radians

robot.src.controller.controller.arm_kinematics.calculate(y: float, z: float, phi: float = None) → tuple[float | None]

Calculates the angles of the robotic arm.

Parameters:

  • y – y value in cm

  • z – z value in cm

  • phi – angle in degrees

Returns:

tuple of theta_1, theta_2, theta_3 in degrees

robot.src.controller.controller.arm_kinematics.convert_thetas_to_servo_angles(thetas: tuple[float]) → tuple[int]

Converts the thetas to servo angles.

Parameters:

thetas – tuple of thetas in degrees

Returns:

tuple of servo angles in degrees

exceptions

exception robot.src.controller.controller.exceptions.ControllerException

Generic exception for the controller module.

exception robot.src.controller.controller.exceptions.NotInProductionMode

Raised when a method requires production mode to be enabled.

Production mode indicates if the expasion board should be used or not.

enums

class robot.src.controller.controller.enums.Direction(value, names=<not given>, *values, module=None, qualname=None, type=None, start=1, boundary=None)

Direction enum.

FORWARD = (1, 1, 1, 1)
BACKWARD = (-1, -1, -1, -1)
STOP = (0, 0, 0, 0)
TURN_LEFT = (0, 0, 1, 1)
TURN_RIGHT = (1, 1, 0, 0)
REVERSE_TURN_LEFT = (-1, -1, 0, 0)
REVERSE_TURN_RIGHT = (0, 0, -1, -1)
LATERAL_LEFT = (-1, 1, 1, -1)
LATERAL_RIGHT = (1, -1, -1, 1)
DIAGONAL_LEFT = (0, 1, 1, 0)
DIAGONAL_RIGHT = (1, 0, 0, 1)
REVERSE_DIAGONAL_LEFT = (0, -1, -1, 0)
REVERSE_DIAGONAL_RIGHT = (-1, 0, 0, -1)
class robot.src.controller.controller.enums.Preset(value, names=<not given>, *values, module=None, qualname=None, type=None, start=1, boundary=None)

Preset enum.

SPEED = 50
UNPINCH_ANGLE = 45
PINCH_ANGLE = 180
WRIST_ANGLE = 100
ARM_TILT_ANGLE = 90
ARM_ROTATION_ANGLE = 90
ARM_SHOULDER_ANGLE = 90
ARM_ELBOW_ANGLE = 90
class robot.src.controller.controller.enums.Arm(value, names=<not given>, *values, module=None, qualname=None, type=None, start=1, boundary=None)

Arm enum.

ROTATION = 1
SHOULDER = 2
ELBOW = 3
TILT = 4
WRIST = 5
PINCH = 6

utils

robot.src.controller.controller.utils.in_production_mode(func)

Decorator which checks if we are in production mode or not.

If we are in production mode, the function will be executed. If not, the NotInProductionMode exception will be raised.

Parameters:

func – function to decorate

Returns:

wrapper function

Return type:

function_wrapper

robot.src.controller.controller.utils.set_last_message(func)

Decorator which sets the last message time and is_sleeping to False.

Parameters:

func – function to decorate

Returns:

wrapper function

Return type:

function_wrapper

robot.src.controller.controller.utils.fill_vector_msg(msg, key: str, data: list)

Fills a std_msgs.msg.Vector3 message for given key.

Parameters:

  • msg – message

  • key – key to fill for

  • data – x, y, z data

Returns:

message with filled data

robot.src.controller.controller.utils.get_config() → dict

Returns the config from a local JSON file.

JSON files do not get compiled, so values can be changed without recompiling the code. Hence the reason for having some options in a JSON file.

Returns:

config

robot.src.controller.controller.utils.get_threshold() → int

Returns the threshold.

Returns:

threshold

robot.src.controller.controller.utils.get_production() → bool

Returns if we are in production mode or not.

Production mode means that the robot will interact with the expansion board (true) or not (false).

Returns:

production mode

robot.src.controller.controller.utils.get_data_hertz() → int

Returns the hertz of which robot data should be sent to the VR headset.

Returns:

hertz

robot.src.controller.controller.utils.get_sleep_mode_hertz() → int

Returns the hertz of which how often the robot should check.

Returns:

hertz

robot.src.controller.controller.utils.get_sleep_mode_after() → int

Returns the time after which the robot should go to sleep mode.

Returns:

seconds

rosmaster

class robot.src.controller.controller.rosmaster.Rosmaster(car_type: int = 1, com: str = '/dev/expbrd', delay: float = 0.002, debug: bool = False)

This is a class for controlling the robot. Which is a modified version of yahboom.net’s code.

__uart_state = 0
__parse_data(ext_type: int, ext_data) → None

Parses the data.

Parameters:

  • ext_type – the type of the data

  • ext_data – the data

set_data() → None

Receives the data.

__request_data(function, param: int = 0) → None

Requests the data.

Parameters:

  • function – the function

  • param – the parameter

__arm_convert_value(s_id: int, s_angle: int) → int

Converts the value of the arm.

Parameters:

  • s_id – the id of the arm

  • s_angle – the angle of the arm

Returns:

the value of the arm

static _arm_convert_angle(s_id: int, s_value: int) → int

Converts the angle of the arm.

Parameters:

  • s_id – the id of the arm

  • s_value – the value of the arm

Returns:

the angle of the arm

static _clamp_motor_value(value: int) → int

Limits (clamps) the motor value.

Parameters:

value – the value

Returns:

the clamped value of the motor between -100 and 100

create_receive_threading() → None

Creates the receive threading.

set_auto_report_state(enable: bool, forever: bool = False) → None

Sets the auto report state.

Parameters:

  • enable – if True, the auto report state is enabled

  • forever – if True, the auto report state is permanent

set_beep(on_time: int) → None

Sets the beep (buzzer).

Parameters:

on_time – the time the beep is on

set_pwm_servo(servo_id: int, angle: int) → None

Sets the PWM servo.

Parameters:

  • servo_id – the id of the servo between 1 and 4

  • angle – the angle of the servo between 0 and 180

set_pwm_servo_all(angle_s1: int, angle_s2: int, angle_s3: int, angle_s4: int) → None

Sets the PWM signal for all servos. Angle values are between 0 and 180.

Parameters:

  • angle_s1 – the angle of the first servo

  • angle_s2 – the angle of the second servo

  • angle_s3 – the angle of the third servo

  • angle_s4 – the angle of the fourth servo

set_colorful_lamps(led_id: int, red: int, green: int, blue: int) → None

Sets the colorful lamps. Color values between 0 and 255.

Parameters:

  • led_id – the id of the led

  • red – the red value

  • green – the green value

  • blue – the blue value

set_colorful_effect(effect: int, speed: int = 255, parm: int = 255) → None

Sets the colorful effect.

Parameters:

  • effect – the effect between 0 and 6

  • speed – the speed between 1 and 10

  • parm – the parameter between 0 and 6

set_motor(speed_1: int, speed_2: int, speed_3: int, speed_4: int) → None

Send PWM pulse to each of the motors. Speeds are between -100 and 100.

Parameters:

  • speed_1 – the speed of motor 1

  • speed_2 – the speed of motor 2

  • speed_3 – the speed of motor 3

  • speed_4 – the speed of motor 4

set_car_run(state: int, speed: int, adjust: bool = False) → None

Control the car to move forward, backward, left, right, etc.

Parameters:

  • state – the state of the car between 0 and 7

  • speed – the speed of the car between -100 and 100

  • adjust – if True, the gyroscope auxiliary motion direction is activated

set_car_motion(v_x: float, v_y: float, v_z: float) → None

Control the car movement.

Parameters:

  • v_x – the speed of the car in the x direction

  • v_y – the speed of the car in the y direction

  • v_z – the speed of the car in the z direction

set_pid_param(kp: int, ki: int, kd: int, forever: bool = False) → None

Sets the PID parameters.

Parameters:

  • kp – the proportional gain

  • ki – the integral gain

  • kd – the derivative gain

  • forever – if True, the PID parameters are permanent

set_car_type(car_type: int) → None

Sets the car type.

Parameters:

car_type – the type of the car

set_uart_servo(servo_id: int, pulse_value: int, run_time: int = 500) → None

Sets the position of one of the servos (on the arm).

Parameters:

  • servo_id – the id of the servo

  • pulse_value – the position of the servo

  • run_time – the running time in milliseconds

set_uart_servo_angle(s_id: int | Arm, s_angle: int, run_time: int = 500) → None

Sets the angle of one of the servos (on the arm). Degrees: 1-4, 6:[0, 180], 5:[0, 270]

1 - Rotation of the arm 2 - Shoulder of the arm 3 - Elbow of the arm 4 - Tilt of the arm 5 - Wrist of the arm (rotation) 6 - Fingers of the arm (pinch)

Parameters:

  • s_id – the id of the servo

  • s_angle – the angle of the servo

  • run_time – the running time in milliseconds

set_uart_servo_id(servo_id: int) → None

Sets the ID of the bus servo.

Parameters:

servo_id – the id of the servo

set_uart_servo_torque(enable: bool) → None

Sets the torque of the bus servo.

Parameters:

enable – if 1, the torque is enabled

set_uart_servo_ctrl_enable(enable: bool) → None

Sets the control switch of the manipulator.

Parameters:

enable – if True, the control protocol is normally sent

set_uart_servo_angle_array(angle_s: list[int] = [90, 90, 90, 90, 90, 180], run_time: int = 500) → None

Sets the angle of all the servos in the arm.

Parameters:

  • angle_s – the angle of the servos

  • run_time – the running time in milliseconds

set_uart_servo_offset(servo_id: int) → int

Sets the offset of the servo.

Parameters:

servo_id – the id of the servo

set_akm_default_angle(angle: int, forever: bool = False) → None

Sets the default angle of the front wheel of the ackerman type (R2) car.

Parameters:

  • angle – the angle between 60 and 120

  • forever – if True, the angle is permanently saved

set_akm_steering_angle(angle: int, ctrl_car: bool = False) → None

Controls the steering angle of the ackerman type (R2) car.

Parameters:

  • angle – the angle between -45 and 45

  • ctrl_car – if True, the steering gear angle and the speed of the left and

  • modified (right motors are)

reset_flash_value() → None

Resets the car flash saved data, restore the factory default value.

reset_car_state() → None

Resets the car status, including parking, lights off, buzzer off.

clear_auto_report_data() → None

Clears the cache data automatically sent by the MCU.

get_akm_default_angle() → int

Reads the default angle of the front wheel of the ackerman type (R2) car.

get_uart_servo_value(servo_id: int) → tuple[int]

Reads the position parameters of the bus servo.

Parameters:

servo_id – the id of the servo

get_uart_servo_angle(s_id: int | Arm) → int

Reads the angle of one of the servos (on the arm).

Parameters:

s_id – the id of the servo

get_uart_servo_angle_array() → list[int]

Reads the angles of all the servos (on the arm).

get_accelerometer_data() → tuple[float]

Get the accelerometer triaxial data, return a_x, a_y, a_z.

get_gyroscope_data() → tuple[float]

Get the gyro triaxial data, return g_x, g_y, g_z.

get_magnetometer_data() → tuple[float]

Get the magnetometer triaxial data, return m_x, m_y, m_z.

get_imu_attitude_data(to_angle: bool = True) → tuple[float]

Get the board attitude Angle, return yaw, roll, pitch.

Parameters:

to_angle – if True, return the Angle; if False, return the radian

get_motion_data() → tuple[float]

Get the car speed, val_vx, val_vy, val_vz.

get_battery_voltage() → float

Get the battery voltage.

get_motor_encoder() → tuple[int]

Obtain data of four-channel motor encoder.

get_motion_pid() → list[int]

Get the motion PID parameters of the dolly and return [kp, ki, kd]

get_car_type_from_machine() → int

Gets the current car type from machine.

get_version() → float

Get the underlying microcontroller version number.