Commit 652f78e2 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added an extra angle in the hip and ankle pitch angle due to differences with...

Added an extra angle in the hip and ankle pitch angle due to differences with the Darwin les structure.
Added a new header file with the initialization contents of the eeprom memory.
Solved a bug with negative motion commands.
parent 7550a739
......@@ -11,6 +11,7 @@ extern "C" {
#define BIOLOID_PELVIS_LENGTH 29.0 //mm
#define BIOLOID_THIGH_LENGTH 76.388 //mm
#define BIOLOID_CALF_LENGTH 76.388 //mm
#define BIOLOID_PITCH_OFFSET 0.1910 //rad
#define BIOLOID_KNEE_DEPTH 14.5 //mm
#define BIOLOID_ANKLE_LENGTH 33.68 //mm
#define BIOLOID_LEG_LENGTH (BIOLOID_THIGH_LENGTH + BIOLOID_CALF_LENGTH + BIOLOID_ANKLE_LENGTH) //mm
......
#ifndef __EEPROM_INIT_H
#define __EEPROM_INIT_H
#ifdef __cplusplus
extern "C" {
#endif
#include "adc_dma.h"
#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 0x1E78
#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
#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_SERVO2_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO3_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO4_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO5_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO6_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO7_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO8_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO9_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO10_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO11_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO12_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO13_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO14_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO15_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO16_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO17_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO18_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO19_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO20_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO21_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO22_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO23_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO24_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO25_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO26_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO27_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO28_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO29_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO30_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO31_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_WALK_X_OFFSET 0xFFF6 // -10 mm
#define DEFAULT_WALK_Y_OFFSET 0x0005 // 5 mm
#define DEFAULT_WALK_Z_OFFSET 0x0014 // 20 mm
#define DEFAULT_WALK_ROLL_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
#define DEFAULT_WALK_PITCH_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
#define DEFAULT_WALK_YAW_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
#define DEFAULT_WALK_HIP_PITCH_OFF 0x0000 // 0 degrees in fixed point format 6 (1+5) | 10
#define DEFAULT_WALK_PERIOD_TIME 0x0258 // 600 ms
#define DEFAULT_WALK_DSP_RATIO 0x0019 // 0.1 in fixed point format 0 | 8
#define DEFAULT_WALK_STEP_FW_BW_RATIO 0x004C // 0.3 in fixed point format 0 | 8
#define DEFAULT_WALK_FOOT_HEIGHT 0x0028 // 40 mm
#define DEFAULT_WALK_SWING_RIGHT_LEFT 0x0014 // 20 mm
#define DEFAULT_WALK_SWING_TOP_DOWN 0x0005 // 5 mm
#define DEFAULT_WALK_PELVIS_OFFSET 0x0018 // 3 degrees in fixed point format 5 (1+4) | 3
#define DEFAULT_WALK_ARM_SWING_GAIN 0x0030 // 1.5 in fixed point format 3 | 5
#define DEFAULT_WALK_MAX_VEL 0x0016 // 20 mm/s
#define DEFAULT_WALK_MAX_ROT_VEL 0x0008 // 8 degrees/s in fixed point format 5 | 3
#ifdef __cplusplus
}
#endif
#endif /* __EEPROM_INIT_H */
......@@ -41,74 +41,11 @@
/* Includes ------------------------------------------------------------------*/
#include "eeprom.h"
#include "eeprom_init.h"
#include "bioloid_registers.h"
#include "adc_dma.h"
/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
#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 0x1E78
#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
#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_SERVO2_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO3_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO4_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO5_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO6_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO7_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO8_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO9_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO10_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO11_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO12_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO13_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO14_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO15_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO16_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO17_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO18_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO19_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO20_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO21_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO22_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO23_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO24_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO25_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO26_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO27_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO28_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO29_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO30_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_SERVO31_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4
#define DEFAULT_WALK_X_OFFSET 0xFFF6 // -10 mm
#define DEFAULT_WALK_Y_OFFSET 0x0005 // 5 mm
#define DEFAULT_WALK_Z_OFFSET 0x0014 // 20 mm
#define DEFAULT_WALK_ROLL_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
#define DEFAULT_WALK_PITCH_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
#define DEFAULT_WALK_YAW_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
#define DEFAULT_WALK_HIP_PITCH_OFF 0x0000 // 0 degrees in fixed point format 6 (1+5) | 10
#define DEFAULT_WALK_PERIOD_TIME 0x0258 // 600 ms
#define DEFAULT_WALK_DSP_RATIO 0x0019 // 0.1 in fixed point format 0 | 8
#define DEFAULT_WALK_STEP_FW_BW_RATIO 0x004C // 0.3 in fixed point format 0 | 8
#define DEFAULT_WALK_FOOT_HEIGHT 0x0028 // 40 mm
#define DEFAULT_WALK_SWING_RIGHT_LEFT 0x0014 // 20 mm
#define DEFAULT_WALK_SWING_TOP_DOWN 0x0005 // 5 mm
#define DEFAULT_WALK_PELVIS_OFFSET 0x0018 // 3 degrees in fixed point format 5 (1+4) | 3
#define DEFAULT_WALK_ARM_SWING_GAIN 0x0030 // 1.5 in fixed point format 3 | 5
#define DEFAULT_WALK_MAX_VEL 0x0016 // 20 mm/s
#define DEFAULT_WALK_MAX_ROT_VEL 0x0008 // 8 degrees/s in fixed point format 5 | 3
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
......
......@@ -3,7 +3,6 @@
#include "bioloid_math.h"
#include "ram.h"
#include <math.h>
#include <stdio.h>
enum {PHASE0=0x00,PHASE1=0x40,PHASE2=0x80,PHASE3=0xC0};
......@@ -115,7 +114,7 @@ void update_param_move()
static float current_y_amplitude=0;
static float current_a_amplitude=0;
target_x_amplitude=((float)ram_data[BIOLOID_WALK_STEP_FW_BW]);
target_x_amplitude=((float)((int8_t)ram_data[BIOLOID_WALK_STEP_FW_BW]));
// change longitudinal and transversal velocities to get to the target ones
if(current_x_amplitude<target_x_amplitude)
{
......@@ -133,7 +132,7 @@ void update_param_move()
m_X_Swap_Amplitude=current_x_amplitude*((float)ram_data[BIOLOID_WALK_STEP_FW_BW_RATIO])/256.0;
// Right/Left
target_y_amplitude=((float)ram_data[BIOLOID_WALK_STEP_LEFT_RIGHT]);
target_y_amplitude=((float)((int8_t)ram_data[BIOLOID_WALK_STEP_LEFT_RIGHT]));
if(current_y_amplitude<target_y_amplitude)
{
current_y_amplitude+=(((float)ram_data[BIOLOID_WALK_MAX_VEL])*m_PeriodTime)/1000.0;
......@@ -159,7 +158,7 @@ void update_param_move()
m_Z_Swap_Amplitude_Shift=m_Z_Swap_Amplitude;
// Direction
target_a_amplitude=((float)ram_data[BIOLOID_WALK_STEP_DIRECTION]);
target_a_amplitude=((float)((int8_t)ram_data[BIOLOID_WALK_STEP_DIRECTION]))/8.0;
if(current_a_amplitude<target_a_amplitude)
{
current_a_amplitude+=(((float)ram_data[BIOLOID_WALK_MAX_ROT_VEL])*m_PeriodTime)/8000.0;
......@@ -450,11 +449,11 @@ void walking_process(void)
if(manager_get_module(R_HIP_ROLL)==MM_WALKING)
manager_current_angles[R_HIP_ROLL]=((180.0*(angle[1]+pelvis_offset_r))/PI)*65536.0;
if(manager_get_module(R_HIP_PITCH)==MM_WALKING)
manager_current_angles[R_HIP_PITCH]=((180.0*angle[2])/PI-m_Hip_Pitch_Offset)*65536.0;
manager_current_angles[R_HIP_PITCH]=((180.0*(angle[2]-BIOLOID_PITCH_OFFSET))/PI-m_Hip_Pitch_Offset)*65536.0;
if(manager_get_module(R_KNEE)==MM_WALKING)
manager_current_angles[R_KNEE]=((-180.0*angle[3])/PI)*65536.0;
if(manager_get_module(R_ANKLE_PITCH)==MM_WALKING)
manager_current_angles[R_ANKLE_PITCH]=((-180.0*angle[4])/PI)*65536.0;
manager_current_angles[R_ANKLE_PITCH]=((-180.0*(angle[4]+BIOLOID_PITCH_OFFSET))/PI)*65536.0;
if(manager_get_module(R_ANKLE_ROLL)==MM_WALKING)
manager_current_angles[R_ANKLE_ROLL]=((-180.0*angle[5])/PI)*65536.0;
if(manager_get_module(L_HIP_YAW)==MM_WALKING)
......@@ -462,11 +461,11 @@ void walking_process(void)
if(manager_get_module(L_HIP_ROLL)==MM_WALKING)
manager_current_angles[L_HIP_ROLL]=((180.0*(angle[7]+pelvis_offset_l))/PI)*65536.0;
if(manager_get_module(L_HIP_PITCH)==MM_WALKING)
manager_current_angles[L_HIP_PITCH]=((-180.0*angle[8])/PI+m_Hip_Pitch_Offset)*65536.0;
manager_current_angles[L_HIP_PITCH]=((-180.0*(angle[8]-BIOLOID_PITCH_OFFSET))/PI+m_Hip_Pitch_Offset)*65536.0;
if(manager_get_module(L_KNEE)==MM_WALKING)
manager_current_angles[L_KNEE]=((180.0*angle[9])/PI)*65536.0;
if(manager_get_module(L_ANKLE_PITCH)==MM_WALKING)
manager_current_angles[L_ANKLE_PITCH]=((180.0*angle[10])/PI)*65536.0;
manager_current_angles[L_ANKLE_PITCH]=((180.0*(angle[10]+BIOLOID_PITCH_OFFSET))/PI)*65536.0;
if(manager_get_module(L_ANKLE_ROLL)==MM_WALKING)
manager_current_angles[L_ANKLE_ROLL]=((-180.0*angle[11])/PI)*65536.0;
if(manager_get_module(R_SHOULDER_PITCH)==MM_WALKING)
......
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