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
5af66819
Commit
5af66819
authored
9 years ago
by
Sergi Hernandez
Browse files
Options
Downloads
Patches
Plain Diff
Implemented the balance feature of the robot using the Gyro.
parent
6fbfc0cf
No related branches found
No related tags found
No related merge requests found
Changes
3
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
include/eeprom_init.h
+4
-4
4 additions, 4 deletions
include/eeprom_init.h
src/imu.c
+2
-2
2 additions, 2 deletions
src/imu.c
src/motion_manager.c
+8
-8
8 additions, 8 deletions
src/motion_manager.c
with
14 additions
and
14 deletions
include/eeprom_init.h
+
4
−
4
View file @
5af66819
...
@@ -11,10 +11,10 @@ extern "C" {
...
@@ -11,10 +11,10 @@ extern "C" {
#define DEFAULT_BAUDRATE 0x0001
#define DEFAULT_BAUDRATE 0x0001
#define DEFAULT_RETURN_DELAY 0x0000
#define DEFAULT_RETURN_DELAY 0x0000
#define DEFAULT_MM_PERIOD 0x1E78
#define DEFAULT_MM_PERIOD 0x1E78
#define DEFAULT_BAL_KNEE_GAIN 0x
04BE // 1/54
in fixed point format 0|16
#define DEFAULT_BAL_KNEE_GAIN 0x
4CCD // 0.3
in fixed point format 0|16
#define DEFAULT_BAL_ANKLE_ROLL_GAIN 0x
0CCD // 1/20
#define DEFAULT_BAL_ANKLE_ROLL_GAIN 0x
FFFF // 0.99999
#define DEFAULT_BAL_ANKLE_PITCH_GAIN 0x
0E39 // 1/18
#define DEFAULT_BAL_ANKLE_PITCH_GAIN 0x
E666 // 0.9
#define DEFAULT_BAL_HIP_ROLL_GAIN 0x
0666 // 1/40
#define DEFAULT_BAL_HIP_ROLL_GAIN 0x
8000 // 0.5
#define DEFAULT_RETURN_LEVEL 0x0002
#define DEFAULT_RETURN_LEVEL 0x0002
#define DEFAULT_SERVO0_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO0_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO1_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO1_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
...
...
This diff is collapsed.
Click to expand it.
src/imu.c
+
2
−
2
View file @
5af66819
...
@@ -298,7 +298,7 @@ void imu_gyro_stop(void)
...
@@ -298,7 +298,7 @@ void imu_gyro_stop(void)
void
imu_gyro_set_config
(
void
)
void
imu_gyro_set_config
(
void
)
{
{
gyro_conf_data
[
0
]
=
0xFF
;
// sampling rate = 800 Hz, BW = 110 Hz , all powered up
gyro_conf_data
[
0
]
=
0xFF
;
// sampling rate = 800 Hz, BW = 110 Hz , all powered up
gyro_conf_data
[
3
]
=
0x
2
0
;
// Full scale =
20
00dps
gyro_conf_data
[
3
]
=
0x
1
0
;
// Full scale =
5
00dps
imu_enable_device
(
GYRO_ID
);
imu_enable_device
(
GYRO_ID
);
imu_spi_start_write
(
gyro_conf_reg
,
gyro_conf_data
,
gyro_conf_len
);
imu_spi_start_write
(
gyro_conf_reg
,
gyro_conf_data
,
gyro_conf_len
);
...
@@ -325,7 +325,7 @@ void imu_gyro_convert_data(uint8_t *data_in,uint8_t *data_out)
...
@@ -325,7 +325,7 @@ void imu_gyro_convert_data(uint8_t *data_in,uint8_t *data_out)
for
(
i
=
0
;
i
<
3
;
i
++
)
for
(
i
=
0
;
i
<
3
;
i
++
)
{
{
gyro_data
[
i
]
=
(((
int16_t
)(
data_in
[
i
*
2
]
+
(
data_in
[
i
*
2
+
1
]
<<
8
)))
*
70
)
-
gyro_center
[
i
];
gyro_data
[
i
]
=
(((
int16_t
)(
data_in
[
i
*
2
]
+
(
data_in
[
i
*
2
+
1
]
<<
8
)))
*
18
)
-
gyro_center
[
i
];
data_out
[
i
*
2
]
=
gyro_data
[
i
]
%
256
;
data_out
[
i
*
2
]
=
gyro_data
[
i
]
%
256
;
data_out
[
i
*
2
+
1
]
=
gyro_data
[
i
]
/
256
;
data_out
[
i
*
2
+
1
]
=
gyro_data
[
i
]
/
256
;
}
}
...
...
This diff is collapsed.
Click to expand it.
src/motion_manager.c
+
8
−
8
View file @
5af66819
...
@@ -124,14 +124,14 @@ void manager_balance(void)
...
@@ -124,14 +124,14 @@ void manager_balance(void)
// get the values of the gyroscope
// get the values of the gyroscope
imu_get_gyro_data
(
&
gyro_x
,
&
gyro_y
,
&
gyro_z
);
imu_get_gyro_data
(
&
gyro_x
,
&
gyro_y
,
&
gyro_z
);
// compensate the servo angle values
// compensate the servo angle values
manager_balance_offset
[
R_KNEE
]
=
(((
gyro_y
*
manager_servos
[
R_KNEE
].
max_angle
)
/
manager_servos
[
R_KNEE
].
encoder_resolution
)
*
knee_gain
)
>>
16
;
manager_balance_offset
[
R_KNEE
]
=
(((
(
int16_t
)
gyro_y
*
(
uint64_t
)
manager_servos
[
R_KNEE
].
max_angle
*
(
uint64_t
)
knee_gain
)
/
(
uint64_t
)
manager_servos
[
R_KNEE
].
encoder_resolution
)
/
1000
)
>>
9
;
manager_balance_offset
[
R_ANKLE_PITCH
]
=
(((
gyro_y
*
manager_servos
[
R_ANKLE_PITCH
].
max_angle
)
/
manager_servos
[
R_ANKLE_PITCH
].
encoder_resolution
)
*
ankle_pitch_gain
)
>>
16
;
manager_balance_offset
[
R_ANKLE_PITCH
]
=
-
(((
(
int64_t
)
gyro_y
*
(
uint64_t
)
manager_servos
[
R_ANKLE_PITCH
].
max_angle
*
(
uint64_t
)
ankle_pitch_gain
)
/
(
uint64_t
)
manager_servos
[
R_ANKLE_PITCH
].
encoder_resolution
)
/
1000
)
>>
9
;
manager_balance_offset
[
L_KNEE
]
=-
(((
gyro_y
*
manager_servos
[
L_KNEE
].
max_angle
)
/
manager_servos
[
L_KNEE
].
encoder_resolution
)
*
knee_gain
)
>>
16
;
manager_balance_offset
[
L_KNEE
]
=-
(((
(
int64_t
)
gyro_y
*
(
uint64_t
)
manager_servos
[
L_KNEE
].
max_angle
*
(
uint64_t
)
knee_gain
)
/
(
uint64_t
)
manager_servos
[
L_KNEE
].
encoder_resolution
)
/
1000
)
>>
9
;
manager_balance_offset
[
L_ANKLE_PITCH
]
=
-
(((
gyro_y
*
manager_servos
[
L_ANKLE_PITCH
].
max_angle
)
/
manager_servos
[
L_ANKLE_PITCH
].
encoder_resolution
)
*
ankle_pitch_gain
)
>>
16
;
manager_balance_offset
[
L_ANKLE_PITCH
]
=
(
(((
int64_t
)
gyro_y
*
(
uint64_t
)
manager_servos
[
L_ANKLE_PITCH
].
max_angle
*
(
uint64_t
)
ankle_pitch_gain
)
/
(
uint64_t
)
manager_servos
[
L_ANKLE_PITCH
].
encoder_resolution
)
/
1000
)
>>
9
;
manager_balance_offset
[
R_HIP_ROLL
]
=
(((
gyro_x
*
manager_servos
[
R_HIP_ROLL
].
max_angle
)
/
manager_servos
[
R_HIP_ROLL
].
encoder_resolution
)
*
hip_roll_gain
)
>>
16
;
manager_balance_offset
[
R_HIP_ROLL
]
=
-
(((
(
int64_t
)
gyro_x
*
(
uint64_t
)
manager_servos
[
R_HIP_ROLL
].
max_angle
*
(
uint64_t
)
hip_roll_gain
)
/
(
uint64_t
)
manager_servos
[
R_HIP_ROLL
].
encoder_resolution
)
/
1000
)
>>
9
;
manager_balance_offset
[
L_HIP_ROLL
]
=
(((
gyro_x
*
manager_servos
[
L_HIP_ROLL
].
max_angle
)
/
manager_servos
[
L_HIP_ROLL
].
encoder_resolution
)
*
hip_roll_gain
)
>>
16
;
manager_balance_offset
[
L_HIP_ROLL
]
=
-
(((
(
int64_t
)
gyro_x
*
(
uint64_t
)
manager_servos
[
L_HIP_ROLL
].
max_angle
*
(
uint64_t
)
hip_roll_gain
)
/
(
uint64_t
)
manager_servos
[
L_HIP_ROLL
].
encoder_resolution
)
/
1000
)
>>
9
;
manager_balance_offset
[
R_ANKLE_ROLL
]
=
-
(((
gyro_x
*
manager_servos
[
R_ANKLE_ROLL
].
max_angle
)
/
manager_servos
[
R_ANKLE_ROLL
].
encoder_resolution
)
*
ankle_roll_gain
)
>>
16
;
manager_balance_offset
[
R_ANKLE_ROLL
]
=
(
(((
int64_t
)
gyro_x
*
(
uint64_t
)
manager_servos
[
R_ANKLE_ROLL
].
max_angle
*
(
uint64_t
)
ankle_roll_gain
)
/
(
uint64_t
)
manager_servos
[
R_ANKLE_ROLL
].
encoder_resolution
)
/
1000
)
>>
9
;
manager_balance_offset
[
L_ANKLE_ROLL
]
=
-
(((
gyro_x
*
manager_servos
[
L_ANKLE_ROLL
].
max_angle
)
/
manager_servos
[
L_ANKLE_ROLL
].
encoder_resolution
)
*
ankle_roll_gain
)
>>
16
;
manager_balance_offset
[
L_ANKLE_ROLL
]
=
(
(((
int64_t
)
gyro_x
*
(
uint64_t
)
manager_servos
[
L_ANKLE_ROLL
].
max_angle
*
(
uint64_t
)
ankle_roll_gain
)
/
(
uint64_t
)
manager_servos
[
L_ANKLE_ROLL
].
encoder_resolution
)
/
1000
)
>>
9
;
}
}
}
}
...
...
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