Commit 43757f7e authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added the action module to the motion manager.

parent 4c501509
......@@ -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
......
......@@ -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);
......
......@@ -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
......
......@@ -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);
......
This diff is collapsed.
......@@ -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]);
......
......@@ -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)
......
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