Commit 65a93f73 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Solved some minor bugs related to the balance feature.

parent 583e3ac3
This diff is collapsed.
...@@ -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
{ {
......
...@@ -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);
......
...@@ -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 ---------------------------------------------------------*/
......
...@@ -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;
......
...@@ -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