Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
D
darwin_stm32_fw
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Model registry
Operate
Environments
Monitor
Incidents
Service Desk
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
humanoides
darwin
darwin_stm32_fw
Commits
a0ed6f38
Commit
a0ed6f38
authored
9 years ago
by
Sergi Hernandez
Browse files
Options
Downloads
Patches
Plain Diff
Updated the darwin register addresses.
Added a flag to indicate the scan for servos is in progress.
parent
e523e00a
No related branches found
Branches containing commit
No related tags found
No related merge requests found
Changes
4
Hide whitespace changes
Inline
Side-by-side
Showing
4 changed files
include/darwin_registers.h
+90
-89
90 additions, 89 deletions
include/darwin_registers.h
src/cm730_fw.c
+1
-0
1 addition, 0 deletions
src/cm730_fw.c
src/darwin_dyn_master.c
+3
-0
3 additions, 0 deletions
src/darwin_dyn_master.c
src/motion_manager.c
+3
-0
3 additions, 0 deletions
src/motion_manager.c
with
97 additions
and
89 deletions
include/darwin_registers.h
+
90
−
89
View file @
a0ed6f38
...
@@ -227,117 +227,117 @@ typedef enum {
...
@@ -227,117 +227,117 @@ typedef enum {
DARWIN_IMU_ACCEL_Z_L
=
0x0148
,
DARWIN_IMU_ACCEL_Z_L
=
0x0148
,
DARWIN_IMU_ACCEL_Z_H
=
0x0149
,
DARWIN_IMU_ACCEL_Z_H
=
0x0149
,
DARWIN_MM_NUM_SERVOS
=
0x01
28
,
DARWIN_MM_NUM_SERVOS
=
0x01
4A
,
DARWIN_MM_CNTRL
=
0x01
29
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
DARWIN_MM_CNTRL
=
0x01
4B
,
//
bit 7
| bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
//
| | | | | Enable power | Enable balance | Enable manager
//
scanning
| | | | | Enable power | Enable balance | Enable manager
DARWIN_MM_MODULE_EN0
=
0x01
2A
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0i
DARWIN_MM_MODULE_EN0
=
0x01
4C
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0i
// Enable servo 0 | assigned module | Enable servo 1 | assigned module
// Enable servo 0 | assigned module | Enable servo 1 | assigned module
// | 000 -> none |
// | 000 -> none |
// | 001 -> action |
// | 001 -> action |
// | 010 -> walk |
// | 010 -> walk |
// | 011 -> joints |
// | 011 -> joints |
DARWIN_MM_MODULE_EN1
=
0x01
2B
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
DARWIN_MM_MODULE_EN1
=
0x01
4D
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// Enable servo 2 | assigned module | Enable servo 3 | assigned module
// Enable servo 2 | assigned module | Enable servo 3 | assigned module
DARWIN_MM_MODULE_EN2
=
0x01
2C
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
DARWIN_MM_MODULE_EN2
=
0x01
4E
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// Enable servo 4 | assigned module | Enable servo 5 | assigned module
// Enable servo 4 | assigned module | Enable servo 5 | assigned module
DARWIN_MM_MODULE_EN3
=
0x01
2D
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
DARWIN_MM_MODULE_EN3
=
0x01
4F
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// Enable servo 6 | assigned module | Enable servo 7 | assigned module
// Enable servo 6 | assigned module | Enable servo 7 | assigned module
DARWIN_MM_MODULE_EN4
=
0x01
2E
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
DARWIN_MM_MODULE_EN4
=
0x01
50
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// Enable servo 8 | assigned module | Enable servo 9 | assigned module
// Enable servo 8 | assigned module | Enable servo 9 | assigned module
DARWIN_MM_MODULE_EN5
=
0x01
2F
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
DARWIN_MM_MODULE_EN5
=
0x01
51
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// Enable servo 10 | assigned module | Enable servo 11 | assigned module
// Enable servo 10 | assigned module | Enable servo 11 | assigned module
DARWIN_MM_MODULE_EN6
=
0x01
30
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
DARWIN_MM_MODULE_EN6
=
0x01
52
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// Enable servo 12 | assigned module | Enable servo 13 | assigned module
// Enable servo 12 | assigned module | Enable servo 13 | assigned module
DARWIN_MM_MODULE_EN7
=
0x013
1
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
DARWIN_MM_MODULE_EN7
=
0x01
5
3
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// Enable servo 14 | assigned module | Enable servo 15 | assigned module
// Enable servo 14 | assigned module | Enable servo 15 | assigned module
DARWIN_MM_MODULE_EN8
=
0x01
32
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
DARWIN_MM_MODULE_EN8
=
0x01
54
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// Enable servo 16 | assigned module | Enable servo 17 | assigned module
// Enable servo 16 | assigned module | Enable servo 17 | assigned module
DARWIN_MM_MODULE_EN9
=
0x01
33
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
DARWIN_MM_MODULE_EN9
=
0x01
55
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// Enable servo 18 | assigned module | Enable servo 19 | assigned module
// Enable servo 18 | assigned module | Enable servo 19 | assigned module
DARWIN_MM_MODULE_EN10
=
0x01
34
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
DARWIN_MM_MODULE_EN10
=
0x01
56
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// Enable servo 20 | assigned module | Enable servo 21 | assigned module
// Enable servo 20 | assigned module | Enable servo 21 | assigned module
DARWIN_MM_MODULE_EN11
=
0x01
3
5
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
DARWIN_MM_MODULE_EN11
=
0x015
7
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// Enable servo 22 | assigned module | Enable servo 23 | assigned module
// Enable servo 22 | assigned module | Enable servo 23 | assigned module
DARWIN_MM_MODULE_EN12
=
0x01
36
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
DARWIN_MM_MODULE_EN12
=
0x01
58
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// Enable servo 24 | assigned module | Enable servo 25 | assigned module
// Enable servo 24 | assigned module | Enable servo 25 | assigned module
DARWIN_MM_MODULE_EN13
=
0x01
37
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
DARWIN_MM_MODULE_EN13
=
0x01
59
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// Enable servo 26 | assigned module | Enable servo 27 | assigned module
// Enable servo 26 | assigned module | Enable servo 27 | assigned module
DARWIN_MM_MODULE_EN14
=
0x01
38
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
DARWIN_MM_MODULE_EN14
=
0x01
5A
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// Enable servo 28 | assigned module | Enable servo 29 | assigned module
// Enable servo 28 | assigned module | Enable servo 29 | assigned module
DARWIN_MM_MODULE_EN15
=
0x01
39
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
DARWIN_MM_MODULE_EN15
=
0x01
5B
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// Enable servo 30 | assigned module | Enable servo 31 | assigned module
// Enable servo 30 | assigned module | Enable servo 31 | assigned module
DARWIN_MM_SERVO0_CUR_POS_L
=
0x01
3A
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO0_CUR_POS_L
=
0x01
5C
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO0_CUR_POS_H
=
0x01
3B
,
DARWIN_MM_SERVO0_CUR_POS_H
=
0x01
5D
,
DARWIN_MM_SERVO1_CUR_POS_L
=
0x01
3C
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO1_CUR_POS_L
=
0x01
5E
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO1_CUR_POS_H
=
0x01
3D
,
DARWIN_MM_SERVO1_CUR_POS_H
=
0x01
5F
,
DARWIN_MM_SERVO2_CUR_POS_L
=
0x01
3E
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO2_CUR_POS_L
=
0x01
60
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO2_CUR_POS_H
=
0x01
3F
,
DARWIN_MM_SERVO2_CUR_POS_H
=
0x01
61
,
DARWIN_MM_SERVO3_CUR_POS_L
=
0x01
40
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO3_CUR_POS_L
=
0x01
62
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO3_CUR_POS_H
=
0x01
41
,
DARWIN_MM_SERVO3_CUR_POS_H
=
0x01
63
,
DARWIN_MM_SERVO4_CUR_POS_L
=
0x014
2
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO4_CUR_POS_L
=
0x01
6
4
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO4_CUR_POS_H
=
0x01
43
,
DARWIN_MM_SERVO4_CUR_POS_H
=
0x01
65
,
DARWIN_MM_SERVO5_CUR_POS_L
=
0x01
44
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO5_CUR_POS_L
=
0x01
66
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO5_CUR_POS_H
=
0x01
45
,
DARWIN_MM_SERVO5_CUR_POS_H
=
0x01
67
,
DARWIN_MM_SERVO6_CUR_POS_L
=
0x01
4
6
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO6_CUR_POS_L
=
0x016
8
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO6_CUR_POS_H
=
0x01
47
,
DARWIN_MM_SERVO6_CUR_POS_H
=
0x01
69
,
DARWIN_MM_SERVO7_CUR_POS_L
=
0x01
48
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO7_CUR_POS_L
=
0x01
6A
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO7_CUR_POS_H
=
0x01
49
,
DARWIN_MM_SERVO7_CUR_POS_H
=
0x01
6B
,
DARWIN_MM_SERVO8_CUR_POS_L
=
0x01
4A
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO8_CUR_POS_L
=
0x01
6C
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO8_CUR_POS_H
=
0x01
4B
,
DARWIN_MM_SERVO8_CUR_POS_H
=
0x01
6D
,
DARWIN_MM_SERVO9_CUR_POS_L
=
0x01
4C
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO9_CUR_POS_L
=
0x01
6E
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO9_CUR_POS_H
=
0x01
4D
,
DARWIN_MM_SERVO9_CUR_POS_H
=
0x01
6F
,
DARWIN_MM_SERVO10_CUR_POS_L
=
0x01
4E
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO10_CUR_POS_L
=
0x01
70
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO10_CUR_POS_H
=
0x01
4F
,
DARWIN_MM_SERVO10_CUR_POS_H
=
0x01
71
,
DARWIN_MM_SERVO11_CUR_POS_L
=
0x01
50
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO11_CUR_POS_L
=
0x01
72
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO11_CUR_POS_H
=
0x01
51
,
DARWIN_MM_SERVO11_CUR_POS_H
=
0x01
73
,
DARWIN_MM_SERVO12_CUR_POS_L
=
0x01
52
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO12_CUR_POS_L
=
0x01
74
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO12_CUR_POS_H
=
0x015
3
,
DARWIN_MM_SERVO12_CUR_POS_H
=
0x01
7
5
,
DARWIN_MM_SERVO13_CUR_POS_L
=
0x01
54
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO13_CUR_POS_L
=
0x01
76
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO13_CUR_POS_H
=
0x01
55
,
DARWIN_MM_SERVO13_CUR_POS_H
=
0x01
77
,
DARWIN_MM_SERVO14_CUR_POS_L
=
0x01
56
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO14_CUR_POS_L
=
0x01
78
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO14_CUR_POS_H
=
0x01
5
7
,
DARWIN_MM_SERVO14_CUR_POS_H
=
0x017
9
,
DARWIN_MM_SERVO15_CUR_POS_L
=
0x01
58
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO15_CUR_POS_L
=
0x01
7A
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO15_CUR_POS_H
=
0x01
59
,
DARWIN_MM_SERVO15_CUR_POS_H
=
0x01
7B
,
DARWIN_MM_SERVO16_CUR_POS_L
=
0x01
5A
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO16_CUR_POS_L
=
0x01
7C
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO16_CUR_POS_H
=
0x01
5B
,
DARWIN_MM_SERVO16_CUR_POS_H
=
0x01
7D
,
DARWIN_MM_SERVO17_CUR_POS_L
=
0x01
5C
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO17_CUR_POS_L
=
0x01
7E
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO17_CUR_POS_H
=
0x01
5D
,
DARWIN_MM_SERVO17_CUR_POS_H
=
0x01
7F
,
DARWIN_MM_SERVO18_CUR_POS_L
=
0x01
5E
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO18_CUR_POS_L
=
0x01
80
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO18_CUR_POS_H
=
0x01
5F
,
DARWIN_MM_SERVO18_CUR_POS_H
=
0x01
81
,
DARWIN_MM_SERVO19_CUR_POS_L
=
0x01
60
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO19_CUR_POS_L
=
0x01
82
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO19_CUR_POS_H
=
0x01
61
,
DARWIN_MM_SERVO19_CUR_POS_H
=
0x01
83
,
DARWIN_MM_SERVO20_CUR_POS_L
=
0x01
62
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO20_CUR_POS_L
=
0x01
84
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO20_CUR_POS_H
=
0x01
63
,
DARWIN_MM_SERVO20_CUR_POS_H
=
0x01
85
,
DARWIN_MM_SERVO21_CUR_POS_L
=
0x016
4
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO21_CUR_POS_L
=
0x01
8
6
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO21_CUR_POS_H
=
0x01
65
,
DARWIN_MM_SERVO21_CUR_POS_H
=
0x01
87
,
DARWIN_MM_SERVO22_CUR_POS_L
=
0x01
66
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO22_CUR_POS_L
=
0x01
88
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO22_CUR_POS_H
=
0x01
67
,
DARWIN_MM_SERVO22_CUR_POS_H
=
0x01
89
,
DARWIN_MM_SERVO23_CUR_POS_L
=
0x01
6
8
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO23_CUR_POS_L
=
0x018
A
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO23_CUR_POS_H
=
0x01
69
,
DARWIN_MM_SERVO23_CUR_POS_H
=
0x01
8B
,
DARWIN_MM_SERVO24_CUR_POS_L
=
0x01
6A
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO24_CUR_POS_L
=
0x01
8C
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO24_CUR_POS_H
=
0x01
6B
,
DARWIN_MM_SERVO24_CUR_POS_H
=
0x01
8D
,
DARWIN_MM_SERVO25_CUR_POS_L
=
0x01
6C
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO25_CUR_POS_L
=
0x01
8E
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO25_CUR_POS_H
=
0x01
6D
,
DARWIN_MM_SERVO25_CUR_POS_H
=
0x01
8F
,
DARWIN_MM_SERVO26_CUR_POS_L
=
0x01
6E
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO26_CUR_POS_L
=
0x01
90
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO26_CUR_POS_H
=
0x01
6F
,
DARWIN_MM_SERVO26_CUR_POS_H
=
0x01
91
,
DARWIN_MM_SERVO27_CUR_POS_L
=
0x01
70
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO27_CUR_POS_L
=
0x01
92
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO27_CUR_POS_H
=
0x01
71
,
DARWIN_MM_SERVO27_CUR_POS_H
=
0x01
93
,
DARWIN_MM_SERVO28_CUR_POS_L
=
0x01
72
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO28_CUR_POS_L
=
0x01
94
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO28_CUR_POS_H
=
0x01
73
,
DARWIN_MM_SERVO28_CUR_POS_H
=
0x01
95
,
DARWIN_MM_SERVO29_CUR_POS_L
=
0x01
74
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO29_CUR_POS_L
=
0x01
96
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO29_CUR_POS_H
=
0x017
5
,
DARWIN_MM_SERVO29_CUR_POS_H
=
0x01
9
7
,
DARWIN_MM_SERVO30_CUR_POS_L
=
0x01
76
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO30_CUR_POS_L
=
0x01
98
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO30_CUR_POS_H
=
0x01
77
,
DARWIN_MM_SERVO30_CUR_POS_H
=
0x01
99
,
DARWIN_MM_SERVO31_CUR_POS_L
=
0x01
78
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO31_CUR_POS_L
=
0x01
9A
,
// angle in fixed point format 9|7
DARWIN_MM_SERVO31_CUR_POS_H
=
0x01
7
9
,
DARWIN_MM_SERVO31_CUR_POS_H
=
0x019
B
,
DARWIN_ACTION_PAGE
=
0x01
7A
,
DARWIN_ACTION_PAGE
=
0x01
9C
,
DARWIN_ACTION_CNTRL
=
0x01
7B
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
DARWIN_ACTION_CNTRL
=
0x01
9D
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// | | | page running | | | stop page | start page
// | | | page running | | | stop page | start page
DARWIN_WALK_CNTRL
=
0x01
7C
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
DARWIN_WALK_CNTRL
=
0x01
9E
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// current phase walking stop walking start walking
// current phase walking stop walking start walking
DARWIN_WALK_STEP_FW_BW
=
0x01
7D
,
DARWIN_WALK_STEP_FW_BW
=
0x01
9F
,
DARWIN_WALK_STEP_LEFT_RIGHT
=
0x01
7E
,
DARWIN_WALK_STEP_LEFT_RIGHT
=
0x01
A0
,
DARWIN_WALK_STEP_DIRECTION
=
0x01
7F
DARWIN_WALK_STEP_DIRECTION
=
0x01
A1
}
darwin_registers
;
}
darwin_registers
;
#define GPIO_BASE_ADDRESS 0x0100
#define GPIO_BASE_ADDRESS 0x0100
...
@@ -382,6 +382,7 @@ typedef enum {
...
@@ -382,6 +382,7 @@ typedef enum {
#define MANAGER_ENABLE 0x01
#define MANAGER_ENABLE 0x01
#define MANAGER_EN_BAL 0x02
#define MANAGER_EN_BAL 0x02
#define MANAGER_EN_PWR 0x04
#define MANAGER_EN_PWR 0x04
#define MANAGER_SCANNING 0x80
#define MANAGER_EVEN_SER_EN 0x80
#define MANAGER_EVEN_SER_EN 0x80
#define MANAGER_EVEN_SER_MOD 0x70
#define MANAGER_EVEN_SER_MOD 0x70
#define MANAGER_ODD_SER_EN 0x08
#define MANAGER_ODD_SER_EN 0x08
...
...
This diff is collapsed.
Click to expand it.
src/cm730_fw.c
+
1
−
0
View file @
a0ed6f38
...
@@ -35,6 +35,7 @@ int main(void)
...
@@ -35,6 +35,7 @@ int main(void)
EE_ReadVariable
(
MM_PERIOD_OFFSET
+
1
,
&
eeprom_data
);
EE_ReadVariable
(
MM_PERIOD_OFFSET
+
1
,
&
eeprom_data
);
period
+=
((
eeprom_data
&
0x00FF
)
<<
8
);
period
+=
((
eeprom_data
&
0x00FF
)
<<
8
);
manager_init
(
period
);
manager_init
(
period
);
gpio_set_led
(
LED_4
);
while
(
1
);
/* main function does not return */
while
(
1
);
/* main function does not return */
}
}
...
...
This diff is collapsed.
Click to expand it.
src/darwin_dyn_master.c
+
3
−
0
View file @
a0ed6f38
#include
"darwin_dyn_master.h"
#include
"darwin_dyn_master.h"
#include
"darwin_time.h"
#include
"darwin_time.h"
#include
"usart1.h"
#include
"usart1.h"
#include
"ram.h"
#define ENABLE_RX_EN_GPIO_CLK __GPIOB_CLK_ENABLE()
#define ENABLE_RX_EN_GPIO_CLK __GPIOB_CLK_ENABLE()
#define RX_EN_PIN GPIO_PIN_5
#define RX_EN_PIN GPIO_PIN_5
...
@@ -94,10 +95,12 @@ void darwin_dyn_master_init(void)
...
@@ -94,10 +95,12 @@ void darwin_dyn_master_init(void)
inline
void
darwin_dyn_master_enable_power
(
void
)
inline
void
darwin_dyn_master_enable_power
(
void
)
{
{
HAL_GPIO_WritePin
(
POWER_GPIO_PORT
,
POWER_PIN
,
GPIO_PIN_SET
);
HAL_GPIO_WritePin
(
POWER_GPIO_PORT
,
POWER_PIN
,
GPIO_PIN_SET
);
ram_data
[
DARWIN_MM_CNTRL
]
|=
MANAGER_EN_PWR
;
}
}
inline
void
darwin_dyn_master_disable_power
(
void
)
inline
void
darwin_dyn_master_disable_power
(
void
)
{
{
HAL_GPIO_WritePin
(
POWER_GPIO_PORT
,
POWER_PIN
,
GPIO_PIN_RESET
);
HAL_GPIO_WritePin
(
POWER_GPIO_PORT
,
POWER_PIN
,
GPIO_PIN_RESET
);
ram_data
[
DARWIN_MM_CNTRL
]
&=
(
~
MANAGER_EN_PWR
);
}
}
This diff is collapsed.
Click to expand it.
src/motion_manager.c
+
3
−
0
View file @
a0ed6f38
...
@@ -183,6 +183,7 @@ void manager_init(uint16_t period_us)
...
@@ -183,6 +183,7 @@ void manager_init(uint16_t period_us)
/* initialize the dynamixel master module for the servos */
/* initialize the dynamixel master module for the servos */
darwin_dyn_master_init
();
darwin_dyn_master_init
();
ram_data
[
DARWIN_MM_CNTRL
]
|=
MANAGER_SCANNING
;
// enable power to the servos
// enable power to the servos
darwin_dyn_master_enable_power
();
darwin_dyn_master_enable_power
();
HAL_Delay
(
1000
);
HAL_Delay
(
1000
);
...
@@ -296,6 +297,7 @@ void manager_init(uint16_t period_us)
...
@@ -296,6 +297,7 @@ void manager_init(uint16_t period_us)
}
}
}
}
darwin_dyn_master_disable_power
();
darwin_dyn_master_disable_power
();
ram_data
[
DARWIN_MM_CNTRL
]
&=
(
~
MANAGER_SCANNING
);
/* configure timer */
/* configure timer */
ENABLE_MANAGER_TIMER_CLK
;
ENABLE_MANAGER_TIMER_CLK
;
...
@@ -304,6 +306,7 @@ void manager_init(uint16_t period_us)
...
@@ -304,6 +306,7 @@ void manager_init(uint16_t period_us)
MANAGER_TIM_Handle
.
Init
.
Prescaler
=
72
;
MANAGER_TIM_Handle
.
Init
.
Prescaler
=
72
;
MANAGER_TIM_Handle
.
Init
.
ClockDivision
=
TIM_CLOCKDIVISION_DIV1
;
MANAGER_TIM_Handle
.
Init
.
ClockDivision
=
TIM_CLOCKDIVISION_DIV1
;
MANAGER_TIM_Handle
.
Init
.
CounterMode
=
TIM_COUNTERMODE_UP
;
MANAGER_TIM_Handle
.
Init
.
CounterMode
=
TIM_COUNTERMODE_UP
;
HAL_TIM_Base_Init
(
&
MANAGER_TIM_Handle
);
HAL_NVIC_SetPriority
(
MANAGER_TIMER_IRQn
,
2
,
1
);
HAL_NVIC_SetPriority
(
MANAGER_TIMER_IRQn
,
2
,
1
);
HAL_NVIC_EnableIRQ
(
MANAGER_TIMER_IRQn
);
HAL_NVIC_EnableIRQ
(
MANAGER_TIMER_IRQn
);
/* use the internal clock */
/* use the internal clock */
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment