diff --git a/Makefile b/Makefile
index c9e730f11fa19b4e8c221b798d516d89350aaf09..95f99f9040ee1fbc2e22ce89b6887282489a3d50 100755
--- a/Makefile
+++ b/Makefile
@@ -4,15 +4,7 @@ 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=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_FILES=$(wildcard src/*.c)
TARGET_PROCESSOR=STM32F407VG
HAL_PATH=$(STM32_HAL_PATH)/f4
@@ -23,19 +15,17 @@ 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 -imacros ./include/bioloid_conf.h
+COMPILE_OPTS += -ffreestanding -nostdlib -D$(PROCESSOR_MACRO) -DARM_MATH_CM4 -D__FPU_PRESENT -DHSE_VALUE=8000000
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$(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/
+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/
TCHAIN_PREFIX=arm-none-eabi-
@@ -47,6 +37,7 @@ 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
@@ -79,12 +70,6 @@ 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))
@@ -101,19 +86,9 @@ $(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) -lm --output $@
+ $(LD) $(LDFLAGS) $(BIOLOID_OBJS) $(BUILD_PATH)/$(STARTUP_FILE:.s=.o) $(EXT_LIB) -lm --output $@
$(MAIN_OUT_HEX): $(MAIN_OUT_ELF)
$(OBJCP) $(OBJCPFLAGS_HEX) $< $@
diff --git a/include/adc_dma.h b/include/adc_dma.h
index 1c552ac7867cf7513a2a25e8ab5b2ee168618106..d360ab07c9f44fb271a20ce56d27876e82714ec5 100644
--- a/include/adc_dma.h
+++ b/include/adc_dma.h
@@ -6,13 +6,11 @@ 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
-uint8_t adc_init(TMemory *memory);
+void adc_init(void);
void adc_start(void);
void adc_set_period(uint8_t period_ms);
inline uint8_t adc_get_period(void);
@@ -20,6 +18,9 @@ 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
deleted file mode 100644
index 58001e683ae250a940ff986c355cc4b443596f07..0000000000000000000000000000000000000000
--- a/include/adc_dma_registers.h
+++ /dev/null
@@ -1,36 +0,0 @@
-#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
deleted file mode 100644
index afeb40183cc054d4637eba471ecc2209e84b1155..0000000000000000000000000000000000000000
--- a/include/bioloid_conf.h
+++ /dev/null
@@ -1,61 +0,0 @@
-#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
deleted file mode 100644
index 6b36ea3986968611db8c042e976054542ff2fbca..0000000000000000000000000000000000000000
--- a/include/bioloid_dyn_manager.h
+++ /dev/null
@@ -1,18 +0,0 @@
-#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 e02812c3be7b8b861eb79d66b337b4be1235f6c4..98554dc5d42abe194303cddb7e9b1fb5347257ab 100644
--- a/include/bioloid_dyn_master_sensors.h
+++ b/include/bioloid_dyn_master_sensors.h
@@ -6,12 +6,10 @@ 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 e0f3e41d3ef7960f9a2d941552abe214fb291334..3152d5673ce5a3eb8b9d2de52aebf0bfa4d961d1 100644
--- a/include/bioloid_dyn_master_servos.h
+++ b/include/bioloid_dyn_master_servos.h
@@ -9,9 +9,6 @@ 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 d90ad919391a9f1561e968a50f3f857713dcb7a8..5f58c04da075d3860462eba7c31e31893d2d5263 100644
--- a/include/bioloid_dyn_slave.h
+++ b/include/bioloid_dyn_slave.h
@@ -7,9 +7,8 @@ extern "C" {
#include "stm32f4xx.h"
#include "dynamixel_slave.h"
-#include "memory.h"
-uint8_t bioloid_dyn_slave_init(TMemory *memory);
+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);
diff --git a/include/bioloid_registers.h b/include/bioloid_registers.h
index 306e371007705a25f02a4eef77f419b10304b231..1acbad0df4c914c71d4852e0bc5667df0307f784 100644
--- a/include/bioloid_registers.h
+++ b/include/bioloid_registers.h
@@ -5,10 +5,17 @@
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)
@@ -64,6 +71,14 @@ 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
@@ -72,6 +87,7 @@ 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
@@ -125,6 +141,49 @@ 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,
@@ -253,6 +312,19 @@ 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 467907d5cefa087e6d1653b6d22de3208dc13ad8..68a136a6dd06dfe2cf74de8f0bda06b465338a17 100755
--- a/include/eeprom.h
+++ b/include/eeprom.h
@@ -83,7 +83,7 @@ extern "C" {
#define PAGE_FULL ((uint8_t)0x80)
/* Variables' number */
-#define EEPROM_NUM_VAR ((uint8_t)EEPROM_SIZE)
+#define NB_OF_VAR ((uint8_t)0x46)
/* Exported types ------------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
diff --git a/include/eeprom_init.h b/include/eeprom_init.h
index d7c327cda5039a283e7470d444bf0bdc12cad203..af23b20a95c60cfaf2ee631184aa90a8eb8919f4 100644
--- a/include/eeprom_init.h
+++ b/include/eeprom_init.h
@@ -7,12 +7,19 @@ 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 976070c66ff4676694dd4d5176944984d5dd2a5d..2cc86d3c84b010b5d49cae1d0e585d77820539bb 100644
--- a/include/gpio.h
+++ b/include/gpio.h
@@ -6,14 +6,12 @@ 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;
-uint8_t gpio_init(TMemory *memory);
+void gpio_init(void);
// LED functions
void gpio_set_led(led_t led_id);
void gpio_clear_led(led_t led_id);
@@ -22,6 +20,10 @@ 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
deleted file mode 100644
index 6e974f2a2555c9276614b6825a75f1b658d4524f..0000000000000000000000000000000000000000
--- a/include/gpio_registers.h
+++ /dev/null
@@ -1,39 +0,0 @@
-#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
new file mode 100644
index 0000000000000000000000000000000000000000..c69e8c5a7d22aa6729154e357c1b1d123b969a32
--- /dev/null
+++ b/include/ram.h
@@ -0,0 +1,37 @@
+#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 436b088652a48fdaa5c00bea826481dc97927b1c..31d3e8e6f87e763704ab3d9a877d16107d9acd1f 100644
--- a/src/adc_dma.c
+++ b/src/adc_dma.c
@@ -52,17 +52,12 @@
#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;
@@ -94,24 +89,6 @@ 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)
{
@@ -170,34 +147,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[ADC_CH1_VOLTAGE]=value%256;
- ram_data[ADC_CH1_VOLTAGE+1]=value/256;
+ ram_data[BIOLOID_ADC_CH1_L]=value%256;
+ ram_data[BIOLOID_ADC_CH1_H]=value/256;
value=adc_convert_voltage((adc_data[0]&0xFFFF0000)>>16);
- ram_data[ADC_CH2_VOLTAGE]=value%256;
- ram_data[ADC_CH2_VOLTAGE+1]=value/256;
+ ram_data[BIOLOID_ADC_CH2_L]=value%256;
+ ram_data[BIOLOID_ADC_CH2_H]=value/256;
value=adc_convert_voltage(adc_data[1]&0x0000FFFF);
- ram_data[ADC_CH3_VOLTAGE]=value%256;
- ram_data[ADC_CH3_VOLTAGE+1]=value/256;
+ ram_data[BIOLOID_ADC_CH3_L]=value%256;
+ ram_data[BIOLOID_ADC_CH3_H]=value/256;
value=adc_convert_voltage((adc_data[1]&0xFFFF0000)>>16);
- ram_data[ADC_CH4_VOLTAGE]=value%256;
- ram_data[ADC_CH4_VOLTAGE+1]=value/256;
+ ram_data[BIOLOID_ADC_CH4_L]=value%256;
+ ram_data[BIOLOID_ADC_CH4_H]=value/256;
value=adc_convert_temperature(adc_data[2]&0x0000FFFF);
- ram_data[ADC_TEMPERATURE]=value%256;
- ram_data[ADC_TEMPERATURE+1]=value/256;
+ ram_data[BIOLOID_ADC_TEMP_L]=value%256;
+ ram_data[BIOLOID_ADC_TEMP_H]=value/256;
value=adc_convert_voltage((adc_data[2]&0xFFFF0000)>>16);
- ram_data[ADC_CH6_VOLTAGE]=value%256;
- ram_data[ADC_CH6_VOLTAGE+1]=value/256;
+ ram_data[BIOLOID_ADC_CH6_L]=value%256;
+ ram_data[BIOLOID_ADC_CH6_H]=value/256;
value=adc_convert_vrefint(adc_data[3]&0x0000FFFF);
- ram_data[ADC_VREF_VOLTAGE]=value%256;
- ram_data[ADC_VREF_VOLTAGE+1]=value/256;
+ ram_data[BIOLOID_ADC_VREF_L]=value%256;
+ ram_data[BIOLOID_ADC_VREF_H]=value/256;
value=adc_convert_voltage((adc_data[3]&0xFFFF0000)>>16);
- ram_data[ADC_CH8_VOLTAGE]=value%256;
- ram_data[ADC_CH8_VOLTAGE+1]=value/256;
+ ram_data[BIOLOID_ADC_CH8_L]=value%256;
+ ram_data[BIOLOID_ADC_CH8_H]=value/256;
}
}
// public functions
-uint8_t adc_init(TMemory *memory)
+void adc_init(void)
{
GPIO_InitTypeDef GPIO_InitStruct;
ADC_MultiModeTypeDef multimode;
@@ -208,7 +185,8 @@ uint8_t adc_init(TMemory *memory)
uint32_t capture;
/* initialize the internal variables */
- adc_set_period(DEFAULT_ADC_PERIOD);
+ adc_period_ms=840;// equivalent to 10 ms
+ ram_data[BIOLOID_ADC_PERIOD]=10;
/* enable clocks */
ADC1_CH1_ENABLE_PORT_CLK;
@@ -371,36 +349,23 @@ uint8_t adc_init(TMemory *memory)
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[ADC_CNTRL]|=0x01;
+ ram_data[BIOLOID_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[ADC_PERIOD]=period_ms;
+ ram_data[BIOLOID_ADC_PERIOD]=period_ms;
}
inline uint8_t adc_get_period(void)
{
- return ram_data[ADC_PERIOD];
+ return ram_data[BIOLOID_ADC_PERIOD];
}
uint16_t adc_get_channel(adc_ch_t channel)
@@ -409,17 +374,17 @@ uint16_t adc_get_channel(adc_ch_t channel)
switch(channel)
{
- case ADC_CH1: value=ram_data[ADC_CH1_VOLTAGE]+ram_data[ADC_CH1_VOLTAGE+1]*256;
+ case ADC_CH1: value=ram_data[BIOLOID_ADC_CH1_L]+ram_data[BIOLOID_ADC_CH1_H]*256;
break;
- case ADC_CH2: value=ram_data[ADC_CH2_VOLTAGE]+ram_data[ADC_CH2_VOLTAGE+1]*256;
+ case ADC_CH2: value=ram_data[BIOLOID_ADC_CH2_L]+ram_data[BIOLOID_ADC_CH2_H]*256;
break;
- case ADC_CH3: value=ram_data[ADC_CH3_VOLTAGE]+ram_data[ADC_CH3_VOLTAGE+1]*256;
+ case ADC_CH3: value=ram_data[BIOLOID_ADC_CH3_L]+ram_data[BIOLOID_ADC_CH3_H]*256;
break;
- case ADC_CH4: value=ram_data[ADC_CH4_VOLTAGE]+ram_data[ADC_CH4_VOLTAGE+1]*256;
+ case ADC_CH4: value=ram_data[BIOLOID_ADC_CH4_L]+ram_data[BIOLOID_ADC_CH4_H]*256;
break;
- case ADC_CH6: value=ram_data[ADC_CH6_VOLTAGE]+ram_data[ADC_CH6_VOLTAGE+1]*256;
+ case ADC_CH6: value=ram_data[BIOLOID_ADC_CH6_L]+ram_data[BIOLOID_ADC_CH6_H]*256;
break;
- case ADC_CH8: value=ram_data[ADC_CH8_VOLTAGE]+ram_data[ADC_CH8_VOLTAGE+1]*256;
+ case ADC_CH8: value=ram_data[BIOLOID_ADC_CH8_L]+ram_data[BIOLOID_ADC_CH8_H]*256;
break;
default: value=0x0000;
}
@@ -453,11 +418,30 @@ uint16_t adc_get_channel_raw(adc_ch_t channel)
uint16_t adc_get_temperature(void)
{
- return ram_data[ADC_TEMPERATURE]+ram_data[ADC_TEMPERATURE+1]*256;
+ return ram_data[BIOLOID_ADC_TEMP_L]+ram_data[BIOLOID_ADC_TEMP_H]*256;
}
void adc_stop(void)
{
- ram_data[ADC_CNTRL]&=0xFE;
+ ram_data[BIOLOID_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
deleted file mode 100644
index ce27eb8ef6d436f81b55e5bf7a0cf3a697f3e844..0000000000000000000000000000000000000000
--- a/src/bioloid_dyn_manager.c
+++ /dev/null
@@ -1,83 +0,0 @@
-#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 98c2d6683c53153f5f975fcb505269702901c238..8ee1a449e95d402f84f2a20e0b76afa6b1afae75 100644
--- a/src/bioloid_dyn_slave.c
+++ b/src/bioloid_dyn_slave.c
@@ -2,6 +2,14 @@
#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
@@ -22,20 +30,72 @@ 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)
{
- mem_do_read(bioloid_dyn_slave_memory,address,length,data);
-
- return 0x00;
+ 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;
}
unsigned char bioloid_on_write(unsigned short int address,unsigned short int length,unsigned char *data)
{
- mem_do_write(bioloid_dyn_slave_memory,address,length,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]);
return 0x00;
}
@@ -58,11 +118,10 @@ void DYN_SLAVE_TIMER_IRQHandler(void)
}
// public functions
-uint8_t bioloid_dyn_slave_init(TMemory *memory)
+void bioloid_dyn_slave_init(void)
{
TUSART_IRQ_Priorities priorities;
GPIO_InitTypeDef GPIO_InitStructure;
- uint8_t status;
// initialize private variables
bioloid_slave_int_count=0;
@@ -72,7 +131,7 @@ uint8_t bioloid_dyn_slave_init(TMemory *memory)
/* initialize the comm object */
comm_init(&bioloid_dyn_slave_comm,0x01,&bioloid_dyn_slave_timer);
- bioloid_comm_init.BaudRate = 2000000/(DEFAULT_BAUDRATE+1);;
+ bioloid_comm_init.BaudRate = 921600;
bioloid_comm_init.WordLength = UART_WORDLENGTH_8B;
bioloid_comm_init.StopBits = UART_STOPBITS_1;
bioloid_comm_init.Parity = UART_PARITY_NONE;
@@ -88,13 +147,12 @@ uint8_t bioloid_dyn_slave_init(TMemory *memory)
priorities.dma_tx_subpriority=0;
usart3_init(&bioloid_dyn_slave_comm,&bioloid_comm_init,&priorities);
- 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;
+ dyn_slave_init(&bioloid_dyn_slave,&bioloid_dyn_slave_comm,ram_data[BIOLOID_ID],DYN_VER2);
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,DEFAULT_RETURN_DELAY);
- dyn_slave_set_return_level(&bioloid_dyn_slave,DEFAULT_RETURN_LEVEL);
+ 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]);
/* initialize the external interrupt pin */
DYN_SLAVE_EXT_INT_EN_GPIO_CLK;
@@ -116,8 +174,6 @@ uint8_t bioloid_dyn_slave_init(TMemory *memory)
// 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 4ac7b0d2a5fbe2a3157601513c9cee5df2c067f1..b34b9d4fe471f0e2f2f7078f754c20ca7a382c6f 100644
--- a/src/bioloid_stm32.c
+++ b/src/bioloid_stm32.c
@@ -1,33 +1,46 @@
-#include "stm32f4xx.h"
-#include "memory.h"
-#include "ram.h"
+#include "stm32f4xx_hal.h"
+#include "gpio.h"
#include "eeprom.h"
+#include "ram.h"
#include "adc_dma.h"
-#include "gpio.h"
+#include "zigbee.h"
+#include "bioloid_gyro.h"
+#include "bioloid_time.h"
#include "bioloid_dyn_slave.h"
-#include "bioloid_dyn_manager.h"
-
-TMemory bioloid_memory;
+#include "bioloid_dyn_master_sensors.h"
+#include "motion_manager.h"
int32_t main(void)
{
+ uint16_t eeprom_data,period;
+
HAL_Init();
/* initialize the gpio */
+ gpio_init();
+ /* initialize the eeprom emmulation */
EE_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(&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);
+ ram_init();
+ /* 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);
while(1);
}
diff --git a/src/eeprom.c b/src/eeprom.c
index d35be7c21012fef08096b79a92f9e5e54746700b..35c1d2266ea43a31a5201dbb41b2d73879531062 100755
--- a/src/eeprom.c
+++ b/src/eeprom.c
@@ -52,9 +52,78 @@
/* 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 ---------------------------------------------------------*/
@@ -147,7 +216,7 @@ uint16_t EE_Init(void)
if (pagestatus1 == VALID_PAGE) /* Page0 receive, Page1 valid */
{
/* Transfer data from Page1 to Page0 */
- for (varidx = 0; varidx < EEPROM_NUM_VAR; varidx++)
+ for (varidx = 0; varidx < NB_OF_VAR; varidx++)
{
if (( *(__IO uint16_t*)(PAGE0_BASE_ADDRESS + 6)) == eeprom_data[varidx])
{
@@ -256,7 +325,7 @@ uint16_t EE_Init(void)
else /* Page0 valid, Page1 receive */
{
/* Transfer data from Page0 to Page1 */
- for (varidx = 0; varidx < EEPROM_NUM_VAR; varidx++)
+ for (varidx = 0; varidx < NB_OF_VAR; varidx++)
{
if ((*(__IO uint16_t*)(PAGE1_BASE_ADDRESS + 6)) == eeprom_data[varidx])
{
@@ -682,7 +751,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 < EEPROM_NUM_VAR; varidx++)
+ for (varidx = 0; varidx < NB_OF_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 db66aa6ac10d3d70137403e486d2d1ee4322c419..a3f13c2cfbc81e771b6566e846d7706dada69706 100644
--- a/src/gpio.c
+++ b/src/gpio.c
@@ -67,39 +67,36 @@ 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[RESET_PB_CNTRL]&GPIO_INT_USED)
+ if(ram_data[BIOLOID_RESET_PB_CNTRL]&GPIO_INT_USED)
{
if(reset_pb_callback!=0)
reset_pb_callback();
}
else
{
- ram_data[RESET_PB_CNTRL]|=GPIO_INT_FLAG;
- if(ram_data[RESET_PB_CNTRL]&GPIO_INT_EN)
+ 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)
{
__HAL_GPIO_EXTI_CLEAR_IT(USER_PB_PIN);
- if(ram_data[USER_PB_CNTRL]&GPIO_INT_USED)
+ if(ram_data[BIOLOID_USER_PB_CNTRL]&GPIO_INT_USED)
{
if(user_pb_callback!=0)
user_pb_callback();
}
else
{
- ram_data[USER_PB_CNTRL]|=GPIO_INT_FLAG;
- if(ram_data[USER_PB_CNTRL]&GPIO_INT_EN)
+ ram_data[BIOLOID_USER_PB_CNTRL]|=GPIO_INT_FLAG;
+ if(ram_data[BIOLOID_USER_PB_CNTRL]&GPIO_INT_EN)
bioloid_dyn_slave_set_int();
}
}
@@ -110,15 +107,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[START_PB_CNTRL]&GPIO_INT_USED)
+ if(ram_data[BIOLOID_START_PB_CNTRL]&GPIO_INT_USED)
{
if(start_pb_callback!=0)
start_pb_callback();
}
else
{
- ram_data[START_PB_CNTRL]|=GPIO_INT_FLAG;
- if(ram_data[START_PB_CNTRL]&GPIO_INT_EN)
+ ram_data[BIOLOID_START_PB_CNTRL]|=GPIO_INT_FLAG;
+ if(ram_data[BIOLOID_START_PB_CNTRL]&GPIO_INT_EN)
bioloid_dyn_slave_set_int();
}
}
@@ -129,15 +126,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[MODE_PB_CNTRL]&GPIO_INT_USED)
+ if(ram_data[BIOLOID_MODE_PB_CNTRL]&GPIO_INT_USED)
{
if(mode_pb_callback!=0)
mode_pb_callback();
}
else
{
- ram_data[MODE_PB_CNTRL]|=GPIO_INT_FLAG;
- if(ram_data[MODE_PB_CNTRL]&GPIO_INT_EN)
+ ram_data[BIOLOID_MODE_PB_CNTRL]|=GPIO_INT_FLAG;
+ if(ram_data[BIOLOID_MODE_PB_CNTRL]&GPIO_INT_EN)
bioloid_dyn_slave_set_int();
}
}
@@ -155,10 +152,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[TXD_LED_CNTRL]&GPIO_VALUE)
- ram_data[TXD_LED_CNTRL]&=(~GPIO_VALUE);
+ if(ram_data[BIOLOID_TXD_LED_CNTRL]&GPIO_VALUE)
+ ram_data[BIOLOID_TXD_LED_CNTRL]&=(~GPIO_VALUE);
else
- ram_data[TXD_LED_CNTRL]|=GPIO_VALUE;
+ ram_data[BIOLOID_TXD_LED_CNTRL]|=GPIO_VALUE;
}
}
if(__HAL_TIM_GET_FLAG(&GPO_TIM_Handle, TIM_FLAG_CC2) != RESET)
@@ -169,10 +166,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[RXD_LED_CNTRL]&GPIO_VALUE)
- ram_data[RXD_LED_CNTRL]&=(~GPIO_VALUE);
+ if(ram_data[BIOLOID_RXD_LED_CNTRL]&GPIO_VALUE)
+ ram_data[BIOLOID_RXD_LED_CNTRL]&=(~GPIO_VALUE);
else
- ram_data[RXD_LED_CNTRL]|=GPIO_VALUE;
+ ram_data[BIOLOID_RXD_LED_CNTRL]|=GPIO_VALUE;
}
}
if(__HAL_TIM_GET_FLAG(&GPO_TIM_Handle, TIM_FLAG_CC3) != RESET)
@@ -183,10 +180,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[USER1_LED_CNTRL]&GPIO_VALUE)
- ram_data[USER1_LED_CNTRL]&=(~GPIO_VALUE);
+ if(ram_data[BIOLOID_USER1_LED_CNTRL]&GPIO_VALUE)
+ ram_data[BIOLOID_USER1_LED_CNTRL]&=(~GPIO_VALUE);
else
- ram_data[USER1_LED_CNTRL]|=GPIO_VALUE;
+ ram_data[BIOLOID_USER1_LED_CNTRL]|=GPIO_VALUE;
}
}
if(__HAL_TIM_GET_FLAG(&GPO_TIM_Handle, TIM_FLAG_CC4) != RESET)
@@ -197,10 +194,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[USER2_LED_CNTRL]&GPIO_VALUE)
- ram_data[USER2_LED_CNTRL]&=(~GPIO_VALUE);
+ if(ram_data[BIOLOID_USER2_LED_CNTRL]&GPIO_VALUE)
+ ram_data[BIOLOID_USER2_LED_CNTRL]&=(~GPIO_VALUE);
else
- ram_data[USER2_LED_CNTRL]|=GPIO_VALUE;
+ ram_data[BIOLOID_USER2_LED_CNTRL]|=GPIO_VALUE;
}
}
/* TIM Update event */
@@ -222,229 +219,9 @@ 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
-uint8_t gpio_init(TMemory *memory)
+void gpio_init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
TIM_ClockConfigTypeDef sClockSourceConfig;
@@ -544,17 +321,6 @@ uint8_t gpio_init(TMemory *memory)
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)
@@ -756,3 +522,224 @@ 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
new file mode 100644
index 0000000000000000000000000000000000000000..34142288c21d200e51380b6f2ece9eb61c3e1f03
--- /dev/null
+++ b/src/ram.c
@@ -0,0 +1,195 @@
+#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;
+}