diff --git a/Makefile b/Makefile
index 95f99f9040ee1fbc2e22ce89b6887282489a3d50..c9e730f11fa19b4e8c221b798d516d89350aaf09 100755
--- a/Makefile
+++ b/Makefile
@@ -4,7 +4,15 @@ STM32_HAL_PATH=$(HOME)/humanoids/stm32_hal
STM32_LIBRARIES_PATH=$(HOME)/humanoids/stm32_libraries
PROJECT_NAME=bioloid_firmware
-TARGET_FILES=$(wildcard src/*.c)
+#TARGET_FILES=$(wildcard src/*.c)
+TARGET_FILES=src/eeprom.c
+TARGET_FILES+=src/adc_dma.c
+TARGET_FILES+=src/system_stm32f4xx.c
+TARGET_FILES+=src/bioloid_stm32.c
+TARGET_FILES+=src/bioloid_time.c
+TARGET_FILES+=src/bioloid_dyn_slave.c
+TARGET_FILES+=src/gpio.c
+TARGET_FILES+=src/bioloid_dyn_manager.c
TARGET_PROCESSOR=STM32F407VG
HAL_PATH=$(STM32_HAL_PATH)/f4
@@ -15,17 +23,19 @@ STM32_STARTUP_FILES_PATH = $(HAL_PATH)/startup_code/
STM32_LINKER_SCRIPTS_PATH = ./linker_script
UTILS_PATH=$(STM32_LIBRARIES_PATH)/utils
COMM_PATH=$(STM32_LIBRARIES_PATH)/comm
+MEMORY_PATH=$(STM32_LIBRARIES_PATH)/memory
USART_PATH=$(STM32_LIBRARIES_PATH)/f4/usart
DYNAMIXEL_PATH=$(STM32_LIBRARIES_PATH)/dynamixel_base
+DYNAMIXEL_MANAGER_PATH=$(STM32_LIBRARIES_PATH)/dynamixel_manager
BUILD_PATH=build
COMPILE_OPTS = -mlittle-endian -mcpu=cortex-m4 -mthumb -mthumb-interwork
COMPILE_OPTS += -Wall -O2 -fno-common -DUSE_HAL_DRIVER
COMPILE_OPTS += -mfloat-abi=hard -mfpu=fpv4-sp-d16
-COMPILE_OPTS += -ffreestanding -nostdlib -D$(PROCESSOR_MACRO) -DARM_MATH_CM4 -D__FPU_PRESENT -DHSE_VALUE=8000000
+COMPILE_OPTS += -ffreestanding -nostdlib -D$(PROCESSOR_MACRO) -DARM_MATH_CM4 -D__FPU_PRESENT -DHSE_VALUE=8000000 -imacros ./include/bioloid_conf.h
INCLUDE_DIRS = -I$(HAL_PATH)/include -I$(HAL_PATH)/include/core -I$(HAL_PATH)/include/devices
-INCLUDE_DIRS += -I$(UTILS_PATH)/include -I$(COMM_PATH)/include -I$(DYNAMIXEL_PATH)/include -I$(USART_PATH)/include -I./include
-LIBRARY_DIRS = -L$(STM32_LIB_PATH)/lib -L$(STM32_DSP_LIB_PATH)/lib -L$(DYNAMIXEL_LIB_PATH)/lib -L/usr/arm-none-eabi/lib/fpu/
+INCLUDE_DIRS += -I$(UTILS_PATH)/include -I$(COMM_PATH)/include -I$(MEMORY_PATH)/include -I$(DYNAMIXEL_PATH)/include -I$(DYNAMIXEL_MANAGER_PATH)/include -I$(USART_PATH)/include -I./include
+LIBRARY_DIRS = -L$(STM32_LIB_PATH)/lib -L$(STM32_DSP_LIB_PATH)/lib -L/usr/arm-none-eabi/lib/fpu/
TCHAIN_PREFIX=arm-none-eabi-
@@ -37,7 +47,6 @@ ASFLAGS = $(COMPILE_OPTS) -c
LD = $(TCHAIN_PREFIX)gcc
LDFLAGS = -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16 -Wl,-Map=$@.map,-cref $(INCLUDE_DIRS) -T $(STM32_LINKER_SCRIPTS_PATH)/bioloid.ld --specs=nosys.specs
-EXT_LIB = $(COMM_PATH)/lib/comm_m4_fpu.a $(UTILS_PATH)/lib/utils_m4_fpu.a $(DYNAMIXEL_PATH)/lib/dynamixel_m4_fpu.a
OBJCP = $(TCHAIN_PREFIX)objcopy
OBJCPFLAGS_HEX = -O ihex
@@ -70,6 +79,12 @@ TARGET_FILES+=$(USART_PATH)/src/usart3.c
TARGET_FILES+=$(USART_PATH)/src/usart2.c
TARGET_FILES+=$(USART_PATH)/src/usart1.c
+TARGET_FILES+=$(wildcard $(UTILS_PATH)/src/*.c)
+TARGET_FILES+=$(wildcard $(COMM_PATH)/src/*.c)
+TARGET_FILES+=$(wildcard $(MEMORY_PATH)/src/*.c)
+TARGET_FILES+=$(wildcard $(DYNAMIXEL_PATH)/src/*.c)
+TARGET_FILES+=$(wildcard $(DYNAMIXEL_MANAGER_PATH)/src/*.c)
+
BIOLOID_OBJS_TMP = $(notdir $(TARGET_FILES:.c=.o))
BIOLOID_OBJS = $(patsubst %,$(BUILD_PATH)/%,$(BIOLOID_OBJS_TMP))
@@ -86,9 +101,19 @@ $(BUILD_PATH)/%.o: $(HAL_PATH)/src/%.c
$(CC) -c $(CFLAGS) -o $@ $<
$(BUILD_PATH)/%.o: $(USART_PATH)/src/%.c
$(CC) -c $(CFLAGS) -o $@ $<
+$(BUILD_PATH)/%.o: $(UTILS_PATH)/src/%.c
+ $(CC) -c $(CFLAGS) -o $@ $<
+$(BUILD_PATH)/%.o: $(COMM_PATH)/src/%.c
+ $(CC) -c $(CFLAGS) -o $@ $<
+$(BUILD_PATH)/%.o: $(MEMORY_PATH)/src/%.c
+ $(CC) -c $(CFLAGS) -o $@ $<
+$(BUILD_PATH)/%.o: $(DYNAMIXEL_PATH)/src/%.c
+ $(CC) -c $(CFLAGS) -o $@ $<
+$(BUILD_PATH)/%.o: $(DYNAMIXEL_MANAGER_PATH)/src/%.c
+ $(CC) -c $(CFLAGS) -o $@ $<
$(MAIN_OUT_ELF): mkdir_build $(BIOLOID_OBJS) $(BUID_PATH)/$(STARTUP_FILE:.s=.o)
- $(LD) $(LDFLAGS) $(BIOLOID_OBJS) $(BUILD_PATH)/$(STARTUP_FILE:.s=.o) $(EXT_LIB) -lm --output $@
+ $(LD) $(LDFLAGS) $(BIOLOID_OBJS) $(BUILD_PATH)/$(STARTUP_FILE:.s=.o) -lm --output $@
$(MAIN_OUT_HEX): $(MAIN_OUT_ELF)
$(OBJCP) $(OBJCPFLAGS_HEX) $< $@
diff --git a/include/adc_dma.h b/include/adc_dma.h
index d360ab07c9f44fb271a20ce56d27876e82714ec5..1c552ac7867cf7513a2a25e8ab5b2ee168618106 100644
--- a/include/adc_dma.h
+++ b/include/adc_dma.h
@@ -6,11 +6,13 @@ extern "C" {
#endif
#include "stm32f4xx.h"
+#include "adc_dma_registers.h"
+#include "memory.h"
typedef enum {ADC_CH1,ADC_CH2,ADC_CH3,ADC_CH4,ADC_CH6,ADC_CH8} adc_ch_t;
// public functions
-void adc_init(void);
+uint8_t adc_init(TMemory *memory);
void adc_start(void);
void adc_set_period(uint8_t period_ms);
inline uint8_t adc_get_period(void);
@@ -18,9 +20,6 @@ unsigned short adc_get_channel(adc_ch_t channel);
unsigned short adc_get_channel_raw(adc_ch_t channel);
unsigned short adc_get_temperature(void);
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/adc_dma_registers.h b/include/adc_dma_registers.h
new file mode 100644
index 0000000000000000000000000000000000000000..58001e683ae250a940ff986c355cc4b443596f07
--- /dev/null
+++ b/include/adc_dma_registers.h
@@ -0,0 +1,36 @@
+#ifndef _ADC_DMA_REGISTERS_H
+#define _ADC_DMA_REGISTERS_H
+
+#ifndef RAM_ADC_DMA_BASE_ADDRESS
+ #define RAM_ADC_DMA_BASE_ADDRESS ((unsigned short int)0x0000)
+#endif
+
+#ifndef EEPROM_ADC_DMA_BASE_ADDRESS
+ #define EEPROM_ADC_DMA_BASE_ADDRESS ((unsigned short int)0x0000)
+#endif
+
+#define RAM_ADC_DMA_LENGTH 17
+
+#define EEPROM_ADC_DMA_LENGTH 1
+
+#define ADC_CNTRL RAM_ADC_DMA_BASE_ADDRESS// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | | | | Enable
+#define ADC_CH1_VOLTAGE (RAM_ADC_DMA_BASE_ADDRESS+1)
+#define ADC_CH2_VOLTAGE (RAM_ADC_DMA_BASE_ADDRESS+3)
+#define ADC_CH3_VOLTAGE (RAM_ADC_DMA_BASE_ADDRESS+5)
+#define ADC_CH4_VOLTAGE (RAM_ADC_DMA_BASE_ADDRESS+7)
+#define ADC_TEMPERATURE (RAM_ADC_DMA_BASE_ADDRESS+9)
+#define ADC_CH6_VOLTAGE (RAM_ADC_DMA_BASE_ADDRESS+11)
+#define ADC_VREF_VOLTAGE (RAM_ADC_DMA_BASE_ADDRESS+13)
+#define ADC_CH8_VOLTAGE (RAM_ADC_DMA_BASE_ADDRESS+15)
+
+#define ADC_ENABLE 0x01
+
+#define ADC_PERIOD EEPROM_ADC_DMA_BASE_ADDRESS
+
+#ifndef DEFAULT_ADC_PERIOD
+ #define DEFAULT_ADC_PERIOD 0x0A
+#endif
+
+#endif
+
diff --git a/include/bioloid_conf.h b/include/bioloid_conf.h
new file mode 100644
index 0000000000000000000000000000000000000000..afeb40183cc054d4637eba471ecc2209e84b1155
--- /dev/null
+++ b/include/bioloid_conf.h
@@ -0,0 +1,61 @@
+#ifndef _BIOLOID_CONF_H
+#define _BIOLOID_CONF_H
+
+#define RAM_SIZE 1024
+#define EEPROM_SIZE 256
+
+/* ADC configuration */
+#define RAM_ADC_DMA_BASE_ADDRESS ((unsigned short int)0x0100)
+#define EEPROM_ADC_DMA_BASE_ADDRESS ((unsigned short int)0x0006)
+
+#define DEFAULT_ADC_PERIOD 0x0A
+
+/* Dynamixel slave configuration */
+#define EEPROM_DYN_SLAVE_BASE_ADDRESS1 ((unsigned short int)0x0000)
+#define EEPROM_DYN_SLAVE_BASE_ADDRESS2 ((unsigned short int)0x0010)
+
+#define DEFAULT_DEVICE_MODEL 0x7300
+#define DEFAULT_FIRMWARE_VERSION 0x0001
+#define DEFAULT_DEVICE_ID 0x0001
+#define DEFAULT_BAUDRATE 0x0010
+#define DEFAULT_RETURN_DELAY 0x0000
+#define DEFAULT_RETURN_LEVEL 0x0002
+
+#define MAX_DYN_SLAVE_TX_BUFFER_LEN 1024
+#define MAX_DYN_SLAVE_RX_BUFFER_LEN 1024
+#define MAX_DYN_SLAVE_REG_BUFFER_LEN 1024
+
+/* Servos Dynamixel master configuration */
+#define MAX_SERVOS_DYN_MASTER_TX_BUFFER_LEN 1024
+#define MAX_SERVOS_DYN_MASTER_RX_BUFFER_LEN 1024
+
+#define MAX_SENSORS_DYN_MASTER_TX_BUFFER_LEN 1024
+#define MAX_SENSORS_DYN_MASTER_RX_BUFFER_LEN 1024
+
+/* Dynamixel manager configuration */
+#define RAM_DYN_MANAGER_BASE_ADDRESS ((unsigned short int)0x0121)
+#define EEPROM_DYN_MANAGER_BASE_ADDRESS ((unsigned short int)0x0007)
+
+#define DEFAULT_DYN_MANAGER_PERIOD 0x1E78
+
+#define DYN_MANAGER_MAX_NUM_MASTERS 4
+#define DYN_MANAGER_MAX_NUM_MODULES 8
+#define DYN_MANAGER_MAX_NUM_DEVICES 32
+#define DYN_MANAGER_MAX_NUM_SINGLE_OP 16
+#define DYN_MANAGER_MAX_NUM_SYNC_OP 4
+#define DYN_MANAGER_MAX_NUM_BULK_OP 4
+
+/* Motion Manager configuration */
+#define RAM_MM_BASE_ADDRESS ((unsigned short int)0x0000)
+#define EEPROM_MM_BASE_ADDRESS ((unsigned short int)0x0000)
+
+#define DEFAULT_MM_PERIOD DEFAULT_DYN_MODULE_PERIOD
+
+#define MM_MAX_NUM_MODELS 32
+#define MM_MAX_NUM_MOTION_MODULES 8
+
+/* GPIO configuration */
+#define RAM_GPIO_BASE_ADDRESS ((unsigned short int)0x0111)
+
+
+#endif
diff --git a/include/bioloid_dyn_manager.h b/include/bioloid_dyn_manager.h
new file mode 100644
index 0000000000000000000000000000000000000000..6b36ea3986968611db8c042e976054542ff2fbca
--- /dev/null
+++ b/include/bioloid_dyn_manager.h
@@ -0,0 +1,18 @@
+#ifndef _BIOLOID_DYN_MANAGER_H
+#define _BIOLOID_DYN_MANAGER_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f4xx.h"
+#include "memory.h"
+
+uint8_t bioloid_dyn_manager_init(TMemory *memory);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
diff --git a/include/bioloid_dyn_master_sensors.h b/include/bioloid_dyn_master_sensors.h
index 98554dc5d42abe194303cddb7e9b1fb5347257ab..e02812c3be7b8b861eb79d66b337b4be1235f6c4 100644
--- a/include/bioloid_dyn_master_sensors.h
+++ b/include/bioloid_dyn_master_sensors.h
@@ -6,10 +6,12 @@ extern "C" {
#endif
#include "stm32f4xx.h"
-#include "stm32_time.h"
#include "dynamixel_master.h"
#include "comm.h"
+#define MAX_DYN_MASTER_TX_BUFFER_LEN MAX_SENSORS_DYN_MASTER_TX_BUFFER_LEN
+#define MAX_DYN_MASTER_RX_BUFFER_LEN MAX_SENSORS_DYN_MASTER_RX_BUFFER_LEN
+
void bioloid_dyn_master_sensors_init(void);
#ifdef __cplusplus
diff --git a/include/bioloid_dyn_master_servos.h b/include/bioloid_dyn_master_servos.h
index 3152d5673ce5a3eb8b9d2de52aebf0bfa4d961d1..e0f3e41d3ef7960f9a2d941552abe214fb291334 100644
--- a/include/bioloid_dyn_master_servos.h
+++ b/include/bioloid_dyn_master_servos.h
@@ -9,6 +9,9 @@ extern "C" {
#include "dynamixel_master.h"
#include "comm.h"
+#define MAX_DYN_MASTER_TX_BUFFER_LEN MAX_SERVOS_DYN_MASTER_TX_BUFFER_LEN
+#define MAX_DYN_MASTER_RX_BUFFER_LEN MAX_SERVOS_DYN_MASTER_RX_BUFFER_LEN
+
extern TDynamixelMaster bioloid_dyn_master_servos;
void bioloid_dyn_master_servos_init(void);
diff --git a/include/bioloid_dyn_slave.h b/include/bioloid_dyn_slave.h
index 5f58c04da075d3860462eba7c31e31893d2d5263..d90ad919391a9f1561e968a50f3f857713dcb7a8 100644
--- a/include/bioloid_dyn_slave.h
+++ b/include/bioloid_dyn_slave.h
@@ -7,8 +7,9 @@ extern "C" {
#include "stm32f4xx.h"
#include "dynamixel_slave.h"
+#include "memory.h"
-void bioloid_dyn_slave_init(void);
+uint8_t bioloid_dyn_slave_init(TMemory *memory);
void bioloid_dyn_slave_start(void);
void bioloid_dyn_slave_stop(void);
void bioloid_dyn_slave_set_int(void);
diff --git a/include/bioloid_registers.h b/include/bioloid_registers.h
index 1acbad0df4c914c71d4852e0bc5667df0307f784..306e371007705a25f02a4eef77f419b10304b231 100644
--- a/include/bioloid_registers.h
+++ b/include/bioloid_registers.h
@@ -5,17 +5,10 @@
extern "C" {
#endif
-#define DEVICE_MODEL_OFFSET ((unsigned short int)0x0000)
-#define FIRMWARE_VERSION_OFFSET ((unsigned short int)0x0002)
-#define DEVICE_ID_OFFSET ((unsigned short int)0x0003)
-#define BAUDRATE_OFFSET ((unsigned short int)0x0004)
-#define RETURN_DELAY_OFFSET ((unsigned short int)0x0005)
-#define MM_PERIOD_OFFSET ((unsigned short int)0x0006)
#define MM_BAL_KNEE_GAIN_OFFSET ((unsigned short int)0x0008)
#define MM_BAL_ANKLE_ROLL_GAIN_OFFSET ((unsigned short int)0x000A)
#define MM_BAL_ANKLE_PITCH_GAIN_OFFSET ((unsigned short int)0x000C)
#define MM_BAL_HIP_ROLL_GAIN_OFFSET ((unsigned short int)0x000E)
-#define RETURN_LEVEL_OFFSET ((unsigned short int)0x0010)
#define MM_SERVO0_OFFSET ((unsigned short int)0x0011)
#define MM_SERVO1_OFFSET ((unsigned short int)0x0012)
#define MM_SERVO2_OFFSET ((unsigned short int)0x0013)
@@ -71,14 +64,6 @@ extern "C" {
#define LAST_EEPROM_OFFSET ((unsigned short int)0x00FF)
typedef enum {
- BIOLOID_MODEL_NUMBER_L = DEVICE_MODEL_OFFSET,
- BIOLOID_MODEL_NUMBER_H = DEVICE_MODEL_OFFSET+1,
- BIOLOID_VERSION = FIRMWARE_VERSION_OFFSET,
- 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_MM_BAL_KNEE_GAIN_L = MM_BAL_KNEE_GAIN_OFFSET,// fixed point format 0|16
BIOLOID_MM_BAL_KNEE_GAIN_H = MM_BAL_KNEE_GAIN_OFFSET+1,
BIOLOID_MM_BAL_ANKLE_ROLL_GAIN_L = MM_BAL_ANKLE_ROLL_GAIN_OFFSET,// fixed point format 0|16
@@ -87,7 +72,6 @@ typedef enum {
BIOLOID_MM_BAL_ANKLE_PITCH_GAIN_H = MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1,
BIOLOID_MM_BAL_HIP_ROLL_GAIN_L = MM_BAL_HIP_ROLL_GAIN_OFFSET,// fixed point format 0|16
BIOLOID_MM_BAL_HIP_ROLL_GAIN_H = MM_BAL_HIP_ROLL_GAIN_OFFSET+1,
- BIOLOID_RETURN_LEVEL = RETURN_LEVEL_OFFSET,
BIOLOID_MM_SERVO0_OFFSET = MM_SERVO0_OFFSET, // angle offset in fixed point format 1-3|4
BIOLOID_MM_SERVO1_OFFSET = MM_SERVO1_OFFSET, // angle offset in fixed point format 1-3|4
BIOLOID_MM_SERVO2_OFFSET = MM_SERVO2_OFFSET, // angle offset in fixed point format 1-3|4
@@ -141,49 +125,6 @@ typedef enum {
BIOLOID_WALK_ARM_SWING_GAIN = WALK_ARM_SWING_GAIN,
BIOLOID_WALK_MAX_VEL = WALK_MAX_VEL,
BIOLOID_WALK_MAX_ROT_VEL = WALK_MAX_ROT_VEL,
- BIOLOID_USER1_LED_CNTRL = 0x0100, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | | | | blink | toggle | value | internally used
- BIOLOID_USER1_LED_PERIOD_L = 0x0101,
- BIOLOID_USER1_LED_PERIOD_H = 0x0102,
- BIOLOID_USER2_LED_CNTRL = 0x0103, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | | | | blink | toggle | value | internally used
- BIOLOID_USER2_LED_PERIOD_L = 0x0104,
- BIOLOID_USER2_LED_PERIOD_H = 0x0105,
- BIOLOID_RXD_LED_CNTRL = 0x0106, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | | | | blink | toggle | value | internally used
- BIOLOID_RXD_LED_PERIOD_L = 0x0107,
- BIOLOID_RXD_LED_PERIOD_H = 0x0108,
- BIOLOID_TXD_LED_CNTRL = 0x0109, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | | | | blink | toggle | value | internally used
- BIOLOID_TXD_LED_PERIOD_L = 0x010A,
- BIOLOID_TXD_LED_PERIOD_H = 0x010B,
- BIOLOID_USER_PB_CNTRL = 0x010C, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | | | | interrupt flag | enable interrupt | value | internally used
- BIOLOID_RESET_PB_CNTRL = 0x010D, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | | | | interrupt flag | enable interrupt | value | internally used
- BIOLOID_START_PB_CNTRL = 0x010E, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | | | | interrupt flag | enable interrupt | value | internally used
- BIOLOID_MODE_PB_CNTRL = 0x010F, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | | | | interrupt flag | enable interrupt | value | internally used
- BIOLOID_ADC_CNTRL = 0x0110, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | | | | | | | Enable
- BIOLOID_ADC_PERIOD = 0x0111,
- BIOLOID_ADC_CH1_L = 0x0112,
- BIOLOID_ADC_CH1_H = 0x0113,
- BIOLOID_ADC_CH2_L = 0x0114,
- BIOLOID_ADC_CH2_H = 0x0115,
- BIOLOID_ADC_CH3_L = 0x0116,
- BIOLOID_ADC_CH3_H = 0x0117,
- BIOLOID_ADC_CH4_L = 0x0118,
- BIOLOID_ADC_CH4_H = 0x0119,
- BIOLOID_ADC_TEMP_L = 0x011A,
- BIOLOID_ADC_TEMP_H = 0x011B,
- BIOLOID_ADC_CH6_L = 0x011C,
- BIOLOID_ADC_CH6_H = 0x011D,
- BIOLOID_ADC_VREF_L = 0x011E,
- BIOLOID_ADC_VREF_H = 0x011F,
- BIOLOID_ADC_CH8_L = 0x0120,
- BIOLOID_ADC_CH8_H = 0x0121,
BIOLOID_ZIGBEE_CNTRL = 0x0122, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// | | | | | | Enable | Enable power
BIOLOID_ZIGBEE_BAUDRATE = 0x0123,
@@ -312,19 +253,6 @@ typedef enum {
BIOLOID_WALK_STEP_DIRECTION = 0x0185
} bioloid_registers;
-#define GPIO_BASE_ADDRESS 0x0100
-#define GPIO_MEM_LENGTH 16
-#define GPIO_INT_USED 0x01
-#define GPIO_VALUE 0x02
-#define GPIO_TOGGLE 0x04
-#define GPIO_BLINK 0x08
-#define GPIO_INT_EN 0x02
-#define GPIO_INT_FLAG 0x04
-
-#define ADC_BASE_ADDRESS 0x0110
-#define ADC_MEM_LENGTH 18
-#define ADC_ENABLE 0x01
-
#define ZIGBEE_BASE_ADDRESS 0x0122
#define ZIGBEE_MEM_LENGTH 6
#define ZIGBEE_EN_PWR 0x01
diff --git a/include/eeprom.h b/include/eeprom.h
index 68a136a6dd06dfe2cf74de8f0bda06b465338a17..467907d5cefa087e6d1653b6d22de3208dc13ad8 100755
--- a/include/eeprom.h
+++ b/include/eeprom.h
@@ -83,7 +83,7 @@ extern "C" {
#define PAGE_FULL ((uint8_t)0x80)
/* Variables' number */
-#define NB_OF_VAR ((uint8_t)0x46)
+#define EEPROM_NUM_VAR ((uint8_t)EEPROM_SIZE)
/* Exported types ------------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
diff --git a/include/eeprom_init.h b/include/eeprom_init.h
index af23b20a95c60cfaf2ee631184aa90a8eb8919f4..d7c327cda5039a283e7470d444bf0bdc12cad203 100644
--- a/include/eeprom_init.h
+++ b/include/eeprom_init.h
@@ -7,19 +7,12 @@ extern "C" {
#include "adc_dma.h"
-#define DEFAULT_DEVICE_MODEL 0x7300
-#define DEFAULT_FIRMWARE_VERSION 0x0001
-#define DEFAULT_DEVICE_ID 0x0002
-#define DEFAULT_BAUDRATE 0x0001
-#define DEFAULT_RETURN_DELAY 0x0000
-#define DEFAULT_MM_PERIOD 0x1E78
#define DEFAULT_BAL_KNEE_GAIN 0x04BE // 1/54 in fixed point format 0|16
#define DEFAULT_BAL_ANKLE_ROLL_GAIN 0x0CCD // 1/20
#define DEFAULT_BAL_ANKLE_PITCH_GAIN 0x0E39 // 1/18
#define DEFAULT_BAL_HIP_ROLL_GAIN 0x0666 // 1/40
#define DEFAULT_GYRO_FB_ADC_CH (uint16_t)ADC_CH1
#define DEFAULT_GYRO_LR_ADC_CH (uint16_t)ADC_CH2
-#define DEFAULT_RETURN_LEVEL 0x0002
#define DEFAULT_SERVO0_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO1_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO2_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
diff --git a/include/gpio.h b/include/gpio.h
index 2cc86d3c84b010b5d49cae1d0e585d77820539bb..976070c66ff4676694dd4d5176944984d5dd2a5d 100644
--- a/include/gpio.h
+++ b/include/gpio.h
@@ -6,12 +6,14 @@ extern "C" {
#endif
#include "stm32f4xx.h"
+#include "gpio_registers.h"
+#include "memory.h"
typedef enum {USER1_LED,USER2_LED,RXD_LED,TXD_LED} led_t;
typedef enum {USER_PB,RESET_PB,START_PB,MODE_PB} pushbutton_t;
-void gpio_init(void);
+uint8_t gpio_init(TMemory *memory);
// LED functions
void gpio_set_led(led_t led_id);
void gpio_clear_led(led_t led_id);
@@ -20,10 +22,6 @@ void gpio_blink_led(led_t led_id, int16_t period_ms);
// Pushbuttons functions
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);
#ifdef __cplusplus
}
diff --git a/include/gpio_registers.h b/include/gpio_registers.h
new file mode 100644
index 0000000000000000000000000000000000000000..6e974f2a2555c9276614b6825a75f1b658d4524f
--- /dev/null
+++ b/include/gpio_registers.h
@@ -0,0 +1,39 @@
+#ifndef _GPIO_REGISTERS_H
+#define _GPIO_REGISTERS_H
+
+#ifndef RAM_GPIO_BASE_ADDRESS
+ #define RAM_GPIO_BASE_ADDRESS ((unsigned short int)0x0000)
+#endif
+
+#define RAM_GPIO_LENGTH 16
+
+#define USER1_LED_CNTRL RAM_GPIO_BASE_ADDRESS// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | blink | toggle | value | internally used
+#define USER1_LED_PERIOD (RAM_GPIO_BASE_ADDRESS+1)
+#define USER2_LED_CNTRL (RAM_GPIO_BASE_ADDRESS+3)// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | blink | toggle | value | internally used
+#define USER2_LED_PERIOD (RAM_GPIO_BASE_ADDRESS+4)
+#define RXD_LED_CNTRL (RAM_GPIO_BASE_ADDRESS+6)// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | blink | toggle | value | internally used
+#define RXD_LED_PERIOD (RAM_GPIO_BASE_ADDRESS+7)
+#define TXD_LED_CNTRL (RAM_GPIO_BASE_ADDRESS+9)// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | blink | toggle | value | internally used
+#define TXD_LED_PERIOD (RAM_GPIO_BASE_ADDRESS+10)
+#define USER_PB_CNTRL (RAM_GPIO_BASE_ADDRESS+12)// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | interrupt flag | enable interrupt | value | internally used
+#define RESET_PB_CNTRL (RAM_GPIO_BASE_ADDRESS+13)// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | interrupt flag | enable interrupt | value | internally used
+#define START_PB_CNTRL (RAM_GPIO_BASE_ADDRESS+14)// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | interrupt flag | enable interrupt | value | internally used
+#define MODE_PB_CNTRL (RAM_GPIO_BASE_ADDRESS+15)// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | interrupt flag | enable interrupt | value | internally used
+
+#define GPIO_INT_USED 0x01
+#define GPIO_VALUE 0x02
+#define GPIO_TOGGLE 0x04
+#define GPIO_BLINK 0x08
+#define GPIO_INT_EN 0x02
+#define GPIO_INT_FLAG 0x04
+
+#endif
+
diff --git a/include/ram.h b/include/ram.h
deleted file mode 100644
index c69e8c5a7d22aa6729154e357c1b1d123b969a32..0000000000000000000000000000000000000000
--- a/include/ram.h
+++ /dev/null
@@ -1,37 +0,0 @@
-#ifndef _RAM_H
-#define _RAM_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "stm32f4xx.h"
-#include "bioloid_registers.h"
-
-#define RAM_SUCCESS 0
-#define RAM_BAD_ADDRESS -1
-#define RAM_WRITE_ERROR -2
-#define RAM_BAD_BIT -3
-#define RAM_BAD_ACCESS -4
-
-#define RAM_SIZE 512
-
-extern uint8_t ram_data[RAM_SIZE];
-
-void ram_init(void);
-inline void ram_read_byte(uint16_t address, uint8_t *data);
-inline void ram_read_word(uint16_t address, uint16_t *data);
-uint8_t ram_read_table(uint16_t address, uint16_t length,uint8_t *data);
-uint8_t ram_set_bit(uint16_t address, uint8_t bit);
-uint8_t ram_clear_bit(uint16_t address, uint8_t bit);
-uint8_t ram_write_byte(uint16_t address, uint8_t data);
-uint8_t ram_write_word(uint16_t address, uint16_t data);
-uint8_t ram_write_table(uint16_t address, uint16_t length,uint8_t *data);
-inline uint8_t ram_in_range(uint16_t reg,uint16_t address,uint16_t length);
-uint8_t ram_in_window(uint16_t start_reg,uint16_t reg_length,uint16_t start_address,uint16_t address_length);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif
diff --git a/src/adc_dma.c b/src/adc_dma.c
index 31d3e8e6f87e763704ab3d9a877d16107d9acd1f..436b088652a48fdaa5c00bea826481dc97927b1c 100644
--- a/src/adc_dma.c
+++ b/src/adc_dma.c
@@ -52,12 +52,17 @@
#define VOLTAGE_DELTA 0.000805664
#define VOLTAGE_GAIN 1.529411765
+uint16_t adc_eeprom_data[] __attribute__ ((section (".eeprom")))={DEFAULT_ADC_PERIOD,ADC_PERIOD};
+
// private variables
ADC_HandleTypeDef hadc1;
ADC_HandleTypeDef hadc2;
DMA_HandleTypeDef hdma_adc1;
TIM_HandleTypeDef ADC_TIM_Handle;
+/* memory module */
+TMemModule adc_mem_module;
+
uint32_t adc_data[4];// temporal buffer to store ADC data before conversion
uint16_t adc_period_ms;
@@ -89,6 +94,24 @@ uint16_t adc_convert_voltage(uint16_t value)
return conv_value*(1<<12);
}
+void adc_write_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ if(ram_in_window(ADC_CNTRL,1,address,length))
+ {
+ if(data[ADC_CNTRL-address]&ADC_ENABLE)
+ adc_start();
+ else
+ adc_stop();
+ }
+ if(ram_in_range(ADC_PERIOD,address,length))
+ adc_set_period(data[ADC_PERIOD-address]);
+}
+
+void adc_read_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ ram_read_table(address,length,data);
+}
+
// interrupt handlers
void ADC_TIMER_IRQHandler(void)
{
@@ -147,34 +170,34 @@ void ADC_DMA_IRQHandler(void)
{
__HAL_DMA_CLEAR_FLAG(&hdma_adc1, __HAL_DMA_GET_TC_FLAG_INDEX(&hdma_adc1));
value=adc_convert_voltage(adc_data[0]&0x0000FFFF);
- ram_data[BIOLOID_ADC_CH1_L]=value%256;
- ram_data[BIOLOID_ADC_CH1_H]=value/256;
+ ram_data[ADC_CH1_VOLTAGE]=value%256;
+ ram_data[ADC_CH1_VOLTAGE+1]=value/256;
value=adc_convert_voltage((adc_data[0]&0xFFFF0000)>>16);
- ram_data[BIOLOID_ADC_CH2_L]=value%256;
- ram_data[BIOLOID_ADC_CH2_H]=value/256;
+ ram_data[ADC_CH2_VOLTAGE]=value%256;
+ ram_data[ADC_CH2_VOLTAGE+1]=value/256;
value=adc_convert_voltage(adc_data[1]&0x0000FFFF);
- ram_data[BIOLOID_ADC_CH3_L]=value%256;
- ram_data[BIOLOID_ADC_CH3_H]=value/256;
+ ram_data[ADC_CH3_VOLTAGE]=value%256;
+ ram_data[ADC_CH3_VOLTAGE+1]=value/256;
value=adc_convert_voltage((adc_data[1]&0xFFFF0000)>>16);
- ram_data[BIOLOID_ADC_CH4_L]=value%256;
- ram_data[BIOLOID_ADC_CH4_H]=value/256;
+ ram_data[ADC_CH4_VOLTAGE]=value%256;
+ ram_data[ADC_CH4_VOLTAGE+1]=value/256;
value=adc_convert_temperature(adc_data[2]&0x0000FFFF);
- ram_data[BIOLOID_ADC_TEMP_L]=value%256;
- ram_data[BIOLOID_ADC_TEMP_H]=value/256;
+ ram_data[ADC_TEMPERATURE]=value%256;
+ ram_data[ADC_TEMPERATURE+1]=value/256;
value=adc_convert_voltage((adc_data[2]&0xFFFF0000)>>16);
- ram_data[BIOLOID_ADC_CH6_L]=value%256;
- ram_data[BIOLOID_ADC_CH6_H]=value/256;
+ ram_data[ADC_CH6_VOLTAGE]=value%256;
+ ram_data[ADC_CH6_VOLTAGE+1]=value/256;
value=adc_convert_vrefint(adc_data[3]&0x0000FFFF);
- ram_data[BIOLOID_ADC_VREF_L]=value%256;
- ram_data[BIOLOID_ADC_VREF_H]=value/256;
+ ram_data[ADC_VREF_VOLTAGE]=value%256;
+ ram_data[ADC_VREF_VOLTAGE+1]=value/256;
value=adc_convert_voltage((adc_data[3]&0xFFFF0000)>>16);
- ram_data[BIOLOID_ADC_CH8_L]=value%256;
- ram_data[BIOLOID_ADC_CH8_H]=value/256;
+ ram_data[ADC_CH8_VOLTAGE]=value%256;
+ ram_data[ADC_CH8_VOLTAGE+1]=value/256;
}
}
// public functions
-void adc_init(void)
+uint8_t adc_init(TMemory *memory)
{
GPIO_InitTypeDef GPIO_InitStruct;
ADC_MultiModeTypeDef multimode;
@@ -185,8 +208,7 @@ void adc_init(void)
uint32_t capture;
/* initialize the internal variables */
- adc_period_ms=840;// equivalent to 10 ms
- ram_data[BIOLOID_ADC_PERIOD]=10;
+ adc_set_period(DEFAULT_ADC_PERIOD);
/* enable clocks */
ADC1_CH1_ENABLE_PORT_CLK;
@@ -349,23 +371,36 @@ void adc_init(void)
HAL_ADC_Start(&hadc2);
HAL_ADCEx_MultiModeStart_DMA(&hadc1,adc_data,4);
adc_start();
+
+ /* initialize memory module */
+ mem_module_init(&adc_mem_module);
+ adc_mem_module.write_cmd=adc_write_cmd;
+ adc_mem_module.read_cmd=adc_read_cmd;
+ if(!mem_module_add_ram_segment(&adc_mem_module,RAM_ADC_DMA_BASE_ADDRESS,RAM_ADC_DMA_LENGTH))
+ return 0x00;
+ if(!mem_module_add_eeprom_segment(&adc_mem_module,EEPROM_ADC_DMA_BASE_ADDRESS,EEPROM_ADC_DMA_LENGTH))
+ return 0x00;
+ if(!mem_add_module(memory,&adc_mem_module))
+ return 0x00;
+
+ return 0x01;
}
void adc_start(void)
{
- ram_data[BIOLOID_ADC_CNTRL]|=0x01;
+ ram_data[ADC_CNTRL]|=0x01;
HAL_TIM_OC_Start_IT(&ADC_TIM_Handle, TIM_CHANNEL_4);
}
void adc_set_period(uint8_t period_ms)
{
adc_period_ms=period_ms*84;
- ram_data[BIOLOID_ADC_PERIOD]=period_ms;
+ ram_data[ADC_PERIOD]=period_ms;
}
inline uint8_t adc_get_period(void)
{
- return ram_data[BIOLOID_ADC_PERIOD];
+ return ram_data[ADC_PERIOD];
}
uint16_t adc_get_channel(adc_ch_t channel)
@@ -374,17 +409,17 @@ uint16_t adc_get_channel(adc_ch_t channel)
switch(channel)
{
- case ADC_CH1: value=ram_data[BIOLOID_ADC_CH1_L]+ram_data[BIOLOID_ADC_CH1_H]*256;
+ case ADC_CH1: value=ram_data[ADC_CH1_VOLTAGE]+ram_data[ADC_CH1_VOLTAGE+1]*256;
break;
- case ADC_CH2: value=ram_data[BIOLOID_ADC_CH2_L]+ram_data[BIOLOID_ADC_CH2_H]*256;
+ case ADC_CH2: value=ram_data[ADC_CH2_VOLTAGE]+ram_data[ADC_CH2_VOLTAGE+1]*256;
break;
- case ADC_CH3: value=ram_data[BIOLOID_ADC_CH3_L]+ram_data[BIOLOID_ADC_CH3_H]*256;
+ case ADC_CH3: value=ram_data[ADC_CH3_VOLTAGE]+ram_data[ADC_CH3_VOLTAGE+1]*256;
break;
- case ADC_CH4: value=ram_data[BIOLOID_ADC_CH4_L]+ram_data[BIOLOID_ADC_CH4_H]*256;
+ case ADC_CH4: value=ram_data[ADC_CH4_VOLTAGE]+ram_data[ADC_CH4_VOLTAGE+1]*256;
break;
- case ADC_CH6: value=ram_data[BIOLOID_ADC_CH6_L]+ram_data[BIOLOID_ADC_CH6_H]*256;
+ case ADC_CH6: value=ram_data[ADC_CH6_VOLTAGE]+ram_data[ADC_CH6_VOLTAGE+1]*256;
break;
- case ADC_CH8: value=ram_data[BIOLOID_ADC_CH8_L]+ram_data[BIOLOID_ADC_CH8_H]*256;
+ case ADC_CH8: value=ram_data[ADC_CH8_VOLTAGE]+ram_data[ADC_CH8_VOLTAGE+1]*256;
break;
default: value=0x0000;
}
@@ -418,30 +453,11 @@ uint16_t adc_get_channel_raw(adc_ch_t channel)
uint16_t adc_get_temperature(void)
{
- return ram_data[BIOLOID_ADC_TEMP_L]+ram_data[BIOLOID_ADC_TEMP_H]*256;
+ return ram_data[ADC_TEMPERATURE]+ram_data[ADC_TEMPERATURE+1]*256;
}
void adc_stop(void)
{
- ram_data[BIOLOID_ADC_CNTRL]&=0xFE;
+ ram_data[ADC_CNTRL]&=0xFE;
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))
- {
- if(data[BIOLOID_ADC_CNTRL-address]&ADC_ENABLE)
- adc_start();
- else
- adc_stop();
- }
- if(ram_in_range(BIOLOID_ADC_PERIOD,address,length))
- adc_set_period(data[BIOLOID_ADC_PERIOD-address]);
-}
diff --git a/src/bioloid_dyn_manager.c b/src/bioloid_dyn_manager.c
new file mode 100644
index 0000000000000000000000000000000000000000..ce27eb8ef6d436f81b55e5bf7a0cf3a697f3e844
--- /dev/null
+++ b/src/bioloid_dyn_manager.c
@@ -0,0 +1,83 @@
+#include "bioloid_dyn_manager.h"
+#include "dyn_manager.h"
+
+#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
+TIM_HandleTypeDef MANAGER_TIM_Handle;
+TDynManager bioloid_manager;
+
+uint16_t bioloid_manager_eeprom_data[] __attribute__ ((section (".eeprom"))) = {DEFAULT_DYN_MANAGER_PERIOD,DYN_MANAGER_PERIOD};
+
+
+// interrupt handlers
+void MANAGER_TIMER_IRQHandler(void)
+{
+ uint16_t capture;
+
+ if(__HAL_TIM_GET_FLAG(&MANAGER_TIM_Handle, TIM_FLAG_CC1) != RESET)
+ {
+ 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 + bioloid_manager.period_us));
+ dyn_manager_loop(&bioloid_manager);
+ }
+ }
+}
+
+/* private functions */
+void bioloid_dyn_manager_timer_init(void)
+{
+ TIM_ClockConfigTypeDef sClockSourceConfig;
+ TIM_MasterConfigTypeDef sMasterConfig;
+ TIM_OC_InitTypeDef TIM_OCInitStructure;
+ uint16_t capture;
+
+ /* configure timer */
+ ENABLE_MANAGER_TIMER_CLK;
+ MANAGER_TIM_Handle.Instance=MANAGER_TIMER;
+ MANAGER_TIM_Handle.Init.Period = 0xFFFF;
+ MANAGER_TIM_Handle.Init.Prescaler = 84;
+ MANAGER_TIM_Handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+ MANAGER_TIM_Handle.Init.CounterMode = TIM_COUNTERMODE_UP;
+ HAL_NVIC_SetPriority(MANAGER_TIMER_IRQn, 2, 1);
+ 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);
+
+ 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+DEFAULT_DYN_MANAGER_PERIOD;
+ HAL_TIM_OC_ConfigChannel(&MANAGER_TIM_Handle, &TIM_OCInitStructure,TIM_CHANNEL_1);
+ HAL_TIM_OC_Start_IT(&MANAGER_TIM_Handle, TIM_CHANNEL_1);
+}
+
+void bioloid_dyn_manager_set_period(unsigned short int period_us)
+{
+ dyn_manager_set_period(&bioloid_manager,period_us);
+}
+
+/* public functions */
+uint8_t bioloid_dyn_manager_init(TMemory *memory)
+{
+ unsigned char status;
+
+ bioloid_manager.init_timer=bioloid_dyn_manager_timer_init;
+ bioloid_manager.set_period=bioloid_dyn_manager_set_period;
+ status=dyn_manager_init(&bioloid_manager,memory);
+
+ return status;
+}
diff --git a/src/bioloid_dyn_slave.c b/src/bioloid_dyn_slave.c
index 8ee1a449e95d402f84f2a20e0b76afa6b1afae75..98c2d6683c53153f5f975fcb505269702901c238 100644
--- a/src/bioloid_dyn_slave.c
+++ b/src/bioloid_dyn_slave.c
@@ -2,14 +2,6 @@
#include "bioloid_time.h"
#include "usart3.h"
#include "ram.h"
-#include "eeprom.h"
-#include "gpio.h"
-#include "adc_dma.h"
-#include "zigbee.h"
-#include "bioloid_gyro.h"
-#include "motion_manager.h"
-#include "action.h"
-#include "walking.h"
/* external interrupt pin */
#define DYN_SLAVE_EXT_INT_PIN GPIO_PIN_8
@@ -30,72 +22,20 @@ UART_InitTypeDef bioloid_comm_init;
TIM_HandleTypeDef bioloid_dyn_slave_tim_handle;
uint8_t bioloid_slave_int_count;
+/* memory module */
+TMemory *bioloid_dyn_slave_memory;
+
// private functions
unsigned char bioloid_on_read(unsigned short int address,unsigned short int length,unsigned char *data)
{
- 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);
- // action commands
- if(ram_in_window(ACTION_BASE_ADDRESS,ACTION_MEM_LENGTH,address,length))
- action_process_read_cmd(address,length,data);
-
- return error;
+ mem_do_read(bioloid_dyn_slave_memory,address,length,data);
+
+ return 0x00;
}
unsigned char bioloid_on_write(unsigned short int address,unsigned short int length,unsigned char *data)
{
- unsigned short int i,j;
-
- /* dynamixel slave internal operation registers */
- if(ram_in_range(BIOLOID_ID,address,length))
- {
- dyn_slave_set_address(&bioloid_dyn_slave,data[BIOLOID_ID-address]);
- ram_data[BIOLOID_ID]=data[BIOLOID_ID-address];
- }
- if(ram_in_range(BIOLOID_BAUD_RATE,address,length))
- {
- bioloid_comm_init.BaudRate=2000000/(data[BIOLOID_BAUD_RATE-address]+1);
- usart3_config(&bioloid_dyn_slave_comm,&bioloid_comm_init);
- ram_data[BIOLOID_BAUD_RATE]=data[BIOLOID_BAUD_RATE-address];
- }
- if(ram_in_range(BIOLOID_RETURN_DELAY_TIME,address,length))
- {
- dyn_slave_set_return_delay(&bioloid_dyn_slave,data[BIOLOID_RETURN_DELAY_TIME-address]);
- ram_data[BIOLOID_RETURN_DELAY_TIME]=data[BIOLOID_RETURN_DELAY_TIME-address];
- }
- if(ram_in_range(BIOLOID_RETURN_LEVEL,address,length))
- {
- dyn_slave_set_return_level(&bioloid_dyn_slave,data[BIOLOID_RETURN_LEVEL-address]);
- ram_data[BIOLOID_RETURN_LEVEL]=data[BIOLOID_RETURN_LEVEL-address];
- }
- // GPIO commands
- if(gpio_in_range(address,length))
- gpio_process_write_cmd(address,length,data);
- // ADC commands
- if(adc_in_range(address,length))
- adc_process_write_cmd(address,length,data);
- // Zigbee commands
- 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);
- // action commands
- if(action_in_range(address,length))
- action_process_write_cmd(address,length,data);
- // gyro commands
- if(gyro_in_range(address,length))
- gyro_process_write_cmd(address,length,data);
- // walk commands
- if(walking_in_range(address,length))
- walking_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]);
+ mem_do_write(bioloid_dyn_slave_memory,address,length,data);
return 0x00;
}
@@ -118,10 +58,11 @@ void DYN_SLAVE_TIMER_IRQHandler(void)
}
// public functions
-void bioloid_dyn_slave_init(void)
+uint8_t bioloid_dyn_slave_init(TMemory *memory)
{
TUSART_IRQ_Priorities priorities;
GPIO_InitTypeDef GPIO_InitStructure;
+ uint8_t status;
// initialize private variables
bioloid_slave_int_count=0;
@@ -131,7 +72,7 @@ void bioloid_dyn_slave_init(void)
/* initialize the comm object */
comm_init(&bioloid_dyn_slave_comm,0x01,&bioloid_dyn_slave_timer);
- bioloid_comm_init.BaudRate = 921600;
+ bioloid_comm_init.BaudRate = 2000000/(DEFAULT_BAUDRATE+1);;
bioloid_comm_init.WordLength = UART_WORDLENGTH_8B;
bioloid_comm_init.StopBits = UART_STOPBITS_1;
bioloid_comm_init.Parity = UART_PARITY_NONE;
@@ -147,12 +88,13 @@ void bioloid_dyn_slave_init(void)
priorities.dma_tx_subpriority=0;
usart3_init(&bioloid_dyn_slave_comm,&bioloid_comm_init,&priorities);
- dyn_slave_init(&bioloid_dyn_slave,&bioloid_dyn_slave_comm,ram_data[BIOLOID_ID],DYN_VER2);
+ status=dyn_slave_init(&bioloid_dyn_slave,memory,&bioloid_dyn_slave_comm,DEFAULT_DEVICE_ID,DYN_VER2);
+ bioloid_dyn_slave.set_baudrate=usart3_set_baudrate;
bioloid_dyn_slave.on_read=bioloid_on_read;
bioloid_dyn_slave.on_write=bioloid_on_write;
bioloid_dyn_slave.on_ping=bioloid_on_ping;
- dyn_slave_set_return_delay(&bioloid_dyn_slave,ram_data[BIOLOID_RETURN_DELAY_TIME]);
- dyn_slave_set_return_level(&bioloid_dyn_slave,ram_data[BIOLOID_RETURN_LEVEL]);
+ dyn_slave_set_return_delay(&bioloid_dyn_slave,DEFAULT_RETURN_DELAY);
+ dyn_slave_set_return_level(&bioloid_dyn_slave,DEFAULT_RETURN_LEVEL);
/* initialize the external interrupt pin */
DYN_SLAVE_EXT_INT_EN_GPIO_CLK;
@@ -174,6 +116,8 @@ void bioloid_dyn_slave_init(void)
// initialize the timer interrupts
HAL_NVIC_SetPriority(DYN_SLAVE_TIMER_IRQn, 2, 0);
HAL_NVIC_EnableIRQ(DYN_SLAVE_TIMER_IRQn);
+
+ return status;
}
void bioloid_dyn_slave_start(void)
diff --git a/src/bioloid_stm32.c b/src/bioloid_stm32.c
index b34b9d4fe471f0e2f2f7078f754c20ca7a382c6f..4ac7b0d2a5fbe2a3157601513c9cee5df2c067f1 100644
--- a/src/bioloid_stm32.c
+++ b/src/bioloid_stm32.c
@@ -1,46 +1,33 @@
-#include "stm32f4xx_hal.h"
-#include "gpio.h"
-#include "eeprom.h"
+#include "stm32f4xx.h"
+#include "memory.h"
#include "ram.h"
+#include "eeprom.h"
#include "adc_dma.h"
-#include "zigbee.h"
-#include "bioloid_gyro.h"
-#include "bioloid_time.h"
+#include "gpio.h"
#include "bioloid_dyn_slave.h"
-#include "bioloid_dyn_master_sensors.h"
-#include "motion_manager.h"
+#include "bioloid_dyn_manager.h"
+
+TMemory bioloid_memory;
int32_t main(void)
{
- uint16_t eeprom_data,period;
-
HAL_Init();
/* initialize the gpio */
- gpio_init();
- /* initialize the eeprom emmulation */
EE_Init();
- /* initialize the ram module */
- ram_init();
+ /* initialize memory structure */
+ mem_init(&bioloid_memory);
+ bioloid_memory.eeprom_write_data=EE_WriteVariable;
+ bioloid_memory.eeprom_read_data=EE_ReadVariable;
/* initialize the ADC module */
- adc_init();
- /* initialize the time module */
- bioloid_time_init();
- /* initialize zigbee module */
- zigbee_init();
- zigbee_enable_power();
- /* initialize the dynamixel slave interface */
- bioloid_dyn_slave_init();
- bioloid_dyn_slave_start();
- /* initialize the dynamixel master module for the sensors */
- bioloid_dyn_master_sensors_init();
- /* initialize gyro module */
- gyro_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);
+ adc_init(&bioloid_memory);
+ /* initialize the GPIO expansion module*/
+ gpio_init(&bioloid_memory);
+ // initialize the dynamixel slave interface
+ bioloid_dyn_slave_init(&bioloid_memory);
+ // initialize dynamixel_manager
+ bioloid_dyn_manager_init(&bioloid_memory);
+ /* initialize the ram module */
+ ram_init(&bioloid_memory);
while(1);
}
diff --git a/src/eeprom.c b/src/eeprom.c
index 35c1d2266ea43a31a5201dbb41b2d73879531062..d35be7c21012fef08096b79a92f9e5e54746700b 100755
--- a/src/eeprom.c
+++ b/src/eeprom.c
@@ -52,78 +52,9 @@
/* Global variable used to store variable value in read sequence */
uint16_t DataVar = 0;
+uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE, 0xFFFF};
+
/* Virtual address defined by the user: 0xFFFF value is prohibited */
-uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE, 0xFFFF,
- DEFAULT_DEVICE_MODEL&0xFF, DEVICE_MODEL_OFFSET,// model number LSB
- DEFAULT_DEVICE_MODEL>>8, DEVICE_MODEL_OFFSET+1,// model number MSB
- DEFAULT_FIRMWARE_VERSION, FIRMWARE_VERSION_OFFSET,// firmware version
- DEFAULT_DEVICE_ID, DEVICE_ID_OFFSET,// default device id
- DEFAULT_BAUDRATE, BAUDRATE_OFFSET,// default baudrate
- DEFAULT_RETURN_DELAY, RETURN_DELAY_OFFSET,// return delay time
- DEFAULT_MM_PERIOD&0xFF, MM_PERIOD_OFFSET,
- DEFAULT_MM_PERIOD>>8, MM_PERIOD_OFFSET+1,
- DEFAULT_BAL_KNEE_GAIN&0xFF, MM_BAL_KNEE_GAIN_OFFSET,
- DEFAULT_BAL_KNEE_GAIN>>8, MM_BAL_KNEE_GAIN_OFFSET+1,
- DEFAULT_BAL_ANKLE_ROLL_GAIN&0xFF, MM_BAL_ANKLE_ROLL_GAIN_OFFSET,
- DEFAULT_BAL_ANKLE_ROLL_GAIN>>8, MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1,
- DEFAULT_BAL_ANKLE_PITCH_GAIN&0xFF, MM_BAL_ANKLE_PITCH_GAIN_OFFSET,
- DEFAULT_BAL_ANKLE_PITCH_GAIN>>8, MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1,
- DEFAULT_BAL_HIP_ROLL_GAIN&0xFF, MM_BAL_HIP_ROLL_GAIN_OFFSET,
- DEFAULT_BAL_HIP_ROLL_GAIN>>8, MM_BAL_HIP_ROLL_GAIN_OFFSET+1,
- DEFAULT_RETURN_LEVEL, RETURN_LEVEL_OFFSET,// return level
- DEFAULT_SERVO0_OFFSET, MM_SERVO0_OFFSET,
- DEFAULT_SERVO1_OFFSET, MM_SERVO1_OFFSET,
- DEFAULT_SERVO2_OFFSET, MM_SERVO2_OFFSET,
- DEFAULT_SERVO3_OFFSET, MM_SERVO3_OFFSET,
- DEFAULT_SERVO4_OFFSET, MM_SERVO4_OFFSET,
- DEFAULT_SERVO5_OFFSET, MM_SERVO5_OFFSET,
- DEFAULT_SERVO6_OFFSET, MM_SERVO6_OFFSET,
- DEFAULT_SERVO7_OFFSET, MM_SERVO7_OFFSET,
- DEFAULT_SERVO8_OFFSET, MM_SERVO8_OFFSET,
- DEFAULT_SERVO9_OFFSET, MM_SERVO9_OFFSET,
- DEFAULT_SERVO10_OFFSET, MM_SERVO10_OFFSET,
- DEFAULT_SERVO11_OFFSET, MM_SERVO11_OFFSET,
- DEFAULT_SERVO12_OFFSET, MM_SERVO12_OFFSET,
- DEFAULT_SERVO13_OFFSET, MM_SERVO13_OFFSET,
- DEFAULT_SERVO14_OFFSET, MM_SERVO14_OFFSET,
- DEFAULT_SERVO15_OFFSET, MM_SERVO15_OFFSET,
- DEFAULT_SERVO16_OFFSET, MM_SERVO16_OFFSET,
- DEFAULT_SERVO17_OFFSET, MM_SERVO17_OFFSET,
- DEFAULT_SERVO18_OFFSET, MM_SERVO18_OFFSET,
- DEFAULT_SERVO19_OFFSET, MM_SERVO19_OFFSET,
- DEFAULT_SERVO20_OFFSET, MM_SERVO20_OFFSET,
- DEFAULT_SERVO21_OFFSET, MM_SERVO21_OFFSET,
- DEFAULT_SERVO22_OFFSET, MM_SERVO22_OFFSET,
- DEFAULT_SERVO23_OFFSET, MM_SERVO23_OFFSET,
- DEFAULT_SERVO24_OFFSET, MM_SERVO24_OFFSET,
- DEFAULT_SERVO25_OFFSET, MM_SERVO25_OFFSET,
- DEFAULT_SERVO26_OFFSET, MM_SERVO26_OFFSET,
- DEFAULT_SERVO27_OFFSET, MM_SERVO27_OFFSET,
- DEFAULT_SERVO28_OFFSET, MM_SERVO28_OFFSET,
- DEFAULT_SERVO29_OFFSET, MM_SERVO29_OFFSET,
- DEFAULT_SERVO30_OFFSET, MM_SERVO30_OFFSET,
- DEFAULT_SERVO31_OFFSET, MM_SERVO31_OFFSET,
- DEFAULT_GYRO_FB_ADC_CH, GYRO_FB_ADC_CH_OFFSET,
- DEFAULT_GYRO_LR_ADC_CH, GYRO_LR_ADC_CH_OFFSET,
- DEFAULT_WALK_X_OFFSET, WALK_X_OFFSET,
- DEFAULT_WALK_Y_OFFSET, WALK_Y_OFFSET,
- DEFAULT_WALK_Z_OFFSET, WALK_Z_OFFSET,
- DEFAULT_WALK_ROLL_OFFSET, WALK_ROLL_OFFSET,
- DEFAULT_WALK_PITCH_OFFSET, WALK_PITCH_OFFSET,
- DEFAULT_WALK_YAW_OFFSET, WALK_YAW_OFFSET,
- DEFAULT_WALK_HIP_PITCH_OFF&0xFF, WALK_HIP_PITCH_OFF,
- DEFAULT_WALK_HIP_PITCH_OFF>>8, WALK_HIP_PITCH_OFF+1,
- DEFAULT_WALK_PERIOD_TIME&0xFF, WALK_PERIOD_TIME,
- DEFAULT_WALK_PERIOD_TIME>>8, WALK_PERIOD_TIME+1,
- DEFAULT_WALK_DSP_RATIO, WALK_DSP_RATIO,
- DEFAULT_WALK_STEP_FW_BW_RATIO, WALK_STEP_FW_BW_RATIO,
- DEFAULT_WALK_FOOT_HEIGHT, WALK_FOOT_HEIGHT,
- DEFAULT_WALK_SWING_RIGHT_LEFT, WALK_SWING_RIGHT_LEFT,
- DEFAULT_WALK_SWING_TOP_DOWN, WALK_SWING_TOP_DOWN,
- DEFAULT_WALK_PELVIS_OFFSET, WALK_PELVIS_OFFSET,
- DEFAULT_WALK_ARM_SWING_GAIN, WALK_ARM_SWING_GAIN,
- DEFAULT_WALK_MAX_VEL, WALK_MAX_VEL,
- DEFAULT_WALK_MAX_ROT_VEL, WALK_MAX_ROT_VEL};
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
@@ -216,7 +147,7 @@ uint16_t EE_Init(void)
if (pagestatus1 == VALID_PAGE) /* Page0 receive, Page1 valid */
{
/* Transfer data from Page1 to Page0 */
- for (varidx = 0; varidx < NB_OF_VAR; varidx++)
+ for (varidx = 0; varidx < EEPROM_NUM_VAR; varidx++)
{
if (( *(__IO uint16_t*)(PAGE0_BASE_ADDRESS + 6)) == eeprom_data[varidx])
{
@@ -325,7 +256,7 @@ uint16_t EE_Init(void)
else /* Page0 valid, Page1 receive */
{
/* Transfer data from Page0 to Page1 */
- for (varidx = 0; varidx < NB_OF_VAR; varidx++)
+ for (varidx = 0; varidx < EEPROM_NUM_VAR; varidx++)
{
if ((*(__IO uint16_t*)(PAGE1_BASE_ADDRESS + 6)) == eeprom_data[varidx])
{
@@ -751,7 +682,7 @@ static uint16_t EE_PageTransfer(uint16_t VirtAddress, uint16_t Data)
}
/* Transfer process: transfer variables from old to the new active page */
- for (varidx = 0; varidx < NB_OF_VAR; varidx++)
+ for (varidx = 0; varidx < EEPROM_NUM_VAR; varidx++)
{
if (eeprom_data[varidx] != VirtAddress) /* Check each variable except the one passed as parameter */
{
diff --git a/src/gpio.c b/src/gpio.c
index a3f13c2cfbc81e771b6566e846d7706dada69706..db66aa6ac10d3d70137403e486d2d1ee4322c419 100644
--- a/src/gpio.c
+++ b/src/gpio.c
@@ -67,36 +67,39 @@ uint8_t led_rx_blinking;
uint8_t led_usr1_blinking;
uint8_t led_usr2_blinking;
+/* memory module */
+TMemModule gpio_mem_module;
+
// IRQ handler functions
void GPI_EXTI1_IRQHandler(void)
{
if(__HAL_GPIO_EXTI_GET_IT(RESET_PB_PIN) != RESET)
{
__HAL_GPIO_EXTI_CLEAR_IT(RESET_PB_PIN);
- if(ram_data[BIOLOID_RESET_PB_CNTRL]&GPIO_INT_USED)
+ if(ram_data[RESET_PB_CNTRL]&GPIO_INT_USED)
{
if(reset_pb_callback!=0)
reset_pb_callback();
}
else
{
- ram_data[BIOLOID_RESET_PB_CNTRL]|=GPIO_INT_FLAG;
- if(ram_data[BIOLOID_RESET_PB_CNTRL]&GPIO_INT_EN)
+ ram_data[RESET_PB_CNTRL]|=GPIO_INT_FLAG;
+ if(ram_data[RESET_PB_CNTRL]&GPIO_INT_EN)
bioloid_dyn_slave_set_int();
}
}
if(__HAL_GPIO_EXTI_GET_IT(USER_PB_PIN) != RESET)
{
__HAL_GPIO_EXTI_CLEAR_IT(USER_PB_PIN);
- if(ram_data[BIOLOID_USER_PB_CNTRL]&GPIO_INT_USED)
+ if(ram_data[USER_PB_CNTRL]&GPIO_INT_USED)
{
if(user_pb_callback!=0)
user_pb_callback();
}
else
{
- ram_data[BIOLOID_USER_PB_CNTRL]|=GPIO_INT_FLAG;
- if(ram_data[BIOLOID_USER_PB_CNTRL]&GPIO_INT_EN)
+ ram_data[USER_PB_CNTRL]|=GPIO_INT_FLAG;
+ if(ram_data[USER_PB_CNTRL]&GPIO_INT_EN)
bioloid_dyn_slave_set_int();
}
}
@@ -107,15 +110,15 @@ void GPI_EXTI2_IRQHandler(void)
if(__HAL_GPIO_EXTI_GET_IT(START_PB_PIN) != RESET)
{
__HAL_GPIO_EXTI_CLEAR_IT(START_PB_PIN);
- if(ram_data[BIOLOID_START_PB_CNTRL]&GPIO_INT_USED)
+ if(ram_data[START_PB_CNTRL]&GPIO_INT_USED)
{
if(start_pb_callback!=0)
start_pb_callback();
}
else
{
- ram_data[BIOLOID_START_PB_CNTRL]|=GPIO_INT_FLAG;
- if(ram_data[BIOLOID_START_PB_CNTRL]&GPIO_INT_EN)
+ ram_data[START_PB_CNTRL]|=GPIO_INT_FLAG;
+ if(ram_data[START_PB_CNTRL]&GPIO_INT_EN)
bioloid_dyn_slave_set_int();
}
}
@@ -126,15 +129,15 @@ void GPI_EXTI3_IRQHandler(void)
if(__HAL_GPIO_EXTI_GET_IT(MODE_PB_PIN) != RESET)
{
__HAL_GPIO_EXTI_CLEAR_IT(MODE_PB_PIN);
- if(ram_data[BIOLOID_MODE_PB_CNTRL]&GPIO_INT_USED)
+ if(ram_data[MODE_PB_CNTRL]&GPIO_INT_USED)
{
if(mode_pb_callback!=0)
mode_pb_callback();
}
else
{
- ram_data[BIOLOID_MODE_PB_CNTRL]|=GPIO_INT_FLAG;
- if(ram_data[BIOLOID_MODE_PB_CNTRL]&GPIO_INT_EN)
+ ram_data[MODE_PB_CNTRL]|=GPIO_INT_FLAG;
+ if(ram_data[MODE_PB_CNTRL]&GPIO_INT_EN)
bioloid_dyn_slave_set_int();
}
}
@@ -152,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]&GPIO_VALUE)
- ram_data[BIOLOID_TXD_LED_CNTRL]&=(~GPIO_VALUE);
+ if(ram_data[TXD_LED_CNTRL]&GPIO_VALUE)
+ ram_data[TXD_LED_CNTRL]&=(~GPIO_VALUE);
else
- ram_data[BIOLOID_TXD_LED_CNTRL]|=GPIO_VALUE;
+ ram_data[TXD_LED_CNTRL]|=GPIO_VALUE;
}
}
if(__HAL_TIM_GET_FLAG(&GPO_TIM_Handle, TIM_FLAG_CC2) != RESET)
@@ -166,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]&GPIO_VALUE)
- ram_data[BIOLOID_RXD_LED_CNTRL]&=(~GPIO_VALUE);
+ if(ram_data[RXD_LED_CNTRL]&GPIO_VALUE)
+ ram_data[RXD_LED_CNTRL]&=(~GPIO_VALUE);
else
- ram_data[BIOLOID_RXD_LED_CNTRL]|=GPIO_VALUE;
+ ram_data[RXD_LED_CNTRL]|=GPIO_VALUE;
}
}
if(__HAL_TIM_GET_FLAG(&GPO_TIM_Handle, TIM_FLAG_CC3) != RESET)
@@ -180,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]&GPIO_VALUE)
- ram_data[BIOLOID_USER1_LED_CNTRL]&=(~GPIO_VALUE);
+ if(ram_data[USER1_LED_CNTRL]&GPIO_VALUE)
+ ram_data[USER1_LED_CNTRL]&=(~GPIO_VALUE);
else
- ram_data[BIOLOID_USER1_LED_CNTRL]|=GPIO_VALUE;
+ ram_data[USER1_LED_CNTRL]|=GPIO_VALUE;
}
}
if(__HAL_TIM_GET_FLAG(&GPO_TIM_Handle, TIM_FLAG_CC4) != RESET)
@@ -194,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]&GPIO_VALUE)
- ram_data[BIOLOID_USER2_LED_CNTRL]&=(~GPIO_VALUE);
+ if(ram_data[USER2_LED_CNTRL]&GPIO_VALUE)
+ ram_data[USER2_LED_CNTRL]&=(~GPIO_VALUE);
else
- ram_data[BIOLOID_USER2_LED_CNTRL]|=GPIO_VALUE;
+ ram_data[USER2_LED_CNTRL]|=GPIO_VALUE;
}
}
/* TIM Update event */
@@ -219,9 +222,229 @@ void GPO_TIMER_IRQHandler(void)
}
// private functions
+void gpio_read_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ if(ram_in_range(RESET_PB_CNTRL,address,length))
+ if(!(ram_data[RESET_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
+ {
+ if(ram_data[RESET_PB_CNTRL]&GPIO_INT_FLAG)
+ {
+ ram_data[RESET_PB_CNTRL]&=(~GPIO_INT_FLAG);
+ if(ram_data[RESET_PB_CNTRL]&GPIO_INT_EN)
+ bioloid_dyn_slave_clear_int();
+ }
+ }
+ if(ram_in_range(USER_PB_CNTRL,address,length))
+ if(!(ram_data[USER_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
+ {
+ if(ram_data[USER_PB_CNTRL]&GPIO_INT_FLAG)
+ {
+ ram_data[USER_PB_CNTRL]&=(~GPIO_INT_FLAG);
+ if(ram_data[USER_PB_CNTRL]&GPIO_INT_EN)
+ bioloid_dyn_slave_clear_int();
+ }
+ }
+ if(ram_in_range(START_PB_CNTRL,address,length))
+ if(!(ram_data[START_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
+ {
+ if(ram_data[START_PB_CNTRL]&GPIO_INT_FLAG)
+ {
+ ram_data[START_PB_CNTRL]&=(~GPIO_INT_FLAG);
+ if(ram_data[START_PB_CNTRL]&GPIO_INT_EN)
+ bioloid_dyn_slave_clear_int();
+ }
+ }
+ if(ram_in_range(MODE_PB_CNTRL,address,length))
+ if(!(ram_data[MODE_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
+ {
+ if(ram_data[MODE_PB_CNTRL]&GPIO_INT_FLAG)
+ {
+ ram_data[MODE_PB_CNTRL]&=(~GPIO_INT_FLAG);
+ if(ram_data[MODE_PB_CNTRL]&GPIO_INT_EN)
+ bioloid_dyn_slave_clear_int();
+ }
+ }
+}
+
+void gpio_write_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ uint16_t period;
+ uint8_t *data_ptr;
+
+ /* LED's */
+ if(ram_in_range(USER1_LED_CNTRL,address,length))
+ {
+ if(!(ram_data[USER1_LED_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
+ {
+ period=led_usr1_period;
+ data_ptr=(uint8_t *).
+ if(ram_in_range(USER1_LED_PERIOD,address,length))
+ data_ptr[0]=data[USER1_LED_PERIOD-address];
+ if(ram_in_range(USER1_LED_PERIOD+1,address,length))
+ data_ptr[1]=data[USER1_LED_PERIOD+1-address];
+ if(data[USER1_LED_CNTRL-address]&GPIO_BLINK)
+ gpio_blink_led(USER1_LED,period);
+ else
+ {
+ gpio_blink_led(USER1_LED,0x0000);
+ if(data[USER1_LED_CNTRL-address]&GPIO_TOGGLE)
+ {
+ gpio_toggle_led(USER1_LED);
+ if(ram_data[USER1_LED_CNTRL]&GPIO_VALUE)
+ ram_data[USER1_LED_CNTRL]&=(~GPIO_VALUE);
+ else
+ ram_data[USER1_LED_CNTRL]|=GPIO_VALUE;
+ }
+ else
+ {
+ if(data[USER1_LED_CNTRL-address]&GPIO_VALUE)
+ {
+ gpio_set_led(USER1_LED);
+ ram_data[USER1_LED_CNTRL]|=GPIO_VALUE;
+ }
+ else
+ {
+ gpio_clear_led(USER1_LED);
+ ram_data[USER1_LED_CNTRL]&=(~GPIO_VALUE);
+ }
+ }
+ }
+ }
+ }
+ if(ram_in_range(USER2_LED_CNTRL,address,length))
+ {
+ if(!(ram_data[USER2_LED_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
+ {
+ period=led_usr2_period;
+ data_ptr=(uint8_t *).
+ if(ram_in_range(USER2_LED_PERIOD,address,length))
+ data_ptr[0]=data[USER2_LED_PERIOD-address];
+ if(ram_in_range(USER2_LED_PERIOD+1,address,length))
+ data_ptr[1]=data[USER2_LED_PERIOD+1-address];
+ if(data[USER2_LED_CNTRL-address]&GPIO_BLINK)
+ gpio_blink_led(USER2_LED,period);
+ else
+ {
+ gpio_blink_led(USER2_LED,0x0000);
+ if(data[USER2_LED_CNTRL-address]&GPIO_TOGGLE)
+ {
+ gpio_toggle_led(USER2_LED);
+ if(ram_data[USER2_LED_CNTRL]&GPIO_VALUE)
+ ram_data[USER2_LED_CNTRL]&=(~GPIO_VALUE);
+ else
+ ram_data[USER2_LED_CNTRL]|=GPIO_VALUE;
+ }
+ else
+ {
+ if(data[USER2_LED_CNTRL-address]&GPIO_VALUE)
+ {
+ gpio_set_led(USER2_LED);
+ ram_data[USER2_LED_CNTRL]|=GPIO_VALUE;
+ }
+ else
+ {
+ gpio_clear_led(USER2_LED);
+ ram_data[USER2_LED_CNTRL]&=(~GPIO_VALUE);
+ }
+ }
+ }
+ }
+ }
+ if(ram_in_range(RXD_LED_CNTRL,address,length))
+ {
+ if(!(ram_data[RXD_LED_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
+ {
+ period=led_rx_period;
+ data_ptr=(uint8_t *).
+ if(ram_in_range(RXD_LED_PERIOD,address,length))
+ data_ptr[0]=data[RXD_LED_PERIOD-address];
+ if(ram_in_range(RXD_LED_PERIOD+1,address,length))
+ data_ptr[1]=data[RXD_LED_PERIOD+1-address];
+ if(data[RXD_LED_CNTRL-address]&GPIO_BLINK)
+ gpio_blink_led(RXD_LED,period);
+ else
+ {
+ gpio_blink_led(RXD_LED,0x0000);
+ if(data[RXD_LED_CNTRL-address]&GPIO_TOGGLE)
+ {
+ gpio_toggle_led(RXD_LED);
+ if(ram_data[RXD_LED_CNTRL]&GPIO_VALUE)
+ ram_data[RXD_LED_CNTRL]&=(~GPIO_VALUE);
+ else
+ ram_data[RXD_LED_CNTRL]|=GPIO_VALUE;
+ }
+ else
+ {
+ if(data[RXD_LED_CNTRL-address]&GPIO_VALUE)
+ {
+ gpio_set_led(RXD_LED);
+ ram_data[RXD_LED_CNTRL]|=GPIO_VALUE;
+ }
+ else
+ {
+ gpio_clear_led(RXD_LED);
+ ram_data[RXD_LED_CNTRL]&=(~GPIO_VALUE);
+ }
+ }
+ }
+ }
+ }
+ if(ram_in_range(TXD_LED_CNTRL,address,length))
+ {
+ if(!(ram_data[TXD_LED_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
+ {
+ period=led_tx_period;
+ data_ptr=(uint8_t *).
+ if(ram_in_range(TXD_LED_PERIOD,address,length))
+ data_ptr[0]=data[TXD_LED_PERIOD-address];
+ if(ram_in_range(TXD_LED_PERIOD+1,address,length))
+ data_ptr[1]=data[TXD_LED_PERIOD+1-address];
+ if(data[TXD_LED_CNTRL-address]&GPIO_BLINK)
+ gpio_blink_led(TXD_LED,period);
+ else
+ {
+ gpio_blink_led(TXD_LED,0x0000);
+ if(data[TXD_LED_CNTRL-address]&GPIO_TOGGLE)
+ {
+ gpio_toggle_led(TXD_LED);
+ if(ram_data[TXD_LED_CNTRL]&GPIO_VALUE)
+ ram_data[TXD_LED_CNTRL]&=(~GPIO_VALUE);
+ else
+ ram_data[TXD_LED_CNTRL]|=GPIO_VALUE;
+ }
+ else
+ {
+ if(data[TXD_LED_CNTRL-address]&GPIO_VALUE)
+ {
+ gpio_set_led(TXD_LED);
+ ram_data[TXD_LED_CNTRL]|=GPIO_VALUE;
+ }
+ else
+ {
+ gpio_clear_led(TXD_LED);
+ ram_data[TXD_LED_CNTRL]&=(~GPIO_VALUE);
+ }
+ }
+ }
+ }
+ }
+ /* pushbuttons */
+ if(ram_in_range(RESET_PB_CNTRL,address,length))
+ if(!(ram_data[RESET_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
+ ram_data[RESET_PB_CNTRL]=data[RESET_PB_CNTRL-address];
+ if(ram_in_range(USER_PB_CNTRL,address,length))
+ if(!(ram_data[USER_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
+ ram_data[USER_PB_CNTRL]=data[USER_PB_CNTRL-address];
+ if(ram_in_range(START_PB_CNTRL,address,length))
+ if(!(ram_data[START_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
+ ram_data[START_PB_CNTRL]=data[START_PB_CNTRL-address];
+ if(ram_in_range(MODE_PB_CNTRL,address,length))
+ if(!(ram_data[MODE_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
+ ram_data[MODE_PB_CNTRL]=data[MODE_PB_CNTRL-address];
+}
// public functions
-void gpio_init(void)
+uint8_t gpio_init(TMemory *memory)
{
GPIO_InitTypeDef GPIO_InitStructure;
TIM_ClockConfigTypeDef sClockSourceConfig;
@@ -321,6 +544,17 @@ void gpio_init(void)
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
HAL_TIMEx_MasterConfigSynchronization(&GPO_TIM_Handle, &sMasterConfig);
+
+ /* initialize memory module */
+ mem_module_init(&gpio_mem_module);
+ gpio_mem_module.write_cmd=gpio_write_cmd;
+ gpio_mem_module.read_cmd=gpio_read_cmd;
+ if(!mem_module_add_ram_segment(&gpio_mem_module,RAM_GPIO_BASE_ADDRESS,RAM_GPIO_LENGTH))
+ return 0x00;
+ if(!mem_add_module(memory,&gpio_mem_module))
+ return 0x00;
+
+ return 0x01;
}
void gpio_set_led(led_t led_id)
@@ -522,224 +756,3 @@ 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))
- if(!(ram_data[BIOLOID_RESET_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
- {
- if(ram_data[BIOLOID_RESET_PB_CNTRL]&GPIO_INT_FLAG)
- {
- 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 */
- {
- if(ram_data[BIOLOID_USER_PB_CNTRL]&GPIO_INT_FLAG)
- {
- 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 */
- {
- if(ram_data[BIOLOID_START_PB_CNTRL]&GPIO_INT_FLAG)
- {
- 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 */
- {
- if(ram_data[BIOLOID_MODE_PB_CNTRL]&GPIO_INT_FLAG)
- {
- 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 */
- {
- period=led_usr1_period;
- if(ram_in_range(BIOLOID_USER1_LED_PERIOD_L,address,length))
- period=(period&0xFF00)+data[BIOLOID_USER1_LED_PERIOD_L-address];
- if(ram_in_range(BIOLOID_USER1_LED_PERIOD_H,address,length))
- period=(period&0x00FF)+(data[BIOLOID_USER1_LED_PERIOD_H-address]<<8);
- if(data[BIOLOID_USER1_LED_CNTRL-address]&GPIO_BLINK)
- gpio_blink_led(USER1_LED,period);
- else
- {
- gpio_blink_led(USER1_LED,0x0000);
- if(data[BIOLOID_USER1_LED_CNTRL-address]&GPIO_TOGGLE)
- {
- gpio_toggle_led(USER1_LED);
- if(ram_data[BIOLOID_USER1_LED_CNTRL]&GPIO_VALUE)
- ram_data[BIOLOID_USER1_LED_CNTRL]&=(~GPIO_VALUE);
- else
- 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]|=GPIO_VALUE;
- }
- else
- {
- gpio_clear_led(USER1_LED);
- ram_data[BIOLOID_USER1_LED_CNTRL]&=(~GPIO_VALUE);
- }
- }
- }
- }
- }
- 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 */
- {
- period=led_usr2_period;
- if(ram_in_range(BIOLOID_USER2_LED_PERIOD_L,address,length))
- period=(period&0xFF00)+data[BIOLOID_USER2_LED_PERIOD_L-address];
- if(ram_in_range(BIOLOID_USER2_LED_PERIOD_H,address,length))
- period=(period&0x00FF)+(data[BIOLOID_USER2_LED_PERIOD_H-address]<<8);
- if(data[BIOLOID_USER2_LED_CNTRL-address]&GPIO_BLINK)
- gpio_blink_led(USER2_LED,period);
- else
- {
- gpio_blink_led(USER2_LED,0x0000);
- if(data[BIOLOID_USER2_LED_CNTRL-address]&GPIO_TOGGLE)
- {
- gpio_toggle_led(USER2_LED);
- if(ram_data[BIOLOID_USER2_LED_CNTRL]&GPIO_VALUE)
- ram_data[BIOLOID_USER2_LED_CNTRL]&=(~GPIO_VALUE);
- else
- 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]|=GPIO_VALUE;
- }
- else
- {
- gpio_clear_led(USER2_LED);
- ram_data[BIOLOID_USER2_LED_CNTRL]&=(~GPIO_VALUE);
- }
- }
- }
- }
- }
- 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 */
- {
- 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))
- period=(period&0x00FF)+(data[BIOLOID_RXD_LED_PERIOD_H-address]<<8);
- if(data[BIOLOID_RXD_LED_CNTRL-address]&GPIO_BLINK)
- gpio_blink_led(RXD_LED,period);
- else
- {
- gpio_blink_led(RXD_LED,0x0000);
- if(data[BIOLOID_RXD_LED_CNTRL-address]&GPIO_TOGGLE)
- {
- gpio_toggle_led(RXD_LED);
- if(ram_data[BIOLOID_RXD_LED_CNTRL]&GPIO_VALUE)
- ram_data[BIOLOID_RXD_LED_CNTRL]&=(~GPIO_VALUE);
- else
- 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]|=GPIO_VALUE;
- }
- else
- {
- gpio_clear_led(RXD_LED);
- ram_data[BIOLOID_RXD_LED_CNTRL]&=(~GPIO_VALUE);
- }
- }
- }
- }
- }
- 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 */
- {
- 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))
- period=(period&0x00FF)+(data[BIOLOID_TXD_LED_PERIOD_H-address]<<8);
- if(data[BIOLOID_TXD_LED_CNTRL-address]&GPIO_BLINK)
- gpio_blink_led(TXD_LED,period);
- else
- {
- gpio_blink_led(TXD_LED,0x0000);
- if(data[BIOLOID_TXD_LED_CNTRL-address]&GPIO_TOGGLE)
- {
- gpio_toggle_led(TXD_LED);
- if(ram_data[BIOLOID_TXD_LED_CNTRL]&GPIO_VALUE)
- ram_data[BIOLOID_TXD_LED_CNTRL]&=(~GPIO_VALUE);
- else
- 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]|=GPIO_VALUE;
- }
- else
- {
- gpio_clear_led(TXD_LED);
- ram_data[BIOLOID_TXD_LED_CNTRL]&=(~GPIO_VALUE);
- }
- }
- }
- }
- }
- /* 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 */
- 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 */
- 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
deleted file mode 100644
index 34142288c21d200e51380b6f2ece9eb61c3e1f03..0000000000000000000000000000000000000000
--- a/src/ram.c
+++ /dev/null
@@ -1,195 +0,0 @@
-#include "ram.h"
-#include "eeprom.h"
-
-uint8_t ram_data[RAM_SIZE];
-
-void ram_init(void)
-{
- uint16_t eeprom_data,i;
-
- for(i=0;i<RAM_SIZE;i++)
- ram_data[i]=0x00;
- // read contents from EEPROM to RAM
- if(EE_ReadVariable(DEVICE_MODEL_OFFSET,&eeprom_data)==0)
- ram_data[DEVICE_MODEL_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(DEVICE_MODEL_OFFSET+1,&eeprom_data)==0)
- ram_data[DEVICE_MODEL_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(FIRMWARE_VERSION_OFFSET,&eeprom_data)==0)
- ram_data[FIRMWARE_VERSION_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(DEVICE_ID_OFFSET,&eeprom_data)==0)
- ram_data[DEVICE_ID_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(BAUDRATE_OFFSET,&eeprom_data)==0)
- ram_data[BAUDRATE_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(RETURN_DELAY_OFFSET,&eeprom_data)==0)
- ram_data[RETURN_DELAY_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(MM_PERIOD_OFFSET,&eeprom_data)==0)
- ram_data[MM_PERIOD_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(MM_PERIOD_OFFSET+1,&eeprom_data)==0)
- ram_data[MM_PERIOD_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(MM_BAL_KNEE_GAIN_OFFSET,&eeprom_data)==0)
- ram_data[MM_BAL_KNEE_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(MM_BAL_KNEE_GAIN_OFFSET+1,&eeprom_data)==0)
- ram_data[MM_BAL_KNEE_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(MM_BAL_ANKLE_ROLL_GAIN_OFFSET,&eeprom_data)==0)
- ram_data[MM_BAL_ANKLE_ROLL_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1,&eeprom_data)==0)
- ram_data[MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(MM_BAL_ANKLE_PITCH_GAIN_OFFSET,&eeprom_data)==0)
- ram_data[MM_BAL_ANKLE_PITCH_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1,&eeprom_data)==0)
- ram_data[MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(MM_BAL_HIP_ROLL_GAIN_OFFSET,&eeprom_data)==0)
- ram_data[MM_BAL_HIP_ROLL_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(MM_BAL_HIP_ROLL_GAIN_OFFSET+1,&eeprom_data)==0)
- ram_data[MM_BAL_HIP_ROLL_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(RETURN_LEVEL_OFFSET,&eeprom_data)==0)
- ram_data[RETURN_LEVEL_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- for(i=0;i<32;i++)
- {
- if(EE_ReadVariable(MM_SERVO0_OFFSET+i,&eeprom_data)==0)
- ram_data[BIOLOID_MM_SERVO0_OFFSET+i]=(uint8_t)(eeprom_data&0x00FF);
- }
- if(EE_ReadVariable(GYRO_FB_ADC_CH_OFFSET,&eeprom_data)==0)
- ram_data[GYRO_FB_ADC_CH_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GYRO_LR_ADC_CH_OFFSET,&eeprom_data)==0)
- ram_data[GYRO_LR_ADC_CH_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_X_OFFSET,&eeprom_data)==0)
- ram_data[WALK_X_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_Y_OFFSET,&eeprom_data)==0)
- ram_data[WALK_Y_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_Z_OFFSET,&eeprom_data)==0)
- ram_data[WALK_Z_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_ROLL_OFFSET,&eeprom_data)==0)
- ram_data[WALK_ROLL_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_PITCH_OFFSET,&eeprom_data)==0)
- ram_data[WALK_PITCH_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_YAW_OFFSET,&eeprom_data)==0)
- ram_data[WALK_YAW_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_HIP_PITCH_OFF,&eeprom_data)==0)
- ram_data[WALK_HIP_PITCH_OFF]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_HIP_PITCH_OFF+1,&eeprom_data)==0)
- ram_data[WALK_HIP_PITCH_OFF+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_PERIOD_TIME,&eeprom_data)==0)
- ram_data[WALK_PERIOD_TIME]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_PERIOD_TIME+1,&eeprom_data)==0)
- ram_data[WALK_PERIOD_TIME+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_DSP_RATIO,&eeprom_data)==0)
- ram_data[WALK_DSP_RATIO]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_STEP_FW_BW_RATIO,&eeprom_data)==0)
- ram_data[WALK_STEP_FW_BW_RATIO]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_FOOT_HEIGHT,&eeprom_data)==0)
- ram_data[WALK_FOOT_HEIGHT]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_SWING_RIGHT_LEFT,&eeprom_data)==0)
- ram_data[WALK_SWING_RIGHT_LEFT]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_SWING_TOP_DOWN,&eeprom_data)==0)
- ram_data[WALK_SWING_TOP_DOWN]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_PELVIS_OFFSET,&eeprom_data)==0)
- ram_data[WALK_PELVIS_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_ARM_SWING_GAIN,&eeprom_data)==0)
- ram_data[WALK_ARM_SWING_GAIN]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_MAX_VEL,&eeprom_data)==0)
- ram_data[WALK_MAX_VEL]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(WALK_MAX_ROT_VEL,&eeprom_data)==0)
- ram_data[WALK_MAX_ROT_VEL]=(uint8_t)(eeprom_data&0x00FF);
-}
-
-inline void ram_read_byte(uint16_t address,uint8_t *data)
-{
- (*data)=ram_data[address];
-}
-
-inline void ram_read_word(uint16_t address, uint16_t *data)
-{
- (*data)=ram_data[address];
- (*data)+=ram_data[address+1]*256;
-}
-
-uint8_t ram_read_table(uint16_t address, uint16_t length,uint8_t *data)
-{
- uint16_t i;
-
- if((address+length)<=(RAM_SIZE-1))
- {
- for(i=0;i<length;i++)
- data[i]=ram_data[address+i];
- return RAM_SUCCESS;
- }
- else
- return RAM_BAD_ADDRESS;
-}
-
-uint8_t ram_set_bit(uint16_t address, uint8_t bit)
-{
- if(bit>=0 && bit<8)
- {
- ram_data[address]|=(0x01<<bit);
- return RAM_SUCCESS;
- }
- else
- return RAM_BAD_BIT;
-}
-
-uint8_t ram_clear_bit(uint16_t address, uint8_t bit)
-{
- if(bit>=0 && bit<8)
- {
- ram_data[address]&=(~(0x01<<bit));
- return RAM_SUCCESS;
- }
- else
- return RAM_BAD_BIT;
-}
-
-uint8_t ram_write_byte(uint16_t address, uint8_t data)
-{
- ram_data[address]=data;
-
- return RAM_SUCCESS;
-}
-
-uint8_t ram_write_word(uint16_t address, uint16_t data)
-{
- if(address < (RAM_SIZE-1))
- {
- ram_data[address]=data%256;
- ram_data[address+1]=data/256;
-
- return RAM_SUCCESS;
- }
- else
- return RAM_BAD_ADDRESS;
-}
-
-uint8_t ram_write_table(uint16_t address, uint16_t length,uint8_t *data)
-{
- uint16_t i;
-
- if((address+length)<RAM_SIZE)
- {
- for(i=0;i<length;i++)
- ram_data[address+i]=data[i];
- return RAM_SUCCESS;
- }
- else
- return RAM_BAD_ADDRESS;
-}
-
-inline uint8_t ram_in_range(uint16_t reg,uint16_t address,uint16_t length)
-{
- if(reg>=address && reg<(address+length))
- return 0x01;
- else
- return 0x00;
-}
-
-uint8_t ram_in_window(uint16_t start_reg,uint16_t reg_length,uint16_t start_address,uint16_t address_length)
-{
- uint16_t end_reg=start_reg+reg_length;
- uint16_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;
-}