Рекомендации по программированию роботов-манипуляторов

1. Расчёт угла поворота сервопривода для редуктора поворота башни роботов RM001 и RM002

Расчёт угла поворота сервопривода для редуктора поворота башни роботов RM001 и RM002

При управлении поворотом башни роботов-манипуляторов моделей RM001 M03 и RM002 M01 необходимо учитывать передаточное число редуктора поворота башни. Это обеспечивает точное позиционирование механизма при программировании.

Передаточные числа редуктора поворота башни #

Модель робота	Передаточное число (N : n)
RM001 M03	20 : 15
RM002 M01	28 : 21

Где:

N — количество зубьев ведомой шестерни
n — количество зубьев ведущей шестерни

Формула расчёта угла поворота сервопривода #

θ_серв = (θ_башни * N) / n,

где:

θ_серв — угол, на который необходимо повернуть сервопривод
θ_башни — угол, на который требуется повернуть башню

Примеры расчётов #

RM002 M01 (передаточное число 28:21) #

Поворот башни на 30°:
θ_серв = (30 * 28) / 21 = 40

RM001 M03 (передаточное число 20:15) #

Поворот башни на 90°:
θ_серв = (90 * 20) / 15 = 120

Пример 1. На языке программирования Python

import ctypes
from ctypes import c_int, create_string_buffer
import time
import sys
import os

# Загрузка библиотеки
lib_path = os.path.abspath('./librisdk.so')  # путь до вашей скомпилированной библиотеки
try:
    librisdk = ctypes.CDLL(lib_path)
except OSError as e:
    print(f"Ошибка загрузки библиотеки: {e}")
    sys.exit(1)

# Глобальные переменные
body_start_pulse = c_int(390)
log_level = c_int(2)

# Структура, содержащая дескрипторы устройств
class Device:
    def __init__(self):
        self.i2c = c_int()
        self.pwm = c_int()
        self.body = c_int()

robohand = Device()

# Структура сервопривода с дескриптором и стартовой позицией
class Servo:
    def __init__(self, descriptor, start_position):
        self.descriptor = descriptor
        self.start_position = start_position

servos = [
    Servo(ctypes.pointer(robohand.body), ctypes.pointer(body_start_pulse))
]

# Инициализация SDK и компонентов
def init_device():
    error_text = create_string_buffer(1000)

    # Инициализация SDK
    err_code = librisdk.RI_SDK_InitSDK(log_level, error_text)
    if err_code != 0:
        return f"errorCode:{err_code} - errorText:{error_text.value.decode()}"

    # Создание компонента PWM
    err_code = librisdk.RI_SDK_CreateModelComponent(
        b"connector", b"pwm", b"pca9685", ctypes.byref(robohand.pwm), error_text)
    if err_code != 0:
        return f"errorCode:{err_code} - errorText:{error_text.value.decode()}"

    # Создание компонента i2c_adapter (CH341)
    err_code = librisdk.RI_SDK_CreateModelComponent(
        b"connector", b"i2c_adapter", b"ch341", ctypes.byref(robohand.i2c), error_text)
    if err_code != 0:
        return f"errorCode:{err_code} - errorText:{error_text.value.decode()}"

    # Линкуем PWM с i2c адаптером
    err_code = librisdk.RI_SDK_LinkPWMToController(
        robohand.pwm, robohand.i2c, 0x40, error_text)
    if err_code != 0:
        # Если CH341 не сработал — пробуем CP2112
        err_code = librisdk.RI_SDK_CreateModelComponent(
            b"connector", b"i2c_adapter", b"cp2112", ctypes.byref(robohand.i2c), error_text)
        if err_code != 0:
            return f"errorCode:{err_code} - errorText:{error_text.value.decode()}"
        err_code = librisdk.RI_SDK_LinkPWMToController(
            robohand.pwm, robohand.i2c, 0x40, error_text)
        if err_code != 0:
            return f"errorCode:{err_code} - errorText:{error_text.value.decode()}"

    # Инициализация сервоприводов
    err = init_servos()
    if err:
        return err

    return None

# Создание и линковка сервоприводов
def init_servos():
    error_text = create_string_buffer(1000)

    # Создание сервопривода MG90S
    err_code = librisdk.RI_SDK_CreateModelComponent(
        b"executor", b"servodrive", b"mg90s", servos[0].descriptor, error_text)
    if err_code != 0:
        return f"errorCode:{err_code} - errorText:{error_text.value.decode()}"

    # Привязка сервопривода к PWM-контроллеру
    err_code = librisdk.RI_SDK_LinkServodriveToController(
        servos[0].descriptor.contents, robohand.pwm, 0, error_text)
    if err_code != 0:
        return f"errorCode:{err_code} - errorText:{error_text.value.decode()}"

    return None

# Удаление сервоприводов
def destruct_servos():
    error_text = create_string_buffer(1000)
    err_code = librisdk.RI_SDK_DestroyComponent(servos[0].descriptor.contents, error_text)
    if err_code != 0:
        return f"errorCode:{err_code} - errorText:{error_text.value.decode()}"
    return None

# Удаление компонентов и завершение работы SDK
def destruct():
    error_text = create_string_buffer(1000)

    # Удаляем сервоприводы
    err = destruct_servos()
    if err:
        return err

    # Сброс всех сигналов на PWM
    err_code = librisdk.RI_SDK_sigmod_PWM_ResetAll(robohand.pwm, error_text)
    if err_code != 0:
        return f"errorCode:{err_code} - errorText:{error_text.value.decode()}"

    # Удаление компонентов
    err_code = librisdk.RI_SDK_DestroyComponent(robohand.pwm, error_text)
    if err_code != 0:
        return f"errorCode:{err_code} - errorText:{error_text.value.decode()}"

    err_code = librisdk.RI_SDK_DestroyComponent(robohand.i2c, error_text)
    if err_code != 0:
        return f"errorCode:{err_code} - errorText:{error_text.value.decode()}"

    # Удаление SDK
    err_code = librisdk.RI_SDK_DestroySDK(True, error_text)
    if err_code != 0:
        return f"errorCode:{err_code} - errorText:{error_text.value.decode()}"

    return None

# Поворот башни
def rotate_body(angle):
    print("Поворот сервопривода на угол:", angle)
    error_text = create_string_buffer(1000)
    err_code = librisdk.RI_SDK_exec_ServoDrive_Turn(
        robohand.body, angle, 400, False, error_text)
    if err_code != 0:
        return f"errorCode:{err_code} - errorText:{error_text.value.decode()}"
    return None

# Вычисление угла поворота сервопривода в зависимости от модели робота
def calculating_turn_angle_servo_drive(model_robot, angle_coordinate_plane):
    print("Поворот башни робота на угол:", angle_coordinate_plane)
    if model_robot == "RM_001":
        return formula_calculating_turn_angle_servo_drive(angle_coordinate_plane, 20, 15), None
    elif model_robot == "RM_002":
        return formula_calculating_turn_angle_servo_drive(angle_coordinate_plane, 28, 21), None
    else:
        return 0, "модель робота не найдена"

# Формула поворота: пересчет координаты в угол
def formula_calculating_turn_angle_servo_drive(angle_coordinate_plane, N, n):
    # Воспользуемся формулой для вычисления угла поворота сервопривода:
	# θ_серв = (angleСoordinatePlane * N) / n (*)
	# θ_серв — угол, на который необходимо повернуть сервопривод
	# θ_башни — угол, на который требуется повернуть башню
	# N — количество зубьев ведомой шестерни
	# n — количество зубьев ведущей шестерни
    return int((angle_coordinate_plane * N) / n)

# Точка входа
if __name__ == "__main__":
    err = init_device()
    if err:
        print("Ошибка инициализации:", err)
    else:
        angle_servo, err = calculating_turn_angle_servo_drive("RM_002", 10)
        if err:
            print("Ошибка вычисления угла поворота сервопривода:", err)
        else:
            err = rotate_body(c_int(angle_servo))
            if err:
                print(err)
            else:
                time.sleep(10)
        destruct()

Пример 2. На языке программирования Golang

package main

import "C"
import (
	"fmt"
	"log"
	"time"
)

/*
#cgo CFLAGS: -I.
#cgo LDFLAGS: -L. -lrisdk
#include <librisdk.h>
*/
import "C"

var (
	// bodyStartPulse - стартовая позиция тела
	bodyStartPulse = C.int(390)
	// logLevel - уровень логирования
	logLevel = C.int(2)
)

// device - структура дескрипторов устройства
type device struct {
	// i2c - дескриптор i2c
	i2c C.int
	// pwm - дескриптор pwm
	pwm C.int
	// body - дескриптор сервопривода тела
	body C.int
}

// robohand - инициализация структуры устройства
var robohand device

// servo - структура для описания сервопривода
type servo struct {
	// descriptor - дескриптор сервопривода
	descriptor *C.int
	// startPosition - стартовая позиция сервопривода
	startPosition *C.int
}

// servos - массив дескрипторов сервоприводов
var servos = [2]servo{
	{
		descriptor:    &robohand.body,
		startPosition: &bodyStartPulse,
	},
}

// initDevice - инициализация библиотеки и устройств
// Возвращает ошибку, если инициализация не удалась
func initDevice() error {
	var (
		// errorTextC - текст ошибки
		errorTextC [1000]C.char
		// errCode - код ошибки
		errCode C.int
	)

	// Инициализируем SDK
	errCode = C.RI_SDK_InitSDK(logLevel, &errorTextC[0])
	if errCode != 0 {
		return fmt.Errorf("errorCode:%d - errorText:%s", errCode, C.GoString(&errorTextC[0]))
	}
	// Создаем компонент ШИМ с конкретной моделью как соединитель и получаем дескриптор компонента
	errCode = C.RI_SDK_CreateModelComponent(C.CString("connector"), C.CString("pwm"), C.CString("pca9685"), &robohand.pwm, &errorTextC[0])
	if errCode != 0 {
		return fmt.Errorf("errorCode:%d - errorText:%s", errCode, C.GoString(&errorTextC[0]))
	}
	// Создаем компонент i2c адаптера
	// Здесь осуществляется примитивное определение подключенной модели адаптера
	// Сначала пробуем создать i2c адаптер модели ch341 и связать с ним ШИМ
	errCode = C.RI_SDK_CreateModelComponent(C.CString("connector"), C.CString("i2c_adapter"), C.CString("ch341"), &robohand.i2c, &errorTextC[0])
	if errCode != 0 {
		return fmt.Errorf("errorCode:%d - errorText:%s", errCode, C.GoString(&errorTextC[0]))
	}
	// Связываем i2c адаптер с ШИМ по адресу 0x40
	errCode = C.RI_SDK_LinkPWMToController(robohand.pwm, robohand.i2c, 0x40, &errorTextC[0])
	if errCode != 0 {
		// Если не получается, то пробуем создать i2c адаптер модели cp2112
		errCode = C.RI_SDK_CreateModelComponent(C.CString("connector"), C.CString("i2c_adapter"), C.CString("cp2112"), &robohand.i2c, &errorTextC[0])
		if errCode != 0 {
			return fmt.Errorf("errorCode:%d - errorText:%s", errCode, C.GoString(&errorTextC[0]))
		}
		// Связываем i2c адаптер с ШИМ по адресу 0x40
		errCode = C.RI_SDK_LinkPWMToController(robohand.pwm, robohand.i2c, 0x40, &errorTextC[0])
		if errCode != 0 {
			return fmt.Errorf("errorCode:%d - errorText:%s", errCode, C.GoString(&errorTextC[0]))
		}
	}

	// Инициализируем сервоприводы
	err := initServos()
	if err != nil {
		return err
	}

	return nil
}

// initServos - создает сервоприводы и линкует их
// Возвращает ошибку, если создание или линковка не удались
func initServos() error {
	var (
		// errorTextC - текст ошибки
		errorTextC [1000]C.char
		// errCode - код ошибки
		errCode C.int
	)
	// Создаем 1 сервопривод и линкуем его к пин 0,
	// также создаем компонент сервопривода с конкретной моделью как исполняемое устройство и получаем дескриптор сервопривода
	errCode = C.RI_SDK_CreateModelComponent(C.CString("executor"), C.CString("servodrive"), C.CString("mg90s"), servos[0].descriptor, &errorTextC[0])
	if errCode != 0 {
		return fmt.Errorf("errorCode:%d - errorText:%s", errCode, C.GoString(&errorTextC[0]))
	}
	// Связываем сервопривод с ШИМ, передаем дескриптор сервопривода и ШИМ
	errCode = C.RI_SDK_LinkServodriveToController(*servos[0].descriptor, robohand.pwm, C.int(0), &errorTextC[0])
	if errCode != 0 {
		return fmt.Errorf("errorCode:%d - errorText:%s", errCode, C.GoString(&errorTextC[0]))
	}

	return nil
}

// destructServos - уничтожает сервопривод,
// возвращает ошибку, если уничтожение не удалось
func destructServos() error {
	var (
		// errorTextC - текст ошибки
		errorTextC [1000]C.char
		// errCode - код ошибки
		errCode C.int
	)
	errCode = C.RI_SDK_DestroyComponent(*servos[0].descriptor, &errorTextC[0])
	if errCode != 0 {
		return fmt.Errorf("errorCode:%d - errorText:%s", errCode, C.GoString(&errorTextC[0]))
	}

	return nil
}

// destruct - уничтожает все компоненты и библиотеку
// Возвращает ошибку, если уничтожение не удалось
func destruct() error {

	var (
		// errorTextC - текст ошибки
		errorTextC [1000]C.char
		// errCode - код ошибки
		errCode C.int
	)

	// Уничтожаем сервоприводы
	err := destructServos()
	if err != nil {
		return err
	}
	// Сбрасываем все порты на ШИМ
	errCode = C.RI_SDK_sigmod_PWM_ResetAll(robohand.pwm, &errorTextC[0])
	if errCode != 0 {
		return fmt.Errorf("errorCode:%d - errorText:%s", errCode, C.GoString(&errorTextC[0]))
	}
	// Удаляем компонент ШИМ
	errCode = C.RI_SDK_DestroyComponent(robohand.pwm, &errorTextC[0])
	if errCode != 0 {
		return fmt.Errorf("errorCode:%d - errorText:%s", errCode, C.GoString(&errorTextC[0]))
	}
	// Удаляем компонент i2c
	errCode = C.RI_SDK_DestroyComponent(robohand.i2c, &errorTextC[0])
	if errCode != 0 {
		return fmt.Errorf("errorCode:%d - errorText:%s", errCode, C.GoString(&errorTextC[0]))
	}
	// Удаляем sdk со всеми компонентами в реестре компонентов
	errCode = C.RI_SDK_DestroySDK(true, &errorTextC[0])
	if errCode != 0 {
		return fmt.Errorf("errorCode:%d - errorText:%s", errCode, C.GoString(&errorTextC[0]))
	}
	return nil
}

// rotateBody - Вращение тела в указанный угол
// angle - угол, на который нужно повернуть тело
// Возвращает ошибку, если вращение не удалось
func rotateBody(angle C.int) error {
	fmt.Println("Поворот сервопривода на угол: ", angle)
	var (
		// errorTextC - текст ошибки
		errorTextC [1000]C.char
		// errCode - код ошибки
		errCode C.int
	)
	errCode = C.RI_SDK_exec_ServoDrive_Turn(robohand.body, angle, 400, false, &errorTextC[0])
	if errCode != 0 {
		return fmt.Errorf("errorCode:%d - errorText:%s", errCode, C.GoString(&errorTextC[0]))
	}
	return nil
}

// calculatingTurnAngleServoDrive - функция для вычисления угла поворота сервопривода
// для поворота башни робота на заданный угол angleСoordinatePlane
func calculatingTurnAngleServoDrive(modelRobot string, angleСoordinatePlane int) (int, error) {
	fmt.Println("Поворот башни робота на угол: ", angleСoordinatePlane)
	switch modelRobot {
	case "RM_001":
		O_serv := formulaCalculatingTurnAngleServoDrive(angleСoordinatePlane, 20, 15)
		return O_serv, nil
	case "RM_002":
		// Воспользуемся формулой для вычисления угла поворота сервопривода (*):
		O_serv := formulaCalculatingTurnAngleServoDrive(angleСoordinatePlane, 28, 21)
		return O_serv, nil
	default:
		return 0, fmt.Errorf("модель робота не найдена")
	}
}

func formulaCalculatingTurnAngleServoDrive(angleСoordinatePlane, N, n int) int {
	// Воспользуемся формулой для вычисления угла поворота сервопривода:
	// θ_серв = (angleСoordinatePlane * N) / n (*)
	// θ_серв — угол, на который необходимо повернуть сервопривод
	// θ_башни — угол, на который требуется повернуть башню
	// N — количество зубьев ведомой шестерни
	// n — количество зубьев ведущей шестерни
	O_serv := (angleСoordinatePlane * N) / n
	return O_serv

}

// main - точка входа в программу.
// Инициализирует устройство, выполняет вращение тела и завершает работу с устройством
func main() {
	err := initDevice()
	if err != nil {
		log.Fatal(err)
	}

	// Уничтожаем компоненты и библиотеку
	defer func() {
		err := destruct()
		fmt.Println("Уничтожаем компоненты и библиотеку")
		if err != nil {
			log.Fatal(err)
		}
	}()

	angleServo, err := calculatingTurnAngleServoDrive("RM_002", 10)

	if err != nil {
		log.Fatalf("Ошибка вычисления угла поворота сервопривода: %s", err)
	}

	err = rotateBody(C.int(angleServo))
	if err != nil {
		fmt.Println(err)
		return
	}
	// Ожидаем, чтобы посмотреть на результат
	time.Sleep(10 * time.Second)
}

40 просмотров0 комментариев