Commit 2763aae1 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added an interrupt to notify when the action has finished.

  It can be enabled or disabled.
  It simplifies the wait for the termination of the action.
parent 0fa835a5
......@@ -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);
......
......@@ -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]);
......
......@@ -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;
}
......
......@@ -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();
}
......
Supports Markdown
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