Commit 583e3ac3 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added the balance feature to the motion manager module.

parent 9cfc070e
......@@ -18,6 +18,7 @@ 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_FILES+=src/bioloid_gyro.c
TARGET_PROCESSOR=STM32F407VG
HAL_PATH=../../STM32_processor/hal/f4
......
......@@ -5,7 +5,7 @@
extern "C" {
#endif
#include "stm32f4xx.h"
#include "stm32f4xx_hal.h"
#include "motion_pages.h"
extern int64_t action_current_angles[PAGE_MAX_NUM_SERVOS];
......
......@@ -15,7 +15,7 @@
#define F_ATTACK 22
#define DEFENCE 23
#define SIT_DOWN 25
#define STANF_UP 26
#define STAND_UP 26
#define F_GET_UP 27
#define B_GET_UP 28
#define CLAP_READY 29
......@@ -137,8 +137,4 @@
#define BALANCE 224
#define CLAP_END 225
#endif
#endif
......@@ -13,6 +13,7 @@ typedef enum {ADC_CH1,ADC_CH2,ADC_CH3,ADC_CH4,ADC_CH6,ADC_CH8} adc_ch_t;
void adc_init(void);
void adc_start(void);
void adc_set_period(uint8_t period_ms);
inline uint8_t adc_get_period(void);
unsigned short adc_get_channel(adc_ch_t channel);
unsigned short adc_get_channel_raw(adc_ch_t channel);
unsigned short adc_get_temperature(void);
......
#ifndef BIOLOID_GYRO_H
#define BIOLOID_GYRO_H
#include "stm32f4xx_hal.h"
#include "adc_dma.h"
/* public functions */
void gyro_init(void);
inline adc_ch_t gyro_get_fb_adc_channel(void);
inline adc_ch_t gyro_get_lr_adc_channel(void);
void gyro_calibrate(void);
inline void gyro_get_offsets(uint16_t *fb_offset,uint16_t *lr_offset);
// operation functions
uint8_t gyro_in_range(unsigned short int address, unsigned short int length);
void gyro_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
void gyro_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
#endif
......@@ -11,8 +11,8 @@ extern "C" {
#define BAUDRATE_OFFSET ((unsigned short int)0x0004)
#define RETURN_DELAY_OFFSET ((unsigned short int)0x0005)
#define MM_PERIOD_OFFSET ((unsigned short int)0x0006)
#define GYRO_FB_ADC_CH_OFFSET ((unsigned short int)0x0007)
#define GYRO_LR_ADC_CH_OFFSET ((unsigned short int)0x0008)
#define GYRO_FB_ADC_CH_OFFSET ((unsigned short int)0x0008)
#define GYRO_LR_ADC_CH_OFFSET ((unsigned short int)0x0009)
#define RETURN_LEVEL_OFFSET ((unsigned short int)0x0010)
#define LAST_EEPROM_OFFSET ((unsigned short int)0x0020)
......@@ -118,76 +118,76 @@ typedef enum {
// Enable servo 28 | assigned module | Enable servo 29 | assigned module
BIOLOID_MM_MODULE_EN15 = 0x59, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// Enable servo 30 | assigned module | Enable servo 31 | assigned module
BIOLOID_MM_SERVO0_CUR_POS_L = 0x5A,
BIOLOID_MM_SERVO0_CUR_POS_L = 0x5A, // angle in fixed point format 9|7
BIOLOID_MM_SERVO0_CUR_POS_H = 0x5B,
BIOLOID_MM_SERVO1_CUR_POS_L = 0x5C,
BIOLOID_MM_SERVO1_CUR_POS_L = 0x5C, // angle in fixed point format 9|7
BIOLOID_MM_SERVO1_CUR_POS_H = 0x5D,
BIOLOID_MM_SERVO2_CUR_POS_L = 0x5E,
BIOLOID_MM_SERVO2_CUR_POS_L = 0x5E, // angle in fixed point format 9|7
BIOLOID_MM_SERVO2_CUR_POS_H = 0x5F,
BIOLOID_MM_SERVO3_CUR_POS_L = 0x60,
BIOLOID_MM_SERVO3_CUR_POS_L = 0x60, // angle in fixed point format 9|7
BIOLOID_MM_SERVO3_CUR_POS_H = 0x61,
BIOLOID_MM_SERVO4_CUR_POS_L = 0x62,
BIOLOID_MM_SERVO4_CUR_POS_L = 0x62, // angle in fixed point format 9|7
BIOLOID_MM_SERVO4_CUR_POS_H = 0x63,
BIOLOID_MM_SERVO5_CUR_POS_L = 0x64,
BIOLOID_MM_SERVO5_CUR_POS_L = 0x64, // angle in fixed point format 9|7
BIOLOID_MM_SERVO5_CUR_POS_H = 0x65,
BIOLOID_MM_SERVO6_CUR_POS_L = 0x66,
BIOLOID_MM_SERVO6_CUR_POS_L = 0x66, // angle in fixed point format 9|7
BIOLOID_MM_SERVO6_CUR_POS_H = 0x67,
BIOLOID_MM_SERVO7_CUR_POS_L = 0x68,
BIOLOID_MM_SERVO7_CUR_POS_L = 0x68, // angle in fixed point format 9|7
BIOLOID_MM_SERVO7_CUR_POS_H = 0x69,
BIOLOID_MM_SERVO8_CUR_POS_L = 0x6A,
BIOLOID_MM_SERVO8_CUR_POS_L = 0x6A, // angle in fixed point format 9|7
BIOLOID_MM_SERVO8_CUR_POS_H = 0x6B,
BIOLOID_MM_SERVO9_CUR_POS_L = 0x6C,
BIOLOID_MM_SERVO9_CUR_POS_L = 0x6C, // angle in fixed point format 9|7
BIOLOID_MM_SERVO9_CUR_POS_H = 0x6D,
BIOLOID_MM_SERVO10_CUR_POS_L = 0x6E,
BIOLOID_MM_SERVO10_CUR_POS_L = 0x6E, // angle in fixed point format 9|7
BIOLOID_MM_SERVO10_CUR_POS_H = 0x6F,
BIOLOID_MM_SERVO11_CUR_POS_L = 0x70,
BIOLOID_MM_SERVO11_CUR_POS_L = 0x70, // angle in fixed point format 9|7
BIOLOID_MM_SERVO11_CUR_POS_H = 0x71,
BIOLOID_MM_SERVO12_CUR_POS_L = 0x72,
BIOLOID_MM_SERVO12_CUR_POS_L = 0x72, // angle in fixed point format 9|7
BIOLOID_MM_SERVO12_CUR_POS_H = 0x73,
BIOLOID_MM_SERVO13_CUR_POS_L = 0x74,
BIOLOID_MM_SERVO13_CUR_POS_L = 0x74, // angle in fixed point format 9|7
BIOLOID_MM_SERVO13_CUR_POS_H = 0x75,
BIOLOID_MM_SERVO14_CUR_POS_L = 0x76,
BIOLOID_MM_SERVO14_CUR_POS_L = 0x76, // angle in fixed point format 9|7
BIOLOID_MM_SERVO14_CUR_POS_H = 0x77,
BIOLOID_MM_SERVO15_CUR_POS_L = 0x78,
BIOLOID_MM_SERVO15_CUR_POS_L = 0x78, // angle in fixed point format 9|7
BIOLOID_MM_SERVO15_CUR_POS_H = 0x79,
BIOLOID_MM_SERVO16_CUR_POS_L = 0x7A,
BIOLOID_MM_SERVO16_CUR_POS_L = 0x7A, // angle in fixed point format 9|7
BIOLOID_MM_SERVO16_CUR_POS_H = 0x7B,
BIOLOID_MM_SERVO17_CUR_POS_L = 0x7C,
BIOLOID_MM_SERVO17_CUR_POS_L = 0x7C, // angle in fixed point format 9|7
BIOLOID_MM_SERVO17_CUR_POS_H = 0x7D,
BIOLOID_MM_SERVO18_CUR_POS_L = 0x7E,
BIOLOID_MM_SERVO18_CUR_POS_L = 0x7E, // angle in fixed point format 9|7
BIOLOID_MM_SERVO18_CUR_POS_H = 0x7F,
BIOLOID_MM_SERVO19_CUR_POS_L = 0x80,
BIOLOID_MM_SERVO19_CUR_POS_L = 0x80, // angle in fixed point format 9|7
BIOLOID_MM_SERVO19_CUR_POS_H = 0x81,
BIOLOID_MM_SERVO20_CUR_POS_L = 0x82,
BIOLOID_MM_SERVO20_CUR_POS_L = 0x82, // angle in fixed point format 9|7
BIOLOID_MM_SERVO20_CUR_POS_H = 0x83,
BIOLOID_MM_SERVO21_CUR_POS_L = 0x84,
BIOLOID_MM_SERVO21_CUR_POS_L = 0x84, // angle in fixed point format 9|7
BIOLOID_MM_SERVO21_CUR_POS_H = 0x85,
BIOLOID_MM_SERVO22_CUR_POS_L = 0x86,
BIOLOID_MM_SERVO22_CUR_POS_L = 0x86, // angle in fixed point format 9|7
BIOLOID_MM_SERVO22_CUR_POS_H = 0x87,
BIOLOID_MM_SERVO23_CUR_POS_L = 0x88,
BIOLOID_MM_SERVO23_CUR_POS_L = 0x88, // angle in fixed point format 9|7
BIOLOID_MM_SERVO23_CUR_POS_H = 0x89,
BIOLOID_MM_SERVO24_CUR_POS_L = 0x8A,
BIOLOID_MM_SERVO24_CUR_POS_L = 0x8A, // angle in fixed point format 9|7
BIOLOID_MM_SERVO24_CUR_POS_H = 0x8B,
BIOLOID_MM_SERVO25_CUR_POS_L = 0x8C,
BIOLOID_MM_SERVO25_CUR_POS_L = 0x8C, // angle in fixed point format 9|7
BIOLOID_MM_SERVO25_CUR_POS_H = 0x8D,
BIOLOID_MM_SERVO26_CUR_POS_L = 0x8E,
BIOLOID_MM_SERVO26_CUR_POS_L = 0x8E, // angle in fixed point format 9|7
BIOLOID_MM_SERVO26_CUR_POS_H = 0x8F,
BIOLOID_MM_SERVO27_CUR_POS_L = 0x90,
BIOLOID_MM_SERVO27_CUR_POS_L = 0x90, // angle in fixed point format 9|7
BIOLOID_MM_SERVO27_CUR_POS_H = 0x91,
BIOLOID_MM_SERVO28_CUR_POS_L = 0x92,
BIOLOID_MM_SERVO28_CUR_POS_L = 0x92, // angle in fixed point format 9|7
BIOLOID_MM_SERVO28_CUR_POS_H = 0x93,
BIOLOID_MM_SERVO29_CUR_POS_L = 0x94,
BIOLOID_MM_SERVO29_CUR_POS_L = 0x94, // angle in fixed point format 9|7
BIOLOID_MM_SERVO29_CUR_POS_H = 0x95,
BIOLOID_MM_SERVO30_CUR_POS_L = 0x96,
BIOLOID_MM_SERVO30_CUR_POS_L = 0x96, // angle in fixed point format 9|7
BIOLOID_MM_SERVO30_CUR_POS_H = 0x97,
BIOLOID_MM_SERVO31_CUR_POS_L = 0x98,
BIOLOID_MM_SERVO31_CUR_POS_L = 0x98, // angle in fixed point format 9|7
BIOLOID_MM_SERVO31_CUR_POS_H = 0x99,
BIOLOID_ACTION_PAGE = 0x9A,
BIOLOID_ACTION_CNTRL = 0x9B, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// | | | page running | interrupt flag | enable interrupt | stop page | start page
BIOLOID_GYRO_CNTRL = 0x9C, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
//
BIOLOID_GYRO_STATUS = 0x9D, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
BIOLOID_GYRO_CNTRL = 0x9C, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// | | | | enable fall int | enable fall det | enable calibrate int | calibrate
BIOLOID_GYRO_STATUS = 0x9D, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
//
BIOLOID_GYRO_FWD_FALL_THD = 0x9E,
BIOLOID_GYRO_BWD_FALL_THD = 0x9F,
......@@ -231,6 +231,13 @@ typedef enum {
#define ACTION_INT_FLAG 0x08
#define ACTION_STATUS 0x10
#define GYRO_BASE_ADDRESS 0x9C
#define GYRO_MEM_LENGTH 6
#define GYRO_CALIBRATE 0x01
#define GYRO_EN_CAL_INT 0x02
#define GYRO_EN_FALL_DET 0x04
#define GYRO_EN_FALL_DET_INT 0x08
#ifdef __cplusplus
}
#endif
......
......@@ -80,6 +80,7 @@ void action_load_next_step(void)
if(action_next_index!=current_index)
{
pages_get_page(action_next_index,&action_next_page);
ram_data[BIOLOID_ACTION_PAGE]=action_next_index;
if(!pages_check_checksum(&action_next_page))
action_end=0x01;
}
......@@ -112,6 +113,7 @@ void action_load_next_step(void)
if(action_next_index!=current_index)
{
pages_get_page(action_next_index,&action_next_page);
ram_data[BIOLOID_ACTION_PAGE]=action_next_index;
if(!pages_check_checksum(&action_next_page))
action_end=0x01;
}
......
......@@ -186,7 +186,7 @@ void adc_init(void)
/* initialize the internal variables */
adc_period_ms=840;// equivalent to 10 ms
ram_data[BIOLOID_ADC_PERIOD]=84;
ram_data[BIOLOID_ADC_PERIOD]=10;
/* enable clocks */
ADC1_CH1_ENABLE_PORT_CLK;
......@@ -360,6 +360,12 @@ void adc_start(void)
void adc_set_period(uint8_t period_ms)
{
adc_period_ms=period_ms*84;
ram_data[BIOLOID_ADC_PERIOD]=period_ms;
}
inline uint8_t adc_get_period(void)
{
return ram_data[BIOLOID_ADC_PERIOD];
}
uint16_t adc_get_channel(adc_ch_t channel)
......@@ -368,18 +374,19 @@ uint16_t adc_get_channel(adc_ch_t channel)
switch(channel)
{
case ADC_CH1: ram_data[BIOLOID_ADC_CH1_L]+ram_data[BIOLOID_ADC_CH1_H]*256;
case ADC_CH1: value=ram_data[BIOLOID_ADC_CH1_L]+ram_data[BIOLOID_ADC_CH1_H]*256;
break;
case ADC_CH2: ram_data[BIOLOID_ADC_CH2_L]+ram_data[BIOLOID_ADC_CH2_H]*256;
case ADC_CH2: value=ram_data[BIOLOID_ADC_CH2_L]+ram_data[BIOLOID_ADC_CH2_H]*256;
break;
case ADC_CH3: ram_data[BIOLOID_ADC_CH3_L]+ram_data[BIOLOID_ADC_CH3_H]*256;
case ADC_CH3: value=ram_data[BIOLOID_ADC_CH3_L]+ram_data[BIOLOID_ADC_CH3_H]*256;
break;
case ADC_CH4: ram_data[BIOLOID_ADC_CH4_L]+ram_data[BIOLOID_ADC_CH4_H]*256;
case ADC_CH4: value=ram_data[BIOLOID_ADC_CH4_L]+ram_data[BIOLOID_ADC_CH4_H]*256;
break;
case ADC_CH6: ram_data[BIOLOID_ADC_CH6_L]+ram_data[BIOLOID_ADC_CH6_H]*256;
case ADC_CH6: value=ram_data[BIOLOID_ADC_CH6_L]+ram_data[BIOLOID_ADC_CH6_H]*256;
break;
case ADC_CH8: ram_data[BIOLOID_ADC_CH8_L]+ram_data[BIOLOID_ADC_CH8_H]*256;
case ADC_CH8: value=ram_data[BIOLOID_ADC_CH8_L]+ram_data[BIOLOID_ADC_CH8_H]*256;
break;
default: value=0x0000;
}
return value;
......@@ -403,6 +410,7 @@ uint16_t adc_get_channel_raw(adc_ch_t channel)
break;
case ADC_CH8: value=(adc_data[3]&0xFFFF0000)>>16;
break;
default: value=0x0000;
}
return value;
......
......@@ -6,6 +6,7 @@
#include "gpio.h"
#include "adc_dma.h"
#include "zigbee.h"
#include "bioloid_gyro.h"
#include "motion_manager.h"
#include "action.h"
......@@ -85,6 +86,9 @@ unsigned char bioloid_on_write(unsigned short int address,unsigned short int len
// action commands
if(action_in_range(address,length))
action_process_write_cmd(address,length,data);
// action commands
if(gyro_in_range(address,length))
gyro_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]);
......
#include "bioloid_gyro.h"
#include "ram.h"
#define GYRO_MAX_CAL_SAMPLES 256
/* private variables */
uint32_t gyro_fb_offset;
uint32_t gyro_lr_offset;
/* private functions */
/* public functions */
void gyro_init(void)
{
/* initialize internal variables */
gyro_fb_offset=0x0000;
gyro_lr_offset=0x0000;
/* calibrate the sensor */
gyro_calibrate();
}
inline adc_ch_t gyro_get_fb_adc_channel(void)
{
return ram_data[BIOLIOD_GYRO_FB_ADC_CH];
}
inline adc_ch_t gyro_get_lr_adc_channel(void)
{
return ram_data[BIOLIOD_GYRO_LR_ADC_CH];
}
void gyro_calibrate(void)
{
adc_ch_t fb_ch=ram_data[BIOLIOD_GYRO_FB_ADC_CH],lr_ch=ram_data[BIOLIOD_GYRO_LR_ADC_CH];
uint16_t i;
gyro_fb_offset=0x0000;
gyro_lr_offset=0x0000;
for(i=0;i<GYRO_MAX_CAL_SAMPLES;i++)
{
gyro_fb_offset+=adc_get_channel_raw(fb_ch);
gyro_lr_offset+=adc_get_channel_raw(lr_ch);
/* wait for the next sample */
HAL_Delay(adc_get_period());
}
gyro_fb_offset/=GYRO_MAX_CAL_SAMPLES;
gyro_lr_offset/=GYRO_MAX_CAL_SAMPLES;
}
inline void gyro_get_offsets(uint16_t *fb_offset,uint16_t *lr_offset)
{
*fb_offset=gyro_fb_offset;
*lr_offset=gyro_lr_offset;
}
// operation functions
uint8_t gyro_in_range(unsigned short int address, unsigned short int length)
{
if(ram_in_window(GYRO_BASE_ADDRESS,GYRO_MEM_LENGTH,address,length) ||
ram_in_window(BIOLIOD_GYRO_FB_ADC_CH,BIOLIOD_GYRO_LR_ADC_CH,address,length))
return 0x01;
else
return 0x00;
}
void gyro_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
{
}
void gyro_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
{
}
#include "bioloid_gyro.h"
#include "adc_dma.h"
/* private functions */
/* public functions */
void gyro_init(void)
{
}
......@@ -4,6 +4,7 @@
#include "ram.h"
#include "adc_dma.h"
#include "zigbee.h"
#include "bioloid_gyro.h"
#include "bioloid_time.h"
#include "bioloid_dyn_slave.h"
#include "bioloid_dyn_master_sensors.h"
......@@ -26,11 +27,14 @@ int32_t main(void)
bioloid_time_init();
/* initialize zigbee module */
zigbee_init();
zigbee_enable_power();
/* initialize the dynamixel slave interface */
bioloid_dyn_slave_init();
bioloid_dyn_slave_start();
/* initialize the dynamixel master module for the sensors */
bioloid_dyn_master_sensors_init();
/* initialize gyro module */
gyro_init();
/* initialize motion manager module */
EE_ReadVariable(MM_PERIOD_OFFSET,&eeprom_data);
period=eeprom_data&0x00FF;
......
......@@ -52,6 +52,8 @@
#define DEFAULT_BAUDRATE 0x0001
#define DEFAULT_RETURN_DELAY 0x0000
#define DEFAULT_MM_PERIOD 0x01FF
#define DEFAULT_GYRO_FB_ADC_CH 0x0002
#define DEFAULT_GYRO_LR_ADC_CH 0x0001
#define DEFAULT_RETURN_LEVEL 0x0002
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
......@@ -78,6 +80,10 @@ uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE,
MM_PERIOD_OFFSET,
DEFAULT_MM_PERIOD>>8,
MM_PERIOD_OFFSET+1,
DEFAULT_GYRO_FB_ADC_CH,
GYRO_FB_ADC_CH_OFFSET,
DEFAULT_GYRO_LR_ADC_CH,
GYRO_LR_ADC_CH_OFFSET,
DEFAULT_RETURN_LEVEL,
RETURN_LEVEL_OFFSET};// return level
......
......@@ -4,6 +4,8 @@
#include "ram.h"
#include "gpio.h"
#include "action.h"
#include "adc_dma.h"
#include "bioloid_gyro.h"
#define MANAGER_TIMER TIM3
#define ENABLE_MANAGER_TIMER_CLK __HAL_RCC_TIM3_CLK_ENABLE()
......@@ -91,7 +93,7 @@ void manager_get_target_position(void)
{
if(manager_servos[i].module==MM_ACTION)
{
manager_servos[i].current_angle=((manager_current_angles[i]+manager_balance_offset[i])>>9);
manager_servos[i].current_angle=((manager_current_angles[i]>>9)+manager_balance_offset[i]);
//>>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);
......@@ -103,6 +105,27 @@ void manager_get_target_position(void)
void manager_balance(void)
{
adc_ch_t fb_ch=gyro_get_fb_adc_channel(),lr_ch=gyro_get_lr_adc_channel();
uint16_t fb_offset,lr_offset;
int16_t fb_value,lr_value;
if(manager_balance_enabled==0x01)// balance is enabled
{
// get the offsets of the gyroscope calibration
gyro_get_offsets(&fb_offset,&lr_offset);
// get the values of the gyroscope
fb_value=adc_get_channel_raw(fb_ch)-fb_offset;
lr_value=adc_get_channel_raw(lr_ch)-lr_offset;
// compensate the servo angle values
manager_balance_offset[13]=(fb_value*manager_servos[13].max_angle)/(manager_servos[13].encoder_resolution*54);
manager_balance_offset[15]=(fb_value*manager_servos[15].max_angle)/(manager_servos[15].encoder_resolution*18);
manager_balance_offset[14]=-(fb_value*manager_servos[14].max_angle)/(manager_servos[14].encoder_resolution*54);
manager_balance_offset[16]=-(fb_value*manager_servos[16].max_angle)/(manager_servos[16].encoder_resolution*18);
manager_balance_offset[9]=(lr_value*manager_servos[9].max_angle)/(manager_servos[9].encoder_resolution*40);
manager_balance_offset[10]=(lr_value*manager_servos[10].max_angle)/(manager_servos[10].encoder_resolution*40);
manager_balance_offset[17]=-(lr_value*manager_servos[17].max_angle)/(manager_servos[17].encoder_resolution*20);
manager_balance_offset[18]=-(lr_value*manager_servos[18].max_angle)/(manager_servos[18].encoder_resolution*20);
}
}
// interrupt handlers
......@@ -123,12 +146,12 @@ void MANAGER_TIMER_IRQHandler(void)
// joint_motion_process();
// call the walking process
// walking_process();
// balance the robot
manager_balance();
// get the target angles from all modules
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();
}
......
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