Gausst松鼠会

这个屌丝很懒，什么也没留下！

热门标签

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

作者：Gausst松鼠会 | 2024-04-11 13:55:45

踩

stm32控制机械臂原理图

➤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 ++;
                }            
            }
        }
    }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52

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
1
2
3
4
5
6
7
8

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;
        }        
    }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

➤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;
    
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

2.角度调节

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

void ElbowAngleSet(int nAngle) {
    g_nShoulderAngleSet = nAngle;
    g_nShoulderAngleAdjustTime = 5;
}
1
2
3
4
5
6
7
8
9

➤※ 附件

/*
**==============================================================================
** 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
//------------------------------------------------------------------------------
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283

/*
**==============================================================================
** 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
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110

■ 相关文献链接:

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

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/Gausst松鼠会/article/detail/405356