Skip to content
GitLab
Menu
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
humanoides
bioloid_stm32_fw
Commits
65a93f73
Commit
65a93f73
authored
Aug 30, 2015
by
Sergi Hernandez
Browse files
Solved some minor bugs related to the balance feature.
parent
583e3ac3
Changes
6
Expand all
Hide whitespace changes
Inline
Side-by-side
include/bioloid_registers.h
View file @
65a93f73
This diff is collapsed.
Click to expand it.
include/motion_manager.h
View file @
65a93f73
...
...
@@ -7,10 +7,31 @@ extern "C" {
#include
"stm32f4xx_hal.h"
typedef
enum
{
MM_NONE
=
0
,
MM_ACTION
=
1
,
MM_WALKING
=
2
,
MM_JOINTS
=
3
}
TModules
;
typedef
enum
{
R_SHOULDER_PITCH
=
1
,
L_SHOULDER_PITCH
=
2
,
R_SHOULDER_ROLL
=
3
,
L_SHOULDER_ROLL
=
4
,
R_ELBOW
=
5
,
L_ELBOW
=
6
,
R_HIP_YAW
=
7
,
L_HIP_YAW
=
8
,
R_HIP_ROLL
=
9
,
L_HIP_ROLL
=
10
,
R_HIP_PITCH
=
11
,
L_HIP_PITCH
=
12
,
R_KNEE
=
13
,
L_KNEE
=
14
,
R_ANKLE_PITCH
=
15
,
L_ANKLE_PITCH
=
16
,
R_ANKLE_ROLL
=
17
,
L_ANKLE_ROLL
=
18
}
servo_id_t
;
typedef
enum
{
MM_NONE
=
0
,
MM_ACTION
=
1
,
MM_WALKING
=
2
,
MM_JOINTS
=
3
}
TModules
;
typedef
struct
{
...
...
src/bioloid_gyro.c
View file @
65a93f73
...
...
@@ -36,6 +36,8 @@ void gyro_calibrate(void)
gyro_fb_offset
=
0x0000
;
gyro_lr_offset
=
0x0000
;
// dummy delay to allow enough time to the ADC to get new data
HAL_Delay
(
2
*
adc_get_period
());
for
(
i
=
0
;
i
<
GYRO_MAX_CAL_SAMPLES
;
i
++
)
{
gyro_fb_offset
+=
adc_get_channel_raw
(
fb_ch
);
...
...
src/eeprom.c
View file @
65a93f73
...
...
@@ -42,19 +42,24 @@
/* Includes ------------------------------------------------------------------*/
#include
"eeprom.h"
#include
"bioloid_registers.h"
#include
"adc_dma.h"
/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
#define EEPROM_SIZE 0x09
#define DEFAULT_DEVICE_MODEL 0x7300
#define DEFAULT_FIRMWARE_VERSION 0x0001
#define DEFAULT_DEVICE_ID 0x0002
#define DEFAULT_BAUDRATE 0x0001
#define DEFAULT_RETURN_DELAY 0x0000
#define DEFAULT_MM_PERIOD 0x01FF
#define DEFAULT_GYRO_FB_ADC_CH 0x0002
#define DEFAULT_GYRO_LR_ADC_CH 0x0001
#define DEFAULT_RETURN_LEVEL 0x0002
#define EEPROM_SIZE 0x20
#define DEFAULT_DEVICE_MODEL 0x7300
#define DEFAULT_FIRMWARE_VERSION 0x0001
#define DEFAULT_DEVICE_ID 0x0002
#define DEFAULT_BAUDRATE 0x0001
#define DEFAULT_RETURN_DELAY 0x0000
#define DEFAULT_MM_PERIOD 0x01FF
#define DEFAULT_BAL_KNEE_GAIN 0x04BE // 1/54 in fixed point format 0|16
#define DEFAULT_BAL_ANKLE_ROLL_GAIN 0x0CCD // 1/20
#define DEFAULT_BAL_ANKLE_PITCH_GAIN 0x0E39 // 1/18
#define DEFAULT_BAL_HIP_ROLL_GAIN 0x0666 // 1/40
#define DEFAULT_GYRO_FB_ADC_CH (uint16_t)ADC_CH1
#define DEFAULT_GYRO_LR_ADC_CH (uint16_t)ADC_CH2
#define DEFAULT_RETURN_LEVEL 0x0002
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
...
...
@@ -80,12 +85,28 @@ uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE,
MM_PERIOD_OFFSET
,
DEFAULT_MM_PERIOD
>>
8
,
MM_PERIOD_OFFSET
+
1
,
DEFAULT_BAL_KNEE_GAIN
&
0xFF
,
MM_BAL_KNEE_GAIN_OFFSET
,
DEFAULT_BAL_KNEE_GAIN
>>
8
,
MM_BAL_KNEE_GAIN_OFFSET
+
1
,
DEFAULT_BAL_ANKLE_ROLL_GAIN
&
0xFF
,
MM_BAL_ANKLE_ROLL_GAIN_OFFSET
,
DEFAULT_BAL_ANKLE_ROLL_GAIN
>>
8
,
MM_BAL_ANKLE_ROLL_GAIN_OFFSET
+
1
,
DEFAULT_BAL_ANKLE_PITCH_GAIN
&
0xFF
,
MM_BAL_ANKLE_PITCH_GAIN_OFFSET
,
DEFAULT_BAL_ANKLE_PITCH_GAIN
>>
8
,
MM_BAL_ANKLE_PITCH_GAIN_OFFSET
+
1
,
DEFAULT_BAL_HIP_ROLL_GAIN
&
0xFF
,
MM_BAL_HIP_ROLL_GAIN_OFFSET
,
DEFAULT_BAL_HIP_ROLL_GAIN
>>
8
,
MM_BAL_HIP_ROLL_GAIN_OFFSET
+
1
,
DEFAULT_RETURN_LEVEL
,
RETURN_LEVEL_OFFSET
,
DEFAULT_GYRO_FB_ADC_CH
,
GYRO_FB_ADC_CH_OFFSET
,
DEFAULT_GYRO_LR_ADC_CH
,
GYRO_LR_ADC_CH_OFFSET
,
DEFAULT_RETURN_LEVEL
,
RETURN_LEVEL_OFFSET
};
// return level
GYRO_LR_ADC_CH_OFFSET
};
// return level
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
...
...
src/motion_manager.c
View file @
65a93f73
...
...
@@ -106,25 +106,31 @@ void manager_get_target_position(void)
void
manager_balance
(
void
)
{
adc_ch_t
fb_ch
=
gyro_get_fb_adc_channel
(),
lr_ch
=
gyro_get_lr_adc_channel
();
int16_t
knee_gain
,
ankle_roll_gain
,
ankle_pitch_gain
,
hip_roll_gain
;
uint16_t
fb_offset
,
lr_offset
;
int16_t
fb_value
,
lr_value
;
if
(
manager_balance_enabled
==
0x01
)
// balance is enabled
{
// get the balance gains
knee_gain
=
ram_data
[
BIOLOID_MM_BAL_KNEE_GAIN_L
]
+
(
ram_data
[
BIOLOID_MM_BAL_KNEE_GAIN_H
]
*
256
);
ankle_roll_gain
=
ram_data
[
BIOLOID_MM_BAL_ANKLE_ROLL_GAIN_L
]
+
(
ram_data
[
BIOLOID_MM_BAL_ANKLE_ROLL_GAIN_H
]
<<
8
);
ankle_pitch_gain
=
ram_data
[
BIOLOID_MM_BAL_ANKLE_PITCH_GAIN_L
]
+
(
ram_data
[
BIOLOID_MM_BAL_ANKLE_PITCH_GAIN_H
]
<<
8
);
hip_roll_gain
=
ram_data
[
BIOLOID_MM_BAL_HIP_ROLL_GAIN_L
]
+
(
ram_data
[
BIOLOID_MM_BAL_HIP_ROLL_GAIN_H
]
<<
8
);
// get the offsets of the gyroscope calibration
gyro_get_offsets
(
&
fb_offset
,
&
lr_offset
);
// get the values of the gyroscope
fb_value
=
adc_get_channel_raw
(
fb_ch
)
-
fb_offset
;
lr_value
=
adc_get_channel_raw
(
lr_ch
)
-
lr_offset
;
// compensate the servo angle values
manager_balance_offset
[
13
]
=
(
fb_value
*
manager_servos
[
13
].
max_angle
)
/
(
manager_servos
[
13
].
encoder_resolution
*
54
)
;
manager_balance_offset
[
15
]
=
(
fb_value
*
manager_servos
[
15
].
max_angle
)
/
(
manager_servos
[
15
].
encoder_resolution
*
18
)
;
manager_balance_offset
[
14
]
=-
(
fb_value
*
manager_servos
[
14
].
max_angle
)
/
(
manager_servos
[
14
].
encoder_resolution
*
54
)
;
manager_balance_offset
[
16
]
=-
(
fb_value
*
manager_servos
[
16
].
max_angle
)
/
(
manager_servos
[
16
].
encoder_resolution
*
18
)
;
manager_balance_offset
[
9
]
=
(
lr_value
*
manager_servos
[
9
].
max_angle
)
/
(
manager_servos
[
9
].
encoder_resolution
*
40
)
;
manager_balance_offset
[
10
]
=
(
lr_value
*
manager_servos
[
10
].
max_angle
)
/
(
manager_servos
[
10
].
encoder_resolution
*
40
)
;
manager_balance_offset
[
17
]
=-
(
lr_value
*
manager_servos
[
17
].
max_angle
)
/
(
manager_servos
[
17
].
encoder_resolution
*
20
)
;
manager_balance_offset
[
18
]
=-
(
lr_value
*
manager_servos
[
18
].
max_angle
)
/
(
manager_servos
[
18
].
encoder_resolution
*
20
)
;
manager_balance_offset
[
R_KNEE
]
=
((
(
fb_value
*
manager_servos
[
R_KNEE
].
max_angle
)
/
manager_servos
[
R_KNEE
].
encoder_resolution
)
*
knee_gain
)
/
65536
;
manager_balance_offset
[
R_ANKLE_PITCH
]
=
((
(
fb_value
*
manager_servos
[
R_ANKLE_PITCH
].
max_angle
)
/
manager_servos
[
R_ANKLE_PITCH
].
encoder_resolution
)
*
ankle_pitch_gain
)
/
65536
;
manager_balance_offset
[
L_KNEE
]
=-
(
((
fb_value
*
manager_servos
[
L_KNEE
].
max_angle
)
/
manager_servos
[
L_KNEE
].
encoder_resolution
)
*
knee_gain
)
/
65536
;
manager_balance_offset
[
L_ANKLE_PITCH
]
=-
(
((
fb_value
*
manager_servos
[
L_ANKLE_PITCH
].
max_angle
)
/
manager_servos
[
L_ANKLE_PITCH
].
encoder_resolution
)
*
ankle_pitch_gain
)
/
65536
;
manager_balance_offset
[
R_HIP_ROLL
]
=
((
(
lr_value
*
manager_servos
[
R_HIP_ROLL
].
max_angle
)
/
manager_servos
[
R_HIP_ROLL
].
encoder_resolution
)
*
hip_roll_gain
)
/
65536
;
manager_balance_offset
[
L_HIP_ROLL
]
=
((
(
lr_value
*
manager_servos
[
L_HIP_ROLL
].
max_angle
)
/
manager_servos
[
L_HIP_ROLL
].
encoder_resolution
)
*
hip_roll_gain
)
/
65536
;
manager_balance_offset
[
R_ANKLE_ROLL
]
=-
(
((
lr_value
*
manager_servos
[
R_ANKLE_ROLL
].
max_angle
)
/
manager_servos
[
R_ANKLE_ROLL
].
encoder_resolution
)
*
ankle_roll_gain
)
/
65536
;
manager_balance_offset
[
L_ANKLE_ROLL
]
=-
(
((
lr_value
*
manager_servos
[
L_ANKLE_ROLL
].
max_angle
)
/
manager_servos
[
L_ANKLE_ROLL
].
encoder_resolution
)
*
ankle_roll_gain
)
/
65536
;
}
}
...
...
@@ -456,7 +462,7 @@ inline int16_t manager_get_ccw_angle_limit(uint8_t servo_id)
uint8_t
manager_in_range
(
unsigned
short
int
address
,
unsigned
short
int
length
)
{
if
(
ram_in_window
(
MANAGER_BASE_ADDRESS
,
MANAGER_MEM_LENGTH
,
address
,
length
)
||
ram_in_window
(
BIOLOID_MM_PERIOD_L
,
BIOLOID_MM_PERIOD_
H
,
address
,
length
))
ram_in_window
(
BIOLOID_MM_PERIOD_L
,
MANAGER_EEPROM_LENGT
H
,
address
,
length
))
return
0x01
;
else
return
0x00
;
...
...
src/ram.c
View file @
65a93f73
...
...
@@ -26,8 +26,28 @@ void ram_init(void)
ram_data
[
MM_PERIOD_OFFSET
]
=
(
uint8_t
)(
eeprom_data
&
0x00FF
);
if
(
EE_ReadVariable
(
MM_PERIOD_OFFSET
+
1
,
&
eeprom_data
)
==
0
)
ram_data
[
MM_PERIOD_OFFSET
+
1
]
=
(
uint8_t
)(
eeprom_data
&
0x00FF
);
if
(
EE_ReadVariable
(
MM_BAL_KNEE_GAIN_OFFSET
,
&
eeprom_data
)
==
0
)
ram_data
[
MM_BAL_KNEE_GAIN_OFFSET
]
=
(
uint8_t
)(
eeprom_data
&
0x00FF
);
if
(
EE_ReadVariable
(
MM_BAL_KNEE_GAIN_OFFSET
+
1
,
&
eeprom_data
)
==
0
)
ram_data
[
MM_BAL_KNEE_GAIN_OFFSET
+
1
]
=
(
uint8_t
)(
eeprom_data
&
0x00FF
);
if
(
EE_ReadVariable
(
MM_BAL_ANKLE_ROLL_GAIN_OFFSET
,
&
eeprom_data
)
==
0
)
ram_data
[
MM_BAL_ANKLE_ROLL_GAIN_OFFSET
]
=
(
uint8_t
)(
eeprom_data
&
0x00FF
);
if
(
EE_ReadVariable
(
MM_BAL_ANKLE_ROLL_GAIN_OFFSET
+
1
,
&
eeprom_data
)
==
0
)
ram_data
[
MM_BAL_ANKLE_ROLL_GAIN_OFFSET
+
1
]
=
(
uint8_t
)(
eeprom_data
&
0x00FF
);
if
(
EE_ReadVariable
(
MM_BAL_ANKLE_PITCH_GAIN_OFFSET
,
&
eeprom_data
)
==
0
)
ram_data
[
MM_BAL_ANKLE_PITCH_GAIN_OFFSET
]
=
(
uint8_t
)(
eeprom_data
&
0x00FF
);
if
(
EE_ReadVariable
(
MM_BAL_ANKLE_PITCH_GAIN_OFFSET
+
1
,
&
eeprom_data
)
==
0
)
ram_data
[
MM_BAL_ANKLE_PITCH_GAIN_OFFSET
+
1
]
=
(
uint8_t
)(
eeprom_data
&
0x00FF
);
if
(
EE_ReadVariable
(
MM_BAL_HIP_ROLL_GAIN_OFFSET
,
&
eeprom_data
)
==
0
)
ram_data
[
MM_BAL_HIP_ROLL_GAIN_OFFSET
]
=
(
uint8_t
)(
eeprom_data
&
0x00FF
);
if
(
EE_ReadVariable
(
MM_BAL_HIP_ROLL_GAIN_OFFSET
+
1
,
&
eeprom_data
)
==
0
)
ram_data
[
MM_BAL_HIP_ROLL_GAIN_OFFSET
+
1
]
=
(
uint8_t
)(
eeprom_data
&
0x00FF
);
if
(
EE_ReadVariable
(
RETURN_LEVEL_OFFSET
,
&
eeprom_data
)
==
0
)
ram_data
[
RETURN_LEVEL_OFFSET
]
=
(
uint8_t
)
eeprom_data
;
if
(
EE_ReadVariable
(
GYRO_FB_ADC_CH_OFFSET
,
&
eeprom_data
)
==
0
)
ram_data
[
GYRO_FB_ADC_CH_OFFSET
]
=
(
uint8_t
)(
eeprom_data
&
0x00FF
);
if
(
EE_ReadVariable
(
GYRO_LR_ADC_CH_OFFSET
,
&
eeprom_data
)
==
0
)
ram_data
[
GYRO_LR_ADC_CH_OFFSET
]
=
(
uint8_t
)(
eeprom_data
&
0x00FF
);
}
inline
void
ram_read_byte
(
uint8_t
address
,
uint8_t
*
data
)
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment