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

Initial implementation of the stairs module.

parent 8c8bd0fb
No related branches found
No related tags found
2 merge requests!5Dynamixel manager,!2Smart charger fw
This commit is part of merge request !5. Comments created here will be created in the context of that merge request.
Hide whitespace changes
Inline Side-by-side
include/darwin_registers.h
+ 48
− 0
View file @ 9b9c4030
......@@ -86,6 +86,24 @@ extern "C" {
#define GRIPPER_RIGHT_MIN_ANGLE ((unsigned short int)0x0060)
#define GRIPPER_RIGHT_MAX_FORCE ((unsigned short int)0x0062)
#define SMART_CHARGER_PERIOD ((unsigned short int)0x0064)
#define STAIRS_PHASE1_TIME ((unsigned short int)0x0066)
#define STAIRS_PHASE2_TIME ((unsigned short int)0x0068)
#define STAIRS_PHASE3_TIME ((unsigned short int)0x006A)
#define STAIRS_PHASE4_TIME ((unsigned short int)0x006C)
#define STAIRS_PHASE5_TIME ((unsigned short int)0x006E)
#define STAIRS_PHASE6_TIME ((unsigned short int)0x0070)
#define STAIRS_PHASE7_TIME ((unsigned short int)0x0072)
#define STAIRS_PHASE8_TIME ((unsigned short int)0x0074)
#define STAIRS_PHASE9_TIME ((unsigned short int)0x0076)
#define STAIRS_X_OFFSET ((unsigned short int)0x0078)
#define STAIRS_Y_OFFSET ((unsigned short int)0x0079)
#define STAIRS_Z_OFFSET ((unsigned short int)0x007A)
#define STAIRS_Y_SHIFT ((unsigned short int)0x007B)
#define STAIRS_X_LEFT_SHIFT ((unsigned short int)0x007C)
#define STAIRS_X_RIGHT_SHIFT ((unsigned short int)0x007D)
#define STAIRS_Z_OVERSHOOT ((unsigned short int)0x007E)
#define STAIRS_Z_HEIGHT ((unsigned short int)0x007F)
#define STAIRS_HIP_PITCH_OFF ((unsigned short int)0x0080)
#define LAST_EEPROM_OFFSET ((unsigned short int)0x00FF)
......@@ -192,6 +210,34 @@ typedef enum {
DARWIN_GRIPPER_RIGHT_MAX_FORCE_H = GRIPPER_RIGHT_MAX_FORCE+1,
DARWIN_SMART_CHARGER_PERIOD_L = SMART_CHARGER_PERIOD, //en ms
DARWIN_SMART_CHARGER_PERIOD_H = SMART_CHARGER_PERIOD+1,
DARWIN_STAIRS_PHASE1_TIME_L = STAIRS_PHASE1_TIME,
DARWIN_STAIRS_PHASE1_TIME_H = STAIRS_PHASE1_TIME+1,
DARWIN_STAIRS_PHASE2_TIME_L = STAIRS_PHASE2_TIME,
DARWIN_STAIRS_PHASE2_TIME_H = STAIRS_PHASE2_TIME+1,
DARWIN_STAIRS_PHASE3_TIME_L = STAIRS_PHASE3_TIME,
DARWIN_STAIRS_PHASE3_TIME_H = STAIRS_PHASE3_TIME+1,
DARWIN_STAIRS_PHASE4_TIME_L = STAIRS_PHASE4_TIME,
DARWIN_STAIRS_PHASE4_TIME_H = STAIRS_PHASE4_TIME+1,
DARWIN_STAIRS_PHASE5_TIME_L = STAIRS_PHASE5_TIME,
DARWIN_STAIRS_PHASE5_TIME_H = STAIRS_PHASE5_TIME+1,
DARWIN_STAIRS_PHASE6_TIME_L = STAIRS_PHASE6_TIME,
DARWIN_STAIRS_PHASE6_TIME_H = STAIRS_PHASE6_TIME+1,
DARWIN_STAIRS_PHASE7_TIME_L = STAIRS_PHASE7_TIME,
DARWIN_STAIRS_PHASE7_TIME_H = STAIRS_PHASE7_TIME+1,
DARWIN_STAIRS_PHASE8_TIME_L = STAIRS_PHASE8_TIME,
DARWIN_STAIRS_PHASE8_TIME_H = STAIRS_PHASE8_TIME+1,
DARWIN_STAIRS_PHASE9_TIME_L = STAIRS_PHASE9_TIME,
DARWIN_STAIRS_PHASE9_TIME_H = STAIRS_PHASE9_TIME+1,
DARWIN_STAIRS_X_OFFSET = STAIRS_X_OFFSET,
DARWIN_STAIRS_Y_OFFSET = STAIRS_Y_OFFSET,
DARWIN_STAIRS_Z_OFFSET = STAIRS_Z_OFFSET,
DARWIN_STAIRS_Y_SHIFT = STAIRS_Y_SHIFT,
DARWIN_STAIRS_X_LEFT_SHIFT = STAIRS_X_LEFT_SHIFT,
DARWIN_STAIRS_X_RIGHT_SHIFT = STAIRS_X_RIGHT_SHIFT,
DARWIN_STAIRS_Z_OVERSHOOT = STAIRS_Z_OVERSHOOT,
DARWIN_STAIRS_Z_HEIGHT = STAIRS_Z_HEIGHT,
DARWIN_STAIRS_HIP_PITCH_OFF = STAIRS_HIP_PITCH_OFF,
//RAM
DARWIN_TX_LED_CNTRL = 0x0100, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// | | | | blink | toggle | value | internally used
......@@ -582,6 +628,8 @@ typedef enum {
// | detected | enable
DARWIN_GRIPPER_CNTRL = 0x0251 // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// left opened | right opened | left moving | right moving | close left | open left | close right | open right
DARWIN_STAIRS_CNTRL = 0x0252, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// current phase | climbing | | | stop stairs | start stairs
}darwin_registers;
#define GPIO_BASE_ADDRESS 0x0100
......
include/eeprom_init.h
+ 19
− 3
View file @ 9b9c4030
......@@ -83,9 +83,25 @@ extern "C" {
#define DEFAULT_GRIPPER_RIGHT_MAX_ANGLE 0x0F00 // 30 in fixed point format 9|7
#define DEFAULT_GRIPPER_RIGHT_MIN_ANGLE 0xF100 // -30 in fixed point format 9|7
#define DEFAULT_GRIPPER_RIGHT_MAX_FORCE 0x0080 // 1023 max force in binary format
#define DEFAULT_SMART_CHARGER_PERIOD 0x05DC //1500 ms
#define DEFAULT_SMART_CHARGER_PERIOD 0x05DC // 1500 ms
#define DEFAULT_STAIRS_PHASE1_TIME 0x0064 // 100 ms
#define DEFAULT_STAIRS_PHASE2_TIME 0x00C8 // 200 ms
#define DEFAULT_STAIRS_PHASE3_TIME 0x012C // 300 ms
#define DEFAULT_STAIRS_PHASE4_TIME 0x0190 // 400 ms
#define DEFAULT_STAIRS_PHASE5_TIME 0x01F4 // 500 ms
#define DEFAULT_STAIRS_PHASE6_TIME 0x0258 // 600 ms
#define DEFAULT_STAIRS_PHASE7_TIME 0x02BC // 700 ms
#define DEFAULT_STAIRS_PHASE8_TIME 0x0320 // 800 ms
#define DEFAULT_STAIRS_PHASE9_TIME 0x0384 // 900 ms
#define DEFAULT_STAIRS_X_OFFSET 0xFFF6 // -10 mm
#define DEFAULT_STAIRS_Y_OFFSET 0x0005 // 5mm
#define DEFAULT_STAIRS_Z_OFFSET 0x0014 // 20 mm
#define DEFAULT_STAIRS_Y_SHIFT 0x0014 // 20 mm
#define DEFAULT_STAIRS_X_LEFT_SHIFT 0x000A // 10 mm
#define DEFAULT_STAIRS_X_RIGHT_SHIFT 0x0032 // 50 mm
#define DEFAULT_STAIRS_Z_OVERSHOOT 0x000A // 10 mm
#define DEFAULT_STAIRS_Z_HEIGHT 0x001E // 30 mm
#define DEFAULT_STAIRS_HIP_PITCH_OFF 0x34B6 // 13.18 degrees in fixed point format 6 (1+5) | 10
#ifdef __cplusplus
}
......
include/stairs.h 0 → 100755
+ 30
− 0
View file @ 9b9c4030
#ifndef _STAIRS_H
#define _STAIRS_H
#ifdef __cplusplus
extern "C" {
#endif
#include "stm32f1xx.h"
#include "motion_manager.h"
// public functions
void stairs_init(uint16_t period_us);
inline void stairs_set_period(uint16_t period_us);
inline uint16_t stairs_get_period(void);
void stairs_start(void);
void stairs_stop(void);
uint8_t is_climbing_stairs(void);
// operation functions
uint8_t stairs_in_range(unsigned short int address, unsigned short int length);
void stairs_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
void stairs_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
// motion manager interface functions
void stairs_process(void);
#ifdef __cplusplus
}
#endif
#endif
src/eeprom.c
+ 28
− 1
View file @ 9b9c4030
......@@ -156,7 +156,34 @@ uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE,
DEFAULT_GRIPPER_RIGHT_MAX_FORCE&0xFF, GRIPPER_RIGHT_MAX_FORCE,
DEFAULT_GRIPPER_RIGHT_MAX_FORCE>>8, GRIPPER_RIGHT_MAX_FORCE+1,
DEFAULT_SMART_CHARGER_PERIOD&0xFF, SMART_CHARGER_PERIOD,
DEFAULT_SMART_CHARGER_PERIOD>>8, SMART_CHARGER_PERIOD+1};
DEFAULT_SMART_CHARGER_PERIOD>>8, SMART_CHARGER_PERIOD+1,
DEFAULT_STAIRS_PHASE1_TIME&0xFF, STAIRS_PHASE1_TIME,
DEFAULT_STAIRS_PHASE1_TIME>>8, STAIRS_PHASE1_TIME+1,
DEFAULT_STAIRS_PHASE2_TIME&0xFF, STAIRS_PHASE2_TIME,
DEFAULT_STAIRS_PHASE2_TIME>>8, STAIRS_PHASE2_TIME+1,
DEFAULT_STAIRS_PHASE3_TIME&0xFF, STAIRS_PHASE3_TIME,
DEFAULT_STAIRS_PHASE3_TIME>>8, STAIRS_PHASE3_TIME+1,
DEFAULT_STAIRS_PHASE4_TIME&0xFF, STAIRS_PHASE4_TIME,
DEFAULT_STAIRS_PHASE4_TIME>>8, STAIRS_PHASE4_TIME+1,
DEFAULT_STAIRS_PHASE5_TIME&0xFF, STAIRS_PHASE5_TIME,
DEFAULT_STAIRS_PHASE5_TIME>>8, STAIRS_PHASE5_TIME+1,
DEFAULT_STAIRS_PHASE6_TIME&0xFF, STAIRS_PHASE6_TIME,
DEFAULT_STAIRS_PHASE6_TIME>>8, STAIRS_PHASE6_TIME+1,
DEFAULT_STAIRS_PHASE7_TIME&0xFF, STAIRS_PHASE7_TIME,
DEFAULT_STAIRS_PHASE7_TIME>>8, STAIRS_PHASE7_TIME+1,
DEFAULT_STAIRS_PHASE8_TIME&0xFF, STAIRS_PHASE8_TIME,
DEFAULT_STAIRS_PHASE8_TIME>>8, STAIRS_PHASE8_TIME+1,
DEFAULT_STAIRS_PHASE9_TIME&0xFF, STAIRS_PHASE9_TIME,
DEFAULT_STAIRS_PHASE9_TIME>>8, STAIRS_PHASE9_TIME+1,
DEFAULT_STAIRS_X_OFFSET, STAIRS_X_OFFSET,
DEFAULT_STAIRS_Y_OFFSET, STAIRS_Y_OFFSET,
DEFAULT_STAIRS_Z_OFFSET, STAIRS_Z_OFFSET,
DEFAULT_STAIRS_Y_SHIFT, STAIRS_Y_SHIFT,
DEFAULT_STAIRS_X_LEFT_SHIFT, STAIRS_X_LEFT_SHIFT,
DEFAULT_STAIRS_X_RIGHT_SHIFT, STAIRS_X_RIGHT_SHIFT,
DEFAULT_STAIRS_Z_OVERSHOOT, STAIRS_Z_OVERSHOOT,
DEFAULT_STAIRS_Z_HEIGHT, STAIRS_Z_HEIGHT,
DEFAULT_STAIRS_HIP_PITCH_OFF, STAIRS_HIP_PITCH_OFF};
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
......
src/stairs.c 0 → 100755
+ 339
− 0
View file @ 9b9c4030
#include "stairs.h"
#include "darwin_kinematics.h"
#include "darwin_math.h"
#include "ram.h"
#include <math.h>
typedef enum {SHIFT_WEIGHT_LEFT,RISE_RIGHT_FOOT,ADVANCE_RIGHT_FOOT,CONTACT_RIGHT_FOOT,SHIFT_WEIGHT_RIGHT,RISE_LEFT_FOOT,ADVANCE_LEFT_FOOT,CONTACT_LEFT_FOOT,CENTER_WEIGHT} stairs_phase_t;
// internal motion variables
float m_Z_stair_height;
float m_Z_overshoot;
float m_Y_swap_amplitude;
float m_X_left_swap_amplitude;
float m_Y_right_swap_amplitude;
// internal offset variables
float m_Hip_Pitch_Offset;
float m_X_Offset;
float m_Y_Offset;
float m_Z_Offset;
// internal time variables
float m_Time;
float m_stairs_period;
float m_shift_weight_left_time;
float m_rise_right_foot_time;
float advance_right_foot_time;
float contact_right_foot_time;
float shift_weight_right_time;
float rise_left_foot_time;
float advance_left_foot_time;
float contact_left_foot_time;
float center_weight_time;
float total_time;
// control variables
uint8_t m_Ctrl_Running;
// private functions
// public functions
void stairs_init(uint16_t period_us)
{
// initialize the internal motion variables
// initialize internal control variables
m_Time=0;
stairs_period=((float)period_us)/1000.0;
m_Ctrl_Running=0x00;
stairs_process();
}
inline void stairs_set_period(uint16_t period_us)
{
stairs_period=((float)period_us)/1000.0;
}
inline uint16_t stairs_get_period(void)
{
return (uint16_t)(stairs_period*1000);
}
void stairs_start(void)
{
ram_data[DARWIN_WALK_CNTRL]|=WALK_STATUS;
m_Ctrl_Running=0x01;
}
void stairs_stop(void)
{
m_Ctrl_Running=0x00;
}
uint8_t is_climbing_stairs(void)
{
if(ram_data[DARWIN_WALK_CNTRL]&WALK_STATUS)
return 0x01;
else
return 0x00;
}
// motion manager interface functions
void stairs_process(void)
{
float x_swap,y_swap,z_swap,a_swap,b_swap,c_swap;
float x_move_r,y_move_r,z_move_r,a_move_r,b_move_r,c_move_r;
float x_move_l,y_move_l,z_move_l,a_move_l,b_move_l,c_move_l;
float pelvis_offset_r,pelvis_offset_l;
// float m_Body_Swing_Y,m_Body_Swing_Z;
float angle[14],ep[12];
if(m_Time==0)
{
update_param_time();
ram_data[DARWIN_WALK_CNTRL]&=(~WALK_PHASE);
ram_data[DARWIN_WALK_CNTRL]|=PHASE0;
if(m_Ctrl_Running==0x00)
{
if(m_X_Move_Amplitude==0 && m_Y_Move_Amplitude==0 && m_A_Move_Amplitude==0)
ram_data[DARWIN_WALK_CNTRL]&=(~WALK_STATUS);
else
{
ram_data[DARWIN_WALK_STEP_FW_BW]=0.0;
ram_data[DARWIN_WALK_STEP_LEFT_RIGHT]=0.0;
ram_data[DARWIN_WALK_STEP_DIRECTION]=0.0;
}
}
}
else if(m_Time>=(m_Phase_Time1-stairs_period/2.0) && m_Time<(m_Phase_Time1+stairs_period/2.0))
{
update_param_move();
ram_data[DARWIN_WALK_CNTRL]&=(~WALK_PHASE);
ram_data[DARWIN_WALK_CNTRL]|=PHASE1;
}
else if(m_Time>=(m_Phase_Time2-stairs_period/2.0) && m_Time<(m_Phase_Time2+stairs_period/2.0))
{
update_param_time();
m_Time=m_Phase_Time2;
ram_data[DARWIN_WALK_CNTRL]&=(~WALK_PHASE);
ram_data[DARWIN_WALK_CNTRL]|=PHASE2;
if(m_Ctrl_Running==0x00)
{
if(m_X_Move_Amplitude==0 && m_Y_Move_Amplitude==0 && m_A_Move_Amplitude==0)
ram_data[DARWIN_WALK_CNTRL]&=~WALK_STATUS;
else
{
ram_data[DARWIN_WALK_STEP_FW_BW]=0.0;
ram_data[DARWIN_WALK_STEP_LEFT_RIGHT]=0.0;
ram_data[DARWIN_WALK_STEP_DIRECTION]=0.0;
}
}
}
else if(m_Time>=(m_Phase_Time3-stairs_period/2.0) && m_Time<(m_Phase_Time3+stairs_period/2.0))
{
update_param_move();
ram_data[DARWIN_WALK_CNTRL]&=(~WALK_PHASE);
ram_data[DARWIN_WALK_CNTRL]|=PHASE3;
}
update_param_balance();
// Compute endpoints
x_swap=wsin(m_Time,m_X_Swap_PeriodTime,m_X_Swap_Phase_Shift,m_X_Swap_Amplitude,m_X_Swap_Amplitude_Shift);
y_swap=wsin(m_Time,m_Y_Swap_PeriodTime,m_Y_Swap_Phase_Shift,m_Y_Swap_Amplitude,m_Y_Swap_Amplitude_Shift);
z_swap=wsin(m_Time,m_Z_Swap_PeriodTime,m_Z_Swap_Phase_Shift,m_Z_Swap_Amplitude,m_Z_Swap_Amplitude_Shift);
a_swap=0;
b_swap=0;
c_swap=0;
if(m_Time<=m_SSP_Time_Start_L)
{
x_move_l=wsin(m_SSP_Time_Start_L,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift);
y_move_l=wsin(m_SSP_Time_Start_L,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift);
z_move_l=wsin(m_SSP_Time_Start_L,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
c_move_l=wsin(m_SSP_Time_Start_L,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift);
x_move_r=wsin(m_SSP_Time_Start_L,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift);
y_move_r=wsin(m_SSP_Time_Start_L,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift);
z_move_r=wsin(m_SSP_Time_Start_R,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
c_move_r=wsin(m_SSP_Time_Start_L,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift);
pelvis_offset_l=0;
pelvis_offset_r=0;
}
else if(m_Time<=m_SSP_Time_End_L)
{
x_move_l=wsin(m_Time,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift);
y_move_l=wsin(m_Time,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift);
z_move_l=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
c_move_l=wsin(m_Time,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift);
x_move_r=wsin(m_Time,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift);
y_move_r=wsin(m_Time,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift);
z_move_r=wsin(m_SSP_Time_Start_R,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
c_move_r=wsin(m_Time,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift);
pelvis_offset_l=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Pelvis_Swing/2.0,m_Pelvis_Swing/2.0);
pelvis_offset_r=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,-m_Pelvis_Offset/2.0,-m_Pelvis_Offset/2.0);
}
else if(m_Time<=m_SSP_Time_Start_R)
{
x_move_l=wsin(m_SSP_Time_End_L,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift);
y_move_l=wsin(m_SSP_Time_End_L,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift);
z_move_l=wsin(m_SSP_Time_End_L,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
c_move_l=wsin(m_SSP_Time_End_L,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift);
x_move_r=wsin(m_SSP_Time_End_L,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_L,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift);
y_move_r=wsin(m_SSP_Time_End_L,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_L,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift);
z_move_r=wsin(m_SSP_Time_Start_R,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
c_move_r=wsin(m_SSP_Time_End_L,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_L,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift);
pelvis_offset_l=0.0;
pelvis_offset_r=0.0;
}
else if(m_Time<=m_SSP_Time_End_R)
{
x_move_l=wsin(m_Time,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift);
y_move_l=wsin(m_Time,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift);
z_move_l=wsin(m_SSP_Time_End_L,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
c_move_l=wsin(m_Time,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift);
x_move_r=wsin(m_Time,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift);
y_move_r=wsin(m_Time,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift);
z_move_r=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
c_move_r=wsin(m_Time,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift);
pelvis_offset_l=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Pelvis_Offset/2,m_Pelvis_Offset/2);
pelvis_offset_r=wsin(m_Time,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,-m_Pelvis_Swing/2,-m_Pelvis_Swing/2);
}
else
{
x_move_l=wsin(m_SSP_Time_End_R,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_X_Move_Amplitude,m_X_Move_Amplitude_Shift);
y_move_l=wsin(m_SSP_Time_End_R,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_Y_Move_Amplitude,m_Y_Move_Amplitude_Shift);
z_move_l=wsin(m_SSP_Time_End_L,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_L,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
c_move_l=wsin(m_SSP_Time_End_R,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_R+PI,m_A_Move_Amplitude,m_A_Move_Amplitude_Shift);
x_move_r=wsin(m_SSP_Time_End_R,m_X_Move_PeriodTime,m_X_Move_Phase_Shift+2.0*PI/m_X_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_X_Move_Amplitude,-m_X_Move_Amplitude_Shift);
y_move_r=wsin(m_SSP_Time_End_R,m_Y_Move_PeriodTime,m_Y_Move_Phase_Shift+2.0*PI/m_Y_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_Y_Move_Amplitude,-m_Y_Move_Amplitude_Shift);
z_move_r=wsin(m_SSP_Time_End_R,m_Z_Move_PeriodTime,m_Z_Move_Phase_Shift+2.0*PI/m_Z_Move_PeriodTime*m_SSP_Time_Start_R,m_Z_Move_Amplitude,m_Z_Move_Amplitude_Shift);
c_move_r=wsin(m_SSP_Time_End_R,m_A_Move_PeriodTime,m_A_Move_Phase_Shift+2.0*PI/m_A_Move_PeriodTime*m_SSP_Time_Start_R+PI,-m_A_Move_Amplitude,-m_A_Move_Amplitude_Shift);
pelvis_offset_l=0.0;
pelvis_offset_r=0.0;
}
a_move_l=0.0;
b_move_l=0.0;
a_move_r=0.0;
b_move_r=0.0;
ep[0]=x_swap+x_move_r+m_X_Offset;
ep[1]=y_swap+y_move_r-m_Y_Offset / 2.0;
ep[2]=z_swap+z_move_r+m_Z_Offset;
ep[3]=a_swap+a_move_r-m_R_Offset / 2.0;
ep[4]=b_swap+b_move_r+m_P_Offset;
ep[5]=c_swap+c_move_r-m_A_Offset / 2.0;
ep[6]=x_swap+x_move_l+m_X_Offset;
ep[7]=y_swap+y_move_l+m_Y_Offset / 2.0;
ep[8]=z_swap+z_move_l+m_Z_Offset;
ep[9]=a_swap+a_move_l+m_R_Offset / 2.0;
ep[10]=b_swap+b_move_l+m_P_Offset;
ep[11]=c_swap+c_move_l+m_A_Offset / 2.0;
// Compute body swing
/* if(m_Time<=m_SSP_Time_End_L)
{
m_Body_Swing_Y=-ep[7];
m_Body_Swing_Z=ep[8];
}
else
{
m_Body_Swing_Y=-ep[1];
m_Body_Swing_Z=ep[2];
}
m_Body_Swing_Z-=DARWIN_LEG_LENGTH;*/
// Compute arm swing
if(m_X_Move_Amplitude==0)
{
angle[12]=0; // Right
angle[13]=0; // Left
}
else
{
angle[12]=wsin(m_Time,m_PeriodTime,0.0,-m_X_Move_Amplitude*m_Arm_Swing_Gain,0.0);
angle[13]=wsin(m_Time,m_PeriodTime,0.0,m_X_Move_Amplitude*m_Arm_Swing_Gain,0.0);
}
if(ram_data[DARWIN_WALK_CNTRL]&WALK_STATUS)
{
m_Time += stairs_period;
if(m_Time >= m_PeriodTime)
m_Time = 0;
}
// Compute angles with the inverse kinematics
if((darwin_leg_ik(&angle[0],ep[0],ep[1],ep[2],ep[3],ep[4],ep[5])!=1) ||
(darwin_leg_ik(&angle[6],ep[6],ep[7],ep[8],ep[9],ep[10],ep[11])!=1))
return;// Do not use angles
// Compute motor value
if(manager_get_module(R_HIP_YAW)==MM_WALKING)
manager_current_angles[R_HIP_YAW]=((-180.0*angle[0])/PI)*65536.0;
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;
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;
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)
manager_current_angles[L_HIP_YAW]=((-180.0*angle[6])/PI)*65536.0;
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;
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;
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)
manager_current_angles[R_SHOULDER_PITCH]=(angle[12]-48.345)*65536.0;
if(manager_get_module(L_SHOULDER_PITCH)==MM_WALKING)
manager_current_angles[L_SHOULDER_PITCH]=(-angle[13]+41.313)*65536.0;
}
// operation functions
uint8_t stairs_in_range(unsigned short int address, unsigned short int length)
{
if(ram_in_window(WALK_BASE_ADDRESS,WALK_MEM_LENGTH,address,length) ||
ram_in_window(WALK_EEPROM_ADDRESS,WALK_EEPROM_LENGTH,address,length))
return 0x01;
else
return 0x00;
}
void stairs_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
{
}
void stairs_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
{
uint16_t i;
// walk control
if(ram_in_range(DARWIN_WALK_CNTRL,address,length))
{
if(data[DARWIN_WALK_CNTRL-address]&WALK_START)
stairs_start();
if(data[DARWIN_WALK_CNTRL-address]&WALK_STOP)
stairs_stop();
}
if(ram_in_range(DARWIN_WALK_STEP_FW_BW,address,length))
ram_data[DARWIN_WALK_STEP_FW_BW]=data[DARWIN_WALK_STEP_FW_BW-address];
if(ram_in_range(DARWIN_WALK_STEP_LEFT_RIGHT,address,length))
ram_data[DARWIN_WALK_STEP_LEFT_RIGHT]=data[DARWIN_WALK_STEP_LEFT_RIGHT-address];
if(ram_in_range(DARWIN_WALK_STEP_DIRECTION,address,length))
ram_data[DARWIN_WALK_STEP_DIRECTION]=data[DARWIN_WALK_STEP_DIRECTION-address];
// walk parameters
for(i=WALK_EEPROM_ADDRESS;i<=WALK_EEPROM_ADDRESS+WALK_EEPROM_LENGTH;i++)
if(ram_in_range(i,address,length))
ram_data[i]=data[i-address];
}
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