diff --git a/include/ram.h b/include/ram.h
index 188dd50047df6072a559b2634d7bfa501163553d..d9378534607db8f8b1d861a65a4822f0e6199b3b 100644
--- a/include/ram.h
+++ b/include/ram.h
@@ -107,10 +107,25 @@ typedef enum {
BIOLOID_IMU_CONTROL = 0x6D,
BIOLOID_IMU_ACCEL_X_L = 0x6E,
BIOLOID_IMU_ACCEL_X_H = 0x6F,
- BIOLOID_IMU_ACCEL_Y_L = 0x7A,
- BIOLOID_IMU_ACCEL_Y_H = 0x7B,
- BIOLOID_IMU_ACCEL_Z_L = 0x7C,
- BIOLOID_IMU_ACCEL_Z_H = 0x7D
+ BIOLOID_IMU_ACCEL_Y_L = 0x70,
+ BIOLOID_IMU_ACCEL_Y_H = 0x71,
+ BIOLOID_IMU_ACCEL_Z_L = 0x72,
+ BIOLOID_IMU_ACCEL_Z_H = 0x73,
+ BIOLOID_IMU_GYRO_X_L = 0x74,
+ BIOLOID_IMU_GYRO_X_H = 0x75,
+ BIOLOID_IMU_GYRO_Y_L = 0x76,
+ BIOLOID_IMU_GYRO_Y_H = 0x77,
+ BIOLOID_IMU_GYRO_Z_L = 0x78,
+ BIOLOID_IMU_GYRO_Z_H = 0x79,
+ BIOLOID_IMU_COMPASS_X_L = 0x7A,
+ BIOLOID_IMU_COMPASS_X_H = 0x7B,
+ BIOLOID_IMU_COMPASS_Y_L = 0x7C,
+ BIOLOID_IMU_COMPASS_Y_H = 0x7D,
+ BIOLOID_IMU_COMPASS_Z_L = 0x7E,
+ BIOLOID_IMU_COMPASS_Z_H = 0x7F,
+ BIOLOID_ACTION_PAGE = 0x80,
+ BIOLOID_ACTION_CONTROL = 0x81,
+ BIOLOID_ACTION_SATUTUS = 0x82
} bioloid_registers;
#define RAM_SIZE 256
diff --git a/src/action.c b/src/action.c
index 0cb22d630121edc529691dbc3afd7ee39bcd3d03..452c2a9635c08878f658f9b605e10b612efa470b 100644
--- a/src/action.c
+++ b/src/action.c
@@ -190,7 +190,7 @@ void action_init(uint16_t period)
{
// angle variables
action_moving_angles[i]=0;// fixed point 48|16 format
- action_current_angles[i]=0;// middle of range
+ //action_current_angles[i]=0;// middle of range
// speed variables
action_start_speed[i]=0;// fixed point 48|16 format
action_main_speed[i]=0;// fixed point 48|16 format
diff --git a/src/bioloid_stm32.c b/src/bioloid_stm32.c
index a69bf5f3fb4bfa0f91a7c3514d7dbc7b48ccca8c..23dfffb4763e556da0966edc99a8eba86ac23458 100644
--- a/src/bioloid_stm32.c
+++ b/src/bioloid_stm32.c
@@ -61,7 +61,7 @@ uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data)
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++)
+ for(i=BIOLOID_MM_MODULE_EN0,j=0;i<=BIOLOID_MM_MODULE_EN15;i++,j+=2)
{
if(ram_in_range(i,i,address,length))
{
@@ -94,6 +94,15 @@ uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data)
// write operation
error=ram_write_table(address,length,data);
// post-write operation
+ if(ram_in_range(BIOLOID_ACTION_PAGE,BIOLOID_ACTION_PAGE,address,length))// load the page identifier
+ action_load_page(data[address-BIOLOID_ACTION_PAGE]);
+ if(ram_in_range(BIOLOID_ACTION_CONTROL,BIOLOID_ACTION_CONTROL,address,length))
+ {
+ if(data[address-BIOLOID_ACTION_CONTROL]&0x01)
+ action_start_page();
+ if(data[address-BIOLOID_ACTION_CONTROL]&0x02)
+ action_stop_page();
+ }
return error;
}
@@ -126,7 +135,7 @@ 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
diff --git a/src/dynamixel_master_uart_dma.c b/src/dynamixel_master_uart_dma.c
index 8abdfcaa9ecbd800671ea7086fe4fd1ca1014a1d..2c545b89e4df0c76cab573514f92f11a9f57be05 100755
--- a/src/dynamixel_master_uart_dma.c
+++ b/src/dynamixel_master_uart_dma.c
@@ -69,6 +69,8 @@ uint8_t dyn_master_tx_buffer[MAX_BUFFER_LEN];
volatile uint8_t dyn_master_packet_ready;
// sending status packet flag
volatile uint8_t dyn_master_sending_packet;
+// no answer for sync write operation
+uint8_t dyn_master_no_answer;
// DMA initialization data structures
DMA_InitTypeDef DMA_TX_InitStructure;
DMA_InitTypeDef DMA_RX_InitStructure;
@@ -106,8 +108,8 @@ uint8_t dyn_master_receive(void)
// wait for the status packet
while(!dyn_master_packet_ready)
{
- delay_ms(10);
- timeout_left-=10;
+ delay_ms(1);
+ timeout_left-=1;
if(timeout_left<=0)
{
dyn_master_reset();
@@ -212,14 +214,19 @@ void USART_IRQHandler(void)
USART_ClearFlag(USART,USART_FLAG_TC);
USART_ITConfig(USART, USART_IT_TC, DISABLE);
dyn_master_sending_packet=0x00;
- // 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;
+ if(dyn_master_no_answer)
+ 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;
+ }
}
}
@@ -307,6 +314,7 @@ void dyn_master_init(void)
dyn_master_packet_ready=0x00;
dyn_master_sending_packet=0x00;
dyn_master_receiving_header=0x01;
+ dyn_master_no_answer=0x00;
USART_DeInit(USART);
USART_StructInit(&USART_InitStructure);
@@ -493,6 +501,7 @@ void dyn_master_sync_write(uint8_t num,uint8_t *ids,uint8_t address, uint8_t len
{
uint8_t i;
+ dyn_master_no_answer=0x01;
// generate the sync write packet
dyn_init_instruction_packet(dyn_master_tx_buffer);
// set the ping instruction
diff --git a/src/eeprom.c b/src/eeprom.c
index cd2df122ceed51f143726c22c2f5af172767d618..37477009408e9a8da640830b2b00f42123f4bcb0 100755
--- a/src/eeprom.c
+++ b/src/eeprom.c
@@ -31,7 +31,7 @@
#define EEPROM_SIZE 0x09
#define DEFAULT_DEVICE_MODEL 0x7300
#define DEFAULT_FIRMWARE_VERSION 0x13
-#define DEFAULT_DEVICE_ID 0x0D
+#define DEFAULT_DEVICE_ID 0xC0
#define DEFAULT_BAUDRATE 0x01
#define DEFAULT_RETURN_DELAY 0x00
#define DEFAULT_MM_PERIOD 0x01FF
diff --git a/src/motion_manager.c b/src/motion_manager.c
index c7dc63d94f1ae580ce8b2ad454eb0da7e333d85a..11ed9bb7355c68119e3e04549b3ce6b8fb14d71c 100644
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -3,12 +3,15 @@
#include "dyn_servos.h"
#include "gpio.h"
#include "ram.h"
+#include "action.h"
#define MOTION_TIMER TIM3
#define MOTION_TIMER_IRQn TIM3_IRQn
#define MOTION_TIMER_IRQHandler TIM3_IRQHandler
#define MOTION_TIMER_CLK RCC_APB1Periph_TIM3
+extern int64_t action_current_angles[MANAGER_MAX_NUM_SERVOS];
+
// private variables
__IO uint16_t manager_motion_period;
uint8_t manager_num_servos;
@@ -19,17 +22,20 @@ void manager_send_motion_command(void)
{
uint8_t servo_ids[MANAGER_MAX_NUM_SERVOS];
uint8_t data[2048];
- uint16_t value;
- uint8_t i;
+ uint8_t i,num=0;
for(i=0;i<manager_num_servos;i++)
{
-// servo_ids[i]=manager_servos[i].id;
-// value=manager_get_index_value(i);
- data[i*2]=value%256;
- data[i*2+1]=value/256;
+ if(manager_servos[i].enabled)
+ {
+ servo_ids[num]=manager_servos[i].id;
+ data[num*2]=manager_servos[i].current_value%256;
+ data[num*2+1]=manager_servos[i].current_value/256;
+ num++;
+ }
}
- dyn_master_sync_write(manager_num_servos,servo_ids,P_GOAL_POSITION_L,2,data);
+ if(num>0)
+ dyn_master_sync_write(manager_num_servos,servo_ids,P_GOAL_POSITION_L,2,data);
}
inline uint16_t manager_angle_to_value(uint8_t servo_id,int16_t angle)
@@ -58,20 +64,30 @@ void manager_get_current_position(void)
{
uint8_t i;
- for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
+ for(i=0;i<manager_num_servos;i++)
{
- if(manager_servos[i].model!=0x0000)// servo is present
+ if(!manager_servos[i].enabled)// servo is enabled
{
- if(manager_servos[i].enabled)// servo is enabled
- {
- // the current position is fixed by one of the motion modules
- }
- else// servo is disabled
+ dyn_master_read_word(manager_servos[i].id,P_PRESENT_POSITION_L,&manager_servos[i].current_value);
+ manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value);
+ }
+ ram_data[BIOLOID_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
+ ram_data[BIOLOID_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8);
+ }
+}
+
+void manager_get_target_position(void)
+{
+ uint8_t i;
+
+ for(i=0;i<manager_num_servos;i++)
+ {
+ if(manager_servos[i].enabled)// servo is enabled
+ {
+ if(manager_servos[i].module==MM_ACTION)
{
- dyn_master_read_word(manager_servos[i].id,P_PRESENT_POSITION_L,&manager_servos[i].current_value);
- manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value);
- ram_data[BIOLOID_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
- ram_data[BIOLOID_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8);
+ manager_servos[i].current_angle=((action_current_angles[i+1])>>16);
+ manager_servos[i].current_value=manager_angle_to_value(i,manager_servos[i].current_angle);
}
}
}
@@ -87,6 +103,8 @@ void MOTION_TIMER_IRQHandler(void)
TIM_ClearITPendingBit(MOTION_TIMER,TIM_IT_CC1);
capture = TIM_GetCapture1(MOTION_TIMER);
TIM_SetCompare1(MOTION_TIMER, capture + manager_motion_period);
+ // get the target angles from all modules
+ // manager_get_target_position();
// call the action process
// action_process();
// call the joint motion process
@@ -96,8 +114,9 @@ void MOTION_TIMER_IRQHandler(void)
// balance the robot
// manager_balance();
// send the motion commands to the servos
- // manager_send_motion_command();
- manager_get_current_position();
+ manager_send_motion_command();
+ // get the disabled servos position
+ // manager_get_current_position();
}
}
@@ -195,6 +214,8 @@ void manager_init(uint16_t period_us)
manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value);
ram_data[BIOLOID_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
ram_data[BIOLOID_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8);
+ // set the action current angles
+ action_current_angles[manager_servos[i].id]=manager_servos[i].current_angle<<16;
manager_num_servos++;
}
else
@@ -276,13 +297,13 @@ inline uint8_t manager_get_num_servos(void)
void manager_set_module(uint8_t servo_id,TModules module)
{
- if(servo_id>0 || servo_id<MANAGER_MAX_NUM_SERVOS)
+ if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
manager_servos[servo_id].module=module;
}
inline TModules manager_get_module(uint8_t servo_id)
{
- if(servo_id>0 || servo_id<MANAGER_MAX_NUM_SERVOS)
+ if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
return manager_servos[servo_id].module;
else
return MM_NONE;
@@ -290,19 +311,22 @@ inline TModules manager_get_module(uint8_t servo_id)
inline void manager_enable_servo(uint8_t servo_id)
{
- if(servo_id>0 || servo_id<MANAGER_MAX_NUM_SERVOS)
+ if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
+ {
manager_servos[servo_id].enabled=0x01;
+ gpio_toggle_led(WEST_LED);
+ }
}
inline void manager_disable_servo(uint8_t servo_id)
{
- if(servo_id>0 || servo_id<MANAGER_MAX_NUM_SERVOS)
+ if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
manager_servos[servo_id].enabled=0x00;
}
inline uint8_t manager_is_servo_enabled(uint8_t servo_id)
{
- if(servo_id>0 || servo_id<MANAGER_MAX_NUM_SERVOS)
+ if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
return manager_servos[servo_id].enabled;
else
return 0x00;
diff --git a/src/system_stm32f4xx.c b/src/system_stm32f4xx.c
index b6a72172346cec06c2cdf05e8277e6466b5fb579..7290a9aca28d73a57011380df2d52a330faab09f 100644
--- a/src/system_stm32f4xx.c
+++ b/src/system_stm32f4xx.c
@@ -260,6 +260,8 @@ void SystemInit(void)
#else
SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */
#endif
+
+ NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
}
/**
diff --git a/src/time.c b/src/time.c
index 343f2cc205d2b34e7642a26e3026860be5b27a70..e36a0d2faf74a0998930771b2746028a49c1e8df 100644
--- a/src/time.c
+++ b/src/time.c
@@ -20,6 +20,7 @@ void time_init(void)
{
// set the time base to 1ms
SysTick_Config(SystemCoreClock / 1000);
+ NVIC_SetPriority(SysTick_IRQn,0);
}
void delay_ms(__IO uint32_t time)