Skip to content
GitLab
Commit e3fcd55d authored by Sergi Hernandez's avatar 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
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 = 84;
bioloid_dyn_slave_tim_handle.Init.Prescaler = 42;
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_IRQHandler TIM3_IRQHandler
#define MOTION_TIMER_CLK RCC_APB1Periph_TIM3
#define MANAGER_TIMER TIM3
#define ENABLE_MANAGER_TIMER_CLK __HAL_RCC_TIM3_CLK_ENABLE()
#define MANAGER_TIMER_IRQn TIM3_IRQn
#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 MOTION_TIMER_IRQHandler(void)
void MANAGER_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();
delay_ms(1000);
bioloid_dyn_master_servos_enable_power();
HAL_Delay(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=0x01;
manager_balance_offset[i]=0;
manager_balance_enabled=0x00;
}
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;