robotModuleFunctions

import time
import matplotlib.pyplot as plt
from collections import deque
from dynamixel_sdk import *
import math
import numpy as np
from pynput import keyboard

# --- Function Definitions ----------------------------------------------------------------
# Signed Conversions:
def to_signed_32(val):
    if val > 2147483647 :  # If greater than (2^31 - 1)
        return val - 4294967296  # Subtract 2^32
    else:   return val
def to_signed_16(val):
    if val > 32767:  # If greater than (2^15 - 1)
        return val - 65536  # Subtract 2^16
    else:   return val

# Read position, velocity, and load from motor:
def read_from_motor(packetHandler, motor_id):
    # Read the current position from the motor
    pos, _, _ = packetHandler.read4ByteTxRx(portHandler, motor_id, ADDR_PRESENT_POSITION)
    # Read the current velocity from the motor
    vel, _, _ = packetHandler.read4ByteTxRx(portHandler, motor_id, ADDR_PRESENT_VELOCITY)
    # Read the load
    load, _, _ = packetHandler.read2ByteTxRx(portHandler, motor_id, ADDR_PRESENT_LOAD)
    
    if pos is None:
        return None, None, None
    if vel is not None:
        # Convert unsigned 32-bit to signed 32-bit
        vel_signed = to_signed_32(vel)       
    if load is not None: 
        # Convert unsigned 16-bit to signed 16-bit
        load_signed = to_signed_16(load)    
    return pos, vel_signed, load_signed

# Normalize function
def normalize(value, min_val, max_val):
    if value < min_val:
        value = min_val
    if value > max_val:
        value = max_val
    return (value - min_val) / (max_val - min_val)

# Binning function
def bin(value, num_bins):
    bin_index = math.ceil(value * num_bins) - 1
    if bin_index < 0:
        bin_index = 0
    if bin_index >= num_bins:
        bin_index = num_bins - 1
    return bin_index

# Convert position and velocity into feature vector X
def featurize(pos, vel):
    # Normalize position and velocity to [0, 1]
    pos_norm = normalize(pos, HAND_POS_1, HAND_POS_2)
    vel_norm = normalize(vel, -MOTOR_VELO, MOTOR_VELO)
    # Determine bin indices
    pos_bin = bin(pos_norm, NUM_POS_BINS)
    vel_bin = bin(vel_norm, NUM_VEL_BINS)
    # Create feature vector
    x = np.zeros(NUM_POS_BINS * NUM_VEL_BINS, dtype=int)
    feature_idx = pos_bin * NUM_VEL_BINS + vel_bin
    x[feature_idx] = 1
    return x

def get_cumulant(load):
    # Convert load into signal of interest (cumulant)
    # c = normalize(load, -MAX_LOAD, MAX_LOAD)
    c = 1 if abs(load) > LOAD_THRESHOLD else 0
    return c

# Kayboard listener for pausing/resuming
def on_press(key):
    global is_paused
    if key == keyboard.Key.space:
        is_paused = not is_paused
        status = "PAUSED" if is_paused else "RESUMED"
        print(f"\n*** {status} ***")
        if is_paused:
            # Tell motor to stop exactly where it is immediately
            pos, _, _ = packetHandler.read4ByteTxRx(portHandler, HAND_ID, ADDR_PRESENT_POSITION)
            packetHandler.write4ByteTxRx(portHandler, HAND_ID, ADDR_GOAL_POSITION, pos)

# Background thread to move the gripper:
def move_logic():
    # This function runs in the background to cycle the hand.
    global running
    while running:
        if is_paused:
            time.sleep(0.1)
            continue
        # Move to Close
        packetHandler.write4ByteTxRx(portHandler, HAND_ID, ADDR_GOAL_POSITION, HAND_POS_1)
        time.sleep(WAIT_TIME)
        # Move to Open
        packetHandler.write4ByteTxRx(portHandler, HAND_ID, ADDR_GOAL_POSITION, HAND_POS_2)
        time.sleep(WAIT_TIME)