From 4861ca1d8fe769e5bfd3249b080724c112fada9d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu> Date: Wed, 6 Aug 2014 17:28:41 +0000 Subject: [PATCH] If servos are missing, the actions now execute properly, assigning each servos the correct angle. When the page 0 is loaded, a default value of 32 is loaded to the compliance slope. --- src/action.c | 7 ++++++- src/motion_manager.c | 31 ++++++++++++++++--------------- 2 files changed, 22 insertions(+), 16 deletions(-) diff --git a/src/action.c b/src/action.c index 2cb59bc..ee7f1e4 100644 --- a/src/action.c +++ b/src/action.c @@ -88,7 +88,12 @@ 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++) - action_slope[i]=pages_get_slope(&action_current_page,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; diff --git a/src/motion_manager.c b/src/motion_manager.c index c8e4040..5d260ac 100644 --- a/src/motion_manager.c +++ b/src/motion_manager.c @@ -23,13 +23,13 @@ void manager_send_motion_command(void) uint8_t data[256]; uint8_t i,num=0; - for(i=0;i<manager_num_servos;i++) + for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++) { if(manager_servos[i].enabled) { servo_ids[num]=manager_servos[i].id; - data[num*4]=action_slope[i+1]; - data[num*4+1]=action_slope[i+1]; + data[num*4]=action_slope[i]; + data[num*4+1]=action_slope[i]; data[num*4+2]=manager_servos[i].current_value%256; data[num*4+3]=manager_servos[i].current_value/256; num++; @@ -65,7 +65,7 @@ void manager_get_current_position(void) { uint8_t i; - for(i=0;i<manager_num_servos;i++) + for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++) { if(!manager_servos[i].enabled)// servo is enabled { @@ -81,13 +81,13 @@ void manager_get_target_position(void) { uint8_t i; - for(i=0;i<manager_num_servos;i++) + for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++) { if(manager_servos[i].enabled)// servo is enabled { if(manager_servos[i].module==MM_ACTION) { - manager_servos[i].current_angle=((action_current_angles[i+1])>>9); + manager_servos[i].current_angle=((action_current_angles[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); } @@ -129,11 +129,11 @@ void MOTION_TIMER_IRQHandler(void) // get the target angles from all modules manager_get_target_position(); // balance the robot - manager_balance(); +// manager_balance(); // send the motion commands to the servos manager_send_motion_command(); // get the disabled servos position - manager_get_current_position(); +// manager_get_current_position(); } } @@ -156,12 +156,13 @@ void manager_init(uint16_t period_us) // detect the servos connected dyn_master_scan(&num,servo_ids); ram_data[BIOLOID_MM_NUM_SERVOS]=num; + manager_num_servos=0; for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++) { - if(i<num) + if(i==servo_ids[manager_num_servos]) { // read the model of the i-th device - dyn_master_read_word(servo_ids[i],P_MODEL_NUMBER_L,&model); + dyn_master_read_word(servo_ids[manager_num_servos],P_MODEL_NUMBER_L,&model); switch(model) { case SERVO_AX12A: manager_servos[i].encoder_resolution=1023; @@ -226,7 +227,7 @@ void manager_init(uint16_t period_us) break; default: break; } - manager_servos[i].id=servo_ids[i]; + manager_servos[i].id=servo_ids[manager_num_servos]; manager_servos[i].model=model; manager_servos[i].module=MM_NONE; manager_servos[i].enabled=0x00; @@ -236,12 +237,12 @@ void manager_init(uint16_t period_us) 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[i+1]=manager_servos[i].current_angle<<9; + action_current_angles[i]=manager_servos[i].current_angle<<9; manager_num_servos++; } else { - manager_servos[i].id=0; + manager_servos[i].id=i; manager_servos[i].model=0x0000; manager_servos[i].module=MM_NONE; manager_servos[i].encoder_resolution=0; @@ -254,7 +255,7 @@ void manager_init(uint16_t period_us) manager_servos[i].enabled=0x00; } } - manager_num_servos=num; + dyn_master_disable_power(); // initialize the timer interrupts NVIC_InitStructure.NVIC_IRQChannel = MOTION_TIMER_IRQn; @@ -348,7 +349,7 @@ void manager_disable_servos(void) uint8_t data[256]; uint8_t i,num=0; - for(i=0;i<manager_num_servos;i++) + for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++) { if(!manager_servos[i].enabled) { -- GitLab