diff --git a/Makefile b/Makefile
index b89b571af64603b5d2b21c23e25a557f0a310875..a79a32c5d258486def72827424a01502ec862523 100755
--- a/Makefile
+++ b/Makefile
@@ -17,6 +17,7 @@ TARGET_FILES+=src/bioloid_dyn_master_sensors.c
TARGET_FILES+=src/bioloid_dyn_master_servos.c
TARGET_FILES+=src/motion_pages.c
TARGET_FILES+=src/motion_manager.c
+TARGET_FILES+=src/action.c
TARGET_PROCESSOR=STM32F407VG
HAL_PATH=../../STM32_processor/hal/f4
diff --git a/include/action.h b/include/action.h
index 74ec9fceb82925eb23c9a117ad15c072a0ca7e8e..79ce518db2a3d6565b86515cd623f904c6e715e2 100644
--- a/include/action.h
+++ b/include/action.h
@@ -19,7 +19,9 @@ 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);
-
+// operation functions
+uint8_t action_in_range(unsigned short int address, unsigned short int length);
+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/include/bioloid_registers.h b/include/bioloid_registers.h
index 24c164a4c60208b38af1d95164f57ef41d1150e2..47a2a89265150e612812b4ea833cb3c13f58cbb6 100644
--- a/include/bioloid_registers.h
+++ b/include/bioloid_registers.h
@@ -177,8 +177,10 @@ typedef enum {
BIOLOID_MM_SERVO30_CUR_POS_L = 0x96,
BIOLOID_MM_SERVO30_CUR_POS_H = 0x97,
BIOLOID_MM_SERVO31_CUR_POS_L = 0x98,
- BIOLOID_MM_SERVO31_CUR_POS_H = 0x99
-
+ BIOLOID_MM_SERVO31_CUR_POS_H = 0x99,
+ BIOLOID_ACTION_PAGE = 0x9A,
+ BIOLOID_ACTION_CNTRL = 0x9B,
+ BIOLOID_ACTION_STATUS = 0x9C
} bioloid_registers;
#define GPIO_BASE_ADDRESS 0x20
@@ -199,6 +201,9 @@ typedef enum {
#define MANAGER_BASE_ADDRESS 0x48
#define MANAGER_MEM_LENGTH 50
+#define ACTION_BASE_ADDRESS 0x9A
+#define ACTION_MEM_LENGTH 3
+
#ifdef __cplusplus
}
#endif
diff --git a/include/motion_manager.h b/include/motion_manager.h
index c6d654c375721f4fc6633ad17caf43a44bb046d3..53867960691ae5878f479b8aab349b44fddb64e6 100644
--- a/include/motion_manager.h
+++ b/include/motion_manager.h
@@ -33,7 +33,7 @@ typedef struct{
}TServoInfo;
// public variables
-extern int64_t current_angles[MANAGER_MAX_NUM_SERVOS];
+extern int64_t manager_current_angles[MANAGER_MAX_NUM_SERVOS];
// public functions
void manager_init(uint16_t period_us);
diff --git a/src/action.c b/src/action.c
index 4089fdda402d932ff1f8657b6939fbba3e87ce9e..c5ed9856fb7b387b6a833c4c632e00970e874989 100644
--- a/src/action.c
+++ b/src/action.c
@@ -23,7 +23,6 @@ uint8_t action_end,action_stop;
uint8_t action_zero_speed_finish[PAGE_MAX_NUM_SERVOS];
uint8_t action_next_index;
uint8_t action_running;
-uint8_t action_slope[PAGE_MAX_NUM_SERVOS];// fixed point 0|8 format
// time variables (in time units (7.8 ms each time unit))
int64_t action_total_time;// fixed point 48|16 format
int64_t action_pre_time;// fixed point 48|16 format
@@ -88,13 +87,6 @@ void action_load_next_step(void)
}
}
pages_copy_page(&action_next_page,&action_current_page);// make the next page active
- for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
- {
- if(action_next_index==0)
- action_slope[i]=32;
- else
- action_slope[i]=pages_get_slope(&action_current_page,i);
- }
if(current_index!=action_next_index)
num_repetitions=action_current_page.header.repeat;
action_current_step_index=0;
@@ -134,46 +126,49 @@ void action_load_next_step(void)
max_angle=0;
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
- angle=action_current_page.steps[action_current_step_index].position[i];
- if(angle==0x5A00)// bigger than 180
- target_angles=current_angles[i];
- else
- target_angles=angle<<9;
- action_moving_angles[i]=target_angles-current_angles[i];
- if(action_end)
- next_angles=target_angles;
- else
+ if(manager_get_module(i)==MM_ACTION)
{
- if(action_current_step_index==action_current_page.header.stepnum-1)
- next_angle=action_next_page.steps[0].position[i];
+ angle=action_current_page.steps[action_current_step_index].position[i];
+ if(angle==0x5A00)// bigger than 180
+ target_angles=manager_current_angles[i];
else
- next_angle=action_current_page.steps[action_current_step_index+1].position[i];
- if(next_angle==0x5A00)
+ target_angles=angle<<9;
+ action_moving_angles[i]=target_angles-manager_current_angles[i];
+ if(action_end)
next_angles=target_angles;
else
- next_angles=next_angle<<9;
- }
- // check changes in direction
- if(((current_angles[i] < target_angles) && (target_angles < next_angles)) ||
- ((current_angles[i] > target_angles) && (target_angles > next_angles)))
- dir_change=0x00;
- else
- dir_change=0x01;
- // check the type of ending
- if(dir_change || action_pause_time>0 || action_end)
- action_zero_speed_finish[i]=0x01;
- else
- action_zero_speed_finish[i]=0x00;
- // find the maximum angle of motion in speed base schedule
- if(action_current_page.header.schedule==SPEED_BASE_SCHEDULE)// speed base schedule
- {
- // fer el valor absolut
- if(action_moving_angles[i]<0)
- tmp_angle=-action_moving_angles[i];
+ {
+ if(action_current_step_index==action_current_page.header.stepnum-1)
+ next_angle=action_next_page.steps[0].position[i];
+ else
+ next_angle=action_current_page.steps[action_current_step_index+1].position[i];
+ if(next_angle==0x5A00)
+ next_angles=target_angles;
+ else
+ next_angles=next_angle<<9;
+ }
+ // check changes in direction
+ if(((manager_current_angles[i] < target_angles) && (target_angles < next_angles)) ||
+ ((manager_current_angles[i] > target_angles) && (target_angles > next_angles)))
+ dir_change=0x00;
+ else
+ dir_change=0x01;
+ // check the type of ending
+ if(dir_change || action_pause_time>0 || action_end)
+ action_zero_speed_finish[i]=0x01;
else
- tmp_angle=action_moving_angles[i];
- if(tmp_angle>max_angle)
- max_angle=tmp_angle;
+ action_zero_speed_finish[i]=0x00;
+ // find the maximum angle of motion in speed base schedule
+ if(action_current_page.header.schedule==SPEED_BASE_SCHEDULE)// speed base schedule
+ {
+ // fer el valor absolut
+ if(action_moving_angles[i]<0)
+ tmp_angle=-action_moving_angles[i];
+ else
+ tmp_angle=action_moving_angles[i];
+ if(tmp_angle>max_angle)
+ max_angle=tmp_angle;
+ }
}
}
if(action_current_page.header.schedule==SPEED_BASE_SCHEDULE)
@@ -202,16 +197,19 @@ void action_load_next_step(void)
zero_speed_divider=action_period;
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
- action_pre_time_initial_speed_angle=(action_start_speed[i]*action_pre_time)>>1;
- moving_angle=action_moving_angles[i]<<16;
- if(action_zero_speed_finish[i])
- action_main_speed[i]=(moving_angle-action_pre_time_initial_speed_angle)/zero_speed_divider;
- else
- action_main_speed[i]=(moving_angle-action_pre_time_initial_speed_angle)/non_zero_speed_divider;
-// if((action_main_speed[i]>>16)>info.max_speed)
-// action_main_speed[i]=(info.max_speed*65536);
-// else if((action_main_speed[i]>>16)<-info.max_speed)
-// action_main_speed[i]=-(info.max_speed*65536);
+ if(manager_get_module(i)==MM_ACTION)
+ {
+ action_pre_time_initial_speed_angle=(action_start_speed[i]*action_pre_time)>>1;
+ moving_angle=action_moving_angles[i]<<16;
+ if(action_zero_speed_finish[i])
+ action_main_speed[i]=(moving_angle-action_pre_time_initial_speed_angle)/zero_speed_divider;
+ else
+ action_main_speed[i]=(moving_angle-action_pre_time_initial_speed_angle)/non_zero_speed_divider;
+// if((action_main_speed[i]>>16)>info.max_speed)
+// action_main_speed[i]=(info.max_speed*65536);
+// else if((action_main_speed[i]>>16)<-info.max_speed)
+// action_main_speed[i]=-(info.max_speed*65536);
+ }
}
}
@@ -220,12 +218,12 @@ void action_init(uint16_t period)
{
unsigned char i;
- // init current_angles with the current position of the servos
+ // init manager_current_angles with the current position of the servos
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
// angle variables
action_moving_angles[i]=0;// fixed point 48|16 format
- action_start_angles[i]=current_angles[i];
+ action_start_angles[i]=manager_current_angles[i];
// speed variables
action_start_speed[i]=0;// fixed point 48|16 format
action_main_speed[i]=0;// fixed point 48|16 format
@@ -273,6 +271,7 @@ uint8_t action_load_page(uint8_t page_id)
return 0x00;
if(!pages_check_checksum(&action_next_page))
return 0x00;
+ ram_data[BIOLOID_ACTION_PAGE]=page_id;
return 0x01;
}
@@ -282,18 +281,20 @@ void action_start_page(void)
uint8_t i;
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
- action_start_angles[i]=current_angles[i];
+ action_start_angles[i]=manager_current_angles[i];
action_stop=0x00;
action_current_time=0;
action_section_time=0;
action_current_step_index=-1;
- ram_set_bit(BIOLOID_ACTION_STATUS,0);
+ ram_data[BIOLOID_ACTION_STATUS]|=0x01;
action_running=0x01;
+ ram_data[BIOLOID_ACTION_CNTRL]|=0x01;
}
void action_stop_page(void)
{
action_stop=0x01;
+ //ram_data[BIOLOID_ACTION_CNTRL]|=0x01;
}
uint8_t action_is_running(void)
@@ -323,28 +324,31 @@ void action_process(void)
{
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
- /* the last segment of the pre section */
- delta_speed=(action_main_speed[i]-action_start_speed[i]);
- current_speed[i]=action_start_speed[i]+delta_speed;
- accel_angles[i]=(action_start_speed[i]*action_section_time+((delta_speed*action_section_time)>>1))>>16;
- current_angles[i]=action_start_angles[i]+accel_angles[i];
- /* update of the state */
- if(!action_zero_speed_finish[i])
+ if(manager_get_module(i)==MM_ACTION)
{
- if((action_step_time-action_pre_time)==0)
- main_angles[i]=0;
+ /* the last segment of the pre section */
+ delta_speed=(action_main_speed[i]-action_start_speed[i]);
+ current_speed[i]=action_start_speed[i]+delta_speed;
+ accel_angles[i]=(action_start_speed[i]*action_section_time+((delta_speed*action_section_time)>>1))>>16;
+ manager_current_angles[i]=action_start_angles[i]+accel_angles[i];
+ /* update of the state */
+ if(!action_zero_speed_finish[i])
+ {
+ if((action_step_time-action_pre_time)==0)
+ main_angles[i]=0;
+ else
+ main_angles[i]=((action_moving_angles[i]-accel_angles[i])*(action_step_time-(action_pre_time<<1)))/(action_step_time-action_pre_time);
+ action_start_angles[i]=manager_current_angles[i];
+ }
else
- main_angles[i]=((action_moving_angles[i]-accel_angles[i])*(action_step_time-(action_pre_time<<1)))/(action_step_time-action_pre_time);
- action_start_angles[i]=current_angles[i];
- }
- else
- {
- main_angles[i]=action_moving_angles[i]-accel_angles[i]-(((action_main_speed[i]*action_pre_time)>>1)>>16);
- action_start_angles[i]=current_angles[i];
+ {
+ main_angles[i]=action_moving_angles[i]-accel_angles[i]-(((action_main_speed[i]*action_pre_time)>>1)>>16);
+ action_start_angles[i]=manager_current_angles[i];
+ }
+ /* the first step of the main section */
+ manager_current_angles[i]=action_start_angles[i]+(main_angles[i]*(action_current_time-action_section_time))/(action_step_time-(action_pre_time<<1));
+ current_speed[i]=action_main_speed[i];
}
- /* the first step of the main section */
- current_angles[i]=action_start_angles[i]+(main_angles[i]*(action_current_time-action_section_time))/(action_step_time-(action_pre_time<<1));
- current_speed[i]=action_main_speed[i];
}
action_current_time=action_current_time-action_section_time;
action_section_time=action_step_time-(action_pre_time<<1);
@@ -354,10 +358,13 @@ void action_process(void)
{
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
- delta_speed=((action_main_speed[i]-action_start_speed[i])*action_current_time)/action_section_time;
- current_speed[i]=action_start_speed[i]+delta_speed;
- accel_angles[i]=((action_start_speed[i]*action_current_time)+((delta_speed*action_current_time)>>1))>>16;
- current_angles[i]=action_start_angles[i]+accel_angles[i];
+ if(manager_get_module(i)==MM_ACTION)
+ {
+ delta_speed=((action_main_speed[i]-action_start_speed[i])*action_current_time)/action_section_time;
+ current_speed[i]=action_start_speed[i]+delta_speed;
+ accel_angles[i]=((action_start_speed[i]*action_current_time)+((delta_speed*action_current_time)>>1))>>16;
+ manager_current_angles[i]=action_start_angles[i]+accel_angles[i];
+ }
}
state=ACTION_PRE;
}
@@ -366,23 +373,26 @@ void action_process(void)
{
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
- /* last segment of the main section */
- current_angles[i]=action_start_angles[i]+main_angles[i];
- current_speed[i]=action_main_speed[i];
- /* update state */
- action_start_angles[i]=current_angles[i];
- main_angles[i]=action_moving_angles[i]-main_angles[i]-accel_angles[i];
- /* first segment of the post section */
- if(action_zero_speed_finish[i])
- {
- delta_speed=((0.0-action_main_speed[i])*(action_current_time-action_section_time))/action_pre_time;
- current_speed[i]=action_main_speed[i]+delta_speed;
- current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16);
- }
- else
+ if(manager_get_module(i)==MM_ACTION)
{
- current_angles[i]=action_start_angles[i]+((main_angles[i]*(action_current_time-action_section_time))/action_pre_time);
+ /* last segment of the main section */
+ manager_current_angles[i]=action_start_angles[i]+main_angles[i];
current_speed[i]=action_main_speed[i];
+ /* update state */
+ action_start_angles[i]=manager_current_angles[i];
+ main_angles[i]=action_moving_angles[i]-main_angles[i]-accel_angles[i];
+ /* first segment of the post section */
+ if(action_zero_speed_finish[i])
+ {
+ delta_speed=((0.0-action_main_speed[i])*(action_current_time-action_section_time))/action_pre_time;
+ current_speed[i]=action_main_speed[i]+delta_speed;
+ manager_current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16);
+ }
+ else
+ {
+ manager_current_angles[i]=action_start_angles[i]+((main_angles[i]*(action_current_time-action_section_time))/action_pre_time);
+ current_speed[i]=action_main_speed[i];
+ }
}
}
action_current_time=action_current_time-action_section_time;
@@ -393,8 +403,11 @@ void action_process(void)
{
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
- current_angles[i]=action_start_angles[i]+(main_angles[i]*action_current_time)/action_section_time;
- current_speed[i]=action_main_speed[i];
+ if(manager_get_module(i)==MM_ACTION)
+ {
+ manager_current_angles[i]=action_start_angles[i]+(main_angles[i]*action_current_time)/action_section_time;
+ current_speed[i]=action_main_speed[i];
+ }
}
state=ACTION_MAIN;
}
@@ -403,30 +416,34 @@ void action_process(void)
{
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
- /* last segment of the post section */
- if(action_zero_speed_finish[i])
+ if(manager_get_module(i)==MM_ACTION)
{
- delta_speed=-action_main_speed[i];
- current_speed[i]=action_main_speed[i]+delta_speed;
- current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*action_section_time)+((delta_speed*action_section_time)>>1))>>16);
- }
- else
- {
- current_angles[i]=action_start_angles[i]+main_angles[i];
- current_speed[i]=action_main_speed[i];
+ /* last segment of the post section */
+ if(action_zero_speed_finish[i])
+ {
+ delta_speed=-action_main_speed[i];
+ current_speed[i]=action_main_speed[i]+delta_speed;
+ manager_current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*action_section_time)+((delta_speed*action_section_time)>>1))>>16);
+ }
+ else
+ {
+ manager_current_angles[i]=action_start_angles[i]+main_angles[i];
+ current_speed[i]=action_main_speed[i];
+ }
+ /* update state */
+ action_start_angles[i]=manager_current_angles[i];
+ action_start_speed[i]=current_speed[i];
}
- /* update state */
- action_start_angles[i]=current_angles[i];
- action_start_speed[i]=current_speed[i];
}
/* load the next step */
if(action_pause_time==0)
{
if(action_end)
{
+ action_load_next_step();
state=ACTION_PAUSE;
action_end=0x00;
- ram_clear_bit(BIOLOID_ACTION_STATUS,0);
+ ram_data[BIOLOID_ACTION_STATUS]&=0xFE;
action_running=0x00;
}
else
@@ -435,10 +452,13 @@ void action_process(void)
/* first step of the next section */
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
- delta_speed=((action_main_speed[i]-action_start_speed[i])*(action_current_time-action_section_time))/action_pre_time;
- current_speed[i]=action_start_speed[i]+delta_speed;
- accel_angles[i]=(((action_start_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16);
- current_angles[i]=action_start_angles[i]+accel_angles[i];
+ if(manager_get_module(i)==MM_ACTION)
+ {
+ delta_speed=((action_main_speed[i]-action_start_speed[i])*(action_current_time-action_section_time))/action_pre_time;
+ current_speed[i]=action_start_speed[i]+delta_speed;
+ accel_angles[i]=(((action_start_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16);
+ manager_current_angles[i]=action_start_angles[i]+accel_angles[i];
+ }
}
action_current_time=action_current_time-action_section_time;
action_section_time=action_pre_time;
@@ -456,16 +476,19 @@ void action_process(void)
{
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
{
- if(action_zero_speed_finish[i])
+ if(manager_get_module(i)==MM_ACTION)
{
- delta_speed=((0.0-action_main_speed[i])*action_current_time)/action_section_time;
- current_speed[i]=action_main_speed[i]+delta_speed;
- current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*action_current_time)+((delta_speed*action_current_time)>>1))>>16);
- }
- else
- {
- current_angles[i]=action_start_angles[i]+(main_angles[i]*action_current_time)/action_section_time;
- current_speed[i]=action_main_speed[i];
+ if(action_zero_speed_finish[i])
+ {
+ delta_speed=((0.0-action_main_speed[i])*action_current_time)/action_section_time;
+ current_speed[i]=action_main_speed[i]+delta_speed;
+ manager_current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*action_current_time)+((delta_speed*action_current_time)>>1))>>16);
+ }
+ else
+ {
+ manager_current_angles[i]=action_start_angles[i]+(main_angles[i]*action_current_time)/action_section_time;
+ current_speed[i]=action_main_speed[i];
+ }
}
}
state=ACTION_POST;
@@ -479,7 +502,7 @@ void action_process(void)
action_load_next_step();
state=ACTION_PAUSE;
action_end=0x00;
- ram_clear_bit(BIOLOID_ACTION_STATUS,0);
+ ram_data[BIOLOID_ACTION_STATUS]&=0xFE;
action_running=0x00;
}
else
@@ -492,7 +515,7 @@ void action_process(void)
delta_speed=((action_main_speed[i]-action_start_speed[i])*(action_current_time-action_section_time))/action_pre_time;
current_speed[i]=action_start_speed[i]+delta_speed;
accel_angles[i]=(((action_start_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16);
- current_angles[i]=action_start_angles[i]+accel_angles[i];
+ manager_current_angles[i]=action_start_angles[i]+accel_angles[i];
}
action_current_time=action_current_time-action_section_time;
action_section_time=action_pre_time;
@@ -507,3 +530,25 @@ void action_process(void)
}
}
+// operation functions
+uint8_t action_in_range(unsigned short int address, unsigned short int length)
+{
+ if(ram_in_window(ACTION_BASE_ADDRESS,ACTION_MEM_LENGTH,address,length))
+ return 0x01;
+ else
+ return 0x00;
+}
+
+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))
+ {
+ if(data[BIOLOID_ACTION_CNTRL-address]&0x01)
+ action_start_page();
+ if(data[BIOLOID_ACTION_CNTRL-address]&0x02)
+ action_stop_page();
+ }
+ 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 4278c010f4b0be431f6dd663fc1f581bbf2652cf..778bef01fd611164d76ea0f08c40effe5028cd56 100644
--- a/src/bioloid_dyn_slave.c
+++ b/src/bioloid_dyn_slave.c
@@ -7,6 +7,7 @@
#include "adc_dma.h"
#include "zigbee.h"
#include "motion_manager.h"
+#include "action.h"
/* external interrupt pin */
#define DYN_SLAVE_EXT_INT_PIN GPIO_PIN_8
@@ -78,6 +79,9 @@ unsigned char bioloid_on_write(unsigned short int address,unsigned short int len
// Manager commands
if(manager_in_range(address,length))
manager_process_write_cmd(address,length,data);
+ // action commands
+ if(action_in_range(address,length))
+ action_process_write_cmd(address,length,data);
// write eeprom
for(i=address,j=0;i<LAST_EEPROM_OFFSET && i<(address+length);i++,j++)
EE_WriteVariable(i,data[j]);
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 64118953ac57ef9f130a4b80e1770f486560dc5e..6ffd8347e49bbfd5a2470a7cec0bfd811180ffe5 100644
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -3,6 +3,7 @@
#include "dyn_servos.h"
#include "ram.h"
#include "gpio.h"
+#include "action.h"
#define MANAGER_TIMER TIM3
#define ENABLE_MANAGER_TIMER_CLK __HAL_RCC_TIM3_CLK_ENABLE()
@@ -69,13 +70,13 @@ void manager_get_current_position(void)
for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
{
- if(!manager_servos[i].enabled)// servo is not enabled
+ if(!manager_servos[i].enabled && manager_servos[i].model!=0x0000)// servo is not enabled but it is present
{
dyn_master_read_word(&bioloid_dyn_master_servos,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);
}
- 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);
}
}
@@ -85,13 +86,15 @@ void manager_get_target_position(void)
for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
{
- if(manager_servos[i].enabled)// servo is enabled
+ if(manager_servos[i].enabled && manager_servos[i].model!=0x0000)// servo is enabled and present
{
if(manager_servos[i].module==MM_ACTION)
{
manager_servos[i].current_angle=((manager_current_angles[i]+manager_balance_offset[i])>>9);
//>>16 from the action codification, <<7 from the manager codification
manager_servos[i].current_value=manager_angle_to_value(i,manager_servos[i].current_angle);
+ 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);
}
}
}
@@ -114,20 +117,19 @@ void MANAGER_TIMER_IRQHandler(void)
capture = HAL_TIM_ReadCapturedValue(&MANAGER_TIM_Handle, TIM_CHANNEL_1);
__HAL_TIM_SET_COMPARE(&MANAGER_TIM_Handle, TIM_CHANNEL_1, (capture + manager_motion_period));
// call the action process
- // action_process();
+ action_process();
// call the joint motion process
// joint_motion_process();
// call the walking process
// walking_process();
// get the target angles from all modules
- // manager_get_target_position();
+ manager_get_target_position();
// 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();
- gpio_toggle_led(RXD_LED);
}
}
}
@@ -283,6 +285,11 @@ void manager_init(uint16_t period_us)
for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
manager_balance_offset[i]=0;
manager_balance_enabled=0x00;
+
+ /* initialize action module */
+ action_init(period_us);
+
+ gpio_blink_led(RXD_LED,1000);
}
uint16_t manager_get_period(void)