diff --git a/include/bioloid_dyn_slave.h b/include/bioloid_dyn_slave.h
index 1bf96b38b4e854885652e380237da13898b933a1..f6d7158fffbbefc629000aafac9d2dada26f50f7 100644
--- a/include/bioloid_dyn_slave.h
+++ b/include/bioloid_dyn_slave.h
@@ -10,9 +10,9 @@ extern "C" {
#include "dynamixel_slave.h"
#include "comm.h"
-extern TDynamixelSlave bioloid_dyn_slave;
-
void bioloid_dyn_slave_init(void);
+void bioloid_dyn_slave_start(void);
+void bioloid_dyn_slave_stop(void);
void bioloid_dyn_slave_set_int(void);
void bioloid_dyn_slave_clear_int(void);
uint8_t bioloid_dyn_slave_is_int_set(void);
diff --git a/include/bioloid_registers.h b/include/bioloid_registers.h
index 4b606f9d3bd2270a26bd69d946850f13439eca09..c18bb7d75d894c644b1df97e7d1a1b1fe63f8c8d 100644
--- a/include/bioloid_registers.h
+++ b/include/bioloid_registers.h
@@ -54,8 +54,8 @@ typedef enum {
#define GPIO_VALUE 0x02
#define GPIO_TOGGLE 0x04
#define GPIO_BLINK 0x08
-#define GPIO_INT_EN 0x04
-#define GPIO_INT_FLAG 0x08
+#define GPIO_INT_EN 0x02
+#define GPIO_INT_FLAG 0x04
#ifdef __cplusplus
}
diff --git a/include/gpio.h b/include/gpio.h
index 4f5ea59546abdde0bca4079660f0fadd901c248c..5200635314774f09a1aae2f9149d844f971aea27 100644
--- a/include/gpio.h
+++ b/include/gpio.h
@@ -21,7 +21,8 @@ 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
-void gpio_process_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
+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);
#ifdef __cplusplus
}
diff --git a/include/ram.h b/include/ram.h
index 3b350e370257bc192a00a155c8ac68b6f17ee3af..4d53763ff5c159e053f216b0bb98de6373098626 100644
--- a/include/ram.h
+++ b/include/ram.h
@@ -28,7 +28,7 @@ uint8_t ram_write_byte(uint8_t address, uint8_t data);
uint8_t ram_write_word(uint8_t address, uint16_t data);
uint8_t ram_write_table(uint8_t address, uint8_t length,uint8_t *data);
inline uint8_t ram_in_range(uint8_t reg,uint8_t address,uint8_t length);
-inline uint8_t ram_in_window(uint8_t start_reg,uint8_t reg_length,uint8_t start_address,uint8_t address_length);
+uint8_t ram_in_window(uint8_t start_reg,uint8_t reg_length,uint8_t start_address,uint8_t address_length);
#ifdef __cplusplus
}
diff --git a/src/bioloid_dyn_slave.c b/src/bioloid_dyn_slave.c
index a4edced5d1a85756ec7e3962d8f8a4f4f4dd8714..ab61ca3450e304032972197e187cf4b199df1711 100644
--- a/src/bioloid_dyn_slave.c
+++ b/src/bioloid_dyn_slave.c
@@ -10,12 +10,19 @@
#define DYN_SLAVE_EXT_INT_GPIO_PORT GPIOE
#define DYN_SLAVE_EXT_INT_EN_GPIO_CLK __HAL_RCC_GPIOE_CLK_ENABLE()
+/* timer for the execution of the dynamixel slave loop */
+#define DYN_SLAVE_TIMER TIM7
+#define DYN_SLAVE_TIMER_IRQn TIM7_IRQn
+#define DYN_SLAVE_TIMER_IRQHandler TIM7_IRQHandler
+#define DYN_SLAVE_TIMER_ENABLE_CLK __HAL_RCC_TIM7_CLK_ENABLE()
+
/* private variables */
TDynamixelSlave bioloid_dyn_slave;
TTime bioloid_dyn_slave_timer;
TComm bioloid_dyn_slave_comm;
UART_InitTypeDef bioloid_comm_init;
-uint8_t bioloid_slave_int_state;
+TIM_HandleTypeDef bioloid_dyn_slave_tim_handle;
+uint8_t bioloid_slave_int_count;
// private functions
unsigned char bioloid_on_read(unsigned short int address,unsigned short int length,unsigned char *data)
@@ -23,6 +30,9 @@ unsigned char bioloid_on_read(unsigned short int address,unsigned short int leng
unsigned char error;
/* dynamixel slave internal operation registers */
error=ram_read_table(address,length,data);
+ // GPIO commands
+ if(ram_in_window(GPIO_BASE_ADDRESS,GPIO_MEM_LENGTH,address,length))
+ gpio_process_read_cmd(address,length,data);
return error;
}
@@ -55,7 +65,7 @@ unsigned char bioloid_on_write(unsigned short int address,unsigned short int len
}
// GPIO commands
if(ram_in_window(GPIO_BASE_ADDRESS,GPIO_MEM_LENGTH,address,length))
- gpio_process_cmd(address,length,data);
+ gpio_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]);
@@ -63,6 +73,19 @@ unsigned char bioloid_on_write(unsigned short int address,unsigned short int len
return 0x00;
}
+/* interrupt service routines */
+void DYN_SLAVE_TIMER_IRQHandler(void)
+{
+ if(__HAL_TIM_GET_FLAG(&bioloid_dyn_slave_tim_handle, TIM_FLAG_UPDATE) != RESET)
+ {
+ if(__HAL_TIM_GET_IT_SOURCE(&bioloid_dyn_slave_tim_handle, TIM_IT_UPDATE) !=RESET)
+ {
+ __HAL_TIM_CLEAR_IT(&bioloid_dyn_slave_tim_handle, TIM_IT_UPDATE);
+ dyn_slave_loop(&bioloid_dyn_slave);
+ }
+ }
+}
+
// public functions
void bioloid_dyn_slave_init(void)
{
@@ -70,7 +93,7 @@ void bioloid_dyn_slave_init(void)
GPIO_InitTypeDef GPIO_InitStructure;
// initialize private variables
- bioloid_slave_int_state=0;
+ bioloid_slave_int_count=0;
// initialize timer structure
time_init(&bioloid_dyn_slave_timer,bioloid_time_get_counts_per_us(),bioloid_time_get_counts);
@@ -102,25 +125,53 @@ void bioloid_dyn_slave_init(void)
/* initialize the external interrupt pin */
DYN_SLAVE_EXT_INT_EN_GPIO_CLK;
GPIO_InitStructure.Pin = DYN_SLAVE_EXT_INT_PIN;
- GPIO_InitStructure.Mode = GPIO_MODE_OUTPUT_OD;
+ GPIO_InitStructure.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStructure.Pull = GPIO_NOPULL;
GPIO_InitStructure.Speed = GPIO_SPEED_HIGH;
HAL_GPIO_Init(DYN_SLAVE_EXT_INT_GPIO_PORT, &GPIO_InitStructure);
+ HAL_GPIO_WritePin(DYN_SLAVE_EXT_INT_GPIO_PORT,DYN_SLAVE_EXT_INT_PIN,GPIO_PIN_SET);
+
+ /* initialize timer for the execution of the dynamixel slave loop */
+ 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.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+ bioloid_dyn_slave_tim_handle.Init.CounterMode = TIM_COUNTERMODE_UP;
+ HAL_TIM_Base_Init(&bioloid_dyn_slave_tim_handle);
+ // initialize the timer interrupts
+ HAL_NVIC_SetPriority(DYN_SLAVE_TIMER_IRQn, 3, 2);
+ HAL_NVIC_EnableIRQ(DYN_SLAVE_TIMER_IRQn);
+}
+
+void bioloid_dyn_slave_start(void)
+{
+ HAL_TIM_Base_Start_IT(&bioloid_dyn_slave_tim_handle);
+}
+
+void bioloid_dyn_slave_stop(void)
+{
+ HAL_TIM_Base_Stop_IT(&bioloid_dyn_slave_tim_handle);
}
void bioloid_dyn_slave_set_int(void)
{
- HAL_GPIO_WritePin(DYN_SLAVE_EXT_INT_GPIO_PORT,DYN_SLAVE_EXT_INT_PIN,GPIO_PIN_RESET);
- bioloid_slave_int_state=1;
+ if(bioloid_slave_int_count==0)
+ HAL_GPIO_WritePin(DYN_SLAVE_EXT_INT_GPIO_PORT,DYN_SLAVE_EXT_INT_PIN,GPIO_PIN_RESET);
+ bioloid_slave_int_count++;
}
void bioloid_dyn_slave_clear_int(void)
{
- HAL_GPIO_WritePin(DYN_SLAVE_EXT_INT_GPIO_PORT,DYN_SLAVE_EXT_INT_PIN,GPIO_PIN_SET);
- bioloid_slave_int_state=0;
+ bioloid_slave_int_count--;
+ if(bioloid_slave_int_count==0)
+ HAL_GPIO_WritePin(DYN_SLAVE_EXT_INT_GPIO_PORT,DYN_SLAVE_EXT_INT_PIN,GPIO_PIN_SET);
}
uint8_t bioloid_dyn_slave_is_int_set(void)
{
- return bioloid_slave_int_state;
+ if(bioloid_slave_int_count==0)
+ return 0x00;
+ else
+ return 0x01;
}
diff --git a/src/bioloid_stm32.c b/src/bioloid_stm32.c
index e5bcd5387cf7547a48eab87c9ec95bd5814df588..7b8d3f0a3e93d31a03ad2155a719893d79f8ed72 100644
--- a/src/bioloid_stm32.c
+++ b/src/bioloid_stm32.c
@@ -18,10 +18,8 @@ int32_t main(void)
bioloid_time_init();
/* initialize the dynamixel slave interface */
bioloid_dyn_slave_init();
+ bioloid_dyn_slave_start();
- while(1)
- {
- dyn_slave_loop(&bioloid_dyn_slave);
- }
+ while(1);
}
diff --git a/src/gpio.c b/src/gpio.c
index 3dd9b2587abb5eb3ceaf79582a2eaf7bb8561596..95dd2f500ccf88431b5da50437873346b7e6246f 100644
--- a/src/gpio.c
+++ b/src/gpio.c
@@ -1,5 +1,6 @@
#include "gpio.h"
#include "ram.h"
+#include "bioloid_dyn_slave.h"
#define ENABLE_LED_TX_GPIO_CLK __HAL_RCC_GPIOE_CLK_ENABLE()
#define LED_TX_PIN GPIO_PIN_10
@@ -79,6 +80,9 @@ void GPI_EXTI1_IRQHandler(void)
}
else
{
+ ram_data[BIOLOID_RESET_PB_CNTRL]|=GPIO_INT_FLAG;
+ if(ram_data[BIOLOID_RESET_PB_CNTRL]&GPIO_INT_EN)
+ bioloid_dyn_slave_set_int();
}
}
if(__HAL_GPIO_EXTI_GET_IT(USER_PB_PIN) != RESET)
@@ -91,6 +95,12 @@ void GPI_EXTI1_IRQHandler(void)
}
else
{
+ ram_data[BIOLOID_USER_PB_CNTRL]|=GPIO_INT_FLAG;
+ if(ram_data[BIOLOID_USER_PB_CNTRL]&GPIO_INT_EN)
+ {
+ bioloid_dyn_slave_set_int();
+ gpio_toggle_led(TXD_LED);
+ }
}
}
}
@@ -107,6 +117,9 @@ void GPI_EXTI2_IRQHandler(void)
}
else
{
+ ram_data[BIOLOID_START_PB_CNTRL]|=GPIO_INT_FLAG;
+ if(ram_data[BIOLOID_START_PB_CNTRL]&GPIO_INT_EN)
+ bioloid_dyn_slave_set_int();
}
}
}
@@ -123,6 +136,9 @@ void GPI_EXTI3_IRQHandler(void)
}
else
{
+ ram_data[BIOLOID_MODE_PB_CNTRL]|=GPIO_INT_FLAG;
+ if(ram_data[BIOLOID_MODE_PB_CNTRL]&GPIO_INT_EN)
+ bioloid_dyn_slave_set_int();
}
}
}
@@ -139,10 +155,10 @@ void GPO_TIMER_IRQHandler(void)
capture = HAL_TIM_ReadCapturedValue(&GPO_TIM_Handle, TIM_CHANNEL_1);
__HAL_TIM_SET_COMPARE(&GPO_TIM_Handle, TIM_CHANNEL_1, (capture + led_tx_period));
HAL_GPIO_TogglePin(LED_TX_GPIO_PORT,LED_TX_PIN);
- if(ram_data[BIOLOID_TXD_LED_CNTRL]&0x02)
- ram_data[BIOLOID_TXD_LED_CNTRL]&=0xFD;
+ if(ram_data[BIOLOID_TXD_LED_CNTRL]&GPIO_VALUE)
+ ram_data[BIOLOID_TXD_LED_CNTRL]&=(~GPIO_VALUE);
else
- ram_data[BIOLOID_TXD_LED_CNTRL]|=0x02;
+ ram_data[BIOLOID_TXD_LED_CNTRL]|=GPIO_VALUE;
}
}
if(__HAL_TIM_GET_FLAG(&GPO_TIM_Handle, TIM_FLAG_CC2) != RESET)
@@ -153,10 +169,10 @@ void GPO_TIMER_IRQHandler(void)
capture = HAL_TIM_ReadCapturedValue(&GPO_TIM_Handle, TIM_CHANNEL_2);
__HAL_TIM_SET_COMPARE(&GPO_TIM_Handle, TIM_CHANNEL_2, (capture + led_rx_period));
HAL_GPIO_TogglePin(LED_RX_GPIO_PORT,LED_RX_PIN);
- if(ram_data[BIOLOID_RXD_LED_CNTRL]&0x02)
- ram_data[BIOLOID_RXD_LED_CNTRL]&=0xFD;
+ if(ram_data[BIOLOID_RXD_LED_CNTRL]&GPIO_VALUE)
+ ram_data[BIOLOID_RXD_LED_CNTRL]&=(~GPIO_VALUE);
else
- ram_data[BIOLOID_RXD_LED_CNTRL]|=0x02;
+ ram_data[BIOLOID_RXD_LED_CNTRL]|=GPIO_VALUE;
}
}
if(__HAL_TIM_GET_FLAG(&GPO_TIM_Handle, TIM_FLAG_CC3) != RESET)
@@ -167,10 +183,10 @@ void GPO_TIMER_IRQHandler(void)
capture = HAL_TIM_ReadCapturedValue(&GPO_TIM_Handle, TIM_CHANNEL_3);
__HAL_TIM_SET_COMPARE(&GPO_TIM_Handle, TIM_CHANNEL_3, (capture + led_usr1_period));
HAL_GPIO_TogglePin(LED_USR1_GPIO_PORT,LED_USR1_PIN);
- if(ram_data[BIOLOID_USER1_LED_CNTRL]&0x02)
- ram_data[BIOLOID_USER1_LED_CNTRL]&=0xFD;
+ if(ram_data[BIOLOID_USER1_LED_CNTRL]&GPIO_VALUE)
+ ram_data[BIOLOID_USER1_LED_CNTRL]&=(~GPIO_VALUE);
else
- ram_data[BIOLOID_USER1_LED_CNTRL]|=0x02;
+ ram_data[BIOLOID_USER1_LED_CNTRL]|=GPIO_VALUE;
}
}
if(__HAL_TIM_GET_FLAG(&GPO_TIM_Handle, TIM_FLAG_CC4) != RESET)
@@ -181,10 +197,10 @@ void GPO_TIMER_IRQHandler(void)
capture = HAL_TIM_ReadCapturedValue(&GPO_TIM_Handle, TIM_CHANNEL_4);
__HAL_TIM_SET_COMPARE(&GPO_TIM_Handle, TIM_CHANNEL_4, (capture + led_usr2_period));
HAL_GPIO_TogglePin(LED_USR2_GPIO_PORT,LED_USR2_PIN);
- if(ram_data[BIOLOID_USER2_LED_CNTRL]&0x02)
- ram_data[BIOLOID_USER2_LED_CNTRL]&=0xFD;
+ if(ram_data[BIOLOID_USER2_LED_CNTRL]&GPIO_VALUE)
+ ram_data[BIOLOID_USER2_LED_CNTRL]&=(~GPIO_VALUE);
else
- ram_data[BIOLOID_USER2_LED_CNTRL]|=0x02;
+ ram_data[BIOLOID_USER2_LED_CNTRL]|=GPIO_VALUE;
}
}
/* TIM Update event */
@@ -278,13 +294,13 @@ void gpio_init(void)
__HAL_GPIO_EXTI_CLEAR_IT(RESET_PB_PIN);
if(__HAL_GPIO_EXTI_GET_IT(USER_PB_PIN) != RESET)
__HAL_GPIO_EXTI_CLEAR_IT(USER_PB_PIN);
- HAL_NVIC_SetPriority(GPI_EXTI1_IRQn, 3, 0);
+ HAL_NVIC_SetPriority(GPI_EXTI1_IRQn, 3, 3);
HAL_NVIC_EnableIRQ(GPI_EXTI1_IRQn);
- HAL_NVIC_SetPriority(GPI_EXTI2_IRQn, 3, 0);
+ HAL_NVIC_SetPriority(GPI_EXTI2_IRQn, 3, 3);
HAL_NVIC_EnableIRQ(GPI_EXTI2_IRQn);
- HAL_NVIC_SetPriority(GPI_EXTI3_IRQn, 3, 0);
+ HAL_NVIC_SetPriority(GPI_EXTI3_IRQn, 3, 3);
HAL_NVIC_EnableIRQ(GPI_EXTI3_IRQn);
/* LED's timer configuration */
@@ -298,7 +314,7 @@ void gpio_init(void)
GPO_TIM_Handle.Init.RepetitionCounter=0;
HAL_TIM_Base_Init(&GPO_TIM_Handle);
// initialize the timer interrupts
- HAL_NVIC_SetPriority(GPO_TIMER_IRQn, 3, 1);
+ HAL_NVIC_SetPriority(GPO_TIMER_IRQn, 3, 3);
HAL_NVIC_EnableIRQ(GPO_TIMER_IRQn);
/* use the internal clock */
sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
@@ -509,14 +525,46 @@ void gpio_set_pushbutton_callback(pushbutton_t pb_id,void (*callback)(void))
}
}
-void gpio_process_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
+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))
+ if(!(ram_data[BIOLOID_RESET_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
+ {
+ ram_data[BIOLOID_RESET_PB_CNTRL]&=(~GPIO_INT_FLAG);
+ if(ram_data[BIOLOID_RESET_PB_CNTRL]&GPIO_INT_EN)
+ bioloid_dyn_slave_clear_int();
+ }
+ if(ram_in_range(BIOLOID_USER_PB_CNTRL,address,length))
+ if(!(ram_data[BIOLOID_USER_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
+ {
+ ram_data[BIOLOID_USER_PB_CNTRL]&=(~GPIO_INT_FLAG);
+ if(ram_data[BIOLOID_USER_PB_CNTRL]&GPIO_INT_EN)
+ bioloid_dyn_slave_clear_int();
+ }
+ if(ram_in_range(BIOLOID_START_PB_CNTRL,address,length))
+ if(!(ram_data[BIOLOID_START_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
+ {
+ ram_data[BIOLOID_START_PB_CNTRL]&=(~GPIO_INT_FLAG);
+ if(ram_data[BIOLOID_START_PB_CNTRL]&GPIO_INT_EN)
+ bioloid_dyn_slave_clear_int();
+ }
+ if(ram_in_range(BIOLOID_MODE_PB_CNTRL,address,length))
+ if(!(ram_data[BIOLOID_MODE_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
+ {
+ ram_data[BIOLOID_MODE_PB_CNTRL]&=(~GPIO_INT_FLAG);
+ if(ram_data[BIOLOID_MODE_PB_CNTRL]&GPIO_INT_EN)
+ bioloid_dyn_slave_clear_int();
+ }
+}
+
+void gpio_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
{
uint16_t period;
/* LED's */
if(ram_in_range(BIOLOID_USER1_LED_CNTRL,address,length))
{
- if(!ram_data[BIOLOID_USER1_LED_CNTRL]&GPIO_INT_USED)/* GPIO is not internally used */
+ if(!(ram_data[BIOLOID_USER1_LED_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
{
period=led_usr1_period;
if(ram_in_range(BIOLOID_USER1_LED_PERIOD_L,address,length))
@@ -531,22 +579,22 @@ void gpio_process_cmd(unsigned short int address,unsigned short int length,unsig
if(data[BIOLOID_USER1_LED_CNTRL-address]&GPIO_TOGGLE)
{
gpio_toggle_led(USER1_LED);
- if(ram_data[BIOLOID_USER1_LED_CNTRL]&0x02)
- ram_data[BIOLOID_USER1_LED_CNTRL]&=0xFD;
+ if(ram_data[BIOLOID_USER1_LED_CNTRL]&GPIO_VALUE)
+ ram_data[BIOLOID_USER1_LED_CNTRL]&=(~GPIO_VALUE);
else
- ram_data[BIOLOID_USER1_LED_CNTRL]|=0x02;
+ ram_data[BIOLOID_USER1_LED_CNTRL]|=GPIO_VALUE;
}
else
{
if(data[BIOLOID_USER1_LED_CNTRL-address]&GPIO_VALUE)
{
gpio_set_led(USER1_LED);
- ram_data[BIOLOID_USER1_LED_CNTRL]|=0x02;
+ ram_data[BIOLOID_USER1_LED_CNTRL]|=GPIO_VALUE;
}
else
{
gpio_clear_led(USER1_LED);
- ram_data[BIOLOID_USER1_LED_CNTRL]&=0xFD;
+ ram_data[BIOLOID_USER1_LED_CNTRL]&=(~GPIO_VALUE);
}
}
}
@@ -554,7 +602,7 @@ void gpio_process_cmd(unsigned short int address,unsigned short int length,unsig
}
if(ram_in_range(BIOLOID_USER2_LED_CNTRL,address,length))
{
- if(!ram_data[BIOLOID_USER2_LED_CNTRL]&GPIO_INT_USED)/* GPIO is not internally used */
+ if(!(ram_data[BIOLOID_USER2_LED_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
{
period=led_usr2_period;
if(ram_in_range(BIOLOID_USER2_LED_PERIOD_L,address,length))
@@ -569,22 +617,22 @@ void gpio_process_cmd(unsigned short int address,unsigned short int length,unsig
if(data[BIOLOID_USER2_LED_CNTRL-address]&GPIO_TOGGLE)
{
gpio_toggle_led(USER2_LED);
- if(ram_data[BIOLOID_USER2_LED_CNTRL]&0x02)
- ram_data[BIOLOID_USER2_LED_CNTRL]&=0xFD;
+ if(ram_data[BIOLOID_USER2_LED_CNTRL]&GPIO_VALUE)
+ ram_data[BIOLOID_USER2_LED_CNTRL]&=(~GPIO_VALUE);
else
- ram_data[BIOLOID_USER2_LED_CNTRL]|=0x02;
+ ram_data[BIOLOID_USER2_LED_CNTRL]|=GPIO_VALUE;
}
else
{
if(data[BIOLOID_USER2_LED_CNTRL-address]&GPIO_VALUE)
{
gpio_set_led(USER2_LED);
- ram_data[BIOLOID_USER2_LED_CNTRL]|=0x02;
+ ram_data[BIOLOID_USER2_LED_CNTRL]|=GPIO_VALUE;
}
else
{
gpio_clear_led(USER2_LED);
- ram_data[BIOLOID_USER2_LED_CNTRL]&=0xFD;
+ ram_data[BIOLOID_USER2_LED_CNTRL]&=(~GPIO_VALUE);
}
}
}
@@ -592,9 +640,9 @@ void gpio_process_cmd(unsigned short int address,unsigned short int length,unsig
}
if(ram_in_range(BIOLOID_RXD_LED_CNTRL,address,length))
{
- if(!ram_data[BIOLOID_RXD_LED_CNTRL]&GPIO_INT_USED)/* GPIO is not internally used */
+ if(!(ram_data[BIOLOID_RXD_LED_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
{
- period=led_usr1_period;
+ period=led_rx_period;
if(ram_in_range(BIOLOID_RXD_LED_PERIOD_L,address,length))
period=(period&0xFF00)+data[BIOLOID_RXD_LED_PERIOD_L-address];
if(ram_in_range(BIOLOID_RXD_LED_PERIOD_H,address,length))
@@ -607,22 +655,22 @@ void gpio_process_cmd(unsigned short int address,unsigned short int length,unsig
if(data[BIOLOID_RXD_LED_CNTRL-address]&GPIO_TOGGLE)
{
gpio_toggle_led(RXD_LED);
- if(ram_data[BIOLOID_RXD_LED_CNTRL]&0x02)
- ram_data[BIOLOID_RXD_LED_CNTRL]&=0xFD;
+ if(ram_data[BIOLOID_RXD_LED_CNTRL]&GPIO_VALUE)
+ ram_data[BIOLOID_RXD_LED_CNTRL]&=(~GPIO_VALUE);
else
- ram_data[BIOLOID_RXD_LED_CNTRL]|=0x02;
+ ram_data[BIOLOID_RXD_LED_CNTRL]|=GPIO_VALUE;
}
else
{
if(data[BIOLOID_RXD_LED_CNTRL-address]&GPIO_VALUE)
{
gpio_set_led(RXD_LED);
- ram_data[BIOLOID_RXD_LED_CNTRL]|=0x02;
+ ram_data[BIOLOID_RXD_LED_CNTRL]|=GPIO_VALUE;
}
else
{
gpio_clear_led(RXD_LED);
- ram_data[BIOLOID_RXD_LED_CNTRL]&=0xFD;
+ ram_data[BIOLOID_RXD_LED_CNTRL]&=(~GPIO_VALUE);
}
}
}
@@ -630,9 +678,9 @@ void gpio_process_cmd(unsigned short int address,unsigned short int length,unsig
}
if(ram_in_range(BIOLOID_TXD_LED_CNTRL,address,length))
{
- if(!ram_data[BIOLOID_TXD_LED_CNTRL]&GPIO_INT_USED)/* GPIO is not internally used */
+ if(!(ram_data[BIOLOID_TXD_LED_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
{
- period=led_usr1_period;
+ period=led_tx_period;
if(ram_in_range(BIOLOID_TXD_LED_PERIOD_L,address,length))
period=(period&0xFF00)+data[BIOLOID_TXD_LED_PERIOD_L-address];
if(ram_in_range(BIOLOID_TXD_LED_PERIOD_H,address,length))
@@ -645,22 +693,22 @@ void gpio_process_cmd(unsigned short int address,unsigned short int length,unsig
if(data[BIOLOID_TXD_LED_CNTRL-address]&GPIO_TOGGLE)
{
gpio_toggle_led(TXD_LED);
- if(ram_data[BIOLOID_TXD_LED_CNTRL]&0x02)
- ram_data[BIOLOID_TXD_LED_CNTRL]&=0xFD;
+ if(ram_data[BIOLOID_TXD_LED_CNTRL]&GPIO_VALUE)
+ ram_data[BIOLOID_TXD_LED_CNTRL]&=(~GPIO_VALUE);
else
- ram_data[BIOLOID_TXD_LED_CNTRL]|=0x02;
+ ram_data[BIOLOID_TXD_LED_CNTRL]|=GPIO_VALUE;
}
else
{
if(data[BIOLOID_TXD_LED_CNTRL-address]&GPIO_VALUE)
{
gpio_set_led(TXD_LED);
- ram_data[BIOLOID_TXD_LED_CNTRL]|=0x02;
+ ram_data[BIOLOID_TXD_LED_CNTRL]|=GPIO_VALUE;
}
else
{
gpio_clear_led(TXD_LED);
- ram_data[BIOLOID_TXD_LED_CNTRL]&=0xFD;
+ ram_data[BIOLOID_TXD_LED_CNTRL]&=(~GPIO_VALUE);
}
}
}
@@ -668,12 +716,18 @@ void gpio_process_cmd(unsigned short int address,unsigned short int length,unsig
}
/* pushbuttons */
if(ram_in_range(BIOLOID_RESET_PB_CNTRL,address,length))
- {
- if(!ram_data[BIOLOID_RESET_PB_CNTRL]&GPIO_INT_USED)/* GPIO is not internally used */
+ if(!(ram_data[BIOLOID_RESET_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
+ ram_data[BIOLOID_RESET_PB_CNTRL]=data[BIOLOID_RESET_PB_CNTRL-address];
+ if(ram_in_range(BIOLOID_USER_PB_CNTRL,address,length))
+ if(!(ram_data[BIOLOID_USER_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
{
- if(data[BIOLOID_RESET_PB_CNTRL-address]&GPIO_INT_EN)
- {
- }
+ gpio_toggle_led(RXD_LED);
+ ram_data[BIOLOID_USER_PB_CNTRL]=data[BIOLOID_USER_PB_CNTRL-address];
}
- }
+ if(ram_in_range(BIOLOID_START_PB_CNTRL,address,length))
+ if(!(ram_data[BIOLOID_START_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
+ ram_data[BIOLOID_START_PB_CNTRL]=data[BIOLOID_START_PB_CNTRL-address];
+ if(ram_in_range(BIOLOID_MODE_PB_CNTRL,address,length))
+ if(!(ram_data[BIOLOID_MODE_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
+ ram_data[BIOLOID_MODE_PB_CNTRL]=data[BIOLOID_MODE_PB_CNTRL-address];
}
diff --git a/src/ram.c b/src/ram.c
index 5d4eb01c505425ac43254b0cd2e736212e34c7de..cce0279314fb3f2a000ae4c4a025556e4433469f 100644
--- a/src/ram.c
+++ b/src/ram.c
@@ -115,10 +115,13 @@ inline uint8_t ram_in_range(uint8_t reg,uint8_t address,uint8_t length)
return 0x00;
}
-inline uint8_t ram_in_window(uint8_t start_reg,uint8_t reg_length,uint8_t start_address,uint8_t address_length)
+uint8_t ram_in_window(uint8_t start_reg,uint8_t reg_length,uint8_t start_address,uint8_t address_length)
{
- if((start_reg>=start_address && start_reg<(start_address+address_length)) ||
- ((start_reg+reg_length)>=start_address && (start_reg+reg_length)<(start_address+address_length)))
+ uint8_t end_reg=start_reg+reg_length;
+ uint8_t end_address=start_address+address_length;
+
+ if((start_reg>=start_address && start_reg<end_address) || (end_reg>=start_address && end_reg<end_address) ||
+ (start_address>=start_reg && start_address<end_reg) || (end_address>=start_reg && end_address<end_reg))
return 0x01;
else
return 0x00;