您现在的位置是：首页 > 其他

当前栏目

对于STM32F103控制的三轴机械臂基本功能测试-关节转动控制

控制基本对于功能测试机械 STM32F103

2023-09-11 14:15:31 时间

➤01 机械臂调试

1.简介

对于 基于STM32对于三轴机械臂控制器设计 的设计已经进行了如下的调试：

下面对于该机械臂的两个关节转动进行调试：

1）肘关节运动：参见： 增加行星轮减速后机械臂调试 、 组装肩部带有减速器双轴机械臂组装与调试
2）肘关节运动：参见： 两轴机械臂+机械爪整体控制板设计与机械爪控制调试 、 双关节机械臂+机械爪运动控制

➤02 关节转动初步调试

1.接口¹

三个步进信号接口定义：

PSM1	PSM2	PSM3
肩关节	肘关节	上下运动

（1）电路图

▲ 三个步进电机信号输出控制端口

^{▲ 三个步进电机信号输出控制端口}

（2）步进电机设置

A. 肩关节步进电机： S1 ~ S6: ON,ON,OFF,ON,OFF,OFF
参考步进电机数据文件： 57HSXXXXEIS一体化步进伺服驱动电机

**B. 肘关节步进电机：**S1 ~ S5: OFF,OFF,OFF,OFF,ON
参考步进电机数据文件： 42HS48EIS步进闭环电机最大转速

（3）角度运行范围

根据 组装肩部带有减速器双轴机械臂组装与调试 给出的测试结果：

（1）肩部角度运行180°，给定脉冲个数：32000
（2）肘部角度运行180°，给定脉冲个数：25600

脉冲数量：正→顺时针旋转；负→逆时针旋转

2.设置中断服务程序²

（1）中断Timer4

通过中断程序：ArthrosisISR来对两个关节步进电机发送脉冲信号。中断程序是在10kHzTimer4的定时中断下工作的。输出脉冲的频率为：5kHz。

（2）中断频率细分

分别通过g_nArthrosisDividerSoulderCount, g_nArthrosisDividerElbowCount对于两个关节脉冲输出频率从5kHz进行细分。

//------------------------------------------------------------------------------
void ArthrosisISR(void) {
    //--------------------------------------------------------------------------
    g_nArthrosisDividerShoulderCount += g_nShoulderDividerInc;
    if(g_nArthrosisDividerShoulderCount >= ARTHROSIS_DIVIDER_SHOULDER_PERIOD) {
        g_nArthrosisDividerShoulderCount -= ARTHROSIS_DIVIDER_SHOULDER_PERIOD;
        g_nArthrosisShoulderCount ++;
        if(g_nShoulderAngle != 0) {
            if(g_nShoulderAngle > 0) {
                ON(SM_DIR1);
                if(g_nArthrosisShoulderCount & 0x1) {
                    ON(SM_PUL1);
                } else {
                    OFF(SM_PUL1);
                    g_nShoulderAngle --;
                }            
            } else {
                OFF(SM_DIR1);
                if(g_nArthrosisShoulderCount & 0x1) {
                    ON(SM_PUL1);
                } else {
                    OFF(SM_PUL1);
                    g_nShoulderAngle ++;
                }            
            }
        }
    }
    g_nArthrosisDividerElbowCount += g_nElbowDividerInc;
    if(g_nArthrosisDividerElbowCount >= ARTHROSIS_DIVIDER_ELBOW_PERIOD) {
        g_nArthrosisDividerElbowCount -= ARTHROSIS_DIVIDER_ELBOW_PERIOD;
        g_nArthrosisElbowCount ++;
        if(g_nElbowAngle != 0) {
            if(g_nElbowAngle > 0) {
                ON(SM_DIR2);
                if(g_nArthrosisElbowCount & 0x1) {
                    ON(SM_PUL2);
                } else {
                    OFF(SM_PUL2);
                    g_nElbowAngle --;
                }            
            } else {
                OFF(SM_DIR2);
                if(g_nArthrosisElbowCount & 0x1) {
                    ON(SM_PUL2);
                } else {
                    OFF(SM_PUL2);
                    g_nElbowAngle ++;
                }            
            }
        }
    }
}

3.设置角度极限检查

使用 ArthrosisLimitCheck()来检查角度的极限。

CONTROL_EXT int g_nArthrosisCheckCount;
#define ARTHROSIS_CHECK_PERIOD                  50
void ArthrosisLimitCheck(void);

#define SHOULDER_ANGLE_MIN          8192
#define SHOULDER_ANGLE_MAX          24576
#define ELBOW_ANGLE_MIN             4096
#define ELBOW_ANGLE_MAX             28672

void ArthrosisLimitCheck(void) {
    if(HAL_GetTick() < g_nArthrosisCheckCount) return;
    g_nArthrosisCheckCount = HAL_GetTick() + ARTHROSIS_CHECK_PERIOD;
    if(g_nShoulderAngle != 0) {        
        int nAngle = ShoulderAngle();
        if(g_nShoulderAngle < -1) {
            if(nAngle < SHOULDER_ANGLE_MIN)
                g_nShoulderAngle = -1;                
        } else if(g_nShoulderAngle > 1) {
            if(nAngle > SHOULDER_ANGLE_MAX)
                g_nShoulderAngle = 1;
        }        
    }
    if(g_nElbowAngle != 0) {        
        int nAngle = ElbowAngle();
        if(g_nElbowAngle < -1) {
            if(nAngle < ELBOW_ANGLE_MIN)
                g_nElbowAngle = -1;                
        } else if(g_nElbowAngle > 1) {            
            if(nAngle > ELBOW_ANGLE_MAX)
               g_nElbowAngle = 1;
        }        
    }
}

➤03 角度调节

1.角度设置

void ShoulderGotoAngle(int nAngle) {
    if(nAngle < SHOULDER_ANGLE_MIN) nAngle = SHOULDER_ANGLE_MIN;
    if(nAngle > SHOULDER_ANGLE_MAX) nAngle = SHOULDER_ANGLE_MAX;
    
    int nNowAngle = ShoulderAngle();
    int nDeltaAngle = (nAngle - nNowAngle) * SHOULDER_HALF_PULSE / 0x4000;
    g_nShoulderAngle += nDeltaAngle;
}

void ElbowGotoAngle(int nAngle) {
    if(nAngle < ELBOW_ANGLE_MIN) nAngle = ELBOW_ANGLE_MIN;
    if(nAngle > ELBOW_ANGLE_MAX) nAngle = ELBOW_ANGLE_MAX;
    
    int nNowAngle = ElbowAngle();
    int nDeltaAngle = (nAngle - nNowAngle) * ELBOW_HALF_PULSE / 0x4000;
    g_nElbowAngle += nDeltaAngle;
    
}

2.角度调节

void ShoulderAngleSet(int nAngle) {
    g_nElbowAngleSet = nAngle;
    g_nElbowAngleAdjustTime = 5;
}

void ElbowAngleSet(int nAngle) {
    g_nShoulderAngleSet = nAngle;
    g_nShoulderAngleAdjustTime = 5;
}

➤※ 附件

/*
**==============================================================================
** CONTROL.C:             -- by Dr. ZhuoQing, 2014-2-4
**
**==============================================================================
*/
#include "stm32f1xx_hal.h"
#include <stdio.h>
#include <math.h>
#include <string.h>
#include "stm32f1xxa.h"
#include "serialtxt.h"
#include "stdsub.h"
#include "config.h"
#include "cmdsub.h"
//------------------------------------------------------------------------------

#define CONTROL_GLOBALS        1                       // Define the global variables
#include "CONTROL.H"
#include "ST3806.H"

//==============================================================================
//                    Initialize function
//------------------------------------------------------------------------------
extern TIM_HandleTypeDef htim1, htim4;
void ControlInit(void) {
    SerialTxtInit();                            // Initialize the console debug interface
    
    //--------------------------------------------------------------------------
    HAL_TIM_Base_Start(&htim1);
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
    
    HAL_TIM_Base_Start_IT(&htim4);
    
    //----------------------------------------------------------------------
    OFF(SM_PUL1);                           // as NL2003 output, OFF means out off.
    OFF(SM_DIR1);
    OFF(SM_PUL2);
    OFF(SM_DIR2);
    OFF(SM_PUL3);
    OFF(SM_DIR3);
    OFF(SM_ENABLE);
    
    OUT(SM_PUL1);
    OUT(SM_DIR1);
    OUT(SM_PUL2);
    OUT(SM_DIR2);
    OUT(SM_PUL3);
    OUT(SM_DIR3);
    OUT(SM_ENABLE);
    
    //------------------------------------------------------------------        
    ON(ARM_POS0);
    ON(ARM_POS1);
    INPU(ARM_POS0);
    INPU(ARM_POS1);    
    g_nArmUpDownFlag = ARM_UPDOWN_STOP;
    g_nArmUpDownCount = 0;
    
    //--------------------------------------------------------------------------
    ST3806Init(1);
    
    //--------------------------------------------------------------------------
    g_nShoulderAngle = 0;
    g_nElbowAngle = 0;
    
    g_nArthrosisShoulderCount = 0;
    g_nArthrosisElbowCount = 0;
    g_nArthrosisDividerShoulderCount = 0;
    g_nArthrosisDividerElbowCount = 0;
    
    g_nShoulderDividerInc = ARTHROSIS_DIVIDER_SHOULDER_INC;
    g_nElbowDividerInc = ARTHROSIS_DIVIDER_ELBOW_INC;
    
    g_nArthrosisCheckCount = HAL_GetTick() + ARTHROSIS_CHECK_PERIOD;
    
    g_nShoulderAngleSet = ShoulderAngle();
    g_nElbowAngleSet = ElbowAngle();
    
    g_nShoulderAngleAdjustTime = 0;
    g_nElbowAngleAdjustTime = 0;
}

//--------------------------------------------------------------------------    
void ArmMoveUpDownISR(void) {
    if(g_nArmUpDownFlag == ARM_UPDOWN_STOP) 
        return;
    
    //--------------------------------------------------------------------------
    g_nArmUpDownISRCount ++;
    
    if(++g_nArmUpDownCount & 0x1) {
        ON(SM_PUL3);
    } else {
        OFF(SM_PUL3);
        
        if(g_nArmUpDownFlag == ARM_UPDOWN_MOVEUP) {
            if(ARM_POS_UP)    g_nArmUpDownFlag = ARM_UPDOWN_STOP;
        } else {
            if(ARM_POS_DOWN)  g_nArmUpDownFlag = ARM_UPDOWN_STOP;
        }
    }    
}

//------------------------------------------------------------------------------
void ArmMoveUp(void) {
    ON(SM_DIR3);
    g_nArmUpDownFlag = ARM_UPDOWN_MOVEUP;
    g_nArmUpDownISRCount = 0;
   
}

//------------------------------------------------------------------------------
void ArmMoveDown(void) {
    OFF(SM_DIR3);
    g_nArmUpDownFlag = ARM_UPDOWN_MOVEDOWN;
    g_nArmUpDownISRCount = 0;
}

void ArmMoveStop(void) {
    g_nArmUpDownFlag = ARM_UPDOWN_STOP;
    g_nArmUpDownISRCount = 0;
}        

//------------------------------------------------------------------------------
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;    
}

//------------------------------------------------------------------------------
void ArthrosisISR(void) {
    //--------------------------------------------------------------------------
    
    g_nArthrosisDividerShoulderCount += g_nShoulderDividerInc;
    if(g_nArthrosisDividerShoulderCount >= ARTHROSIS_DIVIDER_SHOULDER_PERIOD) {
        g_nArthrosisDividerShoulderCount -= ARTHROSIS_DIVIDER_SHOULDER_PERIOD;
        
        g_nArthrosisShoulderCount ++;
    
        if(g_nShoulderAngle != 0) {
            if(g_nShoulderAngle > 0) {
                ON(SM_DIR1);
            
                if(g_nArthrosisShoulderCount & 0x1) {
                    ON(SM_PUL1);
                } else {
                    OFF(SM_PUL1);
                    g_nShoulderAngle --;
                }            
            } else {
                OFF(SM_DIR1);
            
                if(g_nArthrosisShoulderCount & 0x1) {
                    ON(SM_PUL1);
                } else {
                    OFF(SM_PUL1);
                    g_nShoulderAngle ++;
                }            
            }
        
        }
    }

    g_nArthrosisDividerElbowCount += g_nElbowDividerInc;
    if(g_nArthrosisDividerElbowCount >= ARTHROSIS_DIVIDER_ELBOW_PERIOD) {
        g_nArthrosisDividerElbowCount -= ARTHROSIS_DIVIDER_ELBOW_PERIOD;
        
        g_nArthrosisElbowCount ++;
        
        if(g_nElbowAngle != 0) {
            if(g_nElbowAngle > 0) {
                ON(SM_DIR2);
            
                if(g_nArthrosisElbowCount & 0x1) {
                    ON(SM_PUL2);
                } else {
                    OFF(SM_PUL2);
                    g_nElbowAngle --;
                }            
            } else {
                OFF(SM_DIR2);
            
                if(g_nArthrosisElbowCount & 0x1) {
                    ON(SM_PUL2);
                } else {
                    OFF(SM_PUL2);
                    g_nElbowAngle ++;
                }            
            }
        }
        
    }
    
}

//------------------------------------------------------------------------------
void ArthrosisLimitCheck(void) {
    if(HAL_GetTick() < g_nArthrosisCheckCount) return;
    
    g_nArthrosisCheckCount = HAL_GetTick() + ARTHROSIS_CHECK_PERIOD;
    
    if(g_nShoulderAngle != 0) {        
        int nAngle = ShoulderAngle();
        
        if(g_nShoulderAngle < -1) {
            if(nAngle < SHOULDER_ANGLE_MIN)
                g_nShoulderAngle = -1;                
        } else if(g_nShoulderAngle > 1) {
            if(nAngle > SHOULDER_ANGLE_MAX)
                g_nShoulderAngle = 1;
        }        
    } else {
        if(g_nShoulderAngleAdjustTime > 0) {
            g_nShoulderAngleAdjustTime --;
            ShoulderGotoAngle(g_nShoulderAngleSet);
        }
        
    }
    
    if(g_nElbowAngle != 0) {        
        int nAngle = ElbowAngle();
        
        if(g_nElbowAngle < -1) {
            if(nAngle < ELBOW_ANGLE_MIN)
                g_nElbowAngle = -1;                
        } else if(g_nElbowAngle > 1) {            
            if(nAngle > ELBOW_ANGLE_MAX)
               g_nElbowAngle = 1;
        }        
    } else {
        if(g_nElbowAngleAdjustTime > 0) {
            g_nElbowAngleAdjustTime --;
//            ElbowGotoAngle(g_nElbowAngleSet);
        }

    }

}

//------------------------------------------------------------------------------
void ShoulderGotoAngle(int nAngle) {
    if(nAngle < SHOULDER_ANGLE_MIN) nAngle = SHOULDER_ANGLE_MIN;
    if(nAngle > SHOULDER_ANGLE_MAX) nAngle = SHOULDER_ANGLE_MAX;
    
    int nNowAngle = ShoulderAngle();
    int nDeltaAngle = (nAngle - nNowAngle) * SHOULDER_HALF_PULSE / 0x4000;
    g_nShoulderAngle += nDeltaAngle;
}

void ElbowGotoAngle(int nAngle) {
    if(nAngle < ELBOW_ANGLE_MIN) nAngle = ELBOW_ANGLE_MIN;
    if(nAngle > ELBOW_ANGLE_MAX) nAngle = ELBOW_ANGLE_MAX;
    
    int nNowAngle = ElbowAngle();
    int nDeltaAngle = (nAngle - nNowAngle) * ELBOW_HALF_PULSE / 0x4000;
    g_nElbowAngle += nDeltaAngle;
    
}

//------------------------------------------------------------------------------
void ShoulderAngleSet(int nAngle) {
    g_nElbowAngleSet = nAngle;
    g_nElbowAngleAdjustTime = 5;
}

void ElbowAngleSet(int nAngle) {
    g_nShoulderAngleSet = nAngle;
    g_nShoulderAngleAdjustTime = 5;
}

//------------------------------------------------------------------------------

//==============================================================================
//                END OF THE FILE : CONTROL.C
//------------------------------------------------------------------------------

/*
**==============================================================================
** CONTROL.H:             -- by Dr. ZhuoQing, 2014-2-4
**
**  Description:
**
**==============================================================================
*/
#ifndef __CONTROL__
#define __CONTROL__
//------------------------------------------------------------------------------
#ifdef CONTROL_GLOBALS
    #define CONTROL_EXT
#else
    #define CONTROL_EXT extern
#endif // CONTROL_GLOBALS
//------------------------------------------------------------------------------

//==============================================================================
//                INTERFACE INITIALIZE FUNCTION
//------------------------------------------------------------------------------
void ControlInit(void);

//#define ANGLE_DIR1      GPIOB,7
//#define ANGLE_DIR2      GPIOB,6
#define BEEP            GPIOB,5
#define KEYIN           GPIOB,4    

//---------------------------------------------------------------------------   
#define SM_PUL1         GPIOA,4
#define SM_DIR1         GPIOA,5
#define SM_PUL2         GPIOA,6
#define SM_DIR2         GPIOA,7
#define SM_PUL3         GPIOB,0
#define SM_DIR3         GPIOB,1
#define SM_ENABLE       GPIOC,15

#define ARM_MOVE_UP     ON(SM_DIR3)
#define ARM_MOVE_DOWN   OFF(SM_DIR3)

//------------------------------------------------------------------------------
#define ARM_POS0        GPIOA,0
#define ARM_POS1        GPIOA,1

#define ARM_POS_UP      VAL(ARM_POS1)
#define ARM_POS_DOWN    VAL(ARM_POS0)

void ArmMoveUpDownISR(void);
CONTROL_EXT int g_nArmUpDownFlag;           // 0: Stop; 1:Move Up; 2:Move Down
CONTROL_EXT int g_nArmUpDownCount;          // Last bit generate pulse
#define ARM_UPDOWN_STOP         0
#define ARM_UPDOWN_MOVEUP       1
#define ARM_UPDOWN_MOVEDOWN     2

CONTROL_EXT int g_nArmUpDownISRCount;

void ArmMoveUp(void);
void ArmMoveDown(void);
void ArmMoveStop(void);

//------------------------------------------------------------------------------
#define ARM_SHOULDER_OFFSET     15176
#define ARM_ELBOW_OFFSET        7433

unsigned int ShoulderAngle(void);
unsigned int ElbowAngle(void);

//------------------------------------------------------------------------------
CONTROL_EXT int g_nShoulderAngle, g_nElbowAngle;
CONTROL_EXT int g_nArthrosisShoulderCount, g_nArthrosisElbowCount;
CONTROL_EXT int g_nArthrosisDividerShoulderCount, g_nArthrosisDividerElbowCount;

#define ARTHROSIS_DIVIDER_SHOULDER_PERIOD       100
#define ARTHROSIS_DIVIDER_SHOULDER_INC          65
#define ARTHROSIS_DIVIDER_ELBOW_PERIOD          100
#define ARTHROSIS_DIVIDER_ELBOW_INC             60

CONTROL_EXT int g_nShoulderDividerInc, g_nElbowDividerInc;

void ArthrosisISR(void);

//------------------------------------------------------------------------------
CONTROL_EXT int g_nArthrosisCheckCount;
#define ARTHROSIS_CHECK_PERIOD                  50
void ArthrosisLimitCheck(void);

#define SHOULDER_ANGLE_MIN          8192
#define SHOULDER_ANGLE_MAX          24576
#define ELBOW_ANGLE_MIN             4096
#define ELBOW_ANGLE_MAX             28672

#define SHOULDER_HALF_PULSE         32000
#define ELBOW_HALF_PULSE            25600
        
//------------------------------------------------------------------------------
void ShoulderGotoAngle(int nAngle);
void ElbowGotoAngle(int nAngle);

//------------------------------------------------------------------------------
CONTROL_EXT int g_nShoulderAngleSet, g_nElbowAngleSet;
CONTROL_EXT int g_nShoulderAngleAdjustTime;
CONTROL_EXT int g_nElbowAngleAdjustTime;

void ShoulderAngleSet(int nAngle);
void ElbowAngleSet(int nAngle);
    
//==============================================================================
//             END OF THE FILE : CONTROL.H
//------------------------------------------------------------------------------
#endif // __CONTROL__

■ 相关文献链接:

硬件结构AD设计工程文件:AD\XQWF\2020\机械臂\CNTINTERFACE.PcbDoc ↩︎
STM32单片机工程文件:STM32\Application\XQWF\2020\CNTSTM103\ ↩︎

猜你喜欢

ASP.NET MVC - Internet 应用程序
ART世界探险(14) - 快速编译器和优化编译器
【JAVA】Java循环语句中的continue跳转进入下一次循环是否判断循环条件
完全二叉树的权值
ssm使用全注解实现增删改查案例——EmpServiceImpl
【人工智能AI】一、NoSQL 企业级基础入门《NoSQL 企业级基础入门与进阶实战》
从零开始手写Tomcat的教程8节----加载器
C#.NET常见问题(FAQ)-如何改变字符串编码
约瑟夫环问题
移动端整页滑屏示例（上下滑动整屏）demo效果示例（整理）
Spring4 MVC HelloWorld 注解和JavaConfig实例
c语言实例
Rsync+Inotify实现数据实时同步
php单元测试
已解决urllib.error.HTTPError: HTTP Error 403: Forbidden

相关主题

python控制键盘
IOC控制反转
输出控制类
TCP 拥塞控制
TCP流量控制
2.1.3进程控制
10进程控制1
流程控制
3流程控制
域控制
路由控制
url控制
Java控制流程
mysql流程控制
PID控制
12、流程控制
mysql 事务控制
js的流程控制

zl程序教程

当前栏目

对于STM32F103控制的三轴机械臂基本功能测试-关节转动控制

➤01 机械臂调试

1.简介

➤02 关节转动初步调试

1.接口¹

（1）电路图

（2）步进电机设置

（3）角度运行范围

2.设置中断服务程序²

（1）中断Timer4

（2）中断频率细分

3.设置角度极限检查

➤03 角度调节

1.角度设置

2.角度调节

➤※ 附件

相关文章

当前栏目

对于STM32F103控制的三轴机械臂基本功能测试-关节转动控制

➤01 机械臂调试

1.简介

➤02 关节转动初步调试

1.接口1

（1）电路图

（2）步进电机设置

（3）角度运行范围

2.设置中断服务程序2

（1）中断Timer4

（2）中断频率细分

3.设置角度极限检查

➤03 角度调节

1.角度设置

2.角度调节

➤※ 附件

相关文章

1.接口¹

2.设置中断服务程序²