diff --git a/Makefile b/Makefile
index a13c7dae31d512b764d3a622d093d30779061e86..8499dc1db7486f8a13be612e477d253d48b3418f 100755
--- a/Makefile
+++ b/Makefile
@@ -9,7 +9,7 @@ include ../../STM32_processor/libraries/f4/select_processor.mk
STM32_LIB_PATH = ../../STM32_processor/libraries/f4/stm32_lib
STM32_DSP_LIB_PATH = ../../STM32_processor/libraries/f4/stm32_dsp_lib
-DYNAMIXEL_LIB_PATH = ../../STM32_processor/libraries/f4/dynamixel
+DYNAMIXEL_LIB_PATH = ../../STM32_processor/libraries
STM32_STARTUP_FILES_PATH = ../../STM32_processor/startup/f4
STM32_LINKER_SCRIPTS_PATH = ../../STM32_processor/libraries/f4/linker_scripts
BUILD_PATH=build
@@ -18,7 +18,7 @@ COMPILE_OPTS = -mlittle-endian -mcpu=cortex-m4 -mthumb -mthumb-interwork
COMPILE_OPTS += -Wall -g -O2 -fno-common
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
-INCLUDE_DIRS = -I$(STM32_LIB_PATH)/include -I$(STM32_LIB_PATH)/include/core -I$(STM32_LIB_PATH)/include/devices -I$(STM32_DSP_LIB_PATH)/include -I${DYNAMIXEL_LIB_PATH}/include -I./include
+INCLUDE_DIRS = -I$(STM32_LIB_PATH)/include -I$(STM32_LIB_PATH)/include/core -I$(STM32_LIB_PATH)/include/devices -I$(STM32_DSP_LIB_PATH)/include -I${DYNAMIXEL_LIB_PATH}/include/dynamixel -I./include
LIBRARY_DIRS = -L$(STM32_LIB_PATH)/lib -L$(STM32_DSP_LIB_PATH)/lib -L$(DYNAMIXEL_LIB_PATH)/lib -L/usr/arm-none-eabi/lib/fpu/
TCHAIN_PREFIX=arm-none-eabi-
@@ -56,7 +56,7 @@ $(BUILD_PATH)/%.o: src/%.c
$(CC) -c $(CFLAGS) -o $@ $<
$(MAIN_OUT_ELF): mkdir_build $(BIOLOID_OBJS) $(BUID_PATH)/$(STARTUP_FILE:.s=.o) $(STM32_LIB_PATH)/lib/$(LIBRARY)
- $(LD) $(LDFLAGS) $(BIOLOID_OBJS) $(BUILD_PATH)/$(STARTUP_FILE:.s=.o) $(STM32_LIB_PATH)/lib/$(LIBRARY) $(STM32_DSP_LIB_PATH)/lib/libstm32f4_dsp.a ${DYNAMIXEL_LIB_PATH}/lib/dynamixel.a -lm --output $@
+ $(LD) $(LDFLAGS) $(BIOLOID_OBJS) $(BUILD_PATH)/$(STARTUP_FILE:.s=.o) $(STM32_LIB_PATH)/lib/$(LIBRARY) $(STM32_DSP_LIB_PATH)/lib/libstm32f4_dsp.a ${DYNAMIXEL_LIB_PATH}/lib/dynamixel_hard.a -lm --output $@
$(MAIN_OUT_HEX): $(MAIN_OUT_ELF)
$(OBJCP) $(OBJCPFLAGS_HEX) $< $@
diff --git a/include/comm.h b/include/comm.h
new file mode 100644
index 0000000000000000000000000000000000000000..1e8c7b8292bc8b1808ac192b4205707a2e0098df
--- /dev/null
+++ b/include/comm.h
@@ -0,0 +1,10 @@
+#ifndef _COMM_H
+#define _COMM_H
+
+#include "stm32f4xx.h"
+
+// public functions
+void comm_init(void);
+void comm_start(void);
+
+#endif
diff --git a/include/dynamixel_master_uart_dma.h b/include/dynamixel_master_uart_dma.h
index 41b651422fdfe979e25c933fb628e97b3c2e65e5..74763f75ea18c811eef55ef12eec2013a443a169 100755
--- a/include/dynamixel_master_uart_dma.h
+++ b/include/dynamixel_master_uart_dma.h
@@ -3,25 +3,38 @@
#include "stm32f4xx.h"
#include "dynamixel.h"
+#include "dynamixel2.h"
// dynamixel master functions
void dyn_master_init(void);
-void dyn_master_flush(void);
+void dyn_master_reset(void);
void dyn_master_set_timeout(uint16_t timeout_ms);
void dyn_master_scan(uint8_t *num,uint8_t *ids);
+void dyn2_master_scan(uint8_t *num,uint8_t *ids);
inline void dyn_master_enable_power(void);
inline void dyn_master_disable_power(void);
uint8_t dyn_master_is_power_enabled(void);
uint8_t dyn_master_ping(uint8_t id);
+uint8_t dyn2_master_ping(uint8_t id);
uint8_t dyn_master_read_byte(uint8_t id,uint8_t address,uint8_t *data);
+uint8_t dyn2_master_read_byte(uint8_t id,uint16_t address,uint8_t *data);
uint8_t dyn_master_read_word(uint8_t id,uint8_t address,uint16_t *data);
+uint8_t dyn2_master_read_word(uint8_t id,uint16_t address,uint16_t *data);
uint8_t dyn_master_read_table(uint8_t id,uint8_t address,uint8_t length,uint8_t *data);
+uint8_t dyn2_master_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_t *data);
uint8_t dyn_master_write_byte(uint8_t id, uint8_t address, uint8_t data);
+uint8_t dyn2_master_write_byte(uint8_t id, uint16_t address, uint8_t data);
uint8_t dyn_master_write_word(uint8_t id, uint8_t address, uint16_t data);
+uint8_t dyn2_master_write_word(uint8_t id, uint16_t address, uint16_t data);
uint8_t dyn_master_write_table(uint8_t id, uint8_t address, uint8_t length, uint8_t *data);
+uint8_t dyn2_master_write_table(uint8_t id, uint16_t address, uint16_t length, uint8_t *data);
uint8_t dyn_master_reg_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *data);
-void dyn_master_action(void);
-void dyn_master_sync_write(uint8_t num,uint8_t *ids,uint8_t address, uint8_t length, uint8_t *data);
+uint8_t dyn2_master_reg_write(uint8_t id, uint16_t address, uint16_t length, uint8_t *data);
+uint8_t dyn_master_action(void);
+uint8_t dyn2_master_action(void);
+uint8_t dyn_master_sync_write(uint8_t num,uint8_t *ids,uint8_t address, uint8_t length, TWriteData *data);
+uint8_t dyn2_master_sync_write(uint8_t num,uint8_t *ids,uint16_t address, uint16_t length, TWriteData *data);
+uint8_t dyn_master_bulk_read(uint8_t num,uint8_t *ids,uint8_t *address, uint8_t *length, TWriteData *data);
// repeater functions
uint8_t dyn_master_resend_inst_packet(uint8_t *inst_packet, uint8_t *status_packet);
diff --git a/include/ram.h b/include/ram.h
index 485bf26b1bd8aebb1aeebd3a517eec875c457763..b0fb2f38647139114d271535c28eac76d3f8418d 100644
--- a/include/ram.h
+++ b/include/ram.h
@@ -17,7 +17,8 @@ typedef enum {
BIOLOID_ID = DEVICE_ID_OFFSET,
BIOLOID_BAUD_RATE = BAUDRATE_OFFSET,
BIOLOID_RETURN_DELAY_TIME = RETURN_DELAY_OFFSET,
- BIOLOID_MM_PERIOD = MM_PERIOD_OFFSET,
+ BIOLOID_MM_PERIOD_L = MM_PERIOD_OFFSET,
+ BIOLOID_MM_PERIOD_H = MM_PERIOD_OFFSET+1,
BIOLOID_RETURN_LEVEL = RETURN_LEVEL_OFFSET,
BIOLOID_DXL_POWER = 0x18,
BIOLOID_LED = 0x19,
@@ -127,14 +128,14 @@ typedef enum {
BIOLOID_ACTION_PAGE = 0x81,
BIOLOID_ACTION_CONTROL = 0x82,
BIOLOID_ACTION_STATUS = 0x83,
- BIOLOID_MM_PELVIS_ROLL_L = 0x84,
- BIOLOID_MM_PELVIS_ROLL_R = 0x85,
- BIOLOID_MM_ANKLE_ROLL_L = 0x86,
- BIOLOID_MM_ANKLE_ROLL_R = 0x87,
- BIOLOID_MM_ANKLE_PITCH_L = 0x88,
- BIOLOID_MM_ANKLE_PITCH_R = 0x89,
- BIOLOID_MM_KNEE_L = 0x8A,
- BIOLOID_MM_KNEE_R = 0x8B
+ BIOLOID_MM_PELVIS_ROLL_GAIN_L = 0x84,
+ BIOLOID_MM_PELVIS_ROLL_GAIN_R = 0x85,
+ BIOLOID_MM_ANKLE_ROLL_GAIN_L = 0x86,
+ BIOLOID_MM_ANKLE_ROLL_GAIN_R = 0x87,
+ BIOLOID_MM_ANKLE_PITCH_GAIN_L = 0x88,
+ BIOLOID_MM_ANKLE_PITCH_GAIN_R = 0x89,
+ BIOLOID_MM_KNEE_GAIN_L = 0x8A,
+ BIOLOID_MM_KNEE_GAIN_R = 0x8B
} bioloid_registers;
#define RAM_SIZE 256
diff --git a/include/time.h b/include/time.h
index 74f51f2d0d5b1624f5fe86827db7c4db7bb20215..365b6663a0dfa279d807047263be25cb3e37f0c0 100755
--- a/include/time.h
+++ b/include/time.h
@@ -6,5 +6,6 @@
void time_init(void);
void delay_ms(__IO uint32_t time);
+void delay_us(uint32_t us);
#endif
diff --git a/linker_script/STM32F4_1024FLASH_192RAM.ld b/linker_script/STM32F4_1024FLASH_192RAM.ld
index 73182b00d9d292c59164baa5873c626f3b084add..818b7c4effa8498f31a11360c3645973c2ef7132 100644
--- a/linker_script/STM32F4_1024FLASH_192RAM.ld
+++ b/linker_script/STM32F4_1024FLASH_192RAM.ld
@@ -42,8 +42,8 @@ MEMORY
{
FLASH1 (rx) : ORIGIN = 0x08000000, LENGTH = 16K
EEPROM (rw) : ORIGIN = 0x08004000, LENGTH = 32K
-FLASH2 (rx) : ORIGIN = 0x0800C000, LENGTH = 848K
-PAGES (r) : ORIGIN = 0x080E0000, LENGTH = 128K
+PAGES (r) : ORIGIN = 0x0800C000, LENGTH = 128K
+FLASH2 (rx) : ORIGIN = 0x0802C000, LENGTH = 848K
RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K
CCMRAM (rw) : ORIGIN = 0x10000000, LENGTH = 64K
}
diff --git a/src/bioloid_stm32.c b/src/bioloid_stm32.c
index 727737562acd0291a324b6c39a2fa9cf135fa038..81b5f694ee79e66cfa1109e5e7373f0e4551ed13 100644
--- a/src/bioloid_stm32.c
+++ b/src/bioloid_stm32.c
@@ -13,116 +13,10 @@
#include "motion_pages.h"
#include "motion_manager.h"
#include "action.h"
-
-uint8_t read_operation(uint8_t address, uint8_t length, uint8_t *data)
-{
- uint8_t error;
-
- // pre-read operation
- // read operation
- error=ram_read_table(address,length,data);
- // post-read operation
- if(ram_in_range(BIOLOID_PUSHBUTTON,BIOLOID_PUSHBUTTON,address,length))
- ram_data[BIOLOID_PUSHBUTTON]=0x00;
-
- return error;
-}
-
-uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data)
-{
- uint8_t error,byte_value,i,j,do_op=0x00;
- uint16_t word_value;
-
- // pre-write operation
- if(ram_in_range(BIOLOID_DXL_POWER,BIOLOID_DXL_POWER,address,length))
- {
- if(data[BIOLOID_DXL_POWER-address]&0x01) dyn_master_enable_power();
- else dyn_master_disable_power();
- }
- if(ram_in_range(BIOLOID_LED,BIOLOID_LED,address,length))
- {
- byte_value=data[BIOLOID_LED-address];
- if(byte_value&0x01)
- gpio_set_led(NORTH_LED);
- else
- gpio_clear_led(NORTH_LED);
- if(byte_value&0x02)
- gpio_set_led(SOUTH_LED);
- else
- gpio_clear_led(SOUTH_LED);
- if(byte_value&0x04)
- gpio_set_led(EAST_LED);
- else
- gpio_clear_led(EAST_LED);
- if(byte_value&0x08)
- gpio_set_led(WEST_LED);
- else
- gpio_clear_led(WEST_LED);
- }
- if(ram_in_range(BIOLOID_MM_PERIOD,BIOLOID_MM_PERIOD+1,address,length))
- {
- word_value=data[BIOLOID_MM_PERIOD-address]+(data[BIOLOID_MM_PERIOD+1-address]<<8);
- action_set_period(word_value);
- word_value=(word_value*1000000)>>16;
- manager_set_period(word_value);
- }
- for(i=BIOLOID_MM_MODULE_EN0,j=0;i<=BIOLOID_MM_MODULE_EN15;i++,j+=2)
- {
- do_op=0x01;
- if(ram_in_range(i,i,address,length))
- {
- byte_value=data[i-address];
- if(byte_value&0x80) manager_enable_servo(j);
- else manager_disable_servo(j);
- manager_set_module(j,(byte_value&0x70)>>4);
- if(byte_value&0x08) manager_enable_servo(j+1);
- else manager_disable_servo(j+1);
- manager_set_module(j+1,byte_value&0x07);
- }
- }
- if(do_op)
- {
- do_op=0x00;
- manager_disable_servos();
- }
- if(ram_in_range(BIOLOID_IMU_CONTROL,BIOLOID_IMU_CONTROL,address,length))
- {
- if(data[BIOLOID_IMU_CONTROL-address]&0x01)
- imu_start();
- else
- imu_stop();
- }
- if(ram_in_range(BIOLOID_MM_CONTROL,BIOLOID_MM_CONTROL,address,length))
- {
- if(data[BIOLOID_MM_CONTROL-address]&0x01)
- manager_enable();
- else
- manager_disable();
- }
- if(ram_in_range(BIOLOID_ACTION_CONTROL,BIOLOID_ACTION_CONTROL,address,length))
- {
- if(data[BIOLOID_ACTION_CONTROL-address]&0x01)
- action_start_page();
- if(data[BIOLOID_ACTION_CONTROL-address]&0x02)
- action_stop_page();
- }
- if(ram_in_range(BIOLOID_ACTION_PAGE,BIOLOID_ACTION_PAGE,address,length))// load the page identifier
- action_load_page(data[BIOLOID_ACTION_PAGE-address]);
- // write eeprom
- for(i=address,j=0;i<LAST_EEPROM_OFFSET && i<(address+length);i++,j++)
- EE_WriteVariable(i,data[j]);
- // write operation
- error=ram_write_table(address,length,data);
- // post-write operation
-
- return error;
-}
+#include "comm.h"
int32_t main(void)
{
- uint8_t inst_packet[1024];
- uint8_t status_packet[1024];
- uint8_t data[1024],error,length;
uint16_t eeprom_data,i,num_errors=0;
uint16_t action_period,mm_period;
TPage page;
@@ -136,8 +30,8 @@ int32_t main(void)
/* initialize the gpio */
gpio_init();
/* initialize the dynamixel master interface */
- dyn_master_init();
- dyn_master_set_timeout(20);
+// dyn_master_init();
+// dyn_master_set_timeout(20);
/* initialize the dynamixel slave interface*/
dyn_slave_init();
EE_ReadVariable(DEVICE_ID_OFFSET,&eeprom_data);
@@ -147,18 +41,21 @@ int32_t main(void)
EE_ReadVariable(RETURN_LEVEL_OFFSET,&eeprom_data);
dyn_slave_set_return_level((uint8_t)eeprom_data);
/* initialize the IMU */
- imu_init();
+// imu_init();
// initialize the Analog to digital converter
// adc_init();
// initialize motion manager
- EE_ReadVariable(MM_PERIOD_OFFSET,&eeprom_data);
- action_period=eeprom_data&0x00FF;
- EE_ReadVariable(MM_PERIOD_OFFSET+1,&eeprom_data);
- action_period+=((eeprom_data&0x00FF)<<8);
- mm_period=(action_period*1000000)>>16;
- manager_init(mm_period);
+// EE_ReadVariable(MM_PERIOD_OFFSET,&eeprom_data);
+// action_period=eeprom_data&0x00FF;
+// EE_ReadVariable(MM_PERIOD_OFFSET+1,&eeprom_data);
+// action_period+=((eeprom_data&0x00FF)<<8);
+// mm_period=(action_period*1000000)>>16;
+// manager_init(mm_period);
// initialize the action module
- action_init(action_period);
+// action_init(action_period);
+ comm_init();
+
+ comm_start();
gpio_blink_led(NORTH_LED,1000);
@@ -171,65 +68,6 @@ int32_t main(void)
if(num_errors==0)
gpio_set_led(SOUTH_LED);
- while(1) /* main function does not return */
- {
- if(dyn_slave_is_packet_ready())// check if a new instruction packet has been received
- {
- dyn_slave_get_inst_packet(inst_packet);// get the received packet
- // check the packet checksum
- if(dyn_check_checksum(inst_packet)==0xFF)// the incomming packet is okay
- {
- // check address
- if(dyn_get_id(inst_packet)==dyn_slave_get_address())// the packet is addressed to this device
- {
- // process the packet
- switch(dyn_get_instruction(inst_packet))
- {
- case DYN_PING: dyn_slave_send_status_packet(DYN_NO_ERROR,0,data);
- break;
- case DYN_READ: error=read_operation(dyn_get_read_address(inst_packet),dyn_get_read_length(inst_packet),data);
- if(dyn_slave_get_return_level()!=0)
- {
- if(error==RAM_SUCCESS)
- dyn_slave_send_status_packet(DYN_NO_ERROR,dyn_get_read_length(inst_packet),data);
- else
- dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
- }
- break;
- case DYN_WRITE: length=dyn_get_write_data(inst_packet,data);
- if(length!=DYN_BAD_FORMAT)
- error=write_operation(dyn_get_write_address(inst_packet),length,data);
- if(dyn_slave_get_return_level()==2)
- {
- if(error==RAM_SUCCESS || length==DYN_BAD_FORMAT)
- dyn_slave_send_status_packet(DYN_NO_ERROR,0,data);
- else
- dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
- }
-
- break;
- default:
- break;
- }
- }
- else// the packet is addressed to another device
- {
- // send the incomming packet to the dynamixel bus
- if(dyn_master_resend_inst_packet(inst_packet,status_packet)==DYN_NO_ERROR)
- {
- // send the answer back to the computer
- dyn_slave_resend_status_packet(status_packet);
- }
- else
- dyn_slave_reset();// prepare the dynamixel master to accept data
- }
- }
- else
- {
- // send a checksum error answer
- dyn_slave_send_status_packet(DYN_CHECKSUM_ERROR,0,0x00);
- }
- }
- }
+ while(1);
}
diff --git a/src/comm.c b/src/comm.c
new file mode 100644
index 0000000000000000000000000000000000000000..f96d8c08768ae007437a032ade8d9f9f8a8d0247
--- /dev/null
+++ b/src/comm.c
@@ -0,0 +1,287 @@
+#include "comm.h"
+#include "gpio.h"
+#include "ram.h"
+#include "motion_manager.h"
+#include "action.h"
+#include "dynamixel_slave_uart_dma.h"
+#include "dynamixel_master_uart_dma.h"
+#include "adc_dma.h"
+
+#define COMM_TIMER TIM7
+#define COMM_TIMER_IRQn TIM7_IRQn
+#define COMM_TIMER_IRQHandler TIM7_IRQHandler
+#define COMM_TIMER_CLK RCC_APB1Periph_TIM7
+
+// private variables
+uint8_t inst_packet[MAX_DATA_LENGTH];
+uint8_t status_packet[MAX_DATA_LENGTH];
+uint8_t data[MAX_DATA_LENGTH];
+// registered write operation variables
+uint8_t reg_data[MAX_DATA_LENGTH];
+uint8_t reg_address,reg_length,reg_op;
+
+// private functions
+uint8_t read_operation(uint8_t address, uint8_t length, uint8_t *data)
+{
+ uint8_t error;
+
+ // pre-read operation
+ // read operation
+ error=ram_read_table(address,length,data);
+ // post-read operation
+ if(ram_in_range(BIOLOID_PUSHBUTTON,BIOLOID_PUSHBUTTON,address,length))
+ ram_data[BIOLOID_PUSHBUTTON]=0x00;
+
+ return error;
+}
+
+uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data)
+{
+ uint8_t error=RAM_SUCCESS,byte_value,i,j,do_op=0x00,module;
+ uint16_t word_value;
+
+ // pre-write operation
+ if(ram_in_range(BIOLOID_DXL_POWER,BIOLOID_DXL_POWER,address,length))
+ {
+ if(data[BIOLOID_DXL_POWER-address]&0x01) dyn_master_enable_power();
+ else dyn_master_disable_power();
+ }
+ if(ram_in_range(BIOLOID_LED,BIOLOID_LED,address,length))
+ {
+ byte_value=data[BIOLOID_LED-address];
+ if(byte_value&0x01)
+ gpio_set_led(NORTH_LED);
+ else
+ gpio_clear_led(NORTH_LED);
+ if(byte_value&0x02)
+ gpio_set_led(SOUTH_LED);
+ else
+ gpio_clear_led(SOUTH_LED);
+ if(byte_value&0x04)
+ gpio_set_led(EAST_LED);
+ else
+ gpio_clear_led(EAST_LED);
+ if(byte_value&0x08)
+ gpio_set_led(WEST_LED);
+ else
+ gpio_clear_led(WEST_LED);
+ }
+ if(ram_in_range(BIOLOID_MM_PERIOD_L,BIOLOID_MM_PERIOD_H,address,length))
+ {
+ word_value=data[BIOLOID_MM_PERIOD_L-address]+(data[BIOLOID_MM_PERIOD_H-address]<<8);
+ manager_set_period(word_value);
+ }
+ for(i=BIOLOID_MM_MODULE_EN0,j=0;i<=BIOLOID_MM_MODULE_EN15;i++,j+=2)
+ {
+ do_op=0x01;
+ if(ram_in_range(i,i,address,length))
+ {
+ byte_value=data[i-address];
+ if(byte_value&0x80) manager_enable_servo(j);
+ else manager_disable_servo(j);
+ module=(byte_value&0x70)>>4;
+ manager_set_module(j,module);
+ if(byte_value&0x08) manager_enable_servo(j+1);
+ else manager_disable_servo(j+1);
+ module=byte_value&0x07;
+ manager_set_module(j+1,module);
+ }
+ }
+ if(do_op)
+ {
+ do_op=0x00;
+ manager_disable_servos();
+ }
+ if(ram_in_range(BIOLOID_MM_CONTROL,BIOLOID_MM_CONTROL,address,length))
+ {
+ if(data[BIOLOID_MM_CONTROL-address]&0x01)
+ manager_enable();
+ else
+ manager_disable();
+ }
+/* if(ram_in_range(BIOLOID_MM_KNEE_GAIN_L,BIOLOID_MM_KNEE_GAIN_L,address,length))
+ manager_set_knee_gain(data[BIOLOID_MM_KNEE_GAIN_L-address]);
+ if(ram_in_range(BIOLOID_MM_KNEE_GAIN_R,BIOLOID_MM_KNEE_GAIN_R,address,length))
+ manager_set_knee_gain(data[BIOLOID_MM_KNEE_GAIN_R-address]);
+ if(ram_in_range(BIOLOID_MM_ANKLE_PITCH_GAIN_L,BIOLOID_MM_ANKLE_PITCH_GAIN_L,address,length))
+ manager_set_ankle_pitch_gain(data[BIOLOID_MM_ANKLE_PITCH_GAIN_L,address]);
+ if(ram_in_range(BIOLOID_MM_ANKLE_PITCH_GAIN_R,BIOLOID_MM_ANKLE_PITCH_GAIN_R,address,length))
+ manager_set_ankle_pitch_gain(data[BIOLOID_MM_ANKLE_PITCH_GAIN_R,address]);
+ if(ram_in_range(BIOLOID_MM_HIP_ROLL_GAIN_L,BIOLOID_MM_HIP_ROLL_GAIN_L,address,length))
+ manager_set_hip_roll_gain(data[BIOLOID_MM_ROLL_GAIN_L-address]);
+ if(ram_in_range(BIOLOID_MM_HIP_ROLL_GAIN_R,BIOLOID_MM_HIP_ROLL_GAIN_R,address,length))
+ manager_set_hip_roll_gain(data[BIOLOID_MM_ROLL_GAIN_R-address]);
+ if(ram_in_range(BIOLOID_MM_ANKLE_ROLL_GAIN_L,BIOLOID_MM_ANKLE_ROLL_GAIN_L,address,length))
+ manager_set_ankle_roll_gain(data[BIOLOID_MM_CONTROL_L-address]);
+ if(ram_in_range(BIOLOID_MM_ANKLE_ROLL_GAIN_R,BIOLOID_MM_ANKLE_ROLL_GAIN_R,address,length))
+ manager_set_ankle_roll_gain(data[BIOLOID_MM_CONTROL_R-address]);*/
+ if(ram_in_range(BIOLOID_ACTION_CONTROL,BIOLOID_ACTION_CONTROL,address,length))
+ {
+ if(data[BIOLOID_ACTION_CONTROL-address]&0x01)
+ action_start_page();
+ if(data[BIOLOID_ACTION_CONTROL-address]&0x02)
+ action_stop_page();
+ }
+ if(ram_in_range(BIOLOID_ACTION_PAGE,BIOLOID_ACTION_PAGE,address,length))// load the page identifier
+ action_load_page(data[BIOLOID_ACTION_PAGE-address]);
+/* if(ram_in_range(BIOLOID_IMU_CONTROL,BIOLOID_IMU_CONTROL,address,length))
+ {
+ if(data[BIOLOID_IMU_CONTROL-address]&0x01)
+ imu_start();
+ else
+ imu_stop();
+ if(data[BIOLOID_IMU_CONTROL-address]&0x02)
+ imu_start_gyro_cal();
+ }*/
+ // write eeprom
+ for(i=address,j=0;i<LAST_EEPROM_OFFSET && i<(address+length);i++,j++)
+ EE_WriteVariable(i,data[j]);
+ // write operation
+ // error=ram_write_table(address,length,data);
+ // post-write operation
+
+ return error;
+}
+
+// interrupt handlers
+void COMM_TIMER_IRQHandler(void)
+{
+ uint8_t error,length,prev_id,id;
+
+ TIM_ClearITPendingBit(COMM_TIMER,TIM_IT_Update);
+ if(dyn_slave_is_packet_ready())// check if a new instruction packet has been received
+ {
+ dyn_slave_get_inst_packet(inst_packet);// get the received packet
+ // check address
+ id=dyn_get_id(inst_packet);
+ if(id==dyn_slave_get_address() || id==DYN_BROADCAST_ID)// the packet is addressed to this device or it is a broadcast
+ {
+ // check the packet checksum
+ if(dyn_check_checksum(inst_packet)==0xFF)// the incomming packet is okay
+ {
+ // process the packet
+ switch(dyn_get_instruction(inst_packet))
+ {
+ case DYN_PING: if(id!=DYN_BROADCAST_ID)
+ dyn_slave_send_status_packet(DYN_NO_ERROR,0,data);
+ break;
+ case DYN_READ: error=read_operation(dyn_get_read_address(inst_packet),dyn_get_read_length(inst_packet),data);
+ if(dyn_slave_get_return_level()!=0 && id!=DYN_BROADCAST_ID)
+ {
+ if(error==RAM_SUCCESS)
+ dyn_slave_send_status_packet(DYN_NO_ERROR,dyn_get_read_length(inst_packet),data);
+ else
+ dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
+ }
+ break;
+ case DYN_WRITE: length=dyn_get_write_data(inst_packet,data);
+ error=write_operation(dyn_get_write_address(inst_packet),length,data);
+ if(dyn_slave_get_return_level()==2 && id!=DYN_BROADCAST_ID)
+ {
+ if(error==RAM_SUCCESS)
+ dyn_slave_send_status_packet(DYN_NO_ERROR,0,data);
+ else
+ dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
+ }
+ break;
+ case DYN_REG_WRITE: reg_length=dyn_get_reg_write_data(inst_packet,reg_data);
+ reg_address=dyn_get_reg_write_address(inst_packet);
+ reg_op=0x01;// signal there is a registered operation pending
+ if(dyn_slave_get_return_level()==2 && id!=DYN_BROADCAST_ID)
+ dyn_slave_send_status_packet(DYN_NO_ERROR,0,data);
+ break;
+ case DYN_ACTION: if(reg_op)
+ {
+ error=write_operation(reg_address,reg_length,reg_data);
+ reg_op=0x00;
+ }
+ break;
+ case DYN_RESET:
+ break;
+ case DYN_SYNC_READ: dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
+ break;
+ case DYN_SYNC_WRITE: if(dyn_sync_write_id_present(inst_packet,dyn_slave_get_address()))// the device is addressed
+ {
+ length=dyn_get_sync_write_data(inst_packet,dyn_slave_get_address(),data);
+ error=write_operation(dyn_get_sync_write_address(inst_packet),length,data);
+ }
+ break;
+ case DYN_BULK_READ: if(dyn_bulk_read_id_present(inst_packet,dyn_slave_get_address(),&prev_id))
+ {
+ length=dyn_get_bulk_read_length(inst_packet,dyn_slave_get_address());
+ error=read_operation(dyn_get_bulk_read_address(inst_packet,dyn_slave_get_address()),length,data);
+ if(prev_id==0xFF)// first device in the bulk read sequence
+ {
+ if(error==RAM_SUCCESS)
+ dyn_slave_send_status_packet(DYN_NO_ERROR,length,data);
+ else
+ dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
+ }
+ else// wait for the previous device in the sequence to send its data
+ {
+ do{
+ while(!dyn_slave_is_packet_ready());// wait until a new packet is received
+ dyn_slave_get_inst_packet(inst_packet);
+ }while(dyn_get_id(inst_packet)!=prev_id);
+ if(error==RAM_SUCCESS)
+ dyn_slave_send_status_packet(DYN_NO_ERROR,length,data);
+ else
+ dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
+ }
+ }
+ break;
+ case DYN_BULK_WRITE: dyn_slave_send_status_packet(DYN_INST_ERROR,0,data);
+ break;
+ default:
+ break;
+ }
+ }
+ else
+ {
+ // send a checksum error answer
+ if(dyn_get_id(inst_packet)!=DYN_BROADCAST_ID)
+ dyn_slave_send_status_packet(DYN_CHECKSUM_ERROR,0,0x00);
+ }
+ }
+ else// the packet is addressed to another device
+ {
+ // send the incomming packet to the dynamixel bus
+ if(dyn_master_resend_inst_packet(inst_packet,status_packet)!=DYN_TIMEOUT)
+ {
+ // send the answer back to the computer
+ dyn_slave_resend_status_packet(status_packet);
+ }
+ }
+ }
+}
+
+// public functions
+void comm_init(void)
+{
+ TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
+ NVIC_InitTypeDef NVIC_InitStructure;
+
+ RCC_APB1PeriphClockCmd(COMM_TIMER_CLK,ENABLE);
+
+ TIM_TimeBaseStructure.TIM_Period = 1000;
+ TIM_TimeBaseStructure.TIM_Prescaler = 84;
+ TIM_TimeBaseStructure.TIM_ClockDivision = 0;
+ TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+ TIM_TimeBaseInit(COMM_TIMER,&TIM_TimeBaseStructure);
+
+ NVIC_InitStructure.NVIC_IRQChannel = COMM_TIMER_IRQn;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&NVIC_InitStructure);
+
+ /* initialize variables */
+ reg_op=0x00;
+}
+
+void comm_start(void)
+{
+ TIM_Cmd(COMM_TIMER, ENABLE);
+ TIM_ITConfig(COMM_TIMER, TIM_IT_Update, ENABLE);
+}
+
diff --git a/src/dynamixel_master_uart_dma.c b/src/dynamixel_master_uart_dma.c
index e5f0e48c97e7cfc7185feac63dd6c0a5c0296031..b1f98b08cc97c2b94b5d33894f3aa50ee1ce51f4 100755
--- a/src/dynamixel_master_uart_dma.c
+++ b/src/dynamixel_master_uart_dma.c
@@ -1,6 +1,6 @@
#include "dynamixel_master_uart_dma.h"
+#include "motion_manager.h"
#include "time.h"
-#include "gpio.h"
#define USART USART2
#define USART_CLK RCC_APB1Periph_USART2
@@ -65,10 +65,10 @@
#define MAX_BUFFER_LEN 1024
// private variables
+uint8_t dyn_master_version;
uint16_t dyn_master_timeout;// answer reception timeout
// input buffer
uint8_t dyn_master_rx_buffer[MAX_BUFFER_LEN];
-volatile uint8_t dyn_master_receiving_header;
// output buffer
uint8_t dyn_master_tx_buffer[MAX_BUFFER_LEN];
// instruction packet ready flag
@@ -79,6 +79,9 @@ volatile uint8_t dyn_master_sending_packet;
uint8_t dyn_master_no_answer;
// power enabled variable
uint8_t dyn_master_power_enabled;
+// number of packets to receive
+uint8_t dyn_master_num_read_packets;
+uint8_t dyn_master_packet_length;
// DMA initialization data structures
DMA_InitTypeDef DMA_TX_InitStructure;
DMA_InitTypeDef DMA_RX_InitStructure;
@@ -86,42 +89,93 @@ DMA_InitTypeDef DMA_RX_InitStructure;
// private functions
void dyn_master_reset(void)
{
- DMA_ITConfig(USART_RX_DMA_STREAM,DMA_IT_TC,DISABLE);
+ /* Reset reception */
USART_DMACmd(USART, USART_DMAReq_Rx, DISABLE);
DMA_Cmd(USART_RX_DMA_STREAM,DISABLE);
/* wait until the transaction ends */
- while(DMA_GetCmdStatus(USART_RX_DMA_STREAM)==ENABLE);
DMA_ClearFlag(USART_RX_DMA_STREAM,USART_RX_DMA_FLAG_TCIF);
DMA_ClearITPendingBit(USART_RX_DMA_STREAM,USART_RX_DMA_FLAG_TCIF);
- dyn_master_receiving_header=0x01;
+ /* clear any pending data */
+ USART_ReceiveData(USART);
+ /* Reset transmission */
+ USART_DMACmd(USART, USART_DMAReq_Tx, DISABLE);
+ DMA_Cmd(USART_TX_DMA_STREAM,DISABLE);
+ DMA_ClearFlag(USART_TX_DMA_STREAM,USART_TX_DMA_FLAG_TCIF);
+ DMA_ClearITPendingBit(USART_TX_DMA_STREAM,USART_TX_DMA_FLAG_TCIF);
+ USART_ITConfig(USART, USART_IT_TC, DISABLE);
+ /* Reset variables */
dyn_master_packet_ready=0x00;
+ dyn_master_sending_packet=0x00;
}
-void dyn_master_send(void)
+uint8_t dyn_master_wait_sending(void)
{
+ uint16_t timeout_left=dyn_master_timeout*1000;
+
+ while(dyn_master_sending_packet)
+ {
+ delay_us(100);
+ timeout_left-=100;
+ if(timeout_left<=0)
+ {
+ dyn_master_reset();
+ return DYN_TIMEOUT;
+ }
+ }
+
+ return DYN_SUCCESS;
+}
+
+uint8_t dyn_master_send(void)
+{
+ uint8_t error;
+
+ dyn_master_version=1;
// wait until any previous transmission ends
- while(dyn_master_sending_packet);
+ if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
+ return error;
// set the DMA transfer
DMA_SetCurrDataCounter(USART_TX_DMA_STREAM,dyn_get_length(dyn_master_tx_buffer)+4);
DMA_Cmd(USART_TX_DMA_STREAM,ENABLE);
USART_ClearFlag(USART,USART_FLAG_TC);
+ USART_ClearITPendingBit(USART,USART_FLAG_TC);
USART_DMACmd(USART, USART_DMAReq_Tx, ENABLE);
dyn_master_sending_packet=0x01;
+ return error;
+
+}
+
+uint8_t dyn2_master_send(void)
+{
+ uint8_t error;
+
+ dyn_master_version=2;
+ // wait until any previous transmission ends
+ if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
+ return error;
+ // set the DMA transfer
+ DMA_SetCurrDataCounter(USART_TX_DMA_STREAM,dyn2_get_length(dyn_master_tx_buffer)+7);
+ DMA_Cmd(USART_TX_DMA_STREAM,ENABLE);
+ USART_ClearFlag(USART,USART_FLAG_TC);
+ USART_ClearITPendingBit(USART,USART_FLAG_TC);
+ USART_DMACmd(USART, USART_DMAReq_Tx, ENABLE);
+ dyn_master_sending_packet=0x01;
+ return error;
}
uint8_t dyn_master_receive(void)
{
- int16_t timeout_left=dyn_master_timeout;
+ uint16_t timeout_left=dyn_master_timeout*1000;
// wait for the status packet
while(!dyn_master_packet_ready)
{
- delay_ms(1);
- timeout_left-=1;
+ delay_us(100);
+ timeout_left-=100;
if(timeout_left<=0)
{
dyn_master_reset();
- return DYN_TIMEOUT;
+ return DYN_TIMEOUT;
}
}
dyn_master_packet_ready=0x00;
@@ -129,7 +183,36 @@ uint8_t dyn_master_receive(void)
if(dyn_check_checksum(dyn_master_rx_buffer)==0xFF)
return dyn_get_status_error(dyn_master_rx_buffer);
else
+ {
+ dyn_master_reset();
return DYN_CHECKSUM_ERROR;
+ }
+}
+
+uint8_t dyn2_master_receive(void)
+{
+ uint16_t timeout_left=dyn_master_timeout*1000;
+
+ // wait for the status packet
+ while(!dyn_master_packet_ready)
+ {
+ delay_us(100);
+ timeout_left-=100;
+ if(timeout_left<=0)
+ {
+ dyn_master_reset();
+ return DYN_TIMEOUT;
+ }
+ }
+ dyn_master_packet_ready=0x00;
+ // check the input packet checksum
+ if(dyn2_check_checksum(dyn_master_rx_buffer)==0x01)
+ return dyn2_get_status_error(dyn_master_rx_buffer);
+ else
+ {
+ dyn_master_reset();
+ return DYN_CHECKSUM_ERROR;
+ }
}
void dyn_master_enable_tx(void)
@@ -148,80 +231,169 @@ uint8_t dyn_master_read(uint8_t id,uint8_t address,uint8_t length,uint8_t *data)
{
uint8_t error;
+ /* generate the read packet */
+ dyn_init_read_packet(dyn_master_tx_buffer,id,address,length);
+ dyn_master_num_read_packets=0x01;
+ // enable transmission
+ dyn_master_enable_tx();
+ // send the data
+ if((error=dyn_master_send())!=DYN_SUCCESS)
+ {
+ dyn_master_enable_rx();
+ return error;
+ }
// wait for the transmission to end
- while(dyn_master_sending_packet);
- // generate the read packet for the desired device
- dyn_init_instruction_packet(dyn_master_tx_buffer);
- // set the ping instruction
- dyn_set_instruction(dyn_master_tx_buffer,DYN_READ);
- // set the device id
- if((error=dyn_set_id(dyn_master_tx_buffer,id))==DYN_SUCCESS)
- {
- // set the length to read
- dyn_set_read_length(dyn_master_tx_buffer,length);
- // sert the start address to start reading
- dyn_set_read_address(dyn_master_tx_buffer,address);
- // set the checksum
- dyn_set_checksum(dyn_master_tx_buffer);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- dyn_master_send();
- // wait for the transmission to end
- while(dyn_master_sending_packet);
+ if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
+ {
dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn_master_receive();
- if(error==DYN_NO_ERROR)
- dyn_get_status_data(dyn_master_rx_buffer,data);// read the data from the status packet
- else
- dyn_master_reset();
return error;
}
+ dyn_master_enable_rx();
+ // wait for the replay within the given timeout
+ error=dyn_master_receive();
+ if(error==DYN_NO_ERROR)
+ dyn_get_read_status_data(dyn_master_rx_buffer,data);// read the data from the status packet
else
+ dyn_master_reset();
+ return error;
+}
+
+uint8_t dyn2_master_read(uint8_t id,uint16_t address,uint16_t length,uint8_t *data)
+{
+ uint8_t error;
+
+ /* generate the read packet */
+ dyn2_init_read_packet(dyn_master_tx_buffer,id,address,length);
+ dyn_master_num_read_packets=0x01;
+ // enable transmission
+ dyn_master_enable_tx();
+ // send the data
+ if((error=dyn2_master_send())!=DYN_SUCCESS)
+ {
+ dyn_master_enable_rx();
return error;
+ }
+ // wait for the transmission to end
+ if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
+ {
+ dyn_master_enable_rx();
+ return error;
+ }
+ dyn_master_enable_rx();
+ // wait for the replay within the given timeout
+ error=dyn2_master_receive();
+ if(error==DYN_NO_ERROR)
+ dyn2_get_read_status_data(dyn_master_rx_buffer,data);// read the data from the status packet
+ else
+ dyn_master_reset();
+ return error;
}
uint8_t dyn_master_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *data)
{
uint8_t error;
-
+
+ // generate the read packet for the desired device
+ dyn_init_write_packet(dyn_master_tx_buffer,id,address,length,data);
+ dyn_master_num_read_packets=0x01;
+ // enable transmission
+ dyn_master_enable_tx();
+ // send the data
+ if((error=dyn_master_send())!=DYN_SUCCESS)
+ {
+ dyn_master_enable_rx();
+ return error;
+ }
// wait for the transmission to end
- while(dyn_master_sending_packet);
+ if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
+ {
+ dyn_master_enable_rx();
+ return error;
+ }
+ dyn_master_enable_rx();
+ // wait for the replay within the given timeout
+ error=dyn_master_receive();
+ if(error!=DYN_NO_ERROR)
+ dyn_master_reset();
+ return error;
+}
+
+uint8_t dyn2_master_write(uint8_t id, uint16_t address, uint16_t length, uint8_t *data)
+{
+ uint8_t error;
+
// generate the read packet for the desired device
- dyn_init_instruction_packet(dyn_master_tx_buffer);
- // set the ping instruction
- dyn_set_instruction(dyn_master_tx_buffer,DYN_WRITE);
- // set the device id
- if((error=dyn_set_id(dyn_master_tx_buffer,id))==DYN_SUCCESS)
- {
- // sert the start address to start reading
- dyn_set_read_address(dyn_master_tx_buffer,address);
- // set the data to write and its length
- dyn_set_write_data(dyn_master_tx_buffer,length,data);
- // set the checksum
- dyn_set_checksum(dyn_master_tx_buffer);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- dyn_master_send();
- // wait for the transmission to end
- while(dyn_master_sending_packet);
+ dyn2_init_write_packet(dyn_master_tx_buffer,id,address,length,data);
+ dyn_master_num_read_packets=0x01;
+ // enable transmission
+ dyn_master_enable_tx();
+ // send the data
+ if((error=dyn2_master_send())!=DYN_SUCCESS)
+ {
dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn_master_receive();
- if(error!=DYN_NO_ERROR)
- dyn_master_reset();
return error;
- }
- else
+ }
+ // wait for the transmission to end
+ if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
+ {
+ dyn_master_enable_rx();
return error;
+ }
+ dyn_master_enable_rx();
+ // wait for the replay within the given timeout
+ error=dyn2_master_receive();
+ if(error!=DYN_NO_ERROR)
+ dyn_master_reset();
+ return error;
}
// interrupt handlers
void USART_IRQHandler(void)
{
- if(USART_GetITStatus(USART, USART_IT_TC) != RESET)
+ static uint8_t num_bytes=0;
+ uint8_t data;
+
+ if(USART_GetITStatus(USART, USART_IT_RXNE) != RESET)
+ {
+ USART_ClearFlag(USART,USART_FLAG_RXNE);
+ data=USART_ReceiveData(USART);
+ switch(num_bytes)
+ {
+ case 0: if(data==0xFF)
+ {
+ dyn_master_rx_buffer[num_bytes]=data;
+ num_bytes++;
+ }
+ break;
+ case 1: if(data==0xFF)
+ {
+ dyn_master_rx_buffer[num_bytes]=data;
+ num_bytes++;
+ }
+ else num_bytes--;
+ break;
+ case 2: if(data!=0xFF)
+ {
+ dyn_master_rx_buffer[num_bytes]=data;
+ num_bytes++;
+ }
+ break;
+ case 3: dyn_master_rx_buffer[num_bytes]=data;
+ num_bytes=0;
+ /* disable USART RX interrupts */
+ USART_ITConfig(USART,USART_IT_RXNE,DISABLE);
+ /* enable DMA RX */
+ DMA_RX_InitStructure.DMA_BufferSize = data;
+ DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&dyn_master_rx_buffer[dyn_master_packet_length+4];
+ dyn_master_packet_length+=data+4;
+ DMA_Init(USART_RX_DMA_STREAM,&DMA_RX_InitStructure);
+ DMA_Cmd(USART_RX_DMA_STREAM,ENABLE);
+ USART_DMACmd(USART, USART_DMAReq_Rx, ENABLE);
+ break;
+ default: break;
+ }
+ }
+ else if(USART_GetITStatus(USART, USART_IT_TC) != RESET)
{
USART_ClearFlag(USART,USART_FLAG_TC);
USART_ITConfig(USART, USART_IT_TC, DISABLE);
@@ -230,44 +402,37 @@ void USART_IRQHandler(void)
dyn_master_no_answer=0x00;
else
{
- // set up the DMA RX transaction
- DMA_RX_InitStructure.DMA_BufferSize = 4;
- DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)dyn_master_rx_buffer;
- DMA_Init(USART_RX_DMA_STREAM,&DMA_RX_InitStructure);
- DMA_ITConfig(USART_RX_DMA_STREAM,DMA_IT_TC,ENABLE);
- DMA_Cmd(USART_RX_DMA_STREAM,ENABLE);
- USART_DMACmd(USART, USART_DMAReq_Rx, ENABLE);
- dyn_master_receiving_header=0x01;
+ /* enable RX interrupts */
+ USART_ITConfig(USART, USART_IT_RXNE, ENABLE);
}
}
}
void USART_DMA_TX_IRQHandler(void)
{
+ DMA_Cmd(USART_TX_DMA_STREAM,DISABLE);
DMA_ClearFlag(USART_TX_DMA_STREAM,USART_TX_DMA_FLAG_TCIF);
DMA_ClearITPendingBit(USART_TX_DMA_STREAM,USART_TX_DMA_FLAG_TCIF);
+ USART_DMACmd(USART, USART_DMAReq_Tx, DISABLE);
USART_ITConfig(USART, USART_IT_TC, ENABLE);
}
void USART_DMA_RX_IRQHandler(void)
{
- if(dyn_master_receiving_header==0x01)
+ DMA_Cmd(USART_RX_DMA_STREAM,DISABLE);
+ DMA_ClearFlag(USART_RX_DMA_STREAM,USART_RX_DMA_FLAG_TCIF);
+ DMA_ClearITPendingBit(USART_RX_DMA_STREAM,USART_RX_DMA_FLAG_TCIF);
+ USART_DMACmd(USART, USART_DMAReq_Rx, DISABLE);
+ dyn_master_num_read_packets--;
+ if(dyn_master_num_read_packets==0x00)
{
- DMA_ClearFlag(USART_RX_DMA_STREAM,USART_RX_DMA_FLAG_TCIF);
- DMA_ClearITPendingBit(USART_RX_DMA_STREAM,USART_RX_DMA_FLAG_TCIF);
- DMA_RX_InitStructure.DMA_BufferSize = dyn_get_length(dyn_master_rx_buffer);
- DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&dyn_master_rx_buffer[4];
- DMA_Init(USART_RX_DMA_STREAM,&DMA_RX_InitStructure);
- DMA_Cmd(USART_RX_DMA_STREAM,ENABLE);
- USART_DMACmd(USART, USART_DMAReq_Rx, ENABLE);
- dyn_master_receiving_header=0x00;
+ dyn_master_packet_length=0x00;
+ dyn_master_packet_ready=0x01;
}
else
{
- DMA_ClearFlag(USART_RX_DMA_STREAM,USART_RX_DMA_FLAG_TCIF);
- DMA_ClearITPendingBit(USART_RX_DMA_STREAM,USART_RX_DMA_FLAG_TCIF);
- DMA_ITConfig(USART_RX_DMA_STREAM,DMA_IT_TC,DISABLE);
- dyn_master_packet_ready=0x01;
+ /* enable RX interrupts */
+ USART_ITConfig(USART, USART_IT_RXNE, ENABLE);
}
}
@@ -326,8 +491,10 @@ void dyn_master_init(void)
// initialize the flags
dyn_master_packet_ready=0x00;
dyn_master_sending_packet=0x00;
- dyn_master_receiving_header=0x01;
dyn_master_no_answer=0x00;
+ dyn_master_num_read_packets=0x00;
+ dyn_master_packet_length=0x00;
+ dyn_master_version=1;
USART_DeInit(USART);
USART_StructInit(&USART_InitStructure);
@@ -341,11 +508,39 @@ void dyn_master_init(void)
USART_Init(USART, &USART_InitStructure);
NVIC_InitStructure.NVIC_IRQChannel = USART_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
- USART_ITConfig(USART, USART_IT_RXNE | USART_IT_ORE | USART_IT_TXE | USART_IT_CTS | USART_IT_LBD | USART_IT_IDLE | USART_IT_PE | USART_IT_ERR | USART_IT_TC, DISABLE);
+ USART_ITConfig(USART, USART_IT_RXNE |
+ USART_IT_ORE |
+ USART_IT_TXE |
+ USART_IT_CTS |
+ USART_IT_LBD |
+ USART_IT_IDLE |
+ USART_IT_PE |
+ USART_IT_ERR |
+ USART_IT_TC, DISABLE);
+
+ /* Clear all USART interrupt flags */
+ USART_ClearFlag(USART,USART_FLAG_RXNE |
+ USART_FLAG_ORE |
+ USART_FLAG_TXE |
+ USART_FLAG_CTS |
+ USART_FLAG_LBD |
+ USART_FLAG_IDLE |
+ USART_FLAG_PE |
+ USART_FLAG_TC);
+
+ /* Clear all USART interrupt pending bits */
+ USART_ClearITPendingBit(USART, USART_FLAG_RXNE |
+ USART_FLAG_ORE |
+ USART_FLAG_TXE |
+ USART_FLAG_CTS |
+ USART_FLAG_LBD |
+ USART_FLAG_IDLE |
+ USART_FLAG_PE |
+ USART_FLAG_TC);
// configure the DMA channels
/* Configure TX DMA */
@@ -368,8 +563,8 @@ void dyn_master_init(void)
DMA_Init(USART_TX_DMA_STREAM,&DMA_TX_InitStructure);
/* initialize DMA interrupts */
NVIC_InitStructure.NVIC_IRQChannel = USART_DMA_TX_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
DMA_ITConfig(USART_TX_DMA_STREAM,DMA_IT_TC,ENABLE);
@@ -377,7 +572,6 @@ void dyn_master_init(void)
/* Configure RX DMA */
DMA_DeInit(USART_RX_DMA_STREAM);
- DMA_RX_InitStructure.DMA_BufferSize = 4;// transfer the first 3 bytes
DMA_RX_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
DMA_RX_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_Full;
DMA_RX_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
@@ -391,15 +585,16 @@ void dyn_master_init(void)
DMA_RX_InitStructure.DMA_Priority = DMA_Priority_High;
DMA_RX_InitStructure.DMA_Channel = USART_RX_DMA_CHANNEL;
DMA_RX_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;
- DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)dyn_master_rx_buffer;
+ DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&dyn_master_rx_buffer[4];
DMA_Init(USART_RX_DMA_STREAM,&DMA_RX_InitStructure);
/* initialize DMA interrupts */
NVIC_InitStructure.NVIC_IRQChannel = USART_DMA_RX_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
- DMA_ITConfig(USART_RX_DMA_STREAM,DMA_IT_TC | DMA_IT_HT | DMA_IT_TE | DMA_IT_FE | DMA_IT_DME,DISABLE);
+ DMA_ITConfig(USART_RX_DMA_STREAM,DMA_IT_TC,ENABLE);
+ DMA_ITConfig(USART_RX_DMA_STREAM,DMA_IT_HT | DMA_IT_TE | DMA_IT_FE | DMA_IT_DME,DISABLE);
/* Enable the USART2 */
USART_Cmd(USART, ENABLE);
@@ -415,11 +610,6 @@ void dyn_master_init(void)
dyn_master_disable_power();
}
-void dyn_master_flush(void)
-{
- // flush only the reception buffer to avoid interrupting a sync_write command
-}
-
void dyn_master_set_timeout(uint16_t timeout_ms)
{
// save the desired timeout value
@@ -430,6 +620,8 @@ void dyn_master_scan(uint8_t *num,uint8_t *ids)
{
uint8_t i;
+ for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
+ ids[0]=0;
*num=0;
for(i=0;i<254;i++)
{
@@ -441,6 +633,23 @@ void dyn_master_scan(uint8_t *num,uint8_t *ids)
}
}
+void dyn2_master_scan(uint8_t *num,uint8_t *ids)
+{
+ uint8_t i;
+
+ for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
+ ids[0]=0;
+ *num=0;
+ for(i=0;i<254;i++)
+ {
+ if(dyn2_master_ping(i)==DYN_NO_ERROR)// the device exists
+ {
+ ids[*num]=i;
+ (*num)++;
+ }
+ }
+}
+
inline void dyn_master_enable_power(void)
{
GPIO_SetBits(POWER_GPIO_PORT,POWER_PIN);
@@ -459,31 +668,57 @@ uint8_t dyn_master_is_power_enabled(void)
}
uint8_t dyn_master_ping(uint8_t id)
+{
+ uint8_t error;
+
+ // generate the ping packet for the desired device
+ dyn_init_ping_packet(dyn_master_tx_buffer,id);
+ dyn_master_num_read_packets=0x01;
+ // enable transmission
+ dyn_master_enable_tx();
+ // send the data
+ if((error=dyn_master_send())!=DYN_SUCCESS)
+ {
+ dyn_master_enable_rx();
+ return error;
+ }
+ // wait for the transmission to end
+ if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
+ {
+ dyn_master_enable_rx();
+ return error;
+ }
+ dyn_master_enable_rx();
+ // wait for the replay within the given timeout
+ error=dyn_master_receive();
+ return error;
+}
+
+uint8_t dyn2_master_ping(uint8_t id)
{
uint8_t error;
// generate the ping packet for the desired device
- dyn_init_instruction_packet(dyn_master_tx_buffer);
- // set the ping instruction
- dyn_set_instruction(dyn_master_tx_buffer,DYN_PING);
- // set the device id
- if((error=dyn_set_id(dyn_master_tx_buffer,id))==DYN_SUCCESS)
- {
- // set the checksum
- dyn_set_checksum(dyn_master_tx_buffer);
- // enable transmission
- dyn_master_enable_tx();
- // send the data
- dyn_master_send();
- // wait for the transmission to end
- while (dyn_master_sending_packet==0x01);
+ dyn2_init_ping_packet(dyn_master_tx_buffer,id);
+ dyn_master_num_read_packets=0x01;
+ // enable transmission
+ dyn_master_enable_tx();
+ // send the data
+ if((error=dyn2_master_send())!=DYN_SUCCESS)
+ {
dyn_master_enable_rx();
- // wait for the replay within the given timeout
- error=dyn_master_receive();
return error;
}
- else
+ // wait for the transmission to end
+ if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
+ {
+ dyn_master_enable_rx();
return error;
+ }
+ dyn_master_enable_rx();
+ // wait for the replay within the given timeout
+ error=dyn2_master_receive();
+ return error;
}
uint8_t dyn_master_read_byte(uint8_t id,uint8_t address,uint8_t *data)
@@ -491,6 +726,11 @@ uint8_t dyn_master_read_byte(uint8_t id,uint8_t address,uint8_t *data)
return dyn_master_read(id,address,1,data);
}
+uint8_t dyn2_master_read_byte(uint8_t id,uint16_t address,uint8_t *data)
+{
+ return dyn2_master_read(id,address,1,data);
+}
+
uint8_t dyn_master_read_word(uint8_t id,uint8_t address,uint16_t *data)
{
uint8_t value[2];
@@ -503,16 +743,38 @@ uint8_t dyn_master_read_word(uint8_t id,uint8_t address,uint16_t *data)
return error;
}
+uint8_t dyn2_master_read_word(uint8_t id,uint16_t address,uint16_t *data)
+{
+ uint8_t value[2];
+ uint8_t error;
+
+ error=dyn2_master_read(id,address,2,value);
+ if(error==DYN_NO_ERROR)
+ (*data)=value[0]+value[1]*256;
+
+ return error;
+}
+
uint8_t dyn_master_read_table(uint8_t id,uint8_t address,uint8_t length,uint8_t *data)
{
return dyn_master_read(id,address,length,data);
}
+uint8_t dyn2_master_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_t *data)
+{
+ return dyn2_master_read(id,address,length,data);
+}
+
uint8_t dyn_master_write_byte(uint8_t id, uint8_t address, uint8_t data)
{
return dyn_master_write(id,address,1,&data);
}
+uint8_t dyn2_master_write_byte(uint8_t id, uint16_t address, uint8_t data)
+{
+ return dyn2_master_write(id,address,1,&data);
+}
+
uint8_t dyn_master_write_word(uint8_t id, uint8_t address, uint16_t data)
{
uint8_t value[2];
@@ -522,45 +784,121 @@ uint8_t dyn_master_write_word(uint8_t id, uint8_t address, uint16_t data)
return dyn_master_write(id,address,2,value);
}
+uint8_t dyn2_master_write_word(uint8_t id, uint16_t address, uint16_t data)
+{
+ uint8_t value[2];
+
+ value[0]=data%256;
+ value[1]=data/256;
+ return dyn2_master_write(id,address,2,value);
+}
+
uint8_t dyn_master_write_table(uint8_t id, uint8_t address, uint8_t length, uint8_t *data)
{
return dyn_master_write(id,address,length,data);
}
-uint8_t dyn_master_reg_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *data)
+uint8_t dyn2_master_write_table(uint8_t id, uint16_t address, uint16_t length, uint8_t *data)
{
- return DYN_SUCCESS;
+ return dyn2_master_write(id,address,length,data);
}
-void dyn_master_action(void)
+uint8_t dyn_master_reg_write(uint8_t id, uint8_t address, uint8_t length, uint8_t *data)
{
+ uint8_t error;
+ // generate the ping packet for the desired device
+ dyn_init_reg_write_packet(dyn_master_tx_buffer,id,address,length,data);
+ dyn_master_num_read_packets=0x01;
+ // enable transmission
+ dyn_master_enable_tx();
+ // send the data
+ if((error=dyn_master_send())!=DYN_SUCCESS)
+ {
+ dyn_master_enable_rx();
+ return error;
+ }
+ // wait for the transmission to end
+ if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
+ {
+ dyn_master_enable_rx();
+ return error;
+ }
+ dyn_master_enable_rx();
+ // wait for the replay within the given timeout
+ error=dyn_master_receive();
+ return error;
}
-void dyn_master_sync_write(uint8_t num,uint8_t *ids,uint8_t address, uint8_t length, uint8_t *data)
+uint8_t dyn2_master_reg_write(uint8_t id, uint16_t address, uint16_t length, uint8_t *data)
{
- uint8_t i;
+ uint8_t error;
+ // generate the ping packet for the desired device
+ dyn2_init_reg_write_packet(dyn_master_tx_buffer,id,address,length,data);
+ dyn_master_num_read_packets=0x01;
+ // enable transmission
+ dyn_master_enable_tx();
+ // send the data
+ if((error=dyn2_master_send())!=DYN_SUCCESS)
+ {
+ dyn_master_enable_rx();
+ return error;
+ }
// wait for the transmission to end
- while(dyn_master_sending_packet);
+ if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
+ {
+ dyn_master_enable_rx();
+ return error;
+ }
+ dyn_master_enable_rx();
+ // wait for the replay within the given timeout
+ error=dyn2_master_receive();
+ return error;
+}
+
+uint8_t dyn_master_action(void)
+{
+ dyn_master_no_answer=0x01;
+ // generate the ping packet for the desired device
+ dyn_init_action_packet(dyn_master_tx_buffer);
+ // enable transmission
+ dyn_master_enable_tx();
+ // send the data
+ return dyn_master_send();
+}
+
+uint8_t dyn2_master_action(void)
+{
+ dyn_master_no_answer=0x01;
+ // generate the ping packet for the desired device
+ dyn2_init_action_packet(dyn_master_tx_buffer);
+ // enable transmission
+ dyn_master_enable_tx();
+ // send the data
+ return dyn2_master_send();
+}
+
+uint8_t dyn_master_sync_write(uint8_t num,uint8_t *ids,uint8_t address, uint8_t length, TWriteData *data)
+{
dyn_master_no_answer=0x01;
// generate the sync write packet
- dyn_init_instruction_packet(dyn_master_tx_buffer);
- // set the ping instruction
- dyn_set_instruction(dyn_master_tx_buffer,DYN_SYNC_WRITE);
- // set the start address
- dyn_set_write_address(dyn_master_tx_buffer,address);
- // set the data length
- dyn_set_sync_write_length(dyn_master_tx_buffer,length);
- // load the data for each device
- for(i=0;i<num;i++)
- dyn_set_sync_write_data(dyn_master_tx_buffer,ids[i],&data[i*length]);
- // set the checksum
- dyn_set_checksum(dyn_master_tx_buffer);
+ dyn_init_sync_write_packet(dyn_master_tx_buffer,num,ids,address,length,data);
+ // enable transmission
+ dyn_master_enable_tx();
+ // send the data
+ return dyn_master_send();
+}
+
+uint8_t dyn2_master_sync_write(uint8_t num,uint8_t *ids,uint16_t address, uint16_t length, TWriteData *data)
+{
+ dyn_master_no_answer=0x01;
+ // generate the sync write packet
+ dyn2_init_sync_write_packet(dyn_master_tx_buffer,num,ids,address,length,data);
// enable transmission
dyn_master_enable_tx();
// send the data
- dyn_master_send();
+ return dyn2_master_send();
}
// repeater functions
@@ -568,16 +906,23 @@ uint8_t dyn_master_resend_inst_packet(uint8_t *inst_packet, uint8_t *status_pack
{
uint8_t error;
- // generate the read packet for the desired device
- dyn_init_instruction_packet(dyn_master_tx_buffer);
// copy the contents of the instruction packet to the output buffer
dyn_copy_packet(inst_packet,dyn_master_tx_buffer);
+ dyn_master_num_read_packets=0x01;
// enable transmission
dyn_master_enable_tx();
// send the data
- dyn_master_send();
+ if((error=dyn_master_send())!=DYN_SUCCESS)
+ {
+ dyn_master_enable_rx();
+ return error;
+ }
// wait for the transmission to end
- while(dyn_master_sending_packet);
+ if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
+ {
+ dyn_master_enable_rx();
+ return error;
+ }
dyn_master_enable_rx();
// wait for the replay within the given timeout
error=dyn_master_receive();
@@ -590,3 +935,45 @@ uint8_t dyn_master_resend_inst_packet(uint8_t *inst_packet, uint8_t *status_pack
return error;
}
+uint8_t dyn_master_bulk_read(uint8_t num,uint8_t *ids,uint8_t *address,uint8_t *length,TWriteData *data)
+{
+ uint16_t offset=0;
+ uint8_t i,error;
+
+ dyn_master_num_read_packets=num;
+ // generate the sync write packet
+ dyn_init_bulk_read_packet(dyn_master_tx_buffer,num,ids,address,length);
+ // enable transmission
+ dyn_master_enable_tx();
+ // send the data
+ if((error=dyn_master_send())!=DYN_SUCCESS)
+ {
+ dyn_master_enable_rx();
+ return error;
+ }
+ // wait for the transmission to end
+ if((error=dyn_master_wait_sending())!=DYN_SUCCESS)
+ {
+ dyn_master_enable_rx();
+ return error;
+ }
+ dyn_master_enable_rx();
+ // wait for the replay within the given timeout
+ error=dyn_master_receive();
+ if(error!=DYN_NO_ERROR)
+ {
+ dyn_master_reset();
+ return error;
+ }
+ for(i=0;i<num;i++)
+ {
+ error=dyn_get_status_error(&dyn_master_rx_buffer[offset]);
+ if(error!=DYN_NO_ERROR)
+ return error;
+ offset+=dyn_get_read_status_data(&dyn_master_rx_buffer[offset],data[i].data_addr)+4;
+ }
+
+ return DYN_SUCCESS;
+}
+
+
diff --git a/src/dynamixel_slave_uart_dma.c b/src/dynamixel_slave_uart_dma.c
index 6813a6009fc1877aca0882e4286fd6ce753e3136..db8f3390ec70e38f5a488c2551fa357d3053c67a 100644
--- a/src/dynamixel_slave_uart_dma.c
+++ b/src/dynamixel_slave_uart_dma.c
@@ -51,7 +51,6 @@ uint8_t dyn_slave_return_level;// type of response
uint8_t dyn_slave_return_delay;// delay in th response
// input buffer
uint8_t dyn_slave_rx_buffer[MAX_BUFFER_LEN];
-volatile uint8_t dyn_slave_receiving_header;
// output buffer
uint8_t dyn_slave_tx_buffer[MAX_BUFFER_LEN];
// instruction packet ready flag
@@ -67,24 +66,57 @@ DMA_InitTypeDef DYN_SLAVE_DMA_RX_InitStructure;
// interrupt handlers
void DYN_SLAVE_IRQHandler(void)
{
- if(USART_GetITStatus(DYN_SLAVE, USART_IT_TC) != RESET)
+ static uint8_t num_bytes=0;
+ uint8_t data;
+
+ if(USART_GetITStatus(DYN_SLAVE, USART_IT_RXNE) != RESET)
+ {
+ USART_ClearFlag(DYN_SLAVE,USART_FLAG_RXNE);
+ data=USART_ReceiveData(DYN_SLAVE);
+ switch(num_bytes)
+ {
+ case 0: if(data==0xFF)
+ {
+ dyn_slave_rx_buffer[num_bytes]=data;
+ num_bytes++;
+ }
+ break;
+ case 1: if(data==0xFF)
+ {
+ dyn_slave_rx_buffer[num_bytes]=data;
+ num_bytes++;
+ }
+ else num_bytes--;
+ break;
+ case 2: if(data!=0xFF)
+ {
+ dyn_slave_rx_buffer[num_bytes]=data;
+ num_bytes++;
+ }
+ break;
+ case 3: dyn_slave_rx_buffer[num_bytes]=data;
+ num_bytes=0;
+ /* disable USART RX interrupts */
+ USART_ITConfig(DYN_SLAVE,USART_IT_RXNE,DISABLE);
+ /* enable DMA RX */
+ DMA_SetCurrDataCounter(DYN_SLAVE_RX_DMA_STREAM, data);
+ DMA_Cmd(DYN_SLAVE_RX_DMA_STREAM,ENABLE);
+ USART_DMACmd(DYN_SLAVE, USART_DMAReq_Rx, ENABLE);
+ break;
+ default: break;
+ }
+ }
+ else if(USART_GetITStatus(DYN_SLAVE, USART_IT_TC) != RESET)
{
USART_ClearFlag(DYN_SLAVE,USART_FLAG_TC);
USART_ITConfig(DYN_SLAVE, USART_IT_TC, DISABLE);
dyn_slave_sending_packet=0x00;
- // set up the DMA RX transaction
- DYN_SLAVE_DMA_RX_InitStructure.DMA_BufferSize = 4;
- DYN_SLAVE_DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)dyn_slave_rx_buffer;
- DMA_Init(DYN_SLAVE_RX_DMA_STREAM,&DYN_SLAVE_DMA_RX_InitStructure);
- DMA_ITConfig(DYN_SLAVE_RX_DMA_STREAM,DMA_IT_TC,ENABLE);
- DMA_Cmd(DYN_SLAVE_RX_DMA_STREAM,ENABLE);
- USART_DMACmd(DYN_SLAVE, USART_DMAReq_Rx, ENABLE);
- dyn_slave_receiving_header=0x01;
}
}
void DYN_SLAVE_DMA_TX_IRQHandler(void)
{
+ DMA_Cmd(DYN_SLAVE_TX_DMA_STREAM,DISABLE);
DMA_ClearFlag(DYN_SLAVE_TX_DMA_STREAM,DYN_SLAVE_TX_DMA_FLAG_TCIF);
DMA_ClearITPendingBit(DYN_SLAVE_TX_DMA_STREAM,DYN_SLAVE_TX_DMA_FLAG_TCIF);
USART_ITConfig(DYN_SLAVE, USART_IT_TC, ENABLE);
@@ -92,24 +124,13 @@ void DYN_SLAVE_DMA_TX_IRQHandler(void)
void DYN_SLAVE_DMA_RX_IRQHandler(void)
{
- if(dyn_slave_receiving_header==0x01)
- {
- DMA_ClearFlag(DYN_SLAVE_RX_DMA_STREAM,DYN_SLAVE_RX_DMA_FLAG_TCIF);
- DMA_ClearITPendingBit(DYN_SLAVE_RX_DMA_STREAM,DYN_SLAVE_RX_DMA_FLAG_TCIF);
- DYN_SLAVE_DMA_RX_InitStructure.DMA_BufferSize = dyn_get_length(dyn_slave_rx_buffer);
- DYN_SLAVE_DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&dyn_slave_rx_buffer[4];
- DMA_Init(DYN_SLAVE_RX_DMA_STREAM,&DYN_SLAVE_DMA_RX_InitStructure);
- DMA_Cmd(DYN_SLAVE_RX_DMA_STREAM,ENABLE);
- USART_DMACmd(DYN_SLAVE, USART_DMAReq_Rx, ENABLE);
- dyn_slave_receiving_header=0x00;
- }
- else
- {
- DMA_ClearFlag(DYN_SLAVE_RX_DMA_STREAM,DYN_SLAVE_RX_DMA_FLAG_TCIF);
- DMA_ClearITPendingBit(DYN_SLAVE_RX_DMA_STREAM,DYN_SLAVE_RX_DMA_FLAG_TCIF);
- DMA_ITConfig(DYN_SLAVE_RX_DMA_STREAM,DMA_IT_TC,DISABLE);
- dyn_slave_packet_ready=0x01;
- }
+ DMA_Cmd(DYN_SLAVE_RX_DMA_STREAM,DISABLE);
+ DMA_ClearFlag(DYN_SLAVE_RX_DMA_STREAM,DYN_SLAVE_RX_DMA_FLAG_TCIF);
+ DMA_ClearITPendingBit(DYN_SLAVE_RX_DMA_STREAM,DYN_SLAVE_RX_DMA_FLAG_TCIF);
+ USART_DMACmd(DYN_SLAVE, USART_DMAReq_Rx, DISABLE);
+ // enable USART RX interrupt
+ USART_ITConfig(DYN_SLAVE,USART_IT_RXNE,ENABLE);
+ dyn_slave_packet_ready=0x01;
}
// public functions
@@ -149,7 +170,6 @@ void dyn_slave_init(void)
// initialize the flags
dyn_slave_packet_ready=0x00;
dyn_slave_sending_packet=0x00;
- dyn_slave_receiving_header=0x01;
USART_DeInit(DYN_SLAVE);
USART_StructInit(&USART_InitStructure);
@@ -163,14 +183,39 @@ void dyn_slave_init(void)
USART_Init(DYN_SLAVE, &USART_InitStructure);
NVIC_InitStructure.NVIC_IRQChannel = DYN_SLAVE_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
- USART_ITConfig(DYN_SLAVE, USART_IT_RXNE | USART_IT_ORE | USART_IT_TXE | USART_IT_CTS | USART_IT_LBD | USART_IT_IDLE | USART_IT_PE | USART_IT_ERR | USART_IT_TC, DISABLE);
- USART_ClearFlag(DYN_SLAVE,USART_FLAG_RXNE | USART_FLAG_ORE | USART_FLAG_TXE | USART_FLAG_CTS | USART_FLAG_LBD | USART_FLAG_IDLE | USART_FLAG_PE | USART_FLAG_TC);
- USART_ClearITPendingBit(DYN_SLAVE, USART_FLAG_RXNE | USART_FLAG_ORE | USART_FLAG_TXE | USART_FLAG_CTS | USART_FLAG_LBD | USART_FLAG_IDLE | USART_FLAG_PE | USART_FLAG_TC);
+ /* disable all interrupts of the USART */
+ USART_ITConfig(DYN_SLAVE, USART_IT_RXNE |
+ USART_IT_ORE |
+ USART_IT_TXE |
+ USART_IT_CTS |
+ USART_IT_LBD |
+ USART_IT_IDLE |
+ USART_IT_PE |
+ USART_IT_ERR |
+ USART_IT_TC, DISABLE);
+ /* Clear all USART interrupt flags */
+ USART_ClearFlag(DYN_SLAVE,USART_FLAG_RXNE |
+ USART_FLAG_ORE |
+ USART_FLAG_TXE |
+ USART_FLAG_CTS |
+ USART_FLAG_LBD |
+ USART_FLAG_IDLE |
+ USART_FLAG_PE |
+ USART_FLAG_TC);
+ /* Clear all USART interrupt pending bits */
+ USART_ClearITPendingBit(DYN_SLAVE, USART_FLAG_RXNE |
+ USART_FLAG_ORE |
+ USART_FLAG_TXE |
+ USART_FLAG_CTS |
+ USART_FLAG_LBD |
+ USART_FLAG_IDLE |
+ USART_FLAG_PE |
+ USART_FLAG_TC);
/* Enable the DYN_SLAVE3 */
USART_Cmd(DYN_SLAVE, ENABLE);
@@ -204,7 +249,6 @@ void dyn_slave_init(void)
/* Configure RX DMA */
DMA_DeInit(DYN_SLAVE_RX_DMA_STREAM);
- DYN_SLAVE_DMA_RX_InitStructure.DMA_BufferSize = 4;// transfer the first 3 bytes
DYN_SLAVE_DMA_RX_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
DYN_SLAVE_DMA_RX_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_Full;
DYN_SLAVE_DMA_RX_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
@@ -218,7 +262,7 @@ void dyn_slave_init(void)
DYN_SLAVE_DMA_RX_InitStructure.DMA_Priority = DMA_Priority_High;
DYN_SLAVE_DMA_RX_InitStructure.DMA_Channel = DYN_SLAVE_RX_DMA_CHANNEL;
DYN_SLAVE_DMA_RX_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;
- DYN_SLAVE_DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)dyn_slave_rx_buffer;
+ DYN_SLAVE_DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&dyn_slave_rx_buffer[4];
DMA_Init(DYN_SLAVE_RX_DMA_STREAM,&DYN_SLAVE_DMA_RX_InitStructure);
/* initialize DMA interrupts */
NVIC_InitStructure.NVIC_IRQChannel = DYN_SLAVE_DMA_RX_IRQn;
@@ -229,8 +273,8 @@ void dyn_slave_init(void)
DMA_ITConfig(DYN_SLAVE_RX_DMA_STREAM,DMA_IT_TC,ENABLE);
DMA_ITConfig(DYN_SLAVE_RX_DMA_STREAM,DMA_IT_HT | DMA_IT_TE | DMA_IT_FE | DMA_IT_DME,DISABLE);
- DMA_Cmd(DYN_SLAVE_RX_DMA_STREAM,ENABLE);
- USART_DMACmd(DYN_SLAVE, USART_DMAReq_Rx, ENABLE);
+ /* enable USART RX interrupts */
+ USART_ITConfig(DYN_SLAVE,USART_IT_RXNE,ENABLE);
}
inline void dyn_slave_set_address(uint8_t id)
@@ -277,11 +321,7 @@ void dyn_slave_send_status_packet(uint8_t error,uint8_t length, uint8_t *data)
// wait until the previous transmission has ended (if any)
while(dyn_slave_sending_packet==0x01);
// create the status packet
- dyn_init_status_packet(dyn_slave_tx_buffer);
- dyn_set_status_error(dyn_slave_tx_buffer,error);
- dyn_set_id(dyn_slave_tx_buffer,dyn_slave_address);
- dyn_set_status_data(dyn_slave_tx_buffer,length,data);
- dyn_set_checksum(dyn_slave_tx_buffer);
+ dyn_init_status_packet(dyn_slave_tx_buffer,dyn_slave_address,error,length,data);
// set the DMA transfer
DMA_SetCurrDataCounter(DYN_SLAVE_TX_DMA_STREAM,dyn_get_length(dyn_slave_tx_buffer)+4);
DMA_Cmd(DYN_SLAVE_TX_DMA_STREAM,ENABLE);
@@ -295,7 +335,6 @@ void dyn_slave_resend_status_packet(uint8_t *packet)
// wait until the previous transmission has ended (if any)
while(dyn_slave_sending_packet==0x01);
// create the status packet
- dyn_init_status_packet(dyn_slave_tx_buffer);
dyn_copy_packet(packet,dyn_slave_tx_buffer);
// set the DMA transfer
DMA_SetCurrDataCounter(DYN_SLAVE_TX_DMA_STREAM,dyn_get_length(dyn_slave_tx_buffer)+4);
@@ -305,15 +344,3 @@ void dyn_slave_resend_status_packet(uint8_t *packet)
dyn_slave_sending_packet=0x01;
}
-void dyn_slave_reset(void)
-{
- dyn_slave_sending_packet=0x00;
- // set up the DMA RX transaction
- DYN_SLAVE_DMA_RX_InitStructure.DMA_BufferSize = 4;
- DYN_SLAVE_DMA_RX_InitStructure.DMA_Memory0BaseAddr = (uint32_t)dyn_slave_rx_buffer;
- DMA_Init(DYN_SLAVE_RX_DMA_STREAM,&DYN_SLAVE_DMA_RX_InitStructure);
- DMA_ITConfig(DYN_SLAVE_RX_DMA_STREAM,DMA_IT_TC,ENABLE);
- DMA_Cmd(DYN_SLAVE_RX_DMA_STREAM,ENABLE);
- USART_DMACmd(DYN_SLAVE, USART_DMAReq_Rx, ENABLE);
- dyn_slave_receiving_header=0x01;
-}
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 5d260ac2af1da4fb47eea4304ba15e2a58758ee9..0650f7bc74c50fbc4f2cc1fc940423a5387759f0 100644
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -100,14 +100,14 @@ void manager_balance(void)
int16_t x_error=0.0;
int16_t y_error=0.0;
- manager_servos[10].current_value+=(y_error*ram_data[BIOLOID_MM_PELVIS_ROLL_L]*11)/2500;
- manager_servos[9].current_value+=(y_error*ram_data[BIOLOID_MM_PELVIS_ROLL_R]*11)/2500;
- manager_servos[18].current_value-=(y_error*ram_data[BIOLOID_MM_ANKLE_ROLL_L]*11)/2500;
- manager_servos[17].current_value-=(y_error*ram_data[BIOLOID_MM_ANKLE_ROLL_R]*11)/2500;
- manager_servos[16].current_value-=(x_error*ram_data[BIOLOID_MM_ANKLE_PITCH_L]*11)/2500;
- manager_servos[15].current_value+=(x_error*ram_data[BIOLOID_MM_ANKLE_PITCH_R]*11)/2500;
- manager_servos[14].current_value-=(x_error*ram_data[BIOLOID_MM_KNEE_L]*11)/2500;
- manager_servos[13].current_value+=(x_error*ram_data[BIOLOID_MM_KNEE_R]*11)/2500;
+ manager_servos[10].current_value+=(y_error*ram_data[BIOLOID_MM_PELVIS_ROLL_GAIN_L]*11)/2500;
+ manager_servos[9].current_value+=(y_error*ram_data[BIOLOID_MM_PELVIS_ROLL_GAIN_R]*11)/2500;
+ manager_servos[18].current_value-=(y_error*ram_data[BIOLOID_MM_ANKLE_ROLL_GAIN_L]*11)/2500;
+ manager_servos[17].current_value-=(y_error*ram_data[BIOLOID_MM_ANKLE_ROLL_GAIN_R]*11)/2500;
+ manager_servos[16].current_value-=(x_error*ram_data[BIOLOID_MM_ANKLE_PITCH_GAIN_L]*11)/2500;
+ manager_servos[15].current_value+=(x_error*ram_data[BIOLOID_MM_ANKLE_PITCH_GAIN_R]*11)/2500;
+ manager_servos[14].current_value-=(x_error*ram_data[BIOLOID_MM_KNEE_GAIN_L]*11)/2500;
+ manager_servos[13].current_value+=(x_error*ram_data[BIOLOID_MM_KNEE_GAIN_R]*11)/2500;
}
// interrupt handlers
@@ -266,7 +266,7 @@ void manager_init(uint16_t period_us)
/* motion timer configuration */
TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
- TIM_TimeBaseStructure.TIM_Prescaler = 82;
+ TIM_TimeBaseStructure.TIM_Prescaler = 72;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(MOTION_TIMER,&TIM_TimeBaseStructure);
diff --git a/src/time.c b/src/time.c
index e36a0d2faf74a0998930771b2746028a49c1e8df..44dd1b79c4c7c99b968312e1dfbc1bcbe9ab6bc6 100644
--- a/src/time.c
+++ b/src/time.c
@@ -1,6 +1,7 @@
#include "time.h"
static __IO uint32_t timing_delay;
+uint32_t clocks_per_us;
/*******************************************************************************
* Function Name : SysTickHandler
@@ -18,9 +19,23 @@ void SysTick_Handler(void)
// public functions
void time_init(void)
{
+ TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
+
// set the time base to 1ms
SysTick_Config(SystemCoreClock / 1000);
NVIC_SetPriority(SysTick_IRQn,0);
+
+ RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1,ENABLE);
+
+ TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
+ TIM_TimeBaseStructure.TIM_Prescaler = 0;
+ TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
+ TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+ TIM_TimeBaseStructure.TIM_RepetitionCounter=0x0000;
+ TIM_TimeBaseInit(TIM1,&TIM_TimeBaseStructure);
+ TIM_SetCounter(TIM1,0);
+
+ clocks_per_us=(SystemCoreClock/1000000)+1;
}
void delay_ms(__IO uint32_t time)
@@ -30,4 +45,16 @@ void delay_ms(__IO uint32_t time)
while(timing_delay!=0);
}
+void delay_us(uint32_t us) // microseconds
+{
+ uint32_t clocks,current;
+
+ clocks=clocks_per_us*us;
+ TIM_SetCounter(TIM1,0);
+ TIM_Cmd(TIM1, ENABLE);
+ do{
+ current=TIM_GetCounter(TIM1);
+ }while(current<clocks);
+ TIM_Cmd(TIM1, DISABLE);
+}