From 59fde70f4d4bf5063a367799060335c1938f76af Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Sun, 11 Feb 2018 09:54:26 +0100
Subject: [PATCH] Changes to use the new momory data structures to handle the
devices modules memory. Added the registers files for the ADC module, GPIO
module. Added the dynamixel manager library to handle all the devices.
---
Makefile | 37 +-
include/adc_dma.h | 7 +-
include/adc_dma_registers.h | 36 ++
include/bioloid_conf.h | 61 ++++
include/bioloid_dyn_manager.h | 18 +
include/bioloid_dyn_master_sensors.h | 4 +-
include/bioloid_dyn_master_servos.h | 3 +
include/bioloid_dyn_slave.h | 3 +-
include/bioloid_registers.h | 72 ----
include/eeprom.h | 2 +-
include/eeprom_init.h | 7 -
include/gpio.h | 8 +-
include/gpio_registers.h | 39 +++
include/ram.h | 37 --
src/adc_dma.c | 114 +++---
src/bioloid_dyn_manager.c | 83 +++++
src/bioloid_dyn_slave.c | 88 +----
src/bioloid_stm32.c | 53 ++-
src/eeprom.c | 79 +----
src/gpio.c | 505 ++++++++++++++-------------
src/ram.c | 195 -----------
21 files changed, 648 insertions(+), 803 deletions(-)
create mode 100644 include/adc_dma_registers.h
create mode 100644 include/bioloid_conf.h
create mode 100644 include/bioloid_dyn_manager.h
create mode 100644 include/gpio_registers.h
delete mode 100644 include/ram.h
create mode 100644 src/bioloid_dyn_manager.c
delete mode 100644 src/ram.c
diff --git a/Makefile b/Makefile
index 95f99f9..c9e730f 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 d360ab0..1c552ac 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 0000000..58001e6
--- /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 0000000..afeb401
--- /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 0000000..6b36ea3
--- /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 98554dc..e02812c 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 3152d56..e0f3e41 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 5f58c04..d90ad91 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 1acbad0..306e371 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 68a136a..467907d 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 af23b20..d7c327c 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 2cc86d3..976070c 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 0000000..6e974f2
--- /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 c69e8c5..0000000
--- 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 31d3e8e..436b088 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 0000000..ce27eb8
--- /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 8ee1a44..98c2d66 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 b34b9d4..4ac7b0d 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 35c1d22..d35be7c 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 a3f13c2..db66aa6 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 3414228..0000000
--- 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;
-}
--
GitLab