diff --git a/include/action.h b/include/action.h index 79ce518db2a3d6565b86515cd623f904c6e715e2..693ccc167a525e600e8c1f30b103b23f593bf394 100644 --- a/include/action.h +++ b/include/action.h @@ -19,8 +19,12 @@ uint8_t action_load_page(uint8_t page_id); void action_start_page(void); void action_stop_page(void); uint8_t action_is_running(void); +void action_enable_int(void); +void action_disable_int(void); +uint8_t action_is_int_enabled(void); // operation functions uint8_t action_in_range(unsigned short int address, unsigned short int length); +void action_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data); void action_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data); // motion manager interface void action_process(void); diff --git a/src/action.c b/src/action.c index c4262d0c01397f044037cbcecd61f268b96a05e3..c0c6d9773b17d61a2a78ace51c4420b174bd595b 100644 --- a/src/action.c +++ b/src/action.c @@ -2,6 +2,7 @@ #include "motion_pages.h" #include "motion_manager.h" #include "ram.h" +#include "bioloid_dyn_slave.h" #define SPEED_BASE_SCHEDULE 0x00 #define TIME_BASE_SCHEDULE 0x0A @@ -213,6 +214,22 @@ void action_load_next_step(void) } } +void action_finish(void) +{ + // set the internal state machine to the idle state + action_load_next_step(); + action_end=0x00; + // clear the status falg for the action module + ram_data[BIOLOID_ACTION_CNTRL]&=(~ACTION_STATUS); + // set the interrupt falg for the action module + ram_data[BIOLOID_ACTION_CNTRL]|=ACTION_INT_FLAG; + // generate the interrupt in case it is enabled + if(ram_data[BIOLOID_ACTION_CNTRL]&ACTION_INT_EN) + bioloid_dyn_slave_set_int(); + // change the internal state + action_running=0x00; +} + // public functions void action_init(uint16_t period) { @@ -287,6 +304,8 @@ void action_start_page(void) action_section_time=0; action_current_step_index=-1; ram_data[BIOLOID_ACTION_CNTRL]|=ACTION_STATUS; + /* clear the interrupt flag */ + ram_data[BIOLOID_ACTION_CNTRL]&=(~ACTION_INT_FLAG); action_running=0x01; } @@ -300,6 +319,24 @@ uint8_t action_is_running(void) return action_running; } +void action_enable_int(void) +{ + ram_data[BIOLOID_ACTION_CNTRL]|=ACTION_INT_EN; +} + +void action_disable_int(void) +{ + ram_data[BIOLOID_ACTION_CNTRL]&=(~ACTION_INT_EN); +} + +uint8_t action_is_int_enabled(void) +{ + if(ram_data[BIOLOID_ACTION_CNTRL]&ACTION_INT_EN) + return 0x01; + else + return 0x00; +} + // motion manager interface void action_process(void) { @@ -438,11 +475,8 @@ void action_process(void) { if(action_end) { - action_load_next_step(); state=ACTION_PAUSE; - action_end=0x00; - ram_data[BIOLOID_ACTION_CNTRL]&=(~ACTION_STATUS); - action_running=0x00; + action_finish(); } else { @@ -496,12 +530,8 @@ void action_process(void) { if(action_end) { - // find next page to execute - action_load_next_step(); state=ACTION_PAUSE; - action_end=0x00; - ram_data[BIOLOID_ACTION_CNTRL]&=(~ACTION_STATUS); - action_running=0x00; + action_finish(); } else { @@ -534,6 +564,17 @@ uint8_t action_in_range(unsigned short int address, unsigned short int length) return ram_in_window(ACTION_BASE_ADDRESS,ACTION_MEM_LENGTH,address,length); } +void action_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data) +{ + if(ram_in_range(BIOLOID_ACTION_CNTRL,address,length)) + if(ram_data[BIOLOID_ACTION_CNTRL]&ACTION_INT_FLAG) + { + ram_data[BIOLOID_ACTION_CNTRL]&=(~ACTION_INT_FLAG); + if(ram_data[BIOLOID_ACTION_CNTRL]&ACTION_INT_EN) + bioloid_dyn_slave_clear_int(); + } +} + void action_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data) { if(ram_in_range(BIOLOID_ACTION_CNTRL,address,length)) @@ -542,6 +583,10 @@ void action_process_write_cmd(unsigned short int address,unsigned short int leng action_start_page(); if(data[BIOLOID_ACTION_CNTRL-address]&ACTION_STOP) action_stop_page(); + if(data[BIOLOID_ACTION_CNTRL-address]&ACTION_INT_EN) + action_enable_int(); + else + action_disable_int(); } if(ram_in_range(BIOLOID_ACTION_PAGE,address,length))// load the page identifier action_load_page(data[BIOLOID_ACTION_PAGE-address]); diff --git a/src/bioloid_dyn_slave.c b/src/bioloid_dyn_slave.c index 778bef01fd611164d76ea0f08c40effe5028cd56..42305946679fd3738154e7f88622b3eede4c5207 100644 --- a/src/bioloid_dyn_slave.c +++ b/src/bioloid_dyn_slave.c @@ -37,6 +37,9 @@ unsigned char bioloid_on_read(unsigned short int address,unsigned short int leng // GPIO commands if(ram_in_window(GPIO_BASE_ADDRESS,GPIO_MEM_LENGTH,address,length)) gpio_process_read_cmd(address,length,data); + // action commands + if(ram_in_window(ACTION_BASE_ADDRESS,ACTION_MEM_LENGTH,address,length)) + action_process_read_cmd(address,length,data); return error; } diff --git a/src/motion_manager.c b/src/motion_manager.c index 9b294a3b8c1fe8b2dcd30907ba171658712182d6..869d4d9e96ddf3ddce620461c8925297f82874d4 100644 --- a/src/motion_manager.c +++ b/src/motion_manager.c @@ -127,7 +127,7 @@ void MANAGER_TIMER_IRQHandler(void) // balance the robot // manager_balance(); // send the motion commands to the servos - manager_send_motion_command(); + // manager_send_motion_command(); // get the disabled servos position // manager_get_current_position(); }