Starter Kit

from smd.red import *
from serial.tools.list_ports import comports
from platform import system
import math
import os


#  Serial Port Define
def USB_Port():
    if system() == "Windows":
        ports = list(comports())
        if ports:
            for port, desc, hwid in sorted(ports):
                if 'USB Serial Port' in desc:
                    SMD_Port = port
                    return SMD_Port
        else:
            SMD_Port = None
            return SMD_Port

    elif system() == "Linux":
        ports = list(serial.tools.list_ports.comports())
        if ports:
            for port, desc, hwid in sorted(ports):
                if '/dev/ttyUSB' in port:
                    SMD_Port = port
                    return SMD_Port

    elif system() == "Darwin":  # macOS
        ports = list(serial.tools.list_ports.comports())
        if ports:
            for port, desc, hwid in sorted(ports):
                if '/dev/tty.usbserial' in port or '/dev/tty.usbmodem' in port:
                    SMD_Port = port
                    return SMD_Port

        else:
            SMD_Port = None
            return SMD_Port

port = USB_Port()
m = Master(port)

ID = 0
m.attach(Red(ID))

m.update_driver_id(ID, 1)

ID = 1
m.attach(Red(ID))

CPR = 6533
RPM = 100

m.set_shaft_cpr(ID, CPR)
m.set_shaft_rpm(ID, RPM)
m.set_operation_mode(ID, OperationMode.Velocity)
m.set_control_parameters_velocity(ID, 30.0, 5.0, 0.0)
m.set_control_parameters_position(ID, 0.5, 0.0, 20.0)
m.set_control_parameters_torque(ID, 3.0, 0.1, 0.0)

m.enable_torque(ID, True)

mode = 0

"""
Mode 1 - PWM: Motor rotates at a duty cycle corresponding to the value received from the joystick X axis

Mode 2 - Velocity: The velocity of the motor increases or decreases according to the X and Y axis of the joystick

Mode 3 - Position: The joystick acts like the edges of a circle on a coordinate system. It makes the motor rotate at 
                angle of the joystick.

Mode 4 - Current: The motor will draw a current value less than 50 mA which is set as the limit by the joystick. 
                If something physically interrupts motor from moving.
"""

torque_status = True

motor_speed = 0

current_limit = 100
current_value = 0
previous_current = 0

def cmd_clear():
    if system() == "Windows":
            os.system('cls')
    else:
        os.system('clear')

while True:

    try:
        button = m.get_joystick(ID, 1)[2]

        if button:
            pressed_time = time.time()

            while button:
                current_time = time.time()
                if (current_time - pressed_time) > 1.0:
                    mode += 1
                    mode = mode % 4
                    m.enable_torque(ID, False)
                    time.sleep(0.1)
                    m.enable_torque(ID, True)
                    time.sleep(0.1)
                    m.enable_torque(ID, False)
                    time.sleep(0.1)
                    m.enable_torque(ID, True)
                    break
                    
                button = m.get_joystick(ID, 1)[2]

    except:
        print("Button NoneType Error")

    ### MODE 1 ###
    if mode == 0:

        m.set_operation_mode(ID, OperationMode.PWM)

        if not torque_status:
            m.enable_torque(ID, True)
            torque_status = True

        try:
            joystick_x, joystick_y, button = m.get_joystick(ID, 1)
        except:
            joystick_x, joystick_y, button = [0, 0, 0]

        if joystick_x > 10 or joystick_x < -10:
            motor_speed = joystick_x
            m.set_duty_cycle(ID, -motor_speed)
            
        elif 0 <= joystick_x < 10 or 0 >= joystick_x > -10:
            m.set_duty_cycle(ID, 0)
            motor_speed = joystick_x

        cmd_clear()

        print(f"*** MODE 1 ***\nMotor Duty Cycle: {motor_speed}")


    ### MODE 2 ###
    elif mode == 1:
        
        m.set_operation_mode(ID, OperationMode.Velocity)

        if not torque_status:
            m.enable_torque(ID, True)
            torque_status = True

        try:
            joystick_x, joystick_y, button = m.get_joystick(ID, 1)
        except:
            joystick_x, joystick_y, button = [0, 0, 0]

        if (joystick_x > 50 or joystick_y > 50) and motor_speed < 100:
            motor_speed += 1

        elif (joystick_x < -50 or joystick_y < -50) and motor_speed > -100:
            motor_speed -= 1

        m.set_velocity(ID, -motor_speed)

        cmd_clear()

        print(f"*** MODE 2 ***\nMotor Speed: {motor_speed}")

    
    ### MODE 3 ###
    elif mode == 2:
        
        m.set_operation_mode(ID, OperationMode.Position)
        
        if not torque_status:
            m.enable_torque(ID, True)
            torque_status = True

        try:
            joystick_x, joystick_y, button = m.get_joystick(ID, 1)
        except:
            joystick_x, joystick_y, button = [0, 0, 0]
        
        if (-10 < joystick_x < 10) and (-10 < joystick_y < 10):
            try:
                current_position = m.get_position(ID)
                angle_degrees = current_position * (360/CPR)

            except:
                current_position = 0
                previous_angle = 0

        else:
            x = joystick_x / 100.0
            y = joystick_y / 100.0
            
            angle = math.atan2(y, x)
            
            previous_angle = angle_degrees
            angle_degrees = (math.degrees(angle) + 360) % 360

            if angle_degrees - previous_angle > 180:
                angle_degrees -= 360

            position = angle_degrees * (CPR/360)

            m.set_position(ID, position)

        cmd_clear()

        print(f"*** MODE 3 ***\nMotor Angle: {angle_degrees:.2f}°")


    ### MODE 4 ###
    elif mode == 3:
        
        m.set_operation_mode(ID, OperationMode.Torque)

        try:
            joystick_x, joystick_y, button = m.get_joystick(ID, 1)
        except:
            joystick_x, joystick_y, button = [0, 0, 0]

        if joystick_x > 50 or joystick_y > 50:
            current_limit += 1

        elif joystick_x < -50 or joystick_y < -50:
            current_limit -= 1

        m.set_torque(ID, current_limit - 50)

        try:
            previous_current = current_value
            current_value = m.get_torque(ID)
        except:
            current_value = previous_current

        if current_value >= current_limit:
            m.enable_torque(ID, False)
            torque_status = False
        
        cmd_clear()

        print(f"*** MODE 4 ***\nMotor Current: {current_value:.2f}\tCurrent Limit: {current_limit}")

        if not torque_status:
            print("\nMotor movement disabled due to the current exceed!")