Skip to content
GitLab
Commit ec3d0c2f authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Completely integrated the GPIO into the dynamixel interface. 

Solved a problem with the RAM window detection.
parent e14cc94d
Hide whitespace changes
Inline Side-by-side
include/bioloid_dyn_slave.h
View file @ ec3d0c2f
......@@ -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
View file @ ec3d0c2f
......@@ -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
View file @ ec3d0c2f
......@@ -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
View file @ ec3d0c2f
......@@ -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
View file @ ec3d0c2f
......@@ -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
View file @ ec3d0c2f
......@@ -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
View file @ ec3d0c2f
#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
View file @ ec3d0c2f
......@@ -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;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment