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
e3fcd55d
Commit
e3fcd55d
authored
Aug 17, 2015
by
Sergi Hernandez
Browse files
Added initial supprt for the motion manager module.
Changed the way to detect which module is addressed by the dynamixel slave interface.
parent
35ff55ec
Changes
16
Hide whitespace changes
Inline
Side-by-side
Makefile
View file @
e3fcd55d
...
...
@@ -15,6 +15,8 @@ TARGET_FILES+=src/bioloid_time.c
TARGET_FILES
+=
src/bioloid_dyn_slave.c
TARGET_FILES
+=
src/bioloid_dyn_master_sensors.c
TARGET_FILES
+=
src/bioloid_dyn_master_servos.c
TARGET_FILES
+=
src/motion_pages.c
TARGET_FILES
+=
src/motion_manager.c
TARGET_PROCESSOR
=
STM32F407VG
HAL_PATH
=
../../STM32_processor/hal/f4
...
...
include/adc_dma.h
View file @
e3fcd55d
...
...
@@ -13,6 +13,7 @@ void adc_start(void);
void
adc_set_period
(
uint8_t
period_ms
);
void
adc_stop
(
void
);
// operation functions
uint8_t
adc_in_range
(
unsigned
short
int
address
,
unsigned
short
int
length
);
void
adc_process_write_cmd
(
unsigned
short
int
address
,
unsigned
short
int
length
,
unsigned
char
*
data
);
#ifdef __cplusplus
...
...
include/bioloid_registers.h
View file @
e3fcd55d
...
...
@@ -11,6 +11,7 @@ extern "C" {
#define BAUDRATE_OFFSET ((unsigned short int)0x0004)
#define RETURN_DELAY_OFFSET ((unsigned short int)0x0005)
#define RETURN_LEVEL_OFFSET ((unsigned short int)0x0010)
#define MM_PERIOD_OFFSET ((unsigned short int)0x0010)
#define LAST_EEPROM_OFFSET ((unsigned short int)0x0020)
...
...
@@ -21,6 +22,8 @@ typedef enum {
BIOLOID_ID
=
DEVICE_ID_OFFSET
,
BIOLOID_BAUD_RATE
=
BAUDRATE_OFFSET
,
BIOLOID_RETURN_DELAY_TIME
=
RETURN_DELAY_OFFSET
,
BIOLOID_MM_PERIOD_L
=
MM_PERIOD_OFFSET
,
BIOLOID_MM_PERIOD_H
=
MM_PERIOD_OFFSET
+
1
,
BIOLOID_RETURN_LEVEL
=
RETURN_LEVEL_OFFSET
,
BIOLOID_USER1_LED_CNTRL
=
0x20
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// | | | | blink | toggle | value | internally used
...
...
@@ -65,12 +68,116 @@ typedef enum {
BIOLOID_ADC_VREF_H
=
0x3F
,
BIOLOID_ADC_CH8_L
=
0x40
,
BIOLOID_ADC_CH8_H
=
0x41
,
BIOLOID_ZIGBEE_CNTRL
=
0x42
,
BIOLOID_ZIGBEE_CNTRL
=
0x42
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// | | | | | | Enable | Enable power
BIOLOID_ZIGBEE_BAUDRATE
=
0x43
,
BIOLOID_ZIGBEE_OWN_ID_L
=
0x44
,
BIOLOID_ZIGBEE_OWN_ID_H
=
0x45
,
BIOLOID_ZIGBEE_REM_ID_L
=
0x46
,
BIOLOID_ZIGBEE_REM_ID_H
=
0x47
,
BIOLOID_MM_NUM_SERVOS
=
0x48
,
BIOLOID_MM_CNTRL
=
0x49
,
// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// | | | | | Enable power | Enable balance | Enable manager
BIOLOID_MM_MODULE_EN0
=
0x4A
,
// 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
// | 000 -> none |
// | 001 -> action |
// | 010 -> walk |
// | 011 -> joints |
BIOLOID_MM_MODULE_EN1
=
0x4B
,
// 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
BIOLOID_MM_MODULE_EN2
=
0x4C
,
// 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
BIOLOID_MM_MODULE_EN3
=
0x4D
,
// 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
BIOLOID_MM_MODULE_EN4
=
0x4E
,
// 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
BIOLOID_MM_MODULE_EN5
=
0x4F
,
// 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
BIOLOID_MM_MODULE_EN6
=
0x50
,
// 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
BIOLOID_MM_MODULE_EN7
=
0x51
,
// 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
BIOLOID_MM_MODULE_EN8
=
0x52
,
// 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
BIOLOID_MM_MODULE_EN9
=
0x53
,
// 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
BIOLOID_MM_MODULE_EN10
=
0x54
,
// 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
BIOLOID_MM_MODULE_EN11
=
0x55
,
// 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
BIOLOID_MM_MODULE_EN12
=
0x56
,
// 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
BIOLOID_MM_MODULE_EN13
=
0x57
,
// 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
BIOLOID_MM_MODULE_EN14
=
0x58
,
// 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
BIOLOID_MM_MODULE_EN15
=
0x59
,
// 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
BIOLOID_MM_SERVO0_CUR_POS_L
=
0x5A
,
BIOLOID_MM_SERVO0_CUR_POS_H
=
0x5B
,
BIOLOID_MM_SERVO1_CUR_POS_L
=
0x5C
,
BIOLOID_MM_SERVO1_CUR_POS_H
=
0x5D
,
BIOLOID_MM_SERVO2_CUR_POS_L
=
0x5E
,
BIOLOID_MM_SERVO2_CUR_POS_H
=
0x5F
,
BIOLOID_MM_SERVO3_CUR_POS_L
=
0x60
,
BIOLOID_MM_SERVO3_CUR_POS_H
=
0x61
,
BIOLOID_MM_SERVO4_CUR_POS_L
=
0x62
,
BIOLOID_MM_SERVO4_CUR_POS_H
=
0x63
,
BIOLOID_MM_SERVO5_CUR_POS_L
=
0x64
,
BIOLOID_MM_SERVO5_CUR_POS_H
=
0x65
,
BIOLOID_MM_SERVO6_CUR_POS_L
=
0x66
,
BIOLOID_MM_SERVO6_CUR_POS_H
=
0x67
,
BIOLOID_MM_SERVO7_CUR_POS_L
=
0x68
,
BIOLOID_MM_SERVO7_CUR_POS_H
=
0x69
,
BIOLOID_MM_SERVO8_CUR_POS_L
=
0x6A
,
BIOLOID_MM_SERVO8_CUR_POS_H
=
0x6B
,
BIOLOID_MM_SERVO9_CUR_POS_L
=
0x6C
,
BIOLOID_MM_SERVO9_CUR_POS_H
=
0x6D
,
BIOLOID_MM_SERVO10_CUR_POS_L
=
0x6E
,
BIOLOID_MM_SERVO10_CUR_POS_H
=
0x6F
,
BIOLOID_MM_SERVO11_CUR_POS_L
=
0x70
,
BIOLOID_MM_SERVO11_CUR_POS_H
=
0x71
,
BIOLOID_MM_SERVO12_CUR_POS_L
=
0x72
,
BIOLOID_MM_SERVO12_CUR_POS_H
=
0x73
,
BIOLOID_MM_SERVO13_CUR_POS_L
=
0x74
,
BIOLOID_MM_SERVO13_CUR_POS_H
=
0x75
,
BIOLOID_MM_SERVO14_CUR_POS_L
=
0x76
,
BIOLOID_MM_SERVO14_CUR_POS_H
=
0x77
,
BIOLOID_MM_SERVO15_CUR_POS_L
=
0x78
,
BIOLOID_MM_SERVO15_CUR_POS_H
=
0x79
,
BIOLOID_MM_SERVO16_CUR_POS_L
=
0x7A
,
BIOLOID_MM_SERVO16_CUR_POS_H
=
0x7B
,
BIOLOID_MM_SERVO17_CUR_POS_L
=
0x7C
,
BIOLOID_MM_SERVO17_CUR_POS_H
=
0x7D
,
BIOLOID_MM_SERVO18_CUR_POS_L
=
0x7E
,
BIOLOID_MM_SERVO18_CUR_POS_H
=
0x7F
,
BIOLOID_MM_SERVO19_CUR_POS_L
=
0x80
,
BIOLOID_MM_SERVO19_CUR_POS_H
=
0x81
,
BIOLOID_MM_SERVO20_CUR_POS_L
=
0x82
,
BIOLOID_MM_SERVO20_CUR_POS_H
=
0x83
,
BIOLOID_MM_SERVO21_CUR_POS_L
=
0x84
,
BIOLOID_MM_SERVO21_CUR_POS_H
=
0x85
,
BIOLOID_MM_SERVO22_CUR_POS_L
=
0x86
,
BIOLOID_MM_SERVO22_CUR_POS_H
=
0x87
,
BIOLOID_MM_SERVO23_CUR_POS_L
=
0x88
,
BIOLOID_MM_SERVO23_CUR_POS_H
=
0x89
,
BIOLOID_MM_SERVO24_CUR_POS_L
=
0x8A
,
BIOLOID_MM_SERVO24_CUR_POS_H
=
0x8B
,
BIOLOID_MM_SERVO25_CUR_POS_L
=
0x8C
,
BIOLOID_MM_SERVO25_CUR_POS_H
=
0x8D
,
BIOLOID_MM_SERVO26_CUR_POS_L
=
0x8E
,
BIOLOID_MM_SERVO26_CUR_POS_H
=
0x8F
,
BIOLOID_MM_SERVO27_CUR_POS_L
=
0x90
,
BIOLOID_MM_SERVO27_CUR_POS_H
=
0x91
,
BIOLOID_MM_SERVO28_CUR_POS_L
=
0x92
,
BIOLOID_MM_SERVO28_CUR_POS_H
=
0x93
,
BIOLOID_MM_SERVO29_CUR_POS_L
=
0x94
,
BIOLOID_MM_SERVO29_CUR_POS_H
=
0x95
,
BIOLOID_MM_SERVO30_CUR_POS_L
=
0x96
,
BIOLOID_MM_SERVO30_CUR_POS_H
=
0x97
,
BIOLOID_MM_SERVO31_CUR_POS_L
=
0x98
,
BIOLOID_MM_SERVO31_CUR_POS_H
=
0x99
}
bioloid_registers
;
...
...
@@ -89,6 +196,9 @@ typedef enum {
#define ZIGBEE_BASE_ADDRESS 0x42
#define ZIGBEE_MEM_LENGTH 6
#define MANAGER_BASE_ADDRESS 0x42
#define MANAGER_MEM_LENGTH 6
#ifdef __cplusplus
}
#endif
...
...
include/gpio.h
View file @
e3fcd55d
...
...
@@ -21,6 +21,7 @@ void gpio_blink_led(led_t led_id, int16_t period_ms);
uint8_t
gpio_is_pushbutton_pressed
(
pushbutton_t
pb_id
);
void
gpio_set_pushbutton_callback
(
pushbutton_t
pb_id
,
void
(
*
callback
)(
void
));
// operation functions
uint8_t
gpio_in_range
(
unsigned
short
int
address
,
unsigned
short
int
length
);
void
gpio_process_read_cmd
(
unsigned
short
int
address
,
unsigned
short
int
length
,
unsigned
char
*
data
);
void
gpio_process_write_cmd
(
unsigned
short
int
address
,
unsigned
short
int
length
,
unsigned
char
*
data
);
...
...
include/motion_manager.h
View file @
e3fcd55d
...
...
@@ -5,9 +5,9 @@
extern
"C"
{
#endif
#include
"stm32f4xx.h"
#include
"stm32f4xx
_hal
.h"
#define
MANAGER_MAX_NUM_SERVOS 31
#define MANAGER_MAX_NUM_SERVOS 31
typedef
enum
{
MM_NONE
=
0
,
MM_ACTION
=
1
,
...
...
@@ -49,10 +49,12 @@ inline void manager_set_module(uint8_t servo_id,TModules module);
inline
TModules
manager_get_module
(
uint8_t
servo_id
);
inline
void
manager_enable_servo
(
uint8_t
servo_id
);
inline
void
manager_disable_servo
(
uint8_t
servo_id
);
void
manager_disable_servos
(
void
);
inline
uint8_t
manager_is_servo_enabled
(
uint8_t
servo_id
);
inline
int16_t
manager_get_cw_angle_limit
(
uint8_t
servo_id
);
inline
int16_t
manager_get_ccw_angle_limit
(
uint8_t
servo_id
);
// operation functions
uint8_t
manager_in_range
(
unsigned
short
int
address
,
unsigned
short
int
length
);
void
manager_process_write_cmd
(
unsigned
short
int
address
,
unsigned
short
int
length
,
unsigned
char
*
data
);
#ifdef __cplusplus
}
...
...
include/motion_pages.h
View file @
e3fcd55d
...
...
@@ -5,7 +5,7 @@
extern
"C"
{
#endif
#include
"stm32f4xx.h"
#include
"stm32f4xx
_hal
.h"
#define MAX_PAGES 256
#define PAGE_MAX_NUM_SERVOS 31
...
...
include/zigbee.h
View file @
e3fcd55d
...
...
@@ -14,6 +14,7 @@ void zigbee_disable_power(void);
void
zigbee_enable
(
void
);
void
zigbee_disable
(
void
);
// operation functions
uint8_t
zigbee_in_range
(
unsigned
short
int
address
,
unsigned
short
int
length
);
void
zigbee_process_write_cmd
(
unsigned
short
int
address
,
unsigned
short
int
length
,
unsigned
char
*
data
);
#ifdef __cplusplus
...
...
src/adc_dma.c
View file @
e3fcd55d
...
...
@@ -368,6 +368,12 @@ void adc_stop(void)
HAL_TIM_OC_Stop_IT
(
&
ADC_TIM_Handle
,
TIM_CHANNEL_4
);
}
// operation functions
uint8_t
adc_in_range
(
unsigned
short
int
address
,
unsigned
short
int
length
)
{
return
ram_in_window
(
ADC_BASE_ADDRESS
,
ADC_MEM_LENGTH
,
address
,
length
);
}
void
adc_process_write_cmd
(
unsigned
short
int
address
,
unsigned
short
int
length
,
unsigned
char
*
data
)
{
if
(
ram_in_range
(
BIOLOID_ADC_CNTRL
,
address
,
length
))
...
...
src/bioloid_dyn_master_sensors.c
View file @
e3fcd55d
...
...
@@ -75,4 +75,8 @@ void bioloid_dyn_master_sensors_init(void)
dyn_master_init
(
&
bioloid_dyn_master_sensors
,
&
bioloid_dyn_master_sensors_comm
);
bioloid_dyn_master_sensors
.
set_rx_mode
=
bioloid_dyn_master_sensors_set_rx_mode
;
bioloid_dyn_master_sensors
.
set_tx_mode
=
bioloid_dyn_master_sensors_set_tx_mode
;
/* configure dynamixel master module */
dyn_master_set_rx_timeout
(
&
bioloid_dyn_master_sensors
,
50
);
dyn_master_set_return_level
(
&
bioloid_dyn_master_sensors
,
return_all
);
}
src/bioloid_dyn_master_servos.c
View file @
e3fcd55d
...
...
@@ -87,14 +87,9 @@ void bioloid_dyn_master_servos_init(void)
bioloid_dyn_master_servos
.
set_rx_mode
=
bioloid_dyn_master_servos_set_rx_mode
;
bioloid_dyn_master_servos
.
set_tx_mode
=
bioloid_dyn_master_servos_set_tx_mode
;
bioloid_dyn_master_servos_enable_power
();
HAL_Delay
(
2000
);
if
(
dyn_master_ping
(
&
bioloid_dyn_master_servos
,
0x04
)
==
DYN_SUCCESS
)
gpio_set_led
(
RXD_LED
);
else
gpio_clear_led
(
RXD_LED
);
bioloid_dyn_master_servos_disable_power
();
/* configure dynamixel master module */
dyn_master_set_rx_timeout
(
&
bioloid_dyn_master_servos
,
50
);
dyn_master_set_return_level
(
&
bioloid_dyn_master_servos
,
return_all
);
}
inline
void
bioloid_dyn_master_servos_enable_power
(
void
)
...
...
src/bioloid_dyn_slave.c
View file @
e3fcd55d
...
...
@@ -6,6 +6,7 @@
#include
"gpio.h"
#include
"adc_dma.h"
#include
"zigbee.h"
#include
"motion_manager.h"
/* external interrupt pin */
#define DYN_SLAVE_EXT_INT_PIN GPIO_PIN_8
...
...
@@ -66,14 +67,17 @@ unsigned char bioloid_on_write(unsigned short int address,unsigned short int len
ram_write_byte
(
BIOLOID_RETURN_LEVEL
,
data
[
BIOLOID_RETURN_LEVEL
-
address
]);
}
// GPIO commands
if
(
ram_in_window
(
GPIO_BASE_ADDRESS
,
GPIO_MEM_LENGTH
,
address
,
length
))
if
(
gpio_in_range
(
address
,
length
))
gpio_process_write_cmd
(
address
,
length
,
data
);
// ADC commands
if
(
ram
_in_
window
(
ADC_BASE_ADDRESS
,
ADC_MEM_LENGTH
,
address
,
length
))
if
(
adc
_in_
range
(
address
,
length
))
adc_process_write_cmd
(
address
,
length
,
data
);
// Zigbee commands
if
(
ram_in_window
(
ZIGBEE_BASE_ADDRESS
,
ZIGBEE_MEM_LENGTH
,
address
,
length
))
if
(
zigbee_in_range
(
address
,
length
))
zigbee_process_write_cmd
(
address
,
length
,
data
);
// Manager commands
if
(
manager_in_range
(
address
,
length
))
manager_process_write_cmd
(
address
,
length
,
data
);
// write eeprom
for
(
i
=
address
,
j
=
0
;
i
<
LAST_EEPROM_OFFSET
&&
i
<
(
address
+
length
);
i
++
,
j
++
)
EE_WriteVariable
(
i
,
data
[
j
]);
...
...
@@ -143,7 +147,7 @@ void bioloid_dyn_slave_init(void)
DYN_SLAVE_TIMER_ENABLE_CLK
;
bioloid_dyn_slave_tim_handle
.
Instance
=
DYN_SLAVE_TIMER
;
bioloid_dyn_slave_tim_handle
.
Init
.
Period
=
1000
;
bioloid_dyn_slave_tim_handle
.
Init
.
Prescaler
=
8
4
;
bioloid_dyn_slave_tim_handle
.
Init
.
Prescaler
=
4
2
;
bioloid_dyn_slave_tim_handle
.
Init
.
ClockDivision
=
TIM_CLOCKDIVISION_DIV1
;
bioloid_dyn_slave_tim_handle
.
Init
.
CounterMode
=
TIM_COUNTERMODE_UP
;
HAL_TIM_Base_Init
(
&
bioloid_dyn_slave_tim_handle
);
...
...
src/bioloid_stm32.c
View file @
e3fcd55d
...
...
@@ -7,10 +7,12 @@
#include
"bioloid_time.h"
#include
"bioloid_dyn_slave.h"
#include
"bioloid_dyn_master_sensors.h"
#include
"
bioloid_dyn_master_servos
.h"
#include
"
motion_manager
.h"
int32_t
main
(
void
)
{
uint16_t
eeprom_data
,
period
;
HAL_Init
();
/* initialize the gpio */
gpio_init
();
...
...
@@ -29,8 +31,12 @@ int32_t main(void)
bioloid_dyn_slave_start
();
/* initialize the dynamixel master module for the sensors */
bioloid_dyn_master_sensors_init
();
/* initialize the dynamixel master module for the servos */
bioloid_dyn_master_servos_init
();
/* initialize motion manager module */
EE_ReadVariable
(
MM_PERIOD_OFFSET
,
&
eeprom_data
);
period
=
eeprom_data
&
0x00FF
;
EE_ReadVariable
(
MM_PERIOD_OFFSET
+
1
,
&
eeprom_data
);
period
+=
((
eeprom_data
&
0x00FF
)
<<
8
);
manager_init
(
period
);
while
(
1
);
}
...
...
src/eeprom.c
View file @
e3fcd55d
...
...
@@ -51,6 +51,7 @@
#define DEFAULT_DEVICE_ID 0x0002
#define DEFAULT_BAUDRATE 0x0001
#define DEFAULT_RETURN_DELAY 0x0000
#define DEFAULT_MM_PERIOD 0x01FF
#define DEFAULT_RETURN_LEVEL 0x0002
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
...
...
@@ -73,6 +74,10 @@ uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE,
BAUDRATE_OFFSET
,
DEFAULT_RETURN_DELAY
,
// return delay time
RETURN_DELAY_OFFSET
,
DEFAULT_MM_PERIOD
&
0xFF
,
MM_PERIOD_OFFSET
,
DEFAULT_MM_PERIOD
>>
8
,
MM_PERIOD_OFFSET
+
1
,
DEFAULT_RETURN_LEVEL
,
RETURN_LEVEL_OFFSET
};
// return level
...
...
src/gpio.c
View file @
e3fcd55d
...
...
@@ -522,6 +522,12 @@ void gpio_set_pushbutton_callback(pushbutton_t pb_id,void (*callback)(void))
}
}
// operation functions
uint8_t
gpio_in_range
(
unsigned
short
int
address
,
unsigned
short
int
length
)
{
return
ram_in_window
(
GPIO_BASE_ADDRESS
,
GPIO_MEM_LENGTH
,
address
,
length
);
}
void
gpio_process_read_cmd
(
unsigned
short
int
address
,
unsigned
short
int
length
,
unsigned
char
*
data
)
{
if
(
ram_in_range
(
BIOLOID_RESET_PB_CNTRL
,
address
,
length
))
...
...
src/motion_manager.c
View file @
e3fcd55d
#include
"bioloid_dyn_master_servos.h"
#include
"motion_manager.h"
#include
"dynamixel_master_uart_dma.h"
#include
"dyn_servos.h"
#include
"ram.h"
#include
"action.h"
#include
"stm32_time.h"
#define
MOTION
_TIMER TIM3
#define
MOTION_TIMER_IRQn TIM3_IRQn
#define
MOTION
_TIMER_IRQ
Handler
TIM3_IRQ
Handler
#define
MOTION_TIMER_CLK RCC_APB1Periph_TIM3
#define
MANAGER
_TIMER
TIM3
#define
ENABLE_MANAGER_TIMER_CLK __HAL_RCC_TIM3_CLK_ENABLE()
#define
MANAGER
_TIMER_IRQ
n
TIM3_IRQ
n
#define
MANAGER_TIMER_IRQHandler TIM3_IRQHandler
// private variables
__IO
uint16_t
manager_motion_period
;
TIM_HandleTypeDef
MANAGER_TIM_Handle
;
uint16_t
manager_motion_period
;
uint8_t
manager_num_servos
;
TServoInfo
manager_servos
[
MANAGER_MAX_NUM_SERVOS
];
// current angles used for all motion modules
int64_t
current_angles
[
MANAGER_MAX_NUM_SERVOS
];
int64_t
manager_
current_angles
[
MANAGER_MAX_NUM_SERVOS
];
// balance values and configuration
int64_t
balance_offset
[
MANAGER_MAX_NUM_SERVOS
];
uint8_t
balance_enabled
;
// package variables
uint8_t
servo_ids
[
MANAGER_MAX_NUM_SERVOS
];
TWriteData
write_data
[
MANAGER_MAX_NUM_SERVOS
];
uint8_t
length
[
MANAGER_MAX_NUM_SERVOS
];
uint8_t
address
[
MANAGER_MAX_NUM_SERVOS
];
int64_t
manager_balance_offset
[
MANAGER_MAX_NUM_SERVOS
];
uint8_t
manager_balance_enabled
;
// private functions
void
manager_send_motion_command
(
void
)
{
static
uint8_t
servo_ids
[
MANAGER_MAX_NUM_SERVOS
];
static
TWriteData
write_data
[
MANAGER_MAX_NUM_SERVOS
];
uint8_t
i
,
num
=
0
;
for
(
i
=
0
;
i
<
MANAGER_MAX_NUM_SERVOS
;
i
++
)
...
...
@@ -40,7 +37,7 @@ void manager_send_motion_command(void)
}
}
if
(
num
>
0
)
dyn_master_sync_write
(
num
,
servo_ids
,
P_GOAL_POSITION_L
,
2
,
write_data
);
dyn_master_sync_write
(
&
bioloid_dyn_master_servos
,
num
,
servo_ids
,
P_GOAL_POSITION_L
,
2
,
write_data
);
}
inline
uint16_t
manager_angle_to_value
(
uint8_t
servo_id
,
int16_t
angle
)
...
...
@@ -73,7 +70,7 @@ void manager_get_current_position(void)
{
if
(
!
manager_servos
[
i
].
enabled
)
// servo is not enabled
{
dyn_master_read_word
(
manager_servos
[
i
].
id
,
P_PRESENT_POSITION_L
,
&
manager_servos
[
i
].
current_value
);
dyn_master_read_word
(
&
bioloid_dyn_master_servos
,
manager_servos
[
i
].
id
,
P_PRESENT_POSITION_L
,
&
manager_servos
[
i
].
current_value
);
manager_servos
[
i
].
current_angle
=
manager_value_to_angle
(
i
,
manager_servos
[
i
].
current_value
);
}
ram_data
[
BIOLOID_MM_SERVO0_CUR_POS_L
+
i
*
2
]
=
(
manager_servos
[
i
].
current_angle
&
0xFF
);
...
...
@@ -91,7 +88,7 @@ void manager_get_target_position(void)
{
if
(
manager_servos
[
i
].
module
==
MM_ACTION
)
{
manager_servos
[
i
].
current_angle
=
((
current_angles
[
i
]
+
balance_offset
[
i
])
>>
9
);
manager_servos
[
i
].
current_angle
=
((
manager_
current_angles
[
i
]
+
manager_
balance_offset
[
i
])
>>
9
);
//>>16 from the action codification, <<7 from the manager codification
manager_servos
[
i
].
current_value
=
manager_angle_to_value
(
i
,
manager_servos
[
i
].
current_angle
);
}
...
...
@@ -104,50 +101,53 @@ void manager_balance(void)
}
// interrupt handlers
void
M
OTION
_TIMER_IRQHandler
(
void
)
void
M
ANAGER
_TIMER_IRQHandler
(
void
)
{
uint16_t
capture
;
if
(
TIM_GetITStatus
(
MOTION_TIMER
,
TIM_
IT
_CC1
)
!=
RESET
)
if
(
__HAL_TIM_GET_FLAG
(
&
MANAGER_TIM_Handle
,
TIM_
FLAG
_CC1
)
!=
RESET
)
{
TIM_ClearITPendingBit
(
MOTION_TIMER
,
TIM_IT_CC1
);
capture
=
TIM_GetCapture1
(
MOTION_TIMER
);
TIM_SetCompare1
(
MOTION_TIMER
,
capture
+
manager_motion_period
);
// call the action process
action_process
();
// call the joint motion process
// joint_motion_process();
// call the walking process
// walking_process();
// get the target angles from all modules
manager_get_target_position
();
// balance the robot
manager_balance
();
// send the motion commands to the servos
manager_send_motion_command
();
// get the disabled servos position
// manager_get_current_position();
if
(
__HAL_TIM_GET_IT_SOURCE
(
&
MANAGER_TIM_Handle
,
TIM_IT_CC1
)
!=
RESET
)
{
__HAL_TIM_CLEAR_IT
(
&
MANAGER_TIM_Handle
,
TIM_IT_CC1
);
capture
=
HAL_TIM_ReadCapturedValue
(
&
MANAGER_TIM_Handle
,
TIM_CHANNEL_1
);
__HAL_TIM_SET_COMPARE
(
&
MANAGER_TIM_Handle
,
TIM_CHANNEL_1
,
(
capture
+
manager_motion_period
));
// call the action process
// action_process();
// call the joint motion process
// joint_motion_process();
// call the walking process
// walking_process();
// get the target angles from all modules
// manager_get_target_position();
// balance the robot
// manager_balance();
// send the motion commands to the servos
// manager_send_motion_command();
// get the disabled servos position
// manager_get_current_position();
}
}
}
// public functions
void
manager_init
(
uint16_t
period_us
)
{
TIM_TimeBaseInitTypeDef
TIM_TimeBaseStructure
;
TIM_OCInitTypeDef
TIM_OCInitStructure
;
NVIC_InitTypeDef
NVIC_InitStructure
;
TIM_ClockConfigTypeDef
sClockSourceConfig
;
TIM_MasterConfigTypeDef
sMasterConfig
;
uint8_t
servo_ids
[
MANAGER_MAX_NUM_SERVOS
];
uint16_t
model
,
value
;
uint8_t
i
,
num
=
0
;
RCC_APB1PeriphClockCmd
(
MOTION_TIMER_CLK
,
ENABLE
);
/* initialize the dynamixel master module for the servos */
bioloid_dyn_master_servos_init
();
// enable power to the servos
dyn_master_enable_power
();
d
elay
_ms
(
1000
);
bioloid_
dyn_master_
servos_
enable_power
();
HAL_D
elay
(
1000
);
// detect the servos connected
dyn_master_scan
(
&
num
,
servo_ids
);
dyn_master_scan
(
&
bioloid_dyn_master_servos
,
&
num
,
servo_ids
);
ram_data
[
BIOLOID_MM_NUM_SERVOS
]
=
num
;
manager_num_servos
=
0
;
for
(
i
=
0
;
i
<
MANAGER_MAX_NUM_SERVOS
;
i
++
)
...
...
@@ -155,7 +155,7 @@ void manager_init(uint16_t period_us)
if
(
i
==
servo_ids
[
manager_num_servos
])
{
// read the model of the i-th device
dyn_master_read_word
(
servo_ids
[
manager_num_servos
],
P_MODEL_NUMBER_L
,
&
model
);
dyn_master_read_word
(
&
bioloid_dyn_master_servos
,
servo_ids
[
manager_num_servos
],
P_MODEL_NUMBER_L
,
&
model
);
switch
(
model
)
{
case
SERVO_AX12A
:
manager_servos
[
i
].
encoder_resolution
=
1023
;
...
...
@@ -225,17 +225,17 @@ void manager_init(uint16_t period_us)
manager_servos
[
i
].
module
=
MM_NONE
;
manager_servos
[
i
].
enabled
=
0x00
;
// get the servo's current position
dyn_master_read_word
(
manager_servos
[
i
].
id
,
P_PRESENT_POSITION_L
,
&
manager_servos
[
i
].
current_value
);
dyn_master_read_word
(
&
bioloid_dyn_master_servos
,
manager_servos
[
i
].
id
,
P_PRESENT_POSITION_L
,
&
manager_servos
[
i
].
current_value
);
manager_servos
[
i
].
current_angle
=
manager_value_to_angle
(
i
,
manager_servos
[
i
].
current_value
);
ram_data
[
BIOLOID_MM_SERVO0_CUR_POS_L
+
i
*
2
]
=
(
manager_servos
[
i
].
current_angle
&
0xFF
);
ram_data
[
BIOLOID_MM_SERVO0_CUR_POS_H
+
i
*
2
]
=
(
manager_servos
[
i
].
current_angle
>>
8
);
// read the servo limits
dyn_master_read_word
(
manager_servos
[
i
].
id
,
P_CW_ANGLE_LIMIT_L
,
&
value
);
dyn_master_read_word
(
&
bioloid_dyn_master_servos
,
manager_servos
[
i
].
id
,
P_CW_ANGLE_LIMIT_L
,
&
value
);
manager_servos
[
i
].
cw_angle_limit
=
manager_value_to_angle
(
i
,
value
);
dyn_master_read_word
(
manager_servos
[
i
].
id
,
P_CCW_ANGLE_LIMIT_L
,
&
value
);
dyn_master_read_word
(
&
bioloid_dyn_master_servos
,
manager_servos
[
i
].
id
,
P_CCW_ANGLE_LIMIT_L
,
&
value
);
manager_servos
[
i
].
ccw_angle_limit
=
manager_value_to_angle
(
i
,
value
);
// set the action current angles
current_angles
[
i
]
=
manager_servos
[
i
].
current_angle
<<
9
;
manager_
current_angles
[
i
]
=
manager_servos
[
i
].
current_angle
<<
9
;
manager_num_servos
++
;
}
else
...
...
@@ -255,39 +255,32 @@ void manager_init(uint16_t period_us)
manager_servos
[
i
].
ccw_angle_limit
=
0
;
}
}
dyn_master_disable_power
();
// initialize the timer interrupts
NVIC_InitStructure
.
NVIC_IRQChannel
=
MOTION_TIMER_IRQn
;
NVIC_InitStructure
.
NVIC_IRQChannelPreemptionPriority
=
2
;
NVIC_InitStructure
.
NVIC_IRQChannelSubPriority
=
0
;
NVIC_InitStructure
.
NVIC_IRQChannelCmd
=
ENABLE
;
NVIC_Init
(
&
NVIC_InitStructure
);
/* motion timer configuration */
TIM_TimeBaseStructure
.
TIM_Period
=
0xFFFF
;
TIM_TimeBaseStructure
.
TIM_Prescaler
=
42
;
TIM_TimeBaseStructure
.
TIM_ClockDivision
=
0
;
TIM_TimeBaseStructure
.
TIM_CounterMode
=
TIM_CounterMode_Up
;
TIM_TimeBaseInit
(
MOTION_TIMER
,
&
TIM_TimeBaseStructure
);
TIM_OCInitStructure
.
TIM_OCMode
=
TIM_OCMode_Timing
;
TIM_OCInitStructure
.
TIM_OutputState
=
TIM_OutputState_Disable
;
TIM_OCInitStructure
.
TIM_OCPolarity
=
TIM_OCPolarity_High
;
bioloid_dyn_master_servos_disable_power
();
/* configure timer */
ENABLE_MANAGER_TIMER_CLK
;
MANAGER_TIM_Handle
.
Instance
=
MANAGER_TIMER
;
MANAGER_TIM_Handle
.
Init
.
Period
=
0xFFFF
;
MANAGER_TIM_Handle
.
Init
.
Prescaler
=
42
;
MANAGER_TIM_Handle
.
Init
.
ClockDivision
=
TIM_CLOCKDIVISION_DIV1
;
MANAGER_TIM_Handle
.
Init
.
CounterMode
=
TIM_COUNTERMODE_UP
;
HAL_NVIC_SetPriority
(
MANAGER_TIMER_IRQn
,
2
,
0
);
HAL_NVIC_EnableIRQ
(
MANAGER_TIMER_IRQn
);
/* use the internal clock */
sClockSourceConfig
.
ClockSource
=
TIM_CLOCKSOURCE_INTERNAL
;
HAL_TIM_ConfigClockSource
(
&
MANAGER_TIM_Handle
,
&
sClockSourceConfig
);
HAL_TIM_OC_Init
(
&
MANAGER_TIM_Handle
);
/* disable master/slave mode */
sMasterConfig
.
MasterOutputTrigger
=
TIM_TRGO_RESET
;
sMasterConfig
.
MasterSlaveMode
=
TIM_MASTERSLAVEMODE_DISABLE
;
HAL_TIMEx_MasterConfigSynchronization
(
&
MANAGER_TIM_Handle
,
&
sMasterConfig
);
/* configure ouptut counter channel 4 */
manager_motion_period
=
(
period_us
*
1000000
)
>>
16
;
TIM_OCInitStructure
.
TIM_Pulse
=
manager_motion_period
;
TIM_OC1Init
(
MOTION_TIMER
,
&
TIM_OCInitStructure
);
TIM_OC1PreloadConfig
(
MOTION_TIMER
,
TIM_OCPreload_Disable
);
ram_clear_bit
(
BIOLOID_MM_STATUS
,
0
);
/* initialize motion modules */
action_init
(
period_us
);
/* initialize balance parameters */
for
(
i
=
0
;
i
<
MANAGER_MAX_NUM_SERVOS
;
i
++
)
balance_offset
[
i
]
=
0
;
balance_enabled
=
0x0
1
;
manager_
balance_offset
[
i
]
=
0
;
manager_
balance_enabled
=
0x0
0
;
}
uint16_t
manager_get_period
(
void
)
...
...
@@ -298,45 +291,45 @@ uint16_t manager_get_period(void)
void
manager_set_period
(
uint16_t
period_us
)
{
manager_motion_period
=
(
period_us
*
1000000
)
>>
16
;
ram_write_word
(
BIOLOID_MM_PERIOD_H
,
period_us
);
action_set_period
(
period_us
);
ram_data
[
BIOLOID_MM_PERIOD_L
]
=
period_us
;
}
inline
void
manager_enable
(
void
)
{
TIM_OC_InitTypeDef
TIM_OCInitStructure
;
uint16_t
capture
;