Motor Rotation Based on Joystick Counting

This Python code demonstrates a joystick-controlled motor rotation system that enables a user to count joystick inputs and control the motor's rotations accordingly. The application uses a USB-connected motor controller to read joystick inputs and perform precise motor movements based on the input count. Below is a step-by-step explanation of the code.

Required Libraries and Modules

import serial
from smd.red import Master, Red
from threading import Thread
import time
from serial.tools.list_ports import comports
from platform import system

serial: Handles USB serial communication with the motor controller.
smd.red: Provides classes (Master and Red) for interacting with the motor controller and motor.
threading.Thread: Enables multi-threading, allowing joystick input to be handled in parallel with other operations.
time: Used for timing and delays.
serial.tools.list_ports: Lists available USB ports on the system.
platform.system: Detects the operating system (Windows, Linux, or macOS) for platform-specific USB handling.

USB Port Detection Function

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

    elif system() == "Linux":
        ports = list(serial.tools.list_ports.comports())
        if ports:
            for port, desc, hwid in sorted(ports):
                if '/dev/ttyUSB' in port:
                    return 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:
                    return port

    return None

This function detects the USB port where the motor controller is connected:
- Windows: Searches for ports labeled USB Serial Port.
- Linux: Looks for ports starting with /dev/ttyUSB.
- macOS: Searches for ports containing /dev/tty.usbserial or /dev/tty.usbmodem.
If no suitable port is found, the function returns None.

Motor Initialization

port = USB_Port()
m = Master(port)
motor = m.attach(Red(0))

USB_Port(): Detects the USB port for the motor controller.
Master(port): Establishes communication with the motor controller on the detected port.
m.attach(Red(0)): Attaches a motor with ID 0.

Motor Parameter Configuration

m.set_shaft_rpm(0, 100)
m.set_shaft_cpr(0, 6533)
m.set_control_parameters_velocity(0, 10, 1, 0) 
m.set_operation_mode(0, 2)

set_shaft_rpm(0, 100): Sets the motor's maximum speed to 6533 RPM.
set_shaft_cpr(0, 6533): Defines the encoder resolution as 100 counts per revolution (CPR).
set_control_parameters_velocity(0, 10, 1, 0): Configures PID velocity control parameters.
set_operation_mode(0, 2): Sets the motor's operation mode to velocity control.

Counting Mechanism and Motor Rotation

The code implements two key functionalities:

Counting Mode: The user can increment a counter by pressing and releasing the joystick button.
Motor Rotation: The motor rotates a specified number of turns based on the counter value.

Motor Rotation Function

def rotate_motor(turns):
    steps_per_turn = 6533
    target_position = turns * steps_per_turn
    current_position = m.get_position(0)
    m.set_velocity(id=0, sp=10000)
    while abs(m.get_position(0) - current_position) < target_position:
        time.sleep(0.01)
    m.set_velocity(id=0, sp=0)

Calculate Target Position: The number of steps required for the specified turns is calculated.
Control Motor Movement: The motor rotates until the current position reaches the target position.
Stop the Motor: The motor's velocity is set to zero once the target position is achieved.

Joystick Control Function

def joystick_control():
    global counting_mode, turn_count, initial_press_time, final_press_time
    
    while True:
        joystick = m.get_joystick(0, 1)

        if joystick is not None:
            button_pressed = joystick[2]  # Button

            # Switching to counting mode: If the button is held down for 5 seconds for the first time
            if button_pressed and not counting_mode:
                initial_press_time = time.time()
                while button_pressed and time.time() - initial_press_time < 5:
                    button_pressed = m.get_joystick(0, 1)[2]  # Check if the button is still pressed
                    time.sleep(0.1)
                if time.time() - initial_press_time >= 2:
                    counting_mode = True
                    print("Counting mode has been entered. You can count by pressing and pulling.")
            
            # Increase turn_count by push-pull operation in counting mode
            elif counting_mode and button_pressed:
                turn_count += 1
                print(f"The counter was increased: {turn_count}")

            # Rotate the motor: Press and hold the button for 5 seconds in counting mode to rotate the motor.
            if counting_mode and button_pressed:
                final_press_time = time.time()
                while button_pressed and time.time() - final_press_time < 5:
                    button_pressed = m.get_joystick(0, 1)[2]
                    time.sleep(0.1)
                if time.time() - final_press_time >= 2:
                    print(f"The motor will turn {turn_count} times.")
                    rotate_motor(turn_count)
                    turn_count = 0  # Reset the turn count
                    counting_mode = False  # Exit counting mode
                    print("The motor is turned on and the counter is reset to zero.")

        time.sleep(0.01)

Button Press Detection: Reads the joystick button state from the motor controller.
Entering Counting Mode: Holding the button for 5 seconds activates counting mode.
Incrementing the Counter: In counting mode, pressing and releasing the button increases the counter.
Triggering Motor Rotation: Holding the button for another 5 seconds rotates the motor according to the counter value.

Application Workflow

Joystick Monitoring: The joystick_control function continuously monitors joystick input in a separate thread.
Counting Mode Activation: The user can activate counting mode by holding the joystick button for 5 seconds.
Increment Counter: In counting mode, each press-and-release increments the counter.
Motor Rotation: Holding the button for 5 seconds in counting mode rotates the motor by the specified number of turns.

Full Code:

import serial
from smd.red import Master, Red
from threading import Thread
import time
from serial.tools.list_ports import comports
from platform import system

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

    elif system() == "Linux":
        ports = list(serial.tools.list_ports.comports())
        if ports:
            for port, desc, hwid in sorted(ports):
                if '/dev/ttyUSB' in port:
                    return 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:
                    return port

    return None

port = USB_Port()
m = Master(port)
motor = m.attach(Red(0))

# Set motor parameters
m.set_shaft_rpm(0, 6533)
m.set_shaft_cpr(0, 100)
m.set_control_parameters_velocity(0, 10, 1, 0) 
m.set_operation_mode(0, 2)

# Variable to count how many turns to perform when the button is released
counting_mode = False
turn_count = 0
initial_press_time = 0
final_press_time = 0 

# Function to rotate the motor a specific number of turns
def rotate_motor(turns):
    steps_per_turn = 6533  # Set according to the encoder CPR value
    target_position = turns * steps_per_turn  # Calculate the target position
    current_position = m.get_position(0)  # get motor position

    # Rotate the motor until the target position is reached.
    m.set_velocity(id=0, sp=10000)  # Set to maximum speed
    while abs(m.get_position(0) - current_position) < target_position:
        time.sleep(0.01)  # Wait for a short time
    
    m.set_velocity(id=0, sp=0)  # Stop the motor


# Function to handle joystick control
def joystick_control():
    global counting_mode, turn_count, initial_press_time, final_press_time
    
    while True:
        joystick = m.get_joystick(0, 1)

        if joystick is not None:
            button_pressed = joystick[2]  # Button

            # Switching to counting mode: If the button is held down for 5 seconds for the first time
            if button_pressed and not counting_mode:
                initial_press_time = time.time()
                while button_pressed and time.time() - initial_press_time < 5:
                    button_pressed = m.get_joystick(0, 1)[2]  # Check if the button is still pressed
                    time.sleep(0.1)
                if time.time() - initial_press_time >= 2:
                    counting_mode = True
                    print("Counting mode has been entered. You can count by pressing and pulling.")
            
            # Increase turn_count by push-pull operation in counting mode
            elif counting_mode and button_pressed:
                turn_count += 1
                print(f"The counter was increased: {turn_count}")

            # Rotate the motor: Press and hold the button for 5 seconds in counting mode to rotate the motor.
            if counting_mode and button_pressed:
                final_press_time = time.time()
                while button_pressed and time.time() - final_press_time < 5:
                    button_pressed = m.get_joystick(0, 1)[2]
                    time.sleep(0.1)
                if time.time() - final_press_time >= 2:
                    print(f"The motor will turn {turn_count} times.")
                    rotate_motor(turn_count)
                    turn_count = 0  # Reset the turn count
                    counting_mode = False  # Exit counting mode
                    print("The motor is turned on and the counter is reset to zero.")

        time.sleep(0.01)
        
joystick_thread = Thread(target=joystick_control)
joystick_thread.daemon = True
joystick_thread.start()

while True:
    time.sleep(1)

Last updated 6 months ago