diff --git a/include/bioloid_registers.h b/include/bioloid_registers.h
index 47a2a89265150e612812b4ea833cb3c13f58cbb6..5376e83a14e6c7d35db5f116586402a157ad432f 100644
--- a/include/bioloid_registers.h
+++ b/include/bioloid_registers.h
@@ -179,8 +179,8 @@ typedef enum {
BIOLOID_MM_SERVO31_CUR_POS_L = 0x98,
BIOLOID_MM_SERVO31_CUR_POS_H = 0x99,
BIOLOID_ACTION_PAGE = 0x9A,
- BIOLOID_ACTION_CNTRL = 0x9B,
- BIOLOID_ACTION_STATUS = 0x9C
+ BIOLOID_ACTION_CNTRL = 0x9B // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | page running | interrupt flag | enable interrupt | stop page | start page
} bioloid_registers;
#define GPIO_BASE_ADDRESS 0x20
@@ -194,15 +194,30 @@ typedef enum {
#define ADC_BASE_ADDRESS 0x30
#define ADC_MEM_LENGTH 18
+#define ADC_ENABLE 0x01
#define ZIGBEE_BASE_ADDRESS 0x42
#define ZIGBEE_MEM_LENGTH 6
+#define ZIGBEE_EN_PWR 0x01
+#define ZIGBEE_ENABLE 0x02
#define MANAGER_BASE_ADDRESS 0x48
#define MANAGER_MEM_LENGTH 50
+#define MANAGER_ENABLE 0x01
+#define MANAGER_EN_BAL 0x02
+#define MANAGER_EN_PWR 0x04
+#define MANAGER_EVEN_SER_EN 0x80
+#define MANAGER_EVEN_SER_MOD 0x70
+#define MANAGER_ODD_SER_EN 0x08
+#define MANAGER_ODD_SER_MOD 0x07
#define ACTION_BASE_ADDRESS 0x9A
-#define ACTION_MEM_LENGTH 3
+#define ACTION_MEM_LENGTH 2
+#define ACTION_START 0x01
+#define ACTION_STOP 0x02
+#define ACTION_INT_EN 0x04
+#define ACTION_INT_FLAG 0x08
+#define ACTION_STATUS 0x10
#ifdef __cplusplus
}
diff --git a/src/action.c b/src/action.c
index c5ed9856fb7b387b6a833c4c632e00970e874989..c4262d0c01397f044037cbcecd61f268b96a05e3 100644
--- a/src/action.c
+++ b/src/action.c
@@ -286,15 +286,13 @@ void action_start_page(void)
action_current_time=0;
action_section_time=0;
action_current_step_index=-1;
- ram_data[BIOLOID_ACTION_STATUS]|=0x01;
+ ram_data[BIOLOID_ACTION_CNTRL]|=ACTION_STATUS;
action_running=0x01;
- ram_data[BIOLOID_ACTION_CNTRL]|=0x01;
}
void action_stop_page(void)
{
action_stop=0x01;
- //ram_data[BIOLOID_ACTION_CNTRL]|=0x01;
}
uint8_t action_is_running(void)
@@ -443,7 +441,7 @@ void action_process(void)
action_load_next_step();
state=ACTION_PAUSE;
action_end=0x00;
- ram_data[BIOLOID_ACTION_STATUS]&=0xFE;
+ ram_data[BIOLOID_ACTION_CNTRL]&=(~ACTION_STATUS);
action_running=0x00;
}
else
@@ -502,7 +500,7 @@ void action_process(void)
action_load_next_step();
state=ACTION_PAUSE;
action_end=0x00;
- ram_data[BIOLOID_ACTION_STATUS]&=0xFE;
+ ram_data[BIOLOID_ACTION_CNTRL]&=(~ACTION_STATUS);
action_running=0x00;
}
else
@@ -533,19 +531,16 @@ void action_process(void)
// operation functions
uint8_t action_in_range(unsigned short int address, unsigned short int length)
{
- if(ram_in_window(ACTION_BASE_ADDRESS,ACTION_MEM_LENGTH,address,length))
- return 0x01;
- else
- return 0x00;
+ return ram_in_window(ACTION_BASE_ADDRESS,ACTION_MEM_LENGTH,address,length);
}
void action_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
{
if(ram_in_range(BIOLOID_ACTION_CNTRL,address,length))
{
- if(data[BIOLOID_ACTION_CNTRL-address]&0x01)
+ if(data[BIOLOID_ACTION_CNTRL-address]&ACTION_START)
action_start_page();
- if(data[BIOLOID_ACTION_CNTRL-address]&0x02)
+ if(data[BIOLOID_ACTION_CNTRL-address]&ACTION_STOP)
action_stop_page();
}
if(ram_in_range(BIOLOID_ACTION_PAGE,address,length))// load the page identifier
diff --git a/src/adc_dma.c b/src/adc_dma.c
index 5a82d6492bca930e5abfcdbd575ca64ee5ba7f29..074643bcbfa444b91f7e84bd07c73e5ae3df24f0 100644
--- a/src/adc_dma.c
+++ b/src/adc_dma.c
@@ -378,7 +378,7 @@ void adc_process_write_cmd(unsigned short int address,unsigned short int length,
{
if(ram_in_range(BIOLOID_ADC_CNTRL,address,length))
{
- if(data[BIOLOID_ADC_CNTRL-address]&0x01)
+ if(data[BIOLOID_ADC_CNTRL-address]&ADC_ENABLE)
adc_start();
else
adc_stop();
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 6ffd8347e49bbfd5a2470a7cec0bfd811180ffe5..9b294a3b8c1fe8b2dcd30907ba171658712182d6 100644
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -316,30 +316,30 @@ inline void manager_enable(void)
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;
+ ram_data[BIOLOID_MM_CNTRL]|=MANAGER_ENABLE;
}
inline void manager_disable(void)
{
HAL_TIM_OC_Stop_IT(&MANAGER_TIM_Handle, TIM_CHANNEL_1);
- ram_data[BIOLOID_MM_CNTRL]&=0xFE;
+ ram_data[BIOLOID_MM_CNTRL]&=(~MANAGER_ENABLE);
}
inline uint8_t manager_is_enabled(void)
{
- return ram_data[BIOLOID_MM_CNTRL]&0x01;
+ return ram_data[BIOLOID_MM_CNTRL]&MANAGER_ENABLE;
}
void manager_enable_balance(void)
{
manager_balance_enabled=0x01;
- ram_data[BIOLOID_MM_CNTRL]|=0x02;
+ ram_data[BIOLOID_MM_CNTRL]|=MANAGER_EN_BAL;
}
void manager_disable_balance(void)
{
manager_balance_enabled=0x00;
- ram_data[BIOLOID_MM_CNTRL]&=0xFD;
+ ram_data[BIOLOID_MM_CNTRL]&=(~MANAGER_EN_BAL);
}
inline uint8_t manager_get_num_servos(void)
@@ -355,10 +355,10 @@ void manager_set_module(uint8_t servo_id,TModules module)
{
manager_servos[servo_id].module=module;
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);
+ if(servo_id%2)// odd servo
+ byte=(byte&(~MANAGER_ODD_SER_MOD))+((uint8_t)module);
+ else// even servo
+ byte=(byte&(~MANAGER_EVEN_SER_MOD))+(((uint8_t)module)<<4);
ram_data[BIOLOID_MM_MODULE_EN0+servo_id/2]=byte;
}
}
@@ -379,10 +379,10 @@ inline void manager_enable_servo(uint8_t servo_id)
{
manager_servos[servo_id].enabled=0x01;
byte=ram_data[BIOLOID_MM_MODULE_EN0+servo_id/2];;
- if(servo_id%2)
- byte|=0x08;
- else
- byte|=0x80;
+ if(servo_id%2)// odd servo
+ byte|=MANAGER_ODD_SER_EN;
+ else// even servo
+ byte|=MANAGER_EVEN_SER_EN;
ram_data[BIOLOID_MM_MODULE_EN0+servo_id/2]=byte;
}
}
@@ -396,9 +396,9 @@ inline void manager_disable_servo(uint8_t servo_id)
manager_servos[servo_id].enabled=0x00;
byte=ram_data[BIOLOID_MM_MODULE_EN0+servo_id/2];
if(servo_id%2)
- byte&=0xF7;
+ byte&=(~MANAGER_ODD_SER_EN);
else
- byte&=0x7F;
+ byte&=(~MANAGER_EVEN_SER_EN);
ram_data[BIOLOID_MM_MODULE_EN0+servo_id/2]=byte;
}
}
@@ -446,27 +446,27 @@ void manager_process_write_cmd(unsigned short int address,unsigned short int len
if(ram_in_range(i,address,length))
{
byte_value=data[i-address];
- if(byte_value&0x80) manager_enable_servo(j);
+ if(byte_value&MANAGER_EVEN_SER_EN) manager_enable_servo(j);
else manager_disable_servo(j);
- module=(byte_value&0x70)>>4;
+ module=(byte_value&MANAGER_EVEN_SER_MOD)>>4;
manager_set_module(j,module);
- if(byte_value&0x08) manager_enable_servo(j+1);
+ if(byte_value&MANAGER_ODD_SER_EN) manager_enable_servo(j+1);
else manager_disable_servo(j+1);
- module=byte_value&0x07;
+ module=byte_value&MANAGER_ODD_SER_MOD;
manager_set_module(j+1,module);
}
}
if(ram_in_range(BIOLOID_MM_CNTRL,address,length))
{
- if(data[BIOLOID_MM_CNTRL-address]&0x01)
+ if(data[BIOLOID_MM_CNTRL-address]&MANAGER_ENABLE)
manager_enable();
else
manager_disable();
- if(data[BIOLOID_MM_CNTRL-address]&0x02)
+ if(data[BIOLOID_MM_CNTRL-address]&MANAGER_EN_BAL)
manager_enable_balance();
else
manager_disable_balance();
- if(data[BIOLOID_MM_CNTRL-address]&0x04)
+ if(data[BIOLOID_MM_CNTRL-address]&MANAGER_EN_PWR)
bioloid_dyn_master_servos_enable_power();
else
bioloid_dyn_master_servos_disable_power();
diff --git a/src/zigbee.c b/src/zigbee.c
index 9421b7214bd0bfeefb6d4757013ba8aaa730eb9f..debd30f4bcc75e3c7ff96c2fe48464edaa073607 100644
--- a/src/zigbee.c
+++ b/src/zigbee.c
@@ -27,23 +27,23 @@ void zigbee_init(void)
void zigbee_enable_power(void)
{
HAL_GPIO_WritePin(ZIGBEE_PWR_GPIO_PORT,ZIGBEE_PWR_PIN,GPIO_PIN_RESET);
- ram_data[BIOLOID_ZIGBEE_CNTRL]|=0x01;
+ ram_data[BIOLOID_ZIGBEE_CNTRL]|=ZIGBEE_EN_PWR;
}
void zigbee_disable_power(void)
{
HAL_GPIO_WritePin(ZIGBEE_PWR_GPIO_PORT,ZIGBEE_PWR_PIN,GPIO_PIN_SET);
- ram_data[BIOLOID_ZIGBEE_CNTRL]&=0xFE;
+ ram_data[BIOLOID_ZIGBEE_CNTRL]&=(~ZIGBEE_EN_PWR);
}
void zigbee_enable(void)
{
- ram_data[BIOLOID_ZIGBEE_CNTRL]|=0x02;
+ ram_data[BIOLOID_ZIGBEE_CNTRL]|=ZIGBEE_ENABLE;
}
void zigbee_disable(void)
{
- ram_data[BIOLOID_ZIGBEE_CNTRL]&=0xFD;
+ ram_data[BIOLOID_ZIGBEE_CNTRL]&=(~ZIGBEE_ENABLE);
}
// operation functions
@@ -56,11 +56,11 @@ void zigbee_process_write_cmd(unsigned short int address,unsigned short int leng
{
if(ram_in_range(BIOLOID_ZIGBEE_CNTRL,address,length))
{
- if(data[BIOLOID_ZIGBEE_CNTRL-address]&0x01)
+ if(data[BIOLOID_ZIGBEE_CNTRL-address]&ZIGBEE_EN_PWR)
zigbee_enable_power();
else
zigbee_disable_power();
- if(data[BIOLOID_ZIGBEE_CNTRL-address]&0x02)
+ if(data[BIOLOID_ZIGBEE_CNTRL-address]&ZIGBEE_ENABLE)
zigbee_enable();
else
zigbee_disable();