Completely integrated the GPIO into the dynamixel interface.

Solved a problem with the RAM window detection.

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);

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
 }

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
 }

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
 }

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;
 }

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);
 }
 

src/gpio.c

 #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];
 }

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;