Skip to content
Snippets Groups Projects
Commit 4128130b authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Changes to use the new implementation of the dynamixel slave interface.

parent 055e4fbb
No related branches found
No related tags found
1 merge request!5Dynamixel manager
Hide whitespace changes
Inline Side-by-side
Makefile
+ 14
− 6
View file @ 4128130b
......@@ -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 $@
......
include/darwin_conf.h
+ 6
− 5
View file @ 4128130b
#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)
......
include/darwin_dyn_slave.h
+ 3
− 1
View file @ 4128130b
......@@ -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);
......
include/eeprom_init.h
+ 0
− 8
View file @ 4128130b
......@@ -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
......
src/cm730_fw.c
+ 26
− 23
View file @ 4128130b
#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 */
}
......
src/darwin_dyn_slave.c
+ 22
− 43
View file @ 4128130b
......@@ -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);
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment