diff --git a/Makefile b/Makefile
index eeb471a7684a62268bfbd0597f4fd42a3fba967b..d9f8ca6f938025b485289b2e98cff5b00d252123 100755
--- a/Makefile
+++ b/Makefile
@@ -9,10 +9,11 @@ PROJECT_NAME=darwin_firmware
TARGET_FILES=src/cm730_fw.c
TARGET_FILES+=src/eeprom.c
TARGET_FILES+=src/system_stm32f1xx.c
-TARGET_FILES+=src/gpio.c
+#TARGET_FILES+=src/gpio.c
+TARGET_FILES+=src/darwin_sch.c
TARGET_FILES+=src/darwin_time.c
-TARGET_FILES+=src/adc_dma.c
-TARGET_FILES+=src/imu.c
+#TARGET_FILES+=src/adc_dma.c
+#TARGET_FILES+=src/imu.c
TARGET_FILES+=src/darwin_dyn_slave.c
#TARGET_FILES+=src/darwin_dyn_master.c
#TARGET_FILES+=src/darwin_dyn_master_v2.c
@@ -42,13 +43,14 @@ COMM_PATH=$(STM32_LIBRARIES_PATH)/comm
MEMORY_PATH=$(STM32_LIBRARIES_PATH)/memory
USART_PATH=$(STM32_LIBRARIES_PATH)/f1/usart
DYNAMIXEL_PATH=$(STM32_LIBRARIES_PATH)/dynamixel_base
+SCHEDULER_PATH=$(STM32_LIBRARIES_PATH)/scheduler
BUILD_PATH=build
COMPILE_OPTS = -mlittle-endian -mcpu=cortex-m3 -mthumb -mthumb-interwork
COMPILE_OPTS += -Wall -O2 -fno-common -msoft-float -DUSE_HAL_DRIVER
-COMPILE_OPTS += -ffreestanding -nostdlib -D$(PROCESSOR_MACRO)
+COMPILE_OPTS += -ffreestanding -nostdlib -D$(PROCESSOR_MACRO) -DHSE_VALUE=8000000 -imacros ./include/darwin_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$(MEMORY_PATH)/include -I$(DYNAMIXEL_PATH)/include -I$(USART_PATH)/include -I./include
+INCLUDE_DIRS += -I$(UTILS_PATH)/include -I$(COMM_PATH)/include -I$(MEMORY_PATH)/include -I$(DYNAMIXEL_PATH)/include -I$(USART_PATH)/include -I$(SCHEDULER_PATH)/include -I./include
DOC_DIR = ./doc
@@ -98,7 +100,11 @@ TARGET_FILES+=$(USART_PATH)/src/usart1_remap.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+=$(DYNAMIXEL_PATH)/src/dynamixel2.c
+TARGET_FILES+=$(DYNAMIXEL_PATH)/src/dynamixel.c
+TARGET_FILES+=$(DYNAMIXEL_PATH)/src/dynamixel_slave.c
+TARGET_FILES+=$(DYNAMIXEL_PATH)/src/dynamixel_slave_device.c
+TARGET_FILES+=$(wildcard $(SCHEDULER_PATH)/src/*.c)
DARWIN_OBJS_TMP = $(notdir $(TARGET_FILES:.c=.o))
DARWIN_OBJS = $(patsubst %,$(BUILD_PATH)/%,$(DARWIN_OBJS_TMP))
@@ -124,6 +130,8 @@ $(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: $(SCHEDULER_PATH)/src/%.c
+ $(CC) -c $(CFLAGS) -o $@ $<
$(MAIN_OUT_ELF): mkdir_build $(DARWIN_OBJS) $(BUID_PATH)/$(STARTUP_FILE:.s=.o)
$(LD) $(LDFLAGS) $(DARWIN_OBJS) $(BUILD_PATH)/$(STARTUP_FILE:.s=.o) $(EXT_LIB) -lm --output $@
diff --git a/include/darwin_conf.h b/include/darwin_conf.h
index 577308c12f14edc0828bdca1a295743d07161015..44ebbf8622d2d392c3cfbae49e8ccedf0e091c15 100644
--- a/include/darwin_conf.h
+++ b/include/darwin_conf.h
@@ -1,23 +1,24 @@
#ifndef _DARWIN_CONF_H
#define _DARWIN_CONF_H
-#define RAM_SIZE 1024
-#define EEPROM_SIZE 256
+#define RAM_SIZE 1024
+#define EEPROM_SIZE 256
/* 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_DEVICE_MODEL 0x001D
#define DEFAULT_FIRMWARE_VERSION 0x0001
-#define DEFAULT_DEVICE_ID 0x0001
-#define DEFAULT_BAUDRATE 0x0010
+#define DEFAULT_DEVICE_ID 0x0003
+#define DEFAULT_BAUDRATE 0x0022
#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
+#define MAX_NUM_SLAVE_DEVICES 4
/* GPIO configuration */
#define RAM_GPIO_BASE_ADDRESS ((unsigned short int)0x0100)
diff --git a/include/darwin_dyn_slave.h b/include/darwin_dyn_slave.h
index 49437d6860ecfdfe7e5548e671d0b517c9adf31d..493b39530e1e93df9a0169bae902f329accc9fd5 100644
--- a/include/darwin_dyn_slave.h
+++ b/include/darwin_dyn_slave.h
@@ -7,10 +7,12 @@ extern "C" {
#include "stm32f1xx.h"
#include "dynamixel_slave.h"
+#include "dynamixel_slave_device.h"
#include "dynamixel_slave_registers.h"
+#include "scheduler.h"
#include "memory.h"
-uint8_t darwin_dyn_slave_init(TMemory *memory);
+uint8_t darwin_dyn_slave_init(TMemory **memory,TScheduler *scheduler);
void darwin_dyn_slave_start(void);
void darwin_dyn_slave_stop(void);
diff --git a/include/eeprom_init.h b/include/eeprom_init.h
index 09eca5c7ead93c546ad246a4cf536853dbc8d9af..0f1baf36750d925815fdc5a6c47f95a76bdfab9f 100644
--- a/include/eeprom_init.h
+++ b/include/eeprom_init.h
@@ -5,17 +5,11 @@
extern "C" {
#endif
-#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 //7800us
#define DEFAULT_BAL_KNEE_GAIN 0x4CCD // 0.3 in fixed point format 0|16
#define DEFAULT_BAL_ANKLE_ROLL_GAIN 0xFFFF // 0.99999
#define DEFAULT_BAL_ANKLE_PITCH_GAIN 0xE666 // 0.9
#define DEFAULT_BAL_HIP_ROLL_GAIN 0x8000 // 0.5
-#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
@@ -110,8 +104,6 @@ extern "C" {
#define DEFAULT_STAIRS_Y_SPREAD 0x0014 // 20 mm
#define DEFAULT_STAIRS_X_SHIFT_BODY 0x0023 // 35 mm
-#define EEPROM_SIZE 137
-
#ifdef __cplusplus
}
#endif
diff --git a/src/cm730_fw.c b/src/cm730_fw.c
index 859d26a6bf135af392808c3fa6e01c928759f95c..326cbe94588553ddb046f74b1512287e9b9d9947 100755
--- a/src/cm730_fw.c
+++ b/src/cm730_fw.c
@@ -1,43 +1,44 @@
#include "stm32f1xx_hal.h"
#include "darwin_conf.h"
#include "memory.h"
-#include "gpio.h"
-#include "eeprom.h"
-#include "adc_dma.h"
-#include "imu.h"
-#include "darwin_time.h"
+#include "darwin_sch.h"
#include "darwin_dyn_slave.h"
-#include "darwin_dyn_master.h"
-#include "darwin_dyn_master_v2.h"
-#include "motion_manager.h"
-#include "action.h"
-#include "action_id.h"
+#include "darwin_time.h"
+#include "eeprom.h"
+//#include "gpio.h"
+//#include "adc_dma.h"
+//#include "imu.h"
+//#include "darwin_dyn_master.h"
+//#include "darwin_dyn_master_v2.h"
+//#include "motion_manager.h"
+//#include "action.h"
+//#include "action_id.h"
-TMemory darwin_memory;
+TMemory *darwin_memory;
+TScheduler *scheduler;
int main(void)
{
- uint16_t eeprom_data,period;
+// uint16_t eeprom_data,period;
/* initialize the HAL module */
HAL_Init();
/* initialize EEPROM */
EE_Init();
- /* initialize memory structure */
- mem_init(&darwin_memory);
- darwin_memory.eeprom_write_data=EE_WriteVariable;
- darwin_memory.eeprom_read_data=EE_ReadVariable;
+ /* initialize the scheduler */
+ scheduler=darwin_sch_init();
/* initialize the GPIO module */
- gpio_init(&darwin_memory);
+// gpio_init(&darwin_memory);
// initialize adc
- adc_init(&darwin_memory);
+// adc_init(&darwin_memory);
// initialize imu
- imu_init(&darwin_memory);
+// imu_init(&darwin_memory);
// initialize time module
-// darwin_time_init();
+ darwin_time_init();
/* initialize the dynamixel slave interface */
-// darwin_dyn_slave_init();
-// darwin_dyn_slave_start();
+ darwin_dyn_slave_init(&darwin_memory,scheduler);
+ darwin_memory->eeprom_write_data=EE_WriteVariable;
+ darwin_memory->eeprom_read_data=EE_ReadVariable;
/* initialize motion manager module */
// EE_ReadVariable(MM_PERIOD_OFFSET,&eeprom_data);
// period=eeprom_data&0x00FF;
@@ -46,7 +47,9 @@ int main(void)
// manager_init(period);
// gpio_set_led(LED_4);
/* initialize the ram module */
- ram_init(&darwin_memory);
+ ram_init(darwin_memory);
+
+ darwin_dyn_slave_start();
while(1);/* main function does not return */
}
diff --git a/src/darwin_dyn_slave.c b/src/darwin_dyn_slave.c
index eb680dbfe896a92a801e7f72a136ed8a4553b8d0..09c22bc7de4c1b161e080f2d0b1a01071e5f7ab3 100755
--- a/src/darwin_dyn_slave.c
+++ b/src/darwin_dyn_slave.c
@@ -3,22 +3,20 @@
#include "usart3.h"
#include "ram.h"
-/* timer for the execution of the dynamixel slave loop */
-#define DYN_SLAVE_TIMER TIM7
-#define DYN_SLAVE_TIMER_IRQn TIM7_IRQn
-#define DYN_SLAVE_TIMER_IRQHandler TIM7_IRQHandler
-#define DYN_SLAVE_TIMER_ENABLE_CLK __HAL_RCC_TIM7_CLK_ENABLE()
-
/* private variables */
TDynamixelSlave darwin_dyn_slave;
+TDynamixelSlaveDevice darwin_dyn_slave_device;
TTime darwin_dyn_slave_timer;
TComm darwin_dyn_slave_comm;
UART_InitTypeDef darwin_comm_init;
-TIM_HandleTypeDef darwin_dyn_slave_tim_handle;
+TScheduler *darwin_dyn_slave_sch;
/* memory module */
TMemory *darwin_dyn_slave_memory;
+/* eeprom data */
+dyn_slave_control_eeprom_data(darwin_dyn_slave,".eeprom",EEPROM_DYN_SLAVE_BASE_ADDRESS1,EEPROM_DYN_SLAVE_BASE_ADDRESS2);
+
// private functions
unsigned char darwin_on_read(unsigned short int address,unsigned short int length,unsigned char *data)
{
@@ -38,21 +36,8 @@ void darwin_on_ping(void)
{
}
-/* interrupt service routines */
-void DYN_SLAVE_TIMER_IRQHandler(void)
-{
- if(__HAL_TIM_GET_FLAG(&darwin_dyn_slave_tim_handle, TIM_FLAG_UPDATE) != RESET)
- {
- if(__HAL_TIM_GET_IT_SOURCE(&darwin_dyn_slave_tim_handle, TIM_IT_UPDATE) !=RESET)
- {
- __HAL_TIM_CLEAR_IT(&darwin_dyn_slave_tim_handle, TIM_IT_UPDATE);
- dyn_slave_loop(&darwin_dyn_slave);
- }
- }
-}
-
// public functions
-uint8_t darwin_dyn_slave_init(TMemory *memory)
+uint8_t darwin_dyn_slave_init(TMemory **memory,TScheduler *scheduler)
{
TUSART_IRQ_Priorities priorities;
uint8_t status;
@@ -62,7 +47,7 @@ uint8_t darwin_dyn_slave_init(TMemory *memory)
/* initialize the comm object */
comm_init(&darwin_dyn_slave_comm,0x01,&darwin_dyn_slave_timer);
- darwin_comm_init.BaudRate = 2000000/(ram_data[BAUDRATE]+1);
+ darwin_comm_init.BaudRate = 2000000/(ram_data[DEFAULT_BAUDRATE]+1);
darwin_comm_init.WordLength = UART_WORDLENGTH_8B;
darwin_comm_init.StopBits = UART_STOPBITS_1;
darwin_comm_init.Parity = UART_PARITY_NONE;
@@ -78,36 +63,30 @@ uint8_t darwin_dyn_slave_init(TMemory *memory)
priorities.dma_tx_subpriority=0;
usart3_init(&darwin_dyn_slave_comm,&darwin_comm_init,&priorities);
- status=dyn_slave_init(&darwin_dyn_slave,memory,&darwin_dyn_slave_comm,ram_data[DEVICE_ID],DYN_VER2);
- darwin_dyn_slave_memory=memory;
- darwin_dyn_slave.on_read=darwin_on_read;
- darwin_dyn_slave.on_write=darwin_on_write;
- darwin_dyn_slave.on_ping=darwin_on_ping;
- dyn_slave_set_return_delay(&darwin_dyn_slave,ram_data[RETURN_DELAY]);
- dyn_slave_set_return_level(&darwin_dyn_slave,ram_data[RETURN_LEVEL]);
-
- /* initialize timer for the execution of the dynamixel slave loop */
- DYN_SLAVE_TIMER_ENABLE_CLK;
- darwin_dyn_slave_tim_handle.Instance=DYN_SLAVE_TIMER;
- darwin_dyn_slave_tim_handle.Init.Period = 1000;
- darwin_dyn_slave_tim_handle.Init.Prescaler = 72;
- darwin_dyn_slave_tim_handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
- darwin_dyn_slave_tim_handle.Init.CounterMode = TIM_COUNTERMODE_UP;
- HAL_TIM_Base_Init(&darwin_dyn_slave_tim_handle);
- // initialize the timer interrupts
- HAL_NVIC_SetPriority(DYN_SLAVE_TIMER_IRQn, 2, 2);
- HAL_NVIC_EnableIRQ(DYN_SLAVE_TIMER_IRQn);
+ // initialize slave device
+ dyn_slave_device_init(&darwin_dyn_slave_device,DEFAULT_DEVICE_ID,EEPROM_DYN_SLAVE_BASE_ADDRESS1,EEPROM_DYN_SLAVE_BASE_ADDRESS2);
+ darwin_dyn_slave_device.on_read=darwin_on_read;
+ darwin_dyn_slave_device.on_write=darwin_on_write;
+ darwin_dyn_slave_device.on_ping=darwin_on_ping;
+ dyn_slave_device_set_return_delay(&darwin_dyn_slave_device,DEFAULT_RETURN_DELAY);
+ dyn_slave_device_set_return_level(&darwin_dyn_slave_device,DEFAULT_RETURN_LEVEL);
+ (*memory)=dyn_slave_device_get_memory(&darwin_dyn_slave_device);
+ darwin_dyn_slave_memory=dyn_slave_device_get_memory(&darwin_dyn_slave_device);
+
+ status=dyn_slave_init(&darwin_dyn_slave,&darwin_dyn_slave_comm,scheduler,SCHED_CH1,DYN_VER2);
+ darwin_dyn_slave.set_baudrate=usart3_set_baudrate;
+ dyn_slave_add_device(&darwin_dyn_slave,&darwin_dyn_slave_device);
return status;
}
void darwin_dyn_slave_start(void)
{
- HAL_TIM_Base_Start_IT(&darwin_dyn_slave_tim_handle);
+ dyn_slave_start(&darwin_dyn_slave);
}
void darwin_dyn_slave_stop(void)
{
- HAL_TIM_Base_Stop_IT(&darwin_dyn_slave_tim_handle);
+ dyn_slave_stop(&darwin_dyn_slave);
}