Skip to content
Snippets Groups Projects
Commit 5af66819 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Implemented the balance feature of the robot using the Gyro.

parent 6fbfc0cf
No related branches found
No related tags found
No related merge requests found
...@@ -11,10 +11,10 @@ extern "C" { ...@@ -11,10 +11,10 @@ extern "C" {
#define DEFAULT_BAUDRATE 0x0001 #define DEFAULT_BAUDRATE 0x0001
#define DEFAULT_RETURN_DELAY 0x0000 #define DEFAULT_RETURN_DELAY 0x0000
#define DEFAULT_MM_PERIOD 0x1E78 #define DEFAULT_MM_PERIOD 0x1E78
#define DEFAULT_BAL_KNEE_GAIN 0x04BE // 1/54 in fixed point format 0|16 #define DEFAULT_BAL_KNEE_GAIN 0x4CCD // 0.3 in fixed point format 0|16
#define DEFAULT_BAL_ANKLE_ROLL_GAIN 0x0CCD // 1/20 #define DEFAULT_BAL_ANKLE_ROLL_GAIN 0xFFFF // 0.99999
#define DEFAULT_BAL_ANKLE_PITCH_GAIN 0x0E39 // 1/18 #define DEFAULT_BAL_ANKLE_PITCH_GAIN 0xE666 // 0.9
#define DEFAULT_BAL_HIP_ROLL_GAIN 0x0666 // 1/40 #define DEFAULT_BAL_HIP_ROLL_GAIN 0x8000 // 0.5
#define DEFAULT_RETURN_LEVEL 0x0002 #define DEFAULT_RETURN_LEVEL 0x0002
#define DEFAULT_SERVO0_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 #define DEFAULT_SERVO0_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO1_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 #define DEFAULT_SERVO1_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
......
...@@ -298,7 +298,7 @@ void imu_gyro_stop(void) ...@@ -298,7 +298,7 @@ void imu_gyro_stop(void)
void imu_gyro_set_config(void) void imu_gyro_set_config(void)
{ {
gyro_conf_data[0]=0xFF;// sampling rate = 800 Hz, BW = 110 Hz , all powered up gyro_conf_data[0]=0xFF;// sampling rate = 800 Hz, BW = 110 Hz , all powered up
gyro_conf_data[3]=0x20;// Full scale = 2000dps gyro_conf_data[3]=0x10;// Full scale = 500dps
imu_enable_device(GYRO_ID); imu_enable_device(GYRO_ID);
imu_spi_start_write(gyro_conf_reg,gyro_conf_data,gyro_conf_len); imu_spi_start_write(gyro_conf_reg,gyro_conf_data,gyro_conf_len);
...@@ -325,7 +325,7 @@ void imu_gyro_convert_data(uint8_t *data_in,uint8_t *data_out) ...@@ -325,7 +325,7 @@ void imu_gyro_convert_data(uint8_t *data_in,uint8_t *data_out)
for(i=0;i<3;i++) for(i=0;i<3;i++)
{ {
gyro_data[i]=(((int16_t)(data_in[i*2]+(data_in[i*2+1]<<8)))*70)-gyro_center[i]; gyro_data[i]=(((int16_t)(data_in[i*2]+(data_in[i*2+1]<<8)))*18)-gyro_center[i];
data_out[i*2]=gyro_data[i]%256; data_out[i*2]=gyro_data[i]%256;
data_out[i*2+1]=gyro_data[i]/256; data_out[i*2+1]=gyro_data[i]/256;
} }
......
...@@ -124,14 +124,14 @@ void manager_balance(void) ...@@ -124,14 +124,14 @@ void manager_balance(void)
// get the values of the gyroscope // get the values of the gyroscope
imu_get_gyro_data(&gyro_x,&gyro_y,&gyro_z); imu_get_gyro_data(&gyro_x,&gyro_y,&gyro_z);
// compensate the servo angle values // compensate the servo angle values
manager_balance_offset[R_KNEE]=(((gyro_y*manager_servos[R_KNEE].max_angle)/manager_servos[R_KNEE].encoder_resolution)*knee_gain)>>16; manager_balance_offset[R_KNEE]=((((int16_t)gyro_y*(uint64_t)manager_servos[R_KNEE].max_angle*(uint64_t)knee_gain)/(uint64_t)manager_servos[R_KNEE].encoder_resolution)/1000)>>9;
manager_balance_offset[R_ANKLE_PITCH]=(((gyro_y*manager_servos[R_ANKLE_PITCH].max_angle)/manager_servos[R_ANKLE_PITCH].encoder_resolution)*ankle_pitch_gain)>>16; manager_balance_offset[R_ANKLE_PITCH]=-((((int64_t)gyro_y*(uint64_t)manager_servos[R_ANKLE_PITCH].max_angle*(uint64_t)ankle_pitch_gain)/(uint64_t)manager_servos[R_ANKLE_PITCH].encoder_resolution)/1000)>>9;
manager_balance_offset[L_KNEE]=-(((gyro_y*manager_servos[L_KNEE].max_angle)/manager_servos[L_KNEE].encoder_resolution)*knee_gain)>>16; manager_balance_offset[L_KNEE]=-((((int64_t)gyro_y*(uint64_t)manager_servos[L_KNEE].max_angle*(uint64_t)knee_gain)/(uint64_t)manager_servos[L_KNEE].encoder_resolution)/1000)>>9;
manager_balance_offset[L_ANKLE_PITCH]=-(((gyro_y*manager_servos[L_ANKLE_PITCH].max_angle)/manager_servos[L_ANKLE_PITCH].encoder_resolution)*ankle_pitch_gain)>>16; manager_balance_offset[L_ANKLE_PITCH]=((((int64_t)gyro_y*(uint64_t)manager_servos[L_ANKLE_PITCH].max_angle*(uint64_t)ankle_pitch_gain)/(uint64_t)manager_servos[L_ANKLE_PITCH].encoder_resolution)/1000)>>9;
manager_balance_offset[R_HIP_ROLL]=(((gyro_x*manager_servos[R_HIP_ROLL].max_angle)/manager_servos[R_HIP_ROLL].encoder_resolution)*hip_roll_gain)>>16; manager_balance_offset[R_HIP_ROLL]=-((((int64_t)gyro_x*(uint64_t)manager_servos[R_HIP_ROLL].max_angle*(uint64_t)hip_roll_gain)/(uint64_t)manager_servos[R_HIP_ROLL].encoder_resolution)/1000)>>9;
manager_balance_offset[L_HIP_ROLL]=(((gyro_x*manager_servos[L_HIP_ROLL].max_angle)/manager_servos[L_HIP_ROLL].encoder_resolution)*hip_roll_gain)>>16; manager_balance_offset[L_HIP_ROLL]=-((((int64_t)gyro_x*(uint64_t)manager_servos[L_HIP_ROLL].max_angle*(uint64_t)hip_roll_gain)/(uint64_t)manager_servos[L_HIP_ROLL].encoder_resolution)/1000)>>9;
manager_balance_offset[R_ANKLE_ROLL]=-(((gyro_x*manager_servos[R_ANKLE_ROLL].max_angle)/manager_servos[R_ANKLE_ROLL].encoder_resolution)*ankle_roll_gain)>>16; manager_balance_offset[R_ANKLE_ROLL]=((((int64_t)gyro_x*(uint64_t)manager_servos[R_ANKLE_ROLL].max_angle*(uint64_t)ankle_roll_gain)/(uint64_t)manager_servos[R_ANKLE_ROLL].encoder_resolution)/1000)>>9;
manager_balance_offset[L_ANKLE_ROLL]=-(((gyro_x*manager_servos[L_ANKLE_ROLL].max_angle)/manager_servos[L_ANKLE_ROLL].encoder_resolution)*ankle_roll_gain)>>16; manager_balance_offset[L_ANKLE_ROLL]=((((int64_t)gyro_x*(uint64_t)manager_servos[L_ANKLE_ROLL].max_angle*(uint64_t)ankle_roll_gain)/(uint64_t)manager_servos[L_ANKLE_ROLL].encoder_resolution)/1000)>>9;
} }
} }
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment