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

Solved some minor bugs related to the balance feature.

parent 583e3ac3
......@@ -11,9 +11,13 @@ 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)0x0008)
#define GYRO_LR_ADC_CH_OFFSET ((unsigned short int)0x0009)
#define MM_BAL_KNEE_GAIN_OFFSET ((unsigned short int)0x0008)
#define MM_BAL_ANKLE_ROLL_GAIN_OFFSET ((unsigned short int)0x000A)
#define MM_BAL_ANKLE_PITCH_GAIN_OFFSET ((unsigned short int)0x000C)
#define MM_BAL_HIP_ROLL_GAIN_OFFSET ((unsigned short int)0x000E)
#define RETURN_LEVEL_OFFSET ((unsigned short int)0x0010)
#define GYRO_FB_ADC_CH_OFFSET ((unsigned short int)0x0011)
#define GYRO_LR_ADC_CH_OFFSET ((unsigned short int)0x0012)
#define LAST_EEPROM_OFFSET ((unsigned short int)0x0020)
......@@ -26,9 +30,17 @@ typedef enum {
BIOLOID_RETURN_DELAY_TIME = RETURN_DELAY_OFFSET,
BIOLOID_MM_PERIOD_L = MM_PERIOD_OFFSET,
BIOLOID_MM_PERIOD_H = MM_PERIOD_OFFSET+1,
BIOLOID_MM_BAL_KNEE_GAIN_L = MM_BAL_KNEE_GAIN_OFFSET,// fixed point format 0|16
BIOLOID_MM_BAL_KNEE_GAIN_H = MM_BAL_KNEE_GAIN_OFFSET+1,
BIOLOID_MM_BAL_ANKLE_ROLL_GAIN_L = MM_BAL_ANKLE_ROLL_GAIN_OFFSET,// fixed point format 0|16
BIOLOID_MM_BAL_ANKLE_ROLL_GAIN_H = MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1,
BIOLOID_MM_BAL_ANKLE_PITCH_GAIN_L = MM_BAL_ANKLE_PITCH_GAIN_OFFSET,// fixed point format 0|16
BIOLOID_MM_BAL_ANKLE_PITCH_GAIN_H = MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1,
BIOLOID_MM_BAL_HIP_ROLL_GAIN_L = MM_BAL_HIP_ROLL_GAIN_OFFSET,// fixed point format 0|16
BIOLOID_MM_BAL_HIP_ROLL_GAIN_H = MM_BAL_HIP_ROLL_GAIN_OFFSET+1,
BIOLOID_RETURN_LEVEL = RETURN_LEVEL_OFFSET,
BIOLIOD_GYRO_FB_ADC_CH = GYRO_FB_ADC_CH_OFFSET,
BIOLIOD_GYRO_LR_ADC_CH = GYRO_LR_ADC_CH_OFFSET,
BIOLOID_RETURN_LEVEL = RETURN_LEVEL_OFFSET,
BIOLOID_USER1_LED_CNTRL = 0x20, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// | | | | blink | toggle | value | internally used
BIOLOID_USER1_LED_PERIOD_L = 0x21,
......@@ -215,6 +227,7 @@ typedef enum {
#define MANAGER_BASE_ADDRESS 0x48
#define MANAGER_MEM_LENGTH 50
#define MANAGER_EEPROM_LENGTH 10
#define MANAGER_ENABLE 0x01
#define MANAGER_EN_BAL 0x02
#define MANAGER_EN_PWR 0x04
......
......@@ -7,7 +7,28 @@ extern "C" {
#include "stm32f4xx_hal.h"
typedef enum{MM_NONE = 0,
typedef enum {
R_SHOULDER_PITCH = 1,
L_SHOULDER_PITCH = 2,
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;
......
......@@ -36,6 +36,8 @@ void gyro_calibrate(void)
gyro_fb_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++)
{
gyro_fb_offset+=adc_get_channel_raw(fb_ch);
......
......@@ -42,18 +42,23 @@
/* Includes ------------------------------------------------------------------*/
#include "eeprom.h"
#include "bioloid_registers.h"
#include "adc_dma.h"
/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
#define EEPROM_SIZE 0x09
#define EEPROM_SIZE 0x20
#define DEFAULT_DEVICE_MODEL 0x7300
#define DEFAULT_FIRMWARE_VERSION 0x0001
#define DEFAULT_DEVICE_ID 0x0002
#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_BAL_KNEE_GAIN 0x04BE // 1/54 in fixed point format 0|16
#define DEFAULT_BAL_ANKLE_ROLL_GAIN 0x0CCD // 1/20
#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 variables ---------------------------------------------------------*/
......@@ -80,12 +85,28 @@ uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE,
MM_PERIOD_OFFSET,
DEFAULT_MM_PERIOD>>8,
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,
GYRO_FB_ADC_CH_OFFSET,
DEFAULT_GYRO_LR_ADC_CH,
GYRO_LR_ADC_CH_OFFSET,
DEFAULT_RETURN_LEVEL,
RETURN_LEVEL_OFFSET};// return level
GYRO_LR_ADC_CH_OFFSET};// return level
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
......
......@@ -106,25 +106,31 @@ 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();
int16_t knee_gain,ankle_roll_gain,ankle_pitch_gain,hip_roll_gain;
uint16_t fb_offset,lr_offset;
int16_t fb_value,lr_value;
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
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);
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[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[L_KNEE]=-(((fb_value*manager_servos[L_KNEE].max_angle)/manager_servos[L_KNEE].encoder_resolution)*knee_gain)/65536;
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[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[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[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[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)
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) ||
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;
else
return 0x00;
......
......@@ -26,8 +26,28 @@ void ram_init(void)
ram_data[MM_PERIOD_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
if(EE_ReadVariable(MM_PERIOD_OFFSET+1,&eeprom_data)==0)
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)
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)
......
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