STM32
直播中

名士流

8年用户 892经验值
私信 关注
[问答]

如何实现基于STM32对于三轴机械臂控制器设计？

如何实现基于STM32对于三轴机械臂控制器设计？

回帖（1）

张晓宁

2021-12-22 11:13:14

➤01 机械臂调试

  1.简介

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

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

➤02 关节转动初步调试

  1.接口1

三个步进信号接口定义：
[tr]PSM1PSM2PSM3[/tr]
肩关节肘关节上下运动
（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.设置中断服务程序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
#include
#include
#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__
举报

更多回帖

1 0 0

相关问答

STM32
发帖
电子发烧友APP
|投诉反馈|电子发烧友网
© 2021 bbs.elecfans.com
湘ICP备2023018690号
×
20
完善资料，
赚取积分