Skip to content

PICO Servo Driver

Introdction

PICO Servo Driver is a platform based on raspberry pi pico, which is designed for driving and controlling servos. The board comes with an 8-bit level translator and 8 channels GPIO headers that can connect the servo directly. Now you can build your new project about servo by trying out the Raspberry PI Pico.

Model:PICO6DOFR

Pico_Servo_driver_101.jpg

Features

  • Standard Raspberry Pi Pico header.
  • Onboard 8-bit level translator TXB0108PWR for driving servos.
  • 8-channel expansion GPIOs for servos.
  • Onboard Active buzzer.
  • One button.
  • Power supply: 5V.

Interfaces

Pico_Servo_driver_102.jpg

  • Servo header
Index Black Red Yellow
1 GND 5V GP16
2 GND 5V GP17
3 GND 5V GP18
4 GND 5V G019
5 GND 5V GP20
6 GND 5V GP21
7 GND 5V GP22
8 GND 5V GP26
  • Other connection
GPIO
Button GP14
Buzzer GP15

Usage

  • Basic usage with PIO RP2040

Full code is in : PICO_Merchanical_Hand_Driver/Arm6axis/

Most of MCU have some kind of PWM output, for example the analogWrite() in Arduino, it outputs PWM to related pins, with UNO, these pins can be 3/5/6/9/10 and 11, to control the connected LED brightness or motor speed.

But if these hardware pins are already used by some other functions, such as the D11 in UNO(MOSI in SPI communication), most users simulate the PWM in firmware, which needs hardware times/interrupts, which takes big burden to the MCU. and seriously, when the PWM frequency gets high, or multiple PWM are needed, the frequency get unstable, as the MCU maybe too busy to deal with them all, on time.

And more, for servo control, it may need a signal with 20ms frequency, with 0.1%~99.9% logic high duty, hardware PWM can not output such a signal, while with firmware simulating, it will take a lot of timer/interrupt and MCU ability, especially when it comes to multiple servos simultaneously. Here we introduce a simple way, based on the hardware PIO in RP2040(Raspberry Pi PICO), to control multiple servos without the MCU engaging in much.

PIO

The programmable input/output block (PIO) is a versatile hardware interface. It can support a variety of IO standards, including:

  • 8080 and 6800 parallel bus
  • I2C
  • 3-pin I2S
  • SDIO
  • SPI, DSPI, QSPI
  • UART
  • DPI or VGA (via resistor DAC)

There are 2 identical PIO blocks in RP2040. Each PIO block has dedicated connections to the bus fabric, GPIO and interrupt controller.

image9.png

PIO is programmable in the same sense as a processor. There are two PIO blocks with four state machines each, that can independently execute sequential programs to manipulate GPIOs and transfer data. Unlike a general purpose processor, PIO state machines are highly specialized for IO, with a focus on determinism, precise timing, and close integration with fixed-function hardware. Each state machine is equipped with:

  • Two 32-bit shift registers – either direction, any shift count
  • Two 32-bit scratch registers
  • 4×32-bit bus FIFO in each direction (TX/RX), reconfigurable as 8×32 in a single direction
  • Fractional clock divider (16 integers, 8 fractional bits)
  • Flexible GPIO mapping
  • DMA interface, sustained throughput up to 1 word per clock from system DMA
  • IRQ flag set/clear/status

Outputs PWM with PIO

Open Thonny, Create a new Python file, input the following code and execute:

from rp2 import PIO, StateMachine, asm_pio
from machine import Pin
import time


@asm_pio(set_init=PIO.OUT_LOW)
def pwm_1():
    set(pins, 1)
    set(pins, 0)


def test():
    sm1 = StateMachine(1, pwm_1, freq=1000000, set_base=Pin(0))
    sm1.active(1)
    while 1:
        print("Run....")
        time.sleep(5)


test()

Test the GPIO0 with Oscilloscope:

pico1.jpg

PWM with frequency 500K Hz:

pico2.jpg

  • Assembly Parts
@asm_pio(set_init=PIO.OUT_LOW)
def pwm_1():
    set(pins, 1)
    set(pins, 0)

@asm_pio defines this function a PIO assembly function

set(pins, 1) is not a python function, but a PIO assembly function(we will explain it some later,together with other PIO assembly functions), it sets the logistic 1 and 0. each PIO assembly function take 1 PIO cycle, so there 2 assembly functions here , wit 2 PIO cycles.

PIO to Drive Servo

How do drive a servo

The Servo angle is controlled by the duty of input signal, called PCM. Basiclly, it is a cycle with 20ms duration, with logic HIGH 0.5ms~2.5ms, and the logic HIGH finally controls the how the angles. for example, 1.5ms means middle position 90° on the servo(for 180° servo, 90° is the middle position), and if the logic HIHG width smaller than 1.5ms, the servo turns towards to 0°, and vice versa.

servo_motion.gif

Servo control Main

from machine import Pin
import time

T_X = 4000
T_Y0 = 3900
T_Y180 = 3500


#freq=800000 0.5ms y=3900 2.5ms y=3500 unit_y:2.5us == 0.45degree
@asm_pio(set_init=PIO.OUT_LOW)
def servo_ctrl2():

    mov(x, isr)
    set(pins, 1)

    label("for")
    jmp(not_x,"over")
    jmp(x_not_y,"dec")
    set(pins, 0)
    label("dec")
    jmp(x_dec,"continue")
    label("continue")
    jmp("for")

    label("over")
    nop()[3]


class PIO_SERVO:
    def __init__(self, sm_id, pin):
        self._sm = StateMachine(
            sm_id, servo_ctrl2, freq=800000, set_base=Pin(pin))
        # Use exec() to load max count into ISR
        #self._sm.put(T_Y0)
        self._sm.put(T_X)
        self._sm.exec("pull()")
        self._sm.exec("mov(y, osr)")
        self._sm.put(T_X)
        self._sm.exec("pull()")
        self._sm.exec("mov(isr, osr)")
        self._sm.active(1)

    def set(self, pio_cycle):
        self._sm.active(0)
        self._sm.put(pio_cycle)
        self._sm.exec("pull()")
        self._sm.exec("mov(y, osr)")
        self._sm.put(T_X)
        self._sm.exec("pull()")
        self._sm.exec("mov(isr, osr)")
        self._sm.active(1)

    def set_ms(self, ms):
        if ms < 0.5:
            ms = 0.5
        elif ms > 2.5:
            ms = 2.5

        pio_cycle = int(3900 - (ms - 0.5) * 200)
        self.set(pio_cycle)

    def set_angle(self, angle):
        ms = (angle + 90.0) / 90.0 + 0.5
        self.set_ms(ms)


def test():

    servo = PIO_SERVO(0, 16)
    # servo.set(180)
    # servo.set_ms(1.5)
    while 0:
        time.sleep(3)
    while 1:

        # servo.set(T_Y180)
        servo.set_ms(0.5)

        time.sleep(3)

        # servo.set(T_Y0)
        servo.set_ms(2.5)

        time.sleep(3)


#test()

PIO assembly parts

#freq=800000 0.5ms y=3900 2.5ms y=3500 unit_y:2.5us == 0.45degree
@asm_pio(set_init=PIO.OUT_LOW)
def servo_ctrl2():

    mov(x, isr)
    set(pins, 1)

    label("for")
    jmp(not_x,"over")
    jmp(x_not_y,"dec")
    set(pins, 0)
    label("dec")
    jmp(x_dec,"continue")
    label("continue")
    jmp("for")

    label("over")
    nop()[3]

The logic:

  • MOVE data in ISR to x;
  • set the GPIO to HIGH
  • loop
  • Check if X is equal to Y, if yes, set GPIO to LOW
  • X--
  • nops to make it 20 ms duration

PIO MicroPython

T_X = 4000
T_Y0 = 3900
T_Y180 = 3500

class PIO_SERVO:
    def __init__(self, sm_id, pin):
        self._sm = StateMachine(
            sm_id, servo_ctrl2, freq=800000, set_base=Pin(pin))
        # Use exec() to load max count into ISR
        #self._sm.put(T_Y0)
        self._sm.put(T_X)
        self._sm.exec("pull()")
        self._sm.exec("mov(y, osr)")
        self._sm.put(T_X)
        self._sm.exec("pull()")
        self._sm.exec("mov(isr, osr)")
        self._sm.active(1)

    def set(self, pio_cycle):
        self._sm.active(0)
        self._sm.put(pio_cycle)
        self._sm.exec("pull()")
        self._sm.exec("mov(y, osr)")
        self._sm.put(T_X)
        self._sm.exec("pull()")
        self._sm.exec("mov(isr, osr)")
        self._sm.active(1)

Set the state machine frequency 800KHz,1.25us for each command.

Input data to cache and then pull to OSR, and move to registers.

Multiple Servo Control

There 8 state machines, 0~7, in RP2040, to contrl max 8 servo simultaneously with RP2040 PIO.

The following code is a six-axis robot arm controlling based on PIO. Divided into steering gear layer, mechanical arm single shaft, mechanical arm, mechanical arm control four layers. A total of six servos are used, and six state machines of the PIO are used for synchronous control.

pico6.jpg

  • PIO Servo

This part contains a PIO-based servo driver, declares a PIO_SERVO Class, and provides methods based on timing and angle control.

#arm6\pio_servo.py

from rp2 import PIO, StateMachine, asm_pio
from machine import Pin
import time

T_X = 4000
T_Y0 = 3900
T_Y180 = 3500


#freq=800000 0.5ms y=3900 2.5ms y=3500 unit_y:2.5us == 0.45degree
@asm_pio(set_init=PIO.OUT_LOW)
def servo_ctrl2():

    mov(x, isr)
    set(pins, 1)

    label("for")
    jmp(not_x,"over")
    jmp(x_not_y,"dec")
    set(pins, 0)
    label("dec")
    jmp(x_dec,"continue")
    label("continue")
    jmp("for")

    label("over")
    nop()[3]


class PIO_SERVO:
    def __init__(self, sm_id, pin):
        self._sm = StateMachine(
            sm_id, servo_ctrl2, freq=800000, set_base=Pin(pin))
        # Use exec() to load max count into ISR
        #self._sm.put(T_Y0)
        self._sm.put(T_X)
        self._sm.exec("pull()")
        self._sm.exec("mov(y, osr)")
        self._sm.put(T_X)
        self._sm.exec("pull()")
        self._sm.exec("mov(isr, osr)")
        self._sm.active(1)

    def set(self, pio_cycle):
        self._sm.active(0)
        self._sm.put(pio_cycle)
        self._sm.exec("pull()")
        self._sm.exec("mov(y, osr)")
        self._sm.put(T_X)
        self._sm.exec("pull()")
        self._sm.exec("mov(isr, osr)")
        self._sm.active(1)

    def set_ms(self, ms):
        if ms < 0.5:
            ms = 0.5
        elif ms > 2.5:
            ms = 2.5

        pio_cycle = int(3900 - (ms - 0.5) * 200)
        self.set(pio_cycle)

    def set_angle(self, angle):
        ms = (angle + 90.0) / 90.0 + 0.5
        self.set_ms(ms)

Mechanical Arm

Declares Arm_axis Class for a robot arm, each axis containing a PIO_SERVO. The six Arm_axis are packaged as a Arm_6 Class, which provides a series of control methods such as initialization, refresh and so on.

#arm6\arm_6axis.py

from pio_servo import PIO_SERVO
import time

# PIN_LIST = [0, 1, 2, 3, 4, 5]
# DIR_LIST = [0, 0, 0, 1, 0, 0]


class Arm_axis:
    def __init__(self, index, pin, direct):
        self.id = index
        self.pin = pin
        self.dir = direct
        # default position is middle
        self.angle = 0.0
        self.servo = PIO_SERVO(self.id, self.pin)

    def set_angle(self, angle):
        if angle < -90.0:
            angle = -90.0
        elif angle > 90.0:
            angle = 90.0
        self.angle = angle

    def fresh(self):
        real_angle = self.angle
        if self.dir == 1:
            real_angle = -real_angle
        self.servo.set_angle(real_angle)

    def angle_report(self):
        return self.angle


class Arm_6:
    def __init__(self, pin_list, dir_list):
        self.axises = []
        for i in range(6):
            self.axises.append(
                Arm_axis(i, pin_list[i], dir_list[i]))

    def flesh_single(self, index):
        self.axises[index].fresh()

    def flesh(self):
        for i in range(6):
            self.axises[i].fresh()

    def set_single_angle(self, index, angle):
        self.axises[index].set_angle(angle)

    def pos_report(self):
        pos = []
        for i in range(6):
            pos.append(self.axises[i].angle_report())

        return pos

Robot Arm Codes Explanation

Provides a set of control commands, including absolute angle and relative angle two kinds of movement, as well as button, buzzer, delay and other commands. The commands can be read in batches from the file in a way similar to GCODE.

#arm6\arm_controller.py

from arm_6axis import Arm_6
import time

RUNFILE = "arm_code2.txt"
DEBUG = 0

PIN_LIST    = [16, 17, 18, 19, 20, 21]
DIR_LIST    = [0, 1, 1, 0, 0, 1]
ANGLE_LIST  = [-2, -40, 75, 75, 0, -10]


DELAY_TIME = 0.5


class Arm_controller:
    def __init__(self, PIN_LIST, DIR_LIST):
        self.arm = Arm_6(PIN_LIST, DIR_LIST)

    def explain_line(self, TEXT):
        if TEXT[0] == "#":
            if DEBUG:
                print(TEXT)
            return 0

        elif TEXT.find("ABS") == 0:
            command_list = TEXT[4:].split(" ")
            if DEBUG:
                print("ABS")
                print(command_list)
            self.explain_absolate(command_list)
            return 1

        elif TEXT.find("REL") == 0:
            command_list = TEXT[4:].split(" ")
            if DEBUG:
                print("REL")
                print(command_list)
            self.explain_relative(command_list)
            return 1

        elif TEXT.find("END") == 0:
            print("COMMAND END")
            print("Finally POS:")
            print(self.arm.pos_report())
            return 2

        elif TEXT.find("POS") == 0:
            self.arm.flesh()
            time.sleep(2)
            print("REALLY POS:")
            print(self.arm.pos_report())
            return 1

        elif TEXT.find("WAIT") == 0:
            time.sleep(2)
            return 1

        elif TEXT.find("SLEEP") == 0:
            sleep_time = int(TEXT[6:])
            print("Sleep " + str(sleep_time) + "...")
            time.sleep(sleep_time)
            return 1

        elif TEXT.find("BUZZ") == 0:
            alarm()
            return 1

        elif TEXT.find("BUTT") == 0:
            input_button()
            return 1

        else:
            print("WARNING : NOT KNOWN COMMAND")
            print(TEXT)
            return -1

    def explain_absolate(self, COMMAND_LIST):
        for part in COMMAND_LIST:
            index = self.get_index(part[0])
            angle = int(part[1:])

            # print(index)
            # print(angle)

            self.arm.set_single_angle(index, angle)
        self.arm.flesh()
        time.sleep(DELAY_TIME)

    def explain_relative(self, COMMAND_LIST):
        pos_list = self.arm.pos_report()
        for part in COMMAND_LIST:
            index = self.get_index(part[0])
            angle = int(part[1:]) + pos_list[index]

            # print(index)
            # print(angle)

            self.arm.set_single_angle(index, angle)
        self.arm.flesh()
        time.sleep(DELAY_TIME)

    # init by angle list
    def init_arm_angle(self, ANGLE_LIST):
        for index in range(6):
            self.arm.set_single_angle(index, ANGLE_LIST[index])
        self.arm.flesh()
        time.sleep(2)

    def get_index(self, part):
        axis_dic = {'A': 0, 'B': 1, 'C': 2, 'D': 3, 'E': 4, 'F': 5}
        return axis_dic[part]


def control_by_file():

    ct = Arm_controller(PIN_LIST, DIR_LIST)
    ct.init_arm_angle(ANGLE_LIST)

    file = open(RUNFILE, "r")

    for line in file:
        print(line)
        if line.isspace():
            continue
        if ct.explain_line(line) == 2:
            break
    file.close()


def control_by_serial():
    ct = Arm_controller(PIN_LIST, DIR_LIST)
    ct.init_arm_angle(ANGLE_LIST)
    line = ""
    while 1:
        line = input()
        if line.isspace():
            continue
        if ct.explain_line(line) == 2:
            break
        line = ""


def control_by_serial_save():
    file = open("Log.txt", "w")
    file.write("#" + str(PIN_LIST) + "\n")
    file.write("#" + str(DIR_LIST) + "\n")
    file.write("#" + str(ANGLE_LIST) + "\n")

    ct = Arm_controller(PIN_LIST, DIR_LIST)
    ct.init_arm_angle(ANGLE_LIST)
    line = ""
    while 1:
        line = input()
        file.write(str(line) + "\n")
        if line.isspace():
            continue
        if ct.explain_line(line) == 2:
            break
        line = ""

    file.close()

def alarm():
    buzzer = machine.Pin(15, machine.Pin.OUT)
    for i in range(500):
        buzzer.value(1)
        time.sleep(0.001)
        buzzer.value(0)
        time.sleep(0.001)

def input_button():
    button = machine.Pin(14, machine.Pin.IN, machine.Pin.PULL_UP)
    print("Wait button press")
    while 1:
        if button.value() == 0:
            time.sleep(0.5)
            if button.value() == 0:
                alarm()
                break
        time.sleep(0.5)    
def test():

    while 1:
        control_by_file()
        break
    # control_by_serial()
    # control_by_serial_save()

test()

A simple application-Hanoi control

The following codes is Hanoi control, the youtube video following.

- ABS: absolute angle
- REL: Relative angle
- WAIT
- #remark
- POS: return the absolute angle
- BUTT :wait for button
- BUZZ :buzzer work
- SLEEP set the sleep time
#[16, 17, 18, 19, 20, 21]
#[0, 1, 1, 0, 0, 1]
#[-2, -25, 75, 75, 0, -10]

#claw F-10release,F35grap
#C75 D62, B = 3:5 2:9 1:14
#    3       3       3
#    2       2       2
#    1       1       1
#   [1]     [2]     [3]

#sequence
#1:3 -> 3:1
#1:2 -> 2:1
#3:1 -> 2:2
#1:1 -> 3:1
#2:2 -> 1:1
#2:1 -> 3:2
#1:1 -> 3:3

###############################1:3 -> 3:1
#Locate 1:3
ABS A18 B-20 C75 D62 F-10
WAIT

#drop
ABS B5
WAIT

#Grab
ABS F30

# POS
# [18, 5, 75, 62, 0, 30]

#lift
REL B-6
WAIT

#move to A-18
ABS A-18
WAIT

#move to 3:1
REL B3
REL B3
REL B3
REL B3
REL B3

#release
ABS F-10

# POS
# [-18, 14, 75, 62, 0, -10]

#return
ABS B0

FAQ

You can list your question here or contact techsupport@makerfabs.com for technology support. Detailed descriptions of your question will be helped to solve your question.

Resources