diff --git a/src/motion_manager.c b/src/motion_manager.c index 703a7c81dd905b7fdbb6472a91c058efae3ed82f..c461bfaaddf2a4d3f0d41e0f72e08f8432cfa590 100644 --- a/src/motion_manager.c +++ b/src/motion_manager.c @@ -1,7 +1,6 @@ #include "motion_manager.h" #include "dynamixel_master_uart_dma.h" #include "dyn_servos.h" -#include "gpio.h" #include "ram.h" #include "action.h" #include "stm32_time.h" @@ -15,28 +14,33 @@ __IO uint16_t manager_motion_period; uint8_t manager_num_servos; TServoInfo manager_servos[MANAGER_MAX_NUM_SERVOS]; +// current angles used for all motion modules +int64_t current_angles[MANAGER_MAX_NUM_SERVOS]; +// balance values and configuration +int64_t balance_offset[MANAGER_MAX_NUM_SERVOS]; +uint8_t balance_enabled; +// package variables +uint8_t servo_ids[MANAGER_MAX_NUM_SERVOS]; +TWriteData write_data[MANAGER_MAX_NUM_SERVOS]; +uint8_t length[MANAGER_MAX_NUM_SERVOS]; +uint8_t address[MANAGER_MAX_NUM_SERVOS]; // private functions void manager_send_motion_command(void) { - uint8_t servo_ids[MANAGER_MAX_NUM_SERVOS]; - uint8_t data[256]; uint8_t i,num=0; - + for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++) { - if(manager_servos[i].enabled) + if(manager_servos[i].enabled && manager_servos[i].model!=0x0000) { servo_ids[num]=manager_servos[i].id; - 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; + write_data[num].data_addr=(uint8_t *)&(manager_servos[i].current_value); num++; } } if(num>0) - dyn_master_sync_write(num,servo_ids,P_CW_COMPLIANCE_SLOPE,4,data); + dyn_master_sync_write(num,servo_ids,P_GOAL_POSITION_L,2,write_data); } inline uint16_t manager_angle_to_value(uint8_t servo_id,int16_t angle) @@ -67,7 +71,7 @@ void manager_get_current_position(void) for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++) { - if(!manager_servos[i].enabled)// servo is enabled + if(!manager_servos[i].enabled)// servo is not enabled { 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); @@ -87,7 +91,7 @@ void manager_get_target_position(void) { if(manager_servos[i].module==MM_ACTION) { - manager_servos[i].current_angle=((action_current_angles[i])>>9); + manager_servos[i].current_angle=((current_angles[i]+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); } @@ -97,17 +101,6 @@ void manager_get_target_position(void) void manager_balance(void) { - int16_t x_error=0.0; - int16_t y_error=0.0; - - manager_servos[10].current_value+=(y_error*ram_data[BIOLOID_MM_PELVIS_ROLL_GAIN_L]*11)/2500; - manager_servos[9].current_value+=(y_error*ram_data[BIOLOID_MM_PELVIS_ROLL_GAIN_R]*11)/2500; - manager_servos[18].current_value-=(y_error*ram_data[BIOLOID_MM_ANKLE_ROLL_GAIN_L]*11)/2500; - manager_servos[17].current_value-=(y_error*ram_data[BIOLOID_MM_ANKLE_ROLL_GAIN_R]*11)/2500; - manager_servos[16].current_value-=(x_error*ram_data[BIOLOID_MM_ANKLE_PITCH_GAIN_L]*11)/2500; - manager_servos[15].current_value+=(x_error*ram_data[BIOLOID_MM_ANKLE_PITCH_GAIN_R]*11)/2500; - manager_servos[14].current_value-=(x_error*ram_data[BIOLOID_MM_KNEE_GAIN_L]*11)/2500; - manager_servos[13].current_value+=(x_error*ram_data[BIOLOID_MM_KNEE_GAIN_R]*11)/2500; } // interrupt handlers @@ -129,7 +122,7 @@ 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 @@ -144,14 +137,14 @@ void manager_init(uint16_t period_us) TIM_OCInitTypeDef TIM_OCInitStructure; NVIC_InitTypeDef NVIC_InitStructure; uint8_t servo_ids[MANAGER_MAX_NUM_SERVOS]; + uint16_t model,value; uint8_t i,num=0; - uint16_t model; RCC_APB1PeriphClockCmd(MOTION_TIMER_CLK,ENABLE); // enable power to the servos dyn_master_enable_power(); - delay_ms(300); + delay_ms(1000); // detect the servos connected dyn_master_scan(&num,servo_ids); @@ -236,8 +229,13 @@ 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); + // read the servo limits + dyn_master_read_word(manager_servos[i].id,P_CW_ANGLE_LIMIT_L,&value); + manager_servos[i].cw_angle_limit=manager_value_to_angle(i,value); + dyn_master_read_word(manager_servos[i].id,P_CCW_ANGLE_LIMIT_L,&value); + manager_servos[i].ccw_angle_limit=manager_value_to_angle(i,value); // set the action current angles - action_current_angles[i]=manager_servos[i].current_angle<<9; + current_angles[i]=manager_servos[i].current_angle<<9; manager_num_servos++; } else @@ -253,6 +251,8 @@ void manager_init(uint16_t period_us) manager_servos[i].current_value=0; manager_servos[i].current_angle=0; manager_servos[i].enabled=0x00; + manager_servos[i].cw_angle_limit=0; + manager_servos[i].ccw_angle_limit=0; } } dyn_master_disable_power(); @@ -266,21 +266,28 @@ void manager_init(uint16_t period_us) /* motion timer configuration */ TIM_TimeBaseStructure.TIM_Period = 0xFFFF; - TIM_TimeBaseStructure.TIM_Prescaler = 72; + TIM_TimeBaseStructure.TIM_Prescaler = 42; TIM_TimeBaseStructure.TIM_ClockDivision = 0; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseInit(MOTION_TIMER,&TIM_TimeBaseStructure); - TIM_SetClockDivision(MOTION_TIMER,TIM_CKD_DIV1); TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Timing; TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Disable; TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; - manager_motion_period=period_us; + manager_motion_period=(period_us*1000000)>>16; TIM_OCInitStructure.TIM_Pulse = manager_motion_period; TIM_OC1Init(MOTION_TIMER, &TIM_OCInitStructure); TIM_OC1PreloadConfig(MOTION_TIMER, TIM_OCPreload_Disable); ram_clear_bit(BIOLOID_MM_STATUS,0); + + /* initialize motion modules */ + action_init(period_us); + + /* initialize balance parameters */ + for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++) + balance_offset[i]=0; + balance_enabled=0x01; } uint16_t manager_get_period(void) @@ -290,11 +297,17 @@ uint16_t manager_get_period(void) void manager_set_period(uint16_t period_us) { - manager_motion_period=period_us; + manager_motion_period=(period_us*1000000)>>16; + ram_write_word(BIOLOID_MM_PERIOD_H,period_us); + action_set_period(period_us); } inline void manager_enable(void) { + uint16_t capture; + + capture = TIM_GetCapture1(MOTION_TIMER); + TIM_SetCompare1(MOTION_TIMER, capture + manager_motion_period); TIM_Cmd(MOTION_TIMER, ENABLE); TIM_ITConfig(MOTION_TIMER, TIM_IT_CC1, ENABLE); ram_set_bit(BIOLOID_MM_STATUS,0); @@ -312,6 +325,20 @@ inline uint8_t manager_is_enabled(void) return ram_data[BIOLOID_MM_STATUS]&0x01; } +void manager_enable_balance(void) +{ + ram_set_bit(BIOLOID_MM_STATUS,1); + ram_set_bit(BIOLOID_MM_CONTROL,1); + balance_enabled=0x01; +} + +void manager_disable_balance(void) +{ + ram_clear_bit(BIOLOID_MM_STATUS,1); + ram_clear_bit(BIOLOID_MM_CONTROL,1); + balance_enabled=0x00; +} + inline uint8_t manager_get_num_servos(void) { return manager_num_servos; @@ -319,8 +346,18 @@ inline uint8_t manager_get_num_servos(void) void manager_set_module(uint8_t servo_id,TModules module) { + uint8_t byte; + if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS) + { manager_servos[servo_id].module=module; + ram_read_byte(BIOLOID_MM_MODULE_EN0+servo_id/2,&byte); + if(servo_id%2) + byte=(byte&0xF8)+((uint8_t)module); + else + byte=(byte&0x8F)+(((uint8_t)module)<<4); + ram_write_byte(BIOLOID_MM_MODULE_EN0+servo_id/2,byte); + } } inline TModules manager_get_module(uint8_t servo_id) @@ -333,33 +370,38 @@ inline TModules manager_get_module(uint8_t servo_id) inline void manager_enable_servo(uint8_t servo_id) { + uint8_t byte; + if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS) + { manager_servos[servo_id].enabled=0x01; + ram_read_byte(BIOLOID_MM_MODULE_EN0+servo_id/2,&byte); + if(servo_id%2) + byte|=0x08; + else + byte|=0x80; + ram_write_byte(BIOLOID_MM_MODULE_EN0+servo_id/2,byte); + } } inline void manager_disable_servo(uint8_t servo_id) { + uint8_t byte; + if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS) + { manager_servos[servo_id].enabled=0x00; + ram_read_byte(BIOLOID_MM_MODULE_EN0+servo_id/2,&byte); + if(servo_id%2) + byte&=0xF7; + else + byte&=0x7F; + ram_write_byte(BIOLOID_MM_MODULE_EN0+servo_id/2,byte); + } } void manager_disable_servos(void) { - uint8_t servo_ids[MANAGER_MAX_NUM_SERVOS]; - uint8_t data[256]; - uint8_t i,num=0; - - for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++) - { - if(!manager_servos[i].enabled) - { - servo_ids[num]=manager_servos[i].id; - data[num]=0x00; - num++; - } - } - if(num>0) - dyn_master_sync_write(num,servo_ids,P_TORQUE_ENABLE,1,data); } inline uint8_t manager_is_servo_enabled(uint8_t servo_id) @@ -370,3 +412,13 @@ inline uint8_t manager_is_servo_enabled(uint8_t servo_id) return 0x00; } +inline int16_t manager_get_cw_angle_limit(uint8_t servo_id) +{ + return manager_servos[servo_id].cw_angle_limit; +} + +inline int16_t manager_get_ccw_angle_limit(uint8_t servo_id) +{ + return manager_servos[servo_id].ccw_angle_limit; +} +