Skip to content
GitLab
Commit 65a93f73 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Solved some minor bugs related to the balance feature.

parent 583e3ac3
Expand all Hide whitespace changes
Inline Side-by-side
include/bioloid_registers.h
View file @ 65a93f73
This diff is collapsed.
include/motion_manager.h
View file @ 65a93f73
...@@ -7,10 +7,31 @@ extern "C" { ...@@ -7,10 +7,31 @@ extern "C" {
#include "stm32f4xx_hal.h" #include "stm32f4xx_hal.h"
typedef enum{MM_NONE = 0, typedef enum {
MM_ACTION = 1, R_SHOULDER_PITCH = 1,
MM_WALKING = 2, L_SHOULDER_PITCH = 2,
MM_JOINTS = 3} TModules; R_SHOULDER_ROLL = 3,
L_SHOULDER_ROLL = 4,
R_ELBOW = 5,
L_ELBOW = 6,
R_HIP_YAW = 7,
L_HIP_YAW = 8,
R_HIP_ROLL = 9,
L_HIP_ROLL = 10,
R_HIP_PITCH = 11,
L_HIP_PITCH = 12,
R_KNEE = 13,
L_KNEE = 14,
R_ANKLE_PITCH = 15,
L_ANKLE_PITCH = 16,
R_ANKLE_ROLL = 17,
L_ANKLE_ROLL = 18} servo_id_t;
typedef enum {MM_NONE = 0,
MM_ACTION = 1,
MM_WALKING = 2,
MM_JOINTS = 3} TModules;
typedef struct typedef struct
{ {
......
src/bioloid_gyro.c
View file @ 65a93f73
...@@ -36,6 +36,8 @@ void gyro_calibrate(void) ...@@ -36,6 +36,8 @@ void gyro_calibrate(void)
gyro_fb_offset=0x0000; gyro_fb_offset=0x0000;
gyro_lr_offset=0x0000; gyro_lr_offset=0x0000;
// dummy delay to allow enough time to the ADC to get new data
HAL_Delay(2*adc_get_period());
for(i=0;i<GYRO_MAX_CAL_SAMPLES;i++) for(i=0;i<GYRO_MAX_CAL_SAMPLES;i++)
{ {
gyro_fb_offset+=adc_get_channel_raw(fb_ch); gyro_fb_offset+=adc_get_channel_raw(fb_ch);
......
src/eeprom.c
View file @ 65a93f73
...@@ -42,19 +42,24 @@ ...@@ -42,19 +42,24 @@
/* Includes ------------------------------------------------------------------*/ /* Includes ------------------------------------------------------------------*/
#include "eeprom.h" #include "eeprom.h"
#include "bioloid_registers.h" #include "bioloid_registers.h"
#include "adc_dma.h"
/* Private typedef -----------------------------------------------------------*/ /* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/ /* Private define ------------------------------------------------------------*/
#define EEPROM_SIZE 0x09 #define EEPROM_SIZE 0x20
#define DEFAULT_DEVICE_MODEL 0x7300 #define DEFAULT_DEVICE_MODEL 0x7300
#define DEFAULT_FIRMWARE_VERSION 0x0001 #define DEFAULT_FIRMWARE_VERSION 0x0001
#define DEFAULT_DEVICE_ID 0x0002 #define DEFAULT_DEVICE_ID 0x0002
#define DEFAULT_BAUDRATE 0x0001 #define DEFAULT_BAUDRATE 0x0001
#define DEFAULT_RETURN_DELAY 0x0000 #define DEFAULT_RETURN_DELAY 0x0000
#define DEFAULT_MM_PERIOD 0x01FF #define DEFAULT_MM_PERIOD 0x01FF
#define DEFAULT_GYRO_FB_ADC_CH 0x0002 #define DEFAULT_BAL_KNEE_GAIN 0x04BE // 1/54 in fixed point format 0|16
#define DEFAULT_GYRO_LR_ADC_CH 0x0001 #define DEFAULT_BAL_ANKLE_ROLL_GAIN 0x0CCD // 1/20
#define DEFAULT_RETURN_LEVEL 0x0002 #define DEFAULT_BAL_ANKLE_PITCH_GAIN 0x0E39 // 1/18
#define DEFAULT_BAL_HIP_ROLL_GAIN 0x0666 // 1/40
#define DEFAULT_GYRO_FB_ADC_CH (uint16_t)ADC_CH1
#define DEFAULT_GYRO_LR_ADC_CH (uint16_t)ADC_CH2
#define DEFAULT_RETURN_LEVEL 0x0002
/* Private macro -------------------------------------------------------------*/ /* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/ /* Private variables ---------------------------------------------------------*/
...@@ -80,12 +85,28 @@ uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE, ...@@ -80,12 +85,28 @@ uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE,
MM_PERIOD_OFFSET, MM_PERIOD_OFFSET,
DEFAULT_MM_PERIOD>>8, DEFAULT_MM_PERIOD>>8,
MM_PERIOD_OFFSET+1, MM_PERIOD_OFFSET+1,
DEFAULT_BAL_KNEE_GAIN&0xFF,
MM_BAL_KNEE_GAIN_OFFSET,
DEFAULT_BAL_KNEE_GAIN>>8,
MM_BAL_KNEE_GAIN_OFFSET+1,
DEFAULT_BAL_ANKLE_ROLL_GAIN&0xFF,
MM_BAL_ANKLE_ROLL_GAIN_OFFSET,
DEFAULT_BAL_ANKLE_ROLL_GAIN>>8,
MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1,
DEFAULT_BAL_ANKLE_PITCH_GAIN&0xFF,
MM_BAL_ANKLE_PITCH_GAIN_OFFSET,
DEFAULT_BAL_ANKLE_PITCH_GAIN>>8,
MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1,
DEFAULT_BAL_HIP_ROLL_GAIN&0xFF,
MM_BAL_HIP_ROLL_GAIN_OFFSET,
DEFAULT_BAL_HIP_ROLL_GAIN>>8,
MM_BAL_HIP_ROLL_GAIN_OFFSET+1,
DEFAULT_RETURN_LEVEL,
RETURN_LEVEL_OFFSET,
DEFAULT_GYRO_FB_ADC_CH, DEFAULT_GYRO_FB_ADC_CH,
GYRO_FB_ADC_CH_OFFSET, GYRO_FB_ADC_CH_OFFSET,
DEFAULT_GYRO_LR_ADC_CH, DEFAULT_GYRO_LR_ADC_CH,
GYRO_LR_ADC_CH_OFFSET, GYRO_LR_ADC_CH_OFFSET};// return level
DEFAULT_RETURN_LEVEL,
RETURN_LEVEL_OFFSET};// return level
/* Private function prototypes -----------------------------------------------*/ /* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/ /* Private functions ---------------------------------------------------------*/
......
src/motion_manager.c
View file @ 65a93f73
...@@ -106,25 +106,31 @@ void manager_get_target_position(void) ...@@ -106,25 +106,31 @@ void manager_get_target_position(void)
void manager_balance(void) void manager_balance(void)
{ {
adc_ch_t fb_ch=gyro_get_fb_adc_channel(),lr_ch=gyro_get_lr_adc_channel(); adc_ch_t fb_ch=gyro_get_fb_adc_channel(),lr_ch=gyro_get_lr_adc_channel();
int16_t knee_gain,ankle_roll_gain,ankle_pitch_gain,hip_roll_gain;
uint16_t fb_offset,lr_offset; uint16_t fb_offset,lr_offset;
int16_t fb_value,lr_value; int16_t fb_value,lr_value;
if(manager_balance_enabled==0x01)// balance is enabled if(manager_balance_enabled==0x01)// balance is enabled
{ {
// get the balance gains
knee_gain=ram_data[BIOLOID_MM_BAL_KNEE_GAIN_L]+(ram_data[BIOLOID_MM_BAL_KNEE_GAIN_H]*256);
ankle_roll_gain=ram_data[BIOLOID_MM_BAL_ANKLE_ROLL_GAIN_L]+(ram_data[BIOLOID_MM_BAL_ANKLE_ROLL_GAIN_H]<<8);
ankle_pitch_gain=ram_data[BIOLOID_MM_BAL_ANKLE_PITCH_GAIN_L]+(ram_data[BIOLOID_MM_BAL_ANKLE_PITCH_GAIN_H]<<8);
hip_roll_gain=ram_data[BIOLOID_MM_BAL_HIP_ROLL_GAIN_L]+(ram_data[BIOLOID_MM_BAL_HIP_ROLL_GAIN_H]<<8);
// get the offsets of the gyroscope calibration // get the offsets of the gyroscope calibration
gyro_get_offsets(&fb_offset,&lr_offset); gyro_get_offsets(&fb_offset,&lr_offset);
// get the values of the gyroscope // get the values of the gyroscope
fb_value=adc_get_channel_raw(fb_ch)-fb_offset; fb_value=adc_get_channel_raw(fb_ch)-fb_offset;
lr_value=adc_get_channel_raw(lr_ch)-lr_offset; lr_value=adc_get_channel_raw(lr_ch)-lr_offset;
// compensate the servo angle values // 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[R_KNEE]=(((fb_value*manager_servos[R_KNEE].max_angle)/manager_servos[R_KNEE].encoder_resolution)*knee_gain)/65536;
manager_balance_offset[15]=(fb_value*manager_servos[15].max_angle)/(manager_servos[15].encoder_resolution*18); manager_balance_offset[R_ANKLE_PITCH]=(((fb_value*manager_servos[R_ANKLE_PITCH].max_angle)/manager_servos[R_ANKLE_PITCH].encoder_resolution)*ankle_pitch_gain)/65536;
manager_balance_offset[14]=-(fb_value*manager_servos[14].max_angle)/(manager_servos[14].encoder_resolution*54); manager_balance_offset[L_KNEE]=-(((fb_value*manager_servos[L_KNEE].max_angle)/manager_servos[L_KNEE].encoder_resolution)*knee_gain)/65536;
manager_balance_offset[16]=-(fb_value*manager_servos[16].max_angle)/(manager_servos[16].encoder_resolution*18); manager_balance_offset[L_ANKLE_PITCH]=-(((fb_value*manager_servos[L_ANKLE_PITCH].max_angle)/manager_servos[L_ANKLE_PITCH].encoder_resolution)*ankle_pitch_gain)/65536;
manager_balance_offset[9]=(lr_value*manager_servos[9].max_angle)/(manager_servos[9].encoder_resolution*40); manager_balance_offset[R_HIP_ROLL]=(((lr_value*manager_servos[R_HIP_ROLL].max_angle)/manager_servos[R_HIP_ROLL].encoder_resolution)*hip_roll_gain)/65536;
manager_balance_offset[10]=(lr_value*manager_servos[10].max_angle)/(manager_servos[10].encoder_resolution*40); manager_balance_offset[L_HIP_ROLL]=(((lr_value*manager_servos[L_HIP_ROLL].max_angle)/manager_servos[L_HIP_ROLL].encoder_resolution)*hip_roll_gain)/65536;
manager_balance_offset[17]=-(lr_value*manager_servos[17].max_angle)/(manager_servos[17].encoder_resolution*20); manager_balance_offset[R_ANKLE_ROLL]=-(((lr_value*manager_servos[R_ANKLE_ROLL].max_angle)/manager_servos[R_ANKLE_ROLL].encoder_resolution)*ankle_roll_gain)/65536;
manager_balance_offset[18]=-(lr_value*manager_servos[18].max_angle)/(manager_servos[18].encoder_resolution*20); manager_balance_offset[L_ANKLE_ROLL]=-(((lr_value*manager_servos[L_ANKLE_ROLL].max_angle)/manager_servos[L_ANKLE_ROLL].encoder_resolution)*ankle_roll_gain)/65536;
} }
} }
...@@ -456,7 +462,7 @@ inline int16_t manager_get_ccw_angle_limit(uint8_t servo_id) ...@@ -456,7 +462,7 @@ inline int16_t manager_get_ccw_angle_limit(uint8_t servo_id)
uint8_t manager_in_range(unsigned short int address, unsigned short int length) uint8_t manager_in_range(unsigned short int address, unsigned short int length)
{ {
if(ram_in_window(MANAGER_BASE_ADDRESS,MANAGER_MEM_LENGTH,address,length) || if(ram_in_window(MANAGER_BASE_ADDRESS,MANAGER_MEM_LENGTH,address,length) ||
ram_in_window(BIOLOID_MM_PERIOD_L,BIOLOID_MM_PERIOD_H,address,length)) ram_in_window(BIOLOID_MM_PERIOD_L,MANAGER_EEPROM_LENGTH,address,length))
return 0x01; return 0x01;
else else
return 0x00; return 0x00;
......
src/ram.c
View file @ 65a93f73
...@@ -26,8 +26,28 @@ void ram_init(void) ...@@ -26,8 +26,28 @@ void ram_init(void)
ram_data[MM_PERIOD_OFFSET]=(uint8_t)(eeprom_data&0x00FF); ram_data[MM_PERIOD_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(MM_PERIOD_OFFSET+1,&eeprom_data)==0) if(EE_ReadVariable(MM_PERIOD_OFFSET+1,&eeprom_data)==0)
ram_data[MM_PERIOD_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF); ram_data[MM_PERIOD_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(MM_BAL_KNEE_GAIN_OFFSET,&eeprom_data)==0)
ram_data[MM_BAL_KNEE_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(MM_BAL_KNEE_GAIN_OFFSET+1,&eeprom_data)==0)
ram_data[MM_BAL_KNEE_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(MM_BAL_ANKLE_ROLL_GAIN_OFFSET,&eeprom_data)==0)
ram_data[MM_BAL_ANKLE_ROLL_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1,&eeprom_data)==0)
ram_data[MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(MM_BAL_ANKLE_PITCH_GAIN_OFFSET,&eeprom_data)==0)
ram_data[MM_BAL_ANKLE_PITCH_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1,&eeprom_data)==0)
ram_data[MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(MM_BAL_HIP_ROLL_GAIN_OFFSET,&eeprom_data)==0)
ram_data[MM_BAL_HIP_ROLL_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(MM_BAL_HIP_ROLL_GAIN_OFFSET+1,&eeprom_data)==0)
ram_data[MM_BAL_HIP_ROLL_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(RETURN_LEVEL_OFFSET,&eeprom_data)==0) if(EE_ReadVariable(RETURN_LEVEL_OFFSET,&eeprom_data)==0)
ram_data[RETURN_LEVEL_OFFSET]=(uint8_t)eeprom_data; ram_data[RETURN_LEVEL_OFFSET]=(uint8_t)eeprom_data;
if(EE_ReadVariable(GYRO_FB_ADC_CH_OFFSET,&eeprom_data)==0)
ram_data[GYRO_FB_ADC_CH_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(GYRO_LR_ADC_CH_OFFSET,&eeprom_data)==0)
ram_data[GYRO_LR_ADC_CH_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
} }
inline void ram_read_byte(uint8_t address,uint8_t *data) inline void ram_read_byte(uint8_t address,uint8_t *data)
......
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