一、3D结构
外观通过solidworks建模，用大昆3d打印机打印。
关节展示
二、实现原理
首先要看过我的第一篇文章，知道demo的代码功能，如下图。
图中上面三个值，可以理解为选型，就是选择我这个舵机的类型，这个值可以在我再第一篇文章里面介绍的上位机看到。
这个三个值就是主要控制舵机的参数
read_write.py这个程序就是从大约为0°位置转到360°，再转回来。
而0 ~ 4096 就是对应 0 ~ 360°，所以可以有下面的理解：
这是一个PWM脉宽调制，相信有接触过的人都不会陌生。这个就是对应着这个舵机的，从在数值10对应的角度开始，在得到高电平的时候进行旋转到指定位置，每次增加20数值对应的角度，直到达到4000，再按下一次回车就是反转回去。
下面我来说说仿生蛇，首先就是舵机横纵交错连接，是一种运动的结构。只要对控制PWM的输出口输入sinwt的正弦信号，控制转动和正反转，PWM会随着时间的变化而变化，这样就实现了舵机的一种控制，这就是仿生蛇的步态控制，也是从外国的论文文献中看到的。如下图。
根据参数的不同，就会产生不同的步态。
三、代码实现
目前只是加了六个舵机，后面如果继续研究会继续加舵机的。
我之前说用Atom，我后来发现随便用什么的python编译器都可以，这里我之后改用了pycharm编译器。
给大家展示下python代码，供给后来人学习参考，对了我是windows系统上使用的，可能其他系统也会有差别，还有就是这个舵机的通信是写在底层的，我是直接使用的。
大家可以对比官方给的例程read_write.py参考，我这个是在例程demo中直接修改编写的。
3_test_6_2.py
#!/usr/bin/env python
# -*- coding: utf-8 -*-
################################################################################
# Copyright 2017 ROBOTIS CO., LTD.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
################################################################################
# Author: Ryu Woon Jung (Leon)
#
# ********* Read and Write Example *********
#
#
# Available DXL model on this example : All models using Protocol 1.0
# This example is tested with a DXL MX-28, and an USB2DYNAMIXEL
# Be sure that DXL MX properties are already set as %% ID : 1 / Baudnum : 34 (Baudrate : 57600)
#
import os
import numpy as np
if os.name == 'nt':
import msvcrt
def getch():
return msvcrt.getch().decode()
else:
import sys, tty, termios
fd = sys.stdin.fileno()
old_settings = termios.tcgetattr(fd)
def getch():
try:
tty.setraw(sys.stdin.fileno())
ch = sys.stdin.read(1)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
return ch
from dynamixel_sdk import * # Uses Dynamixel SDK library
import math
import time
def sin_fun(phi, t, T, A=math.pi / 4, B=math.pi / 2):
'''
input : A, T, phi, t, B : Amplitude Period Phase Time Offset
return : theta (0-360 Convert to 0-4096)
'''
w = 2 * math.pi / T
theta = A * math.sin(w * t - phi) + B
theta = int(theta / math.pi * 0.5 * 4096)
return theta
if __name__ == '__main__':
# Control table address
ADDR_MX_TORQUE_ENABLE = 24 # Control table address is different in Dynamixel model
ADDR_MX_GOAL_POSITION = 30
ADDR_MX_PRESENT_POSITION = 36
# Protocol version
PROTOCOL_VERSION = 1.0 # See which protocol version is used in the Dynamixel
# Default setting
servo_num = 6
DXL_ID = [3,4,5,6,7,8] # Dynamixel ID
BAUDRATE = 57600 # Dynamixel default baudrate : 57600
DEVICENAME = 'COM13' # Check which port is being used on your controller
# ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"
TORQUE_ENABLE = 1 # Value for enabling the torque
TORQUE_DISABLE = 0 # Value for disabling the torque
DXL_MOVING_STATUS_THRESHOLD = [] # Dynamixel moving status threshold
for i in range(servo_num):
DXL_MOVING_STATUS_THRESHOLD.append(20)
# Initialize PortHandler instance
# Set the port path
# Get methods and members of PortHandlerLinux or PortHandlerWindows
portHandler = PortHandler(DEVICENAME)
# Initialize PacketHandler instance
# Set the protocol version
# Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
packetHandler = PacketHandler(PROTOCOL_VERSION)
# Open port
if portHandler.openPort():
print("Succeeded to open the port")
else:
print("Failed to open the port")
print("Press any key to terminate...")
getch()
quit()
# Set port baudrate
if portHandler.setBaudRate(BAUDRATE):
print("Succeeded to change the baudrate")
else:
print("Failed to change the baudrate")
print("Press any key to terminate...")
getch()
quit()
# Enable Dynamixel Torque
for i in range(servo_num):
dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID[i], ADDR_MX_TORQUE_ENABLE,
TORQUE_ENABLE)
if dxl_comm_result != COMM_SUCCESS:
print("##### Enable Dynamixel Torque", "%s" % packetHandler.getTxRxResult(dxl_comm_result))
elif dxl_error != 0:
print("##### Enable Dynamixel Torque", "%s" % packetHandler.getRxPacketError(dxl_error))
else:
print("Dynamixel " + str(i) + " has been successfully connected")
# Define parameters
T = 2 # T > 1, w < 2*pi/1s
phi = [0, 0, math.pi / 6, math.pi / 6, math.pi / 3, math.pi / 3, math.pi / 2, math.pi / 2]
start = 0
end = 100
dxl_goal_position = []
dxl_present_position = []
for i in range(servo_num):
dxl_goal_position.append(0)
dxl_present_position.append(0)
# ----------------------------------------------------------------------------
# Set initial position
for i in range(servo_num):
dxl_goal_position[i] = sin_fun(phi=phi[i], t=start, T=T)
print('initial position = ', dxl_goal_position)
for i in range(servo_num):
dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL_ID[i], ADDR_MX_GOAL_POSITION,
dxl_goal_position[i])
if dxl_comm_result != COMM_SUCCESS:
print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
elif dxl_error != 0:
print("%s" % packetHandler.getRxPacketError(dxl_error))
judge = []
for i in range(servo_num):
judge.append('1')
while True:
# Read present position
for i in range(servo_num):
dxl_present_position[i], dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, DXL_ID[i],
ADDR_MX_PRESENT_POSITION)
if dxl_comm_result != COMM_SUCCESS:
print("%s" % packetHandler.getTxRxResult(dxl_comm_result), "servo_num is :", i + 1)
elif dxl_error != 0:
print("%s" % packetHandler.getRxPacketError(dxl_error), "servo_num is :", i + 1)
if abs(dxl_goal_position[i] - dxl_present_position[i]) < DXL_MOVING_STATUS_THRESHOLD[i]:
judge[i] = '0'
else:
dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL_ID[i], ADDR_MX_GOAL_POSITION,
dxl_goal_position[i])
if dxl_comm_result != COMM_SUCCESS:
print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
elif dxl_error != 0:
print("%s" % packetHandler.getRxPacketError(dxl_error))
print('Initial present position=', dxl_present_position)
print('Initial goal_position=', dxl_goal_position)
print('judge=', judge)
if '1' not in judge:
break
# -------------------------------------------------------------------------------------------------
print("Press any key to continue! (or press ESC to end the program !)")
# if getch() == chr(0x1b):
# import sys
#
# sys.exit(0)
servo_time = np.arange(start, end, 0.1)
for t in servo_time:
# Calculate goal position
for i in range(servo_num):
dxl_goal_position[i] = sin_fun(phi=phi[i], t=t, T=T)
# Write goal position
for i in range(servo_num):
dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL_ID[i], ADDR_MX_GOAL_POSITION,
dxl_goal_position[i])
time.sleep(0.1) # delay 0.1s---------------------------------------------------------------------------------
if dxl_comm_result != COMM_SUCCESS:
print("%s" % packetHandler.getTxRxResult(dxl_comm_result), "servo_num is :", i + 1)
if dxl_error != 0:
print("%s" % packetHandler.getRxPacketError(dxl_error), "servo_num is :", i + 1)
judge = []
for i in range(servo_num):
judge.append('1')
while True:
# Read present position
for i in range(servo_num):
dxl_present_position[i], dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler,
DXL_ID[i],
ADDR_MX_PRESENT_POSITION)
if dxl_comm_result != COMM_SUCCESS:
print("%s" % packetHandler.getTxRxResult(dxl_comm_result), "servo_num is :", i + 1)
if dxl_error != 0:
print("%s" % packetHandler.getRxPacketError(dxl_error), "servo_num is :", i + 1)
if abs(dxl_goal_position[i] - dxl_present_position[i]) < DXL_MOVING_STATUS_THRESHOLD[i]:
judge[i] = '0'
else:
dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL_ID[i],
ADDR_MX_GOAL_POSITION,
dxl_goal_position[i])
if dxl_comm_result != COMM_SUCCESS:
print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
elif dxl_error != 0:
print("%s" % packetHandler.getRxPacketError(dxl_error))
print('t=', t)
print('present position=', dxl_present_position)
print('goal_position=', dxl_goal_position)
print('judge=', judge)
if '1' not in judge:
break
# Disable Dynamixel Torque
# for i in range(servo_num):
# dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID[i], ADDR_MX_TORQUE_ENABLE,
# TORQUE_DISABLE)
# if dxl_comm_result != COMM_SUCCESS:
# print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
# elif dxl_error != 0:
# print("%s" % packetHandler.getRxPacketError(dxl_error))
# # Close port
# portHandler.closePort()
四、成果演示
目前只实现了翻滚动作，但是核心代码基本完成，下面就是增加舵机和拟定参数，拟定参数可能要加仿真，不然非人力所为。（视频在上传…）
总结
怎么说呢，代码在编写时候咨询了别人，自己做这个学了solidworks和python，但是觉得自己学的不够深入，在应用方面很欠缺，做这个时候也咨询了很多次老师，乔老师真的是很厉害，其他的指导老师不一定会给你讲太多指导太多，但是他是真的会的，我希望通过这次项目制作能够有提升，然后就是细细规划之后的事情，学习，考研，科研比赛等等…
下图是乔老师给的指导
下图是组装时候拍摄的
本文含有隐藏内容，请 开通VIP 后查看