diff --git a/Makefile b/Makefile
index 3850d7527273ad63213d5530fa4d436164cb22d7..b89b571af64603b5d2b21c23e25a557f0a310875 100755
--- a/Makefile
+++ b/Makefile
@@ -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
diff --git a/include/adc_dma.h b/include/adc_dma.h
index 7b82e293cc776bcd8e840fe07f2ccdc06936442b..a9359ae138bbc01d81b06fc9bd6dfb3ee10d3093 100644
--- a/include/adc_dma.h
+++ b/include/adc_dma.h
@@ -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
diff --git a/include/bioloid_registers.h b/include/bioloid_registers.h
index cb379365db8af1cbe5cf478ca8d38bb02875845e..0dd14ed1245c9a2d2e65a905469a2234fb3b1557 100644
--- a/include/bioloid_registers.h
+++ b/include/bioloid_registers.h
@@ -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
diff --git a/include/gpio.h b/include/gpio.h
index 5200635314774f09a1aae2f9149d844f971aea27..76907b0abb8222c682ee649ab5aebd5c70ef96f3 100644
--- a/include/gpio.h
+++ b/include/gpio.h
@@ -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);
diff --git a/include/motion_manager.h b/include/motion_manager.h
index fcc07e6457ec18e36fb0a414a8f8e5f98cf0b796..c6d654c375721f4fc6633ad17caf43a44bb046d3 100644
--- a/include/motion_manager.h
+++ b/include/motion_manager.h
@@ -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
}
diff --git a/include/motion_pages.h b/include/motion_pages.h
index a195e1705a164993678da09238ef0047fb8a1a95..5ff494fad35ea7b9892fe3d3851c9c8b141dace8 100644
--- a/include/motion_pages.h
+++ b/include/motion_pages.h
@@ -5,7 +5,7 @@
extern "C" {
#endif
-#include "stm32f4xx.h"
+#include "stm32f4xx_hal.h"
#define MAX_PAGES 256
#define PAGE_MAX_NUM_SERVOS 31
diff --git a/include/zigbee.h b/include/zigbee.h
index bc0b98dadefc0925da51af3fbce93100ede0b3c8..87526c65f0d1b2efe76ea77fe57ee64f8fa755ef 100644
--- a/include/zigbee.h
+++ b/include/zigbee.h
@@ -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
diff --git a/src/adc_dma.c b/src/adc_dma.c
index 35da17e307433d6a9e012807a2b87113f76a4ea1..5a82d6492bca930e5abfcdbd575ca64ee5ba7f29 100644
--- a/src/adc_dma.c
+++ b/src/adc_dma.c
@@ -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))
diff --git a/src/bioloid_dyn_master_sensors.c b/src/bioloid_dyn_master_sensors.c
index ff619e83fd9ab862e97fbc157b756b3723287c6a..6c0a87e50496688facbc03e05ee497c648530250 100755
--- a/src/bioloid_dyn_master_sensors.c
+++ b/src/bioloid_dyn_master_sensors.c
@@ -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);
}
diff --git a/src/bioloid_dyn_master_servos.c b/src/bioloid_dyn_master_servos.c
index 955e10f0e740dfc897a8f8b714e4fb45e0d92dde..b4ac7e7c1cf85ce92e139801c2d3d9108b7f40b4 100755
--- a/src/bioloid_dyn_master_servos.c
+++ b/src/bioloid_dyn_master_servos.c
@@ -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)
diff --git a/src/bioloid_dyn_slave.c b/src/bioloid_dyn_slave.c
index 1c8f7613535aa54ffd10422af41f9d7ecef41a78..8e28e2d382d72dca9bb8ec9e6bca3bc64ee88088 100644
--- a/src/bioloid_dyn_slave.c
+++ b/src/bioloid_dyn_slave.c
@@ -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);
diff --git a/src/bioloid_stm32.c b/src/bioloid_stm32.c
index e747769a5091f1357c7e655beed7ee7d81cc0e53..37533df226e5d8baebbd7556d7f153cec0a79f68 100644
--- a/src/bioloid_stm32.c
+++ b/src/bioloid_stm32.c
@@ -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);
}
diff --git a/src/eeprom.c b/src/eeprom.c
index 8696f8cca185f890aced9f3a72435486ef5d0a3f..b41aaaf57739324b675d7cd87a6b2d181882eed2 100755
--- a/src/eeprom.c
+++ b/src/eeprom.c
@@ -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
diff --git a/src/gpio.c b/src/gpio.c
index 2d1e1fb798045af17722ee0fd6490e2708439643..a3f13c2cfbc81e771b6566e846d7706dada69706 100644
--- a/src/gpio.c
+++ b/src/gpio.c
@@ -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))
diff --git a/src/motion_manager.c b/src/motion_manager.c
index c461bfaaddf2a4d3f0d41e0f72e08f8432cfa590..0841cbac0f9cc97f44bf35010e89c9948853e8b5 100644
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -1,33 +1,30 @@
+#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;
- capture = TIM_GetCapture1(MOTION_TIMER);
- TIM_SetCompare1(MOTION_TIMER, capture + manager_motion_period);
- TIM_Cmd(MOTION_TIMER, ENABLE);
- TIM_ITConfig(MOTION_TIMER, TIM_IT_CC1, ENABLE);
- ram_set_bit(BIOLOID_MM_STATUS,0);
+ TIM_OCInitStructure.OCMode = TIM_OCMODE_TIMING;
+ TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH;
+ TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE;
+ capture = HAL_TIM_ReadCapturedValue(&MANAGER_TIM_Handle, TIM_CHANNEL_1);
+ TIM_OCInitStructure.Pulse = capture+manager_motion_period;
+ HAL_TIM_OC_ConfigChannel(&MANAGER_TIM_Handle, &TIM_OCInitStructure,TIM_CHANNEL_1);
+ HAL_TIM_OC_Start_IT(&MANAGER_TIM_Handle, TIM_CHANNEL_1);
+ ram_data[BIOLOID_MM_CNTRL]|=0x01;
}
inline void manager_disable(void)
{
- TIM_Cmd(MOTION_TIMER, DISABLE);
- TIM_ITConfig(MOTION_TIMER, TIM_IT_CC1, DISABLE);
- ram_clear_bit(BIOLOID_MM_STATUS,0);
+ HAL_TIM_OC_Stop_IT(&MANAGER_TIM_Handle, TIM_CHANNEL_1);
+ ram_data[BIOLOID_MM_CNTRL]&=0xFE;
}
inline uint8_t manager_is_enabled(void)
{
- return ram_data[BIOLOID_MM_STATUS]&0x01;
+ return ram_data[BIOLOID_MM_CNTRL]&0x01;
}
void manager_enable_balance(void)
{
- ram_set_bit(BIOLOID_MM_STATUS,1);
- ram_set_bit(BIOLOID_MM_CONTROL,1);
- balance_enabled=0x01;
+ manager_balance_enabled=0x01;
+ ram_data[BIOLOID_MM_CNTRL]|=0x02;
}
void manager_disable_balance(void)
{
- ram_clear_bit(BIOLOID_MM_STATUS,1);
- ram_clear_bit(BIOLOID_MM_CONTROL,1);
- balance_enabled=0x00;
+ manager_balance_enabled=0x00;
+ ram_data[BIOLOID_MM_CNTRL]&=0xFD;
}
inline uint8_t manager_get_num_servos(void)
@@ -351,12 +344,12 @@ void manager_set_module(uint8_t servo_id,TModules module)
if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
{
manager_servos[servo_id].module=module;
- ram_read_byte(BIOLOID_MM_MODULE_EN0+servo_id/2,&byte);
+ byte=ram_data[BIOLOID_MM_MODULE_EN0+servo_id/2];
if(servo_id%2)
byte=(byte&0xF8)+((uint8_t)module);
else
byte=(byte&0x8F)+(((uint8_t)module)<<4);
- ram_write_byte(BIOLOID_MM_MODULE_EN0+servo_id/2,byte);
+ ram_data[BIOLOID_MM_MODULE_EN0+servo_id/2]=byte;
}
}
@@ -375,12 +368,12 @@ inline void manager_enable_servo(uint8_t servo_id)
if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
{
manager_servos[servo_id].enabled=0x01;
- ram_read_byte(BIOLOID_MM_MODULE_EN0+servo_id/2,&byte);
+ byte=ram_data[BIOLOID_MM_MODULE_EN0+servo_id/2];;
if(servo_id%2)
byte|=0x08;
else
byte|=0x80;
- ram_write_byte(BIOLOID_MM_MODULE_EN0+servo_id/2,byte);
+ ram_data[BIOLOID_MM_MODULE_EN0+servo_id/2]=byte;
}
}
@@ -391,19 +384,15 @@ inline void manager_disable_servo(uint8_t servo_id)
if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
{
manager_servos[servo_id].enabled=0x00;
- ram_read_byte(BIOLOID_MM_MODULE_EN0+servo_id/2,&byte);
+ byte=ram_data[BIOLOID_MM_MODULE_EN0+servo_id/2];
if(servo_id%2)
byte&=0xF7;
else
byte&=0x7F;
- ram_write_byte(BIOLOID_MM_MODULE_EN0+servo_id/2,byte);
+ ram_data[BIOLOID_MM_MODULE_EN0+servo_id/2]=byte;
}
}
-void manager_disable_servos(void)
-{
-}
-
inline uint8_t manager_is_servo_enabled(uint8_t servo_id)
{
if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
@@ -422,3 +411,54 @@ inline int16_t manager_get_ccw_angle_limit(uint8_t servo_id)
return manager_servos[servo_id].ccw_angle_limit;
}
+// operation functions
+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))
+ return 0x01;
+ else
+ return 0x00;
+}
+
+void manager_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ uint16_t word_value;
+ uint8_t byte_value,module,i,j;
+
+ if(ram_in_range(BIOLOID_MM_PERIOD_L,address,length) && ram_in_range(BIOLOID_MM_PERIOD_H,address,length))
+ {
+ word_value=data[BIOLOID_MM_PERIOD_L-address]+(data[BIOLOID_MM_PERIOD_H-address]<<8);
+ manager_set_period(word_value);
+ }
+ for(i=BIOLOID_MM_MODULE_EN0,j=0;i<=BIOLOID_MM_MODULE_EN15;i++,j+=2)
+ {
+ if(ram_in_range(i,address,length))
+ {
+ byte_value=data[i-address];
+ if(byte_value&0x80) manager_enable_servo(j);
+ else manager_disable_servo(j);
+ module=(byte_value&0x70)>>4;
+ manager_set_module(j,module);
+ if(byte_value&0x08) manager_enable_servo(j+1);
+ else manager_disable_servo(j+1);
+ module=byte_value&0x07;
+ manager_set_module(j+1,module);
+ }
+ }
+ if(ram_in_range(BIOLOID_MM_CNTRL,address,length))
+ {
+ if(data[BIOLOID_MM_CNTRL-address]&0x01)
+ manager_enable();
+ else
+ manager_disable();
+ if(data[BIOLOID_MM_CNTRL-address]&0x02)
+ manager_enable_balance();
+ else
+ manager_disable_balance();
+ if(data[BIOLOID_MM_CNTRL-address]&0x04)
+ bioloid_dyn_master_servos_enable_power();
+ else
+ bioloid_dyn_master_servos_disable_power();
+ }
+}
diff --git a/src/zigbee.c b/src/zigbee.c
index 69e2d82626b1cf4d74e01f7d7640855d550efecd..9421b7214bd0bfeefb6d4757013ba8aaa730eb9f 100644
--- a/src/zigbee.c
+++ b/src/zigbee.c
@@ -47,6 +47,11 @@ void zigbee_disable(void)
}
// operation functions
+uint8_t zigbee_in_range(unsigned short int address,unsigned short int length)
+{
+ return ram_in_window(ZIGBEE_BASE_ADDRESS,ZIGBEE_MEM_LENGTH,address,length);
+}
+
void zigbee_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
{
if(ram_in_range(BIOLOID_ZIGBEE_CNTRL,address,length))