对于STM32F103三轴机械臂控制器进行基本功能测试-关节角度读取
2024-06-01 10:12:44
➤01 机械臂调试
1.简介
对 基于STM32对于三轴机械臂控制器设计 中对应的控制电路读取肩部和肘部两个 角度编码器 ST-3806-15-RS 数据进行调试。
2.接口电路1
▲ 角度读取的相关电路和PCB
2.单片机软件
单片机软件所在目录：
D:\zhuoqing\window\ARM\IAR\STM32\Application\XQWF\2020\CNTSTM103\Src\main.c
➤02 基本调试2
1. UART2输入输出
通过UART2发送字符，测试在MAX485的输出波形显示互补的信号。
2.建立ST3806文件
从 角度编码器 ST-3806-15-RS 拷贝 ST3806的头文件和C文件并建立相应的文件，加入Project。 具体文件内容参见附录。
注意：Control中调用ST3806Init()初始化的时候，需要注意到它的输入参数应该设置为1。
通过下面的过程，可以测量读取两个角度值所需要的缠绵大约在3 ~ 4ms。
if(++nShowCount >= 500) {nShowCount = 0;int nBegin = HAL_GetTick();unsigned int nNumber1 = ShoulderAngle(); //ST3806ReadNumber(ST3806_CHANNEL_1);unsigned int nNumber2 = ElbowAngle(); //ST3806ReadNumber(ST3806_CHANNEL_2);int nEnd = HAL_GetTick();printf("%d %d %d\r\n", nNumber1, nNumber2, nEnd-nBegin);}
3.读取两个关节的最大值和最小值
从顶部看肩关节和肘关节的角度极值分别为：
（1）肩关节角度极值范围
- 顺时针： 11157
- 逆时针： 23376
▲ 顺时针旋转，从左90°旋转刀右90°
（2）肘关节角度极值范围
- 顺时针： 23913
- 逆时针： 27646
▲ 顺时针旋转，从左135°旋转刀右135°
4.角度左右补偿
（1）目标
由于传感器一周的分辨率是15bit，角度是以θT=0x8000\theta _T = 0x8000θT=0x8000为周期重复的。所以机械臂在旋转过程中它的数值可能会从0x8000→0，或者0→0x8000突变。
通过角度增加一个偏置θoffset\theta _{offset}θoffset，使得修正后的角度，在机械臂旋转范围内，不会出现上述的突变。实际的角度通过如下公式计算：
θnew=(θ+θoffset)%0x8000\theta _{new} = \left( {\theta + \theta _{offset} } \right)\% 0x8000θnew=(θ+θoffset)%0x8000
（2）获得偏移量
如何获得最佳的偏移量θoffset\theta _{offset}θoffset使得输出的角度值范围能够位于0x0 ~ 0x8000的中间位置0x4000呢？
下面给出了角度偏移量θoffset=ΔA\theta _{offset} = \Delta Aθoffset=ΔA的计算公式。
#!/usr/local/bin/python
# -*- coding: gbk -*-
#============================================================
# TEST1.PY -- by Dr. ZhuoQing 2020-12-21
#
# Note:
#============================================================
from headm import *
from tsmodule.tsstm32 import *
def deltaA(amin, amax):amax = amax if amax >= amin else 0x8000+amaxa0 = (amin + amax) // 2delA = 0x4000-a0 if a0 <= 0x4000 else 0xc000-a0return delA
#------------------------------------------------------------
data = stm32memo(1)
delA = deltaA(data[0], data[-1])
newdata = [(a + delA) % 0x8000 for a in data]
printf('DeltaA:%d'%delA)
printf(data, newdata)
plt.plot(data, label='Origin')
plt.plot(newdata, label='Modifier')
plt.xlabel("Sample")
plt.ylabel("Angle")
plt.grid(True)
plt.tight_layout()
plt.legend(loc='upper right')
plt.show()
#------------------------------------------------------------
# END OF FILE : TEST1.PY
#============================================================
（3）测试数值
对于肩关节：ΔA=15176\Delta A = 15176ΔA=15176
▲ 肩关节经过修正后的角度变化范围
对于肘关节的数据：ΔA=7433\Delta A = 7433ΔA=7433
▲ 肘关节角度补偿前和补偿后的曲线
➤03 程序接口
下面分别是在control.h, control.c下的两个接口程序：
//------------------------------------------------------------------------------
#define ARM_SHOULDER_OFFSET 15176
#define ARM_ELBOW_OFFSET 7433unsigned int ShoulderAngle(void);
unsigned int ElbowAngle(void);//------------------------------------------------------------------------------
unsigned int ShoulderAngle(void) {unsigned int nAngle;nAngle = ST3806ReadNumber(ST3806_CHANNEL_1);return (nAngle + ARM_SHOULDER_OFFSET) & 0x7fff;
}unsigned int ElbowAngle(void) {unsigned int nAngle;nAngle = ST3806ReadNumber(ST3806_CHANNEL_2);return (nAngle + ARM_ELBOW_OFFSET) & 0x7fff;
}
➤※ 附录
1.ST3806角度传感器的读写程序
/*
**==============================================================================
** ST3806.C: -- by Dr. ZhuoQing, 2020-09-14
**
**==============================================================================
*/#include "stm32f1xx_hal.h"
#include <stdio.h>
#include <math.h>
#include <string.h>
#include "stm32f1xxa.h"//------------------------------------------------------------------------------
#define ST3806_GLOBALS 1 // Define the global variables
#include "ST3806.H"//------------------------------------------------------------------------------//------------------------------------------------------------------------------
void ST3806Init(unsigned char ucAddress) { // Default ucAddress should be 1OFF(ST3806_DIR1_PIN);OUT(ST3806_DIR1_PIN);OFF(ST3806_DIR2_PIN);OUT(ST3806_DIR2_PIN);g_ucST3806Address = ucAddress; g_ucST3806Channel = ST3806_CHANNEL_1;
}//------------------------------------------------------------------------------
void UART2ClearBuffer(void) {while(1) {if(!UART2_CANRECE) return;unsigned char ucChar;UART2ReceChar(&ucChar);}}//------------------------------------------------------------------------------
void ST3806SetChannel(unsigned char ucChannel) {g_ucST3806Channel = ucChannel;OFF(ST3806_DIR1_PIN);OFF(ST3806_DIR2_PIN);
}void ST3806DirON(void) {if(g_ucST3806Channel == ST3806_CHANNEL_1) {OFF(ST3806_DIR2_PIN);ON(ST3806_DIR1_PIN);} else {OFF(ST3806_DIR1_PIN);ON(ST3806_DIR2_PIN); }}void ST3806DirOFF(void) {OFF(ST3806_DIR2_PIN);OFF(ST3806_DIR1_PIN);
}//------------------------------------------------------------------------------
unsigned int ModbusCRC(unsigned char * pByte, int nLength) {unsigned int nCRC;unsigned int i;unsigned char ucByte, j;nCRC = 0xffff;for(i = 0; i < nLength; i ++) {ucByte = *(pByte + i);nCRC ^= (unsigned int)ucByte;for(j = 0; j < 8; j ++) {if((nCRC & 0x1) != 0) {nCRC >>= 1;nCRC ^= 0xA001;} else nCRC >>= 1;} }return nCRC;
}//------------------------------------------------------------------------------
void ST3806AppendCRC(unsigned char ucLength) {unsigned int nCRC;nCRC = ModbusCRC(g_ucST3806Buffer, ucLength);g_ucST3806Buffer[ucLength + 1] = (unsigned char)(nCRC >> 8);g_ucST3806Buffer[ucLength] = (unsigned char)nCRC;
}//------------------------------------------------------------------------------
void ST3806SendBuffer(unsigned char ucLength) {unsigned char i;ST3806AppendCRC(ucLength);ST3806DirON();for(i = 0; i < ucLength + 2; i ++) {UART2SendChar(g_ucST3806Buffer[i]);}ST3806DirOFF();UART2_CLEAR;}//------------------------------------------------------------------------------
unsigned char ST3806SetBaud115200(void) {unsigned char i, c;g_ucST3806Buffer[0] = g_ucST3806Address;g_ucST3806Buffer[1] = 0x10;g_ucST3806Buffer[2] = 0x0;g_ucST3806Buffer[3] = 0x3;g_ucST3806Buffer[4] = 0x0;g_ucST3806Buffer[5] = 0x1;g_ucST3806Buffer[6] = 0x2;g_ucST3806Buffer[7] = 0x0;g_ucST3806Buffer[8] = 0x5;ST3806SendBuffer(9);for(i = 0; i < 100; i ++) {if(UART2ReceChar(&c) == 0) break;}if(i < 100)g_ucST3806Buffer[0] = c;else return 1;/* for(i = 1; i < 8; i ++) {if(UART2ReceChar(&c)) return 10+i;g_ucST3806Buffer[i] = c;}
*/ return 0;
}//------------------------------------------------------------------------------
unsigned char ST3806ReadData2Buffer(void) {unsigned char i, c;g_ucST3806Buffer[0] = g_ucST3806Address;g_ucST3806Buffer[1] = 0x3;g_ucST3806Buffer[2] = 0x0;g_ucST3806Buffer[3] = 0x0;g_ucST3806Buffer[4] = 0x0;g_ucST3806Buffer[5] = 0x1;ST3806SendBuffer(6);g_ucST3806Buffer[3] = 0x0;g_ucST3806Buffer[4] = 0x0;g_ucST3806Buffer[5] = 0x0;g_ucST3806Buffer[6] = 0x0;for(i = 0; i < 10; i ++) {if(UART2ReceChar(&c) == 0) break;}if(i < 10)g_ucST3806Buffer[0] = c;else return 1;for(i = 1; i < 7; i ++) {if(UART2ReceChar(&c)) return 1;g_ucST3806Buffer[i] = c;}return 0;}//------------------------------------------------------------------------------
unsigned char ST3806ReadConfig2Buffer(void) {unsigned char i, c;g_ucST3806Buffer[0] = g_ucST3806Address;g_ucST3806Buffer[1] = 0x3;g_ucST3806Buffer[2] = 0x0;g_ucST3806Buffer[3] = 0x0;g_ucST3806Buffer[4] = 0x0;g_ucST3806Buffer[5] = 0x3;ST3806SendBuffer(6);for(i = 0; i < 10; i ++) {if(UART2ReceChar(&c) == 0) break;}if(i < 10)g_ucST3806Buffer[0] = c;else return 1;for(i = 1; i < 11; i ++) {if(UART2ReceChar(&c)) return 1;g_ucST3806Buffer[i] = c;}return 0;
}//------------------------------------------------------------------------------
unsigned int ST3806Buffer2Number(void) {unsigned int nNumber;nNumber = g_ucST3806Buffer[3];nNumber = (nNumber << 8) + g_ucST3806Buffer[4];return nNumber;
}//------------------------------------------------------------------------------
unsigned int ST3806ReadNumber(unsigned char ucChannel) { ST3806SetChannel(ucChannel);ST3806ReadData2Buffer();return ST3806Buffer2Number();
}//==============================================================================
// END OF THE FILE : ST3806.C
//------------------------------------------------------------------------------
/*
**==============================================================================
** ST3806.H: -- by Dr. ZhuoQing, 2020-12-18
**
** Description:
**
**==============================================================================
*/
#ifndef __ST3806__
#define __ST3806__
//------------------------------------------------------------------------------
#ifdef ST3806_GLOBALS#define ST3806_EXT
#else#define ST3806_EXT extern
#endif // ST3806_GLOBALS
//------------------------------------------------------------------------------
#define ST3806_ADDRESS 1#define ST3806_DIR1_PIN GPIOB,7
#define ST3806_DIR2_PIN GPIOB,6//==============================================================================
void ST3806Init(unsigned char ucAddress); // ucAddress: Default is 1#define ST3806_CHANNEL_1 0
#define ST3806_CHANNEL_2 1ST3806_EXT unsigned char g_ucST3806Channel;
void ST3806SetChannel(unsigned char ucChannel);
void ST3806DirON(void);
void ST3806DirOFF(void);//------------------------------------------------------------------------------
#define ST3806_BUFFER 16ST3806_EXT unsigned char g_ucST3806Buffer[ST3806_BUFFER];
ST3806_EXT unsigned char g_ucST3806Address;unsigned int ModbusCRC(unsigned char * pByte, int nLength);
void ST3806AppendCRC(unsigned char ucLength);//------------------------------------------------------------------------------
void ST3806SendCharBuffer(unsigned char ucLength);unsigned char ST3806ReadData2Buffer(void);
unsigned char ST3806ReadConfig2Buffer(void);unsigned int ST3806Buffer2Number(void);//------------------------------------------------------------------------------
unsigned int ST3806ReadNumber(unsigned char ucChannel); // Return about 2ms(&115200bps)unsigned char ST3806SetBaud115200(void);//------------------------------------------------------------------------------
#define UART2_CLEAR UART2ClearBuffer()
void UART2ClearBuffer(void);//==============================================================================
// END OF THE FILE : ST3806.H
//------------------------------------------------------------------------------
#endif // __ST3806__
■ 相关文献链接:
- 基于STM32对于三轴机械臂控制器设计
- 角度编码器 ST-3806-15-RS
- 对于STM32F103三轴机械臂控制器进行基本功能测试-上下运动功能
接口板上的电路AD工程文件:AD\XQWF\2020\机械臂\CNTINTERFACE.SchDoc ↩︎
调试工程文件：STM32工程文件:STM32\Application\XQWF\2020\CNTSTM103\ ↩︎
对于STM32F103三轴机械臂控制器进行基本功能测试-关节角度读取相关推荐
- 对于STM32F103三轴机械臂控制器进行基本功能测试-上下运动功能
简 介: 本文针对基于在基于STM32对于三轴机械臂控制器设计 中设计了控制电路板.本文记录对其在机械臂实际部件进行调试的过程. 关键词: STM32F103,机械臂,三轴,上下运动 ➤01 机械臂控 ...
- 基于STM32对于三轴机械臂控制器设计
简 介: 本文使用了STM32对于一款三轴机械臂进行初步驱动,调试他的各个关节的运动情况. 关键词: 机械臂,三轴机械臂,STM32 ➤ 01背景 在 组装肩部带有减速器双轴机械臂组装与调试 的调试基 ...
- 对于STM32F103控制的三轴机械臂基本功能测试-关节转动控制
➤01 机械臂调试 1.简介 对于 基于STM32对于三轴机械臂控制器设计 的设计已经进行了如下的调试: 对于STM32F103三轴机械臂控制器进行基本功能测试-关节角度读取 对于STM32F103三 ...
- 基于STM32F103双轴机械臂完整电路板设计
➤01 机械臂设计 在 基于STM32对于三轴机械臂控制器设计 设计了机械臂的控制电路板.采用了双板分开设计方式.并分别进行了如下的测试: 1. 对于STM32F103三轴机械臂控制器进行基本 ...
- Rviz玩转三轴机械臂
前言 最近想加深ROS仿真机械臂的理解,所以笔者参考一些资料与博客,在ROS下搭个简单的三轴机械臂,在Rviz下实现各轴关节转动,如果后续有时间的话,可能会更新下Gazebo下仿真,如果时间不够的话, ...
- 六轴机械臂控制器 控制卡 软件 机械臂
六轴机械臂控制器 控制卡 软件 机械臂27400624681113128教学实训设备
- grbl控制3轴机械臂 原理 实现 （四） 之GRBL源码修改驱动三轴机械臂
往期回顾: 第一篇:grbl控制3轴机械臂 原理 实现 (一) 之2D机械臂模拟及实现 第二篇:grbl控制3轴机械臂 原理 实现 (二) 之3D机械臂模拟及实现 第三篇:grbl控制3轴机械臂 原理 ...
- 逆运动学：RRR型 2D 三轴机械臂的IK求解 | 机械臂运动学笔记（二）
任务: 给定末端的(x,y, \phi),求各轴角度( θ 1 , θ 2 , θ 3 \theta_1 , \theta_2, \theta_3 θ1,θ2,θ3) 先将多个空间几何拆解成平面 ...
- 三轴机械臂/三自由度四足单腿DH正逆运动学及matlab验证
实物模型 DH建立坐标系以及正逆运动学推导 Matlab验证 clear; clc; a1=-9.57*0.001;alpha1=pi/2; a2=-59.2*0.001; a3=-77*0.001; ...
最新文章
- 第三部分：Android 应用程序接口指南---第三节：应用程序资源---第四章 本地化...
- 5 HBase命令行接口
- pytorch中的torch.rand(),torch.randn(),torch.randerm()的关系
- 即将改变软件开发的5个Java9新特性
- ThreadPoolExecutor 中的workerDone(this); 为什么会时不时的就会在这个地方停下来???
- php bloginfo templatedirectory,PHP变量不显示使用bloginfo（'template_directory'）的图像
- 分布式消息系列：详解RocketMQ的简介与演进、架构设计、关键特性与应用场景
- 系统性能优化- Session丢失
- jsoncpp添加对象、数组与json对象的解析
- C++编程读取注册表文件
- netsetman使用教程_NetSetMan配置网络参数教程
- Mac 文本对比工具（比较两份文件差异）
- css svg做动图,如何制作svg动态图
- 慌乱的表情，泄露了我的悲伤：伤感心情日志
- 广告roi怎么计算公式_什么是广告ROI?ROI计算公式是什么？ ROI怎么计算?ROI影响因素有哪些?...
- 电子元件-温湿度元件与传感器
- 【人因工程】熵值法与CRITIC法求权重
- 网络原理——网络层与数据链路层
- Mahout in action 中文版-3.推荐器的数据表达
- 单元测试、注解、枚举、反射（5）JavaSE