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

Changed all the hardcoded values to registers in the dynamixel interface. 

Cleaned up the stairs code.
parent ba914090
No related branches found
No related tags found
2 merge requests!5Dynamixel manager,!2Smart charger fw
Hide whitespace changes
Inline Side-by-side
include/darwin_registers.h
+ 22
− 11
View file @ 7d7b711d
......@@ -95,16 +95,22 @@ extern "C" {
#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_PHASE10_TIME ((unsigned short int)0x0078)
#define STAIRS_X_OFFSET ((unsigned short int)0x007A)
#define STAIRS_Y_OFFSET ((unsigned short int)0x007B)
#define STAIRS_Z_OFFSET ((unsigned short int)0x007C)
#define STAIRS_Y_SHIFT ((unsigned short int)0x007D)
#define STAIRS_X_LEFT_SHIFT ((unsigned short int)0x007E)
#define STAIRS_X_RIGHT_SHIFT ((unsigned short int)0x007F)
#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_R_OFFSET ((unsigned short int)0x007B)
#define STAIRS_P_OFFSET ((unsigned short int)0x007C)
#define STAIRS_A_OFFSET ((unsigned short int)0x007D)
#define STAIRS_Y_SHIFT ((unsigned short int)0x007E)
#define STAIRS_X_SHIFT ((unsigned short int)0x007F)
#define STAIRS_Z_OVERSHOOT ((unsigned short int)0x0080)
#define STAIRS_Z_HEIGHT ((unsigned short int)0x0081)
#define STAIRS_HIP_PITCH_OFF ((unsigned short int)0x0082)
#define STAIRS_R_SHIFT ((unsigned short int)0x0084)
#define STAIRS_P_SHIFT ((unsigned short int)0x0085)
#define STAIRS_A_SHIFT ((unsigned short int)0x0086)
#define STAIRS_Y_SPREAD ((unsigned short int)0x0087)
#define STAIRS_X_SHIFT_BODY ((unsigned short int)0x0088)
#define LAST_EEPROM_OFFSET ((unsigned short int)0x00FF)
......@@ -229,18 +235,23 @@ typedef enum {
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_PHASE10_TIME_L = STAIRS_PHASE10_TIME,
DARWIN_STAIRS_PHASE10_TIME_H = STAIRS_PHASE10_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_R_OFFSET = STAIRS_R_OFFSET,
DARWIN_STAIRS_P_OFFSET = STAIRS_P_OFFSET,
DARWIN_STAIRS_A_OFFSET = STAIRS_A_OFFSET,
DARWIN_STAIRS_X_SHIFT = STAIRS_X_SHIFT,
DARWIN_STAIRS_Z_OVERSHOOT = STAIRS_Z_OVERSHOOT,
DARWIN_STAIRS_Z_HEIGHT = STAIRS_Z_HEIGHT,
DARWIN_STAIRS_HIP_PITCH_OFF_L = STAIRS_HIP_PITCH_OFF,
DARWIN_STAIRS_HIP_PITCH_OFF_H = STAIRS_HIP_PITCH_OFF+1,
DARWIN_STAIRS_R_SHIFT = STAIRS_R_SHIFT,
DARWIN_STAIRS_P_SHIFT = STAIRS_P_SHIFT,
DARWIN_STAIRS_A_SHIFT = STAIRS_A_SHIFT,
DARWIN_STAIRS_Y_SPREAD = STAIRS_Y_SPREAD,
DARWIN_STAIRS_X_SHIFT_BODY = STAIRS_X_SHIFT_BODY,
//RAM
DARWIN_TX_LED_CNTRL = 0x0100, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
......
include/eeprom.h
+ 1
− 1
View file @ 7d7b711d
......@@ -83,7 +83,7 @@ extern "C" {
#define PAGE_FULL ((uint8_t)0x80)
/* Variables' number */
#define NB_OF_VAR ((uint8_t)0x83)
#define NB_OF_VAR ((uint8_t)0x89)
/* Exported types ------------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
......
include/eeprom_init.h
+ 19
− 13
View file @ 7d7b711d
......@@ -84,27 +84,33 @@ extern "C" {
#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_STAIRS_PHASE1_TIME 0x0640 // 100 ms
#define DEFAULT_STAIRS_PHASE2_TIME 0x0C80 // 200 ms
#define DEFAULT_STAIRS_PHASE3_TIME 0x12C0 // 300 ms
#define DEFAULT_STAIRS_PHASE4_TIME 0x1900 // 400 ms
#define DEFAULT_STAIRS_PHASE5_TIME 0x1F40 // 500 ms
#define DEFAULT_STAIRS_PHASE6_TIME 0x2580 // 600 ms
#define DEFAULT_STAIRS_PHASE7_TIME 0x2BC0 // 700 ms
#define DEFAULT_STAIRS_PHASE8_TIME 0x3200 // 800 ms
#define DEFAULT_STAIRS_PHASE9_TIME 0x3840 // 900 ms
#define DEFAULT_STAIRS_PHASE10_TIME 0x3E80 // 1000 ms
#define DEFAULT_STAIRS_PHASE1_TIME 0x0640 // 1600 ms
#define DEFAULT_STAIRS_PHASE2_TIME 0x0C80 // 3200 ms
#define DEFAULT_STAIRS_PHASE3_TIME 0x12C0 // 4800 ms
#define DEFAULT_STAIRS_PHASE4_TIME 0x1900 // 6400 ms
#define DEFAULT_STAIRS_PHASE5_TIME 0x1F40 // 8000 ms
#define DEFAULT_STAIRS_PHASE6_TIME 0x2580 // 9600 ms
#define DEFAULT_STAIRS_PHASE7_TIME 0x2BC0 // 11200 ms
#define DEFAULT_STAIRS_PHASE8_TIME 0x3200 // 12800 ms
#define DEFAULT_STAIRS_PHASE9_TIME 0x3840 // 14400 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_R_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
#define DEFAULT_STAIRS_P_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
#define DEFAULT_STAIRS_A_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
#define DEFAULT_STAIRS_Y_SHIFT 0x0028 // 40 mm
#define DEFAULT_STAIRS_X_LEFT_SHIFT 0x0000 // 0 mm
#define DEFAULT_STAIRS_X_RIGHT_SHIFT 0x0050 // 80 mm
#define DEFAULT_STAIRS_X_SHIFT 0x0050 // 80 mm
#define DEFAULT_STAIRS_Z_OVERSHOOT 0x000F // 15 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
#define DEFAULT_STAIRS_R_SHIFT 0x0017 // 2.875 degrees in fixed point format 5 (1+4) | 3
#define DEFAULT_STAIRS_P_SHIFT 0x002E // 5.73 degrees in fixed point format 5 (1+4) | 3
#define DEFAULT_STAIRS_A_SHIFT 0x0045 // 17.19 degrees in fixed point format 6 (1+5) | 2
#define DEFAULT_STAIRS_Y_SPREAD 0x0014 // 20 mm
#define DEFAULT_STAIRS_X_SHIFT_BODY 0x0023 // 35 mm
#define EEPROM_SIZE 131
#define EEPROM_SIZE 137
#ifdef __cplusplus
}
......
src/eeprom.c
+ 11
− 5
View file @ 7d7b711d
......@@ -175,17 +175,23 @@ uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE,
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_PHASE10_TIME&0xFF, STAIRS_PHASE10_TIME,
DEFAULT_STAIRS_PHASE10_TIME>>8, STAIRS_PHASE10_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_R_OFFSET, STAIRS_R_OFFSET,
DEFAULT_STAIRS_P_OFFSET, STAIRS_P_OFFSET,
DEFAULT_STAIRS_A_OFFSET, STAIRS_A_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_X_SHIFT, STAIRS_X_SHIFT,
DEFAULT_STAIRS_Z_OVERSHOOT, STAIRS_Z_OVERSHOOT,
DEFAULT_STAIRS_Z_HEIGHT, STAIRS_Z_HEIGHT,
DEFAULT_STAIRS_HIP_PITCH_OFF, STAIRS_HIP_PITCH_OFF};
DEFAULT_STAIRS_HIP_PITCH_OFF&0xFF, STAIRS_HIP_PITCH_OFF,
DEFAULT_STAIRS_HIP_PITCH_OFF>>8, STAIRS_HIP_PITCH_OFF+1,
DEFAULT_STAIRS_R_SHIFT, STAIRS_R_SHIFT,
DEFAULT_STAIRS_P_SHIFT, STAIRS_P_SHIFT,
DEFAULT_STAIRS_A_SHIFT, STAIRS_A_SHIFT,
DEFAULT_STAIRS_Y_SPREAD, STAIRS_Y_SPREAD,
DEFAULT_STAIRS_X_SHIFT_BODY, STAIRS_X_SHIFT_BODY};
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
......
src/stairs.c
+ 114
− 92
View file @ 7d7b711d
......@@ -5,20 +5,27 @@
#include <stdio.h>
#include <math.h>
typedef enum {SHIFT_WEIGHT_LEFT=0x10,RISE_RIGHT_FOOT=0x20,ADVANCE_RIGHT_FOOT=0x30,CONTACT_RIGHT_FOOT=0x40,SHIFT_WEIGHT_RIGHT=0x50,RISE_LEFT_FOOT=0x60,ADVANCE_LEFT_FOOT=0x70,CONTACT_LEFT_FOOT=0x80,CENTER_WEIGHT=0x90,STAND_UP=0xA0} stairs_phase_t;
typedef enum {SHIFT_WEIGHT_LEFT=0x10,RISE_RIGHT_FOOT=0x20,ADVANCE_RIGHT_FOOT=0x30,CONTACT_RIGHT_FOOT=0x40,SHIFT_WEIGHT_RIGHT=0x50,RISE_LEFT_FOOT=0x60,ADVANCE_LEFT_FOOT=0x70,CONTACT_LEFT_FOOT=0x80,CENTER_WEIGHT=0x90} stairs_phase_t;
// internal motion variables
float stairs_Z_stair_height;
float stairs_Z_overshoot;
float stairs_Y_shift_amplitude;
float stairs_X_left_shift_amplitude;
float stairs_X_right_shift_amplitude;
float stairs_X_shift_amplitude;
float stairs_R_shift_amplitude;
float stairs_P_shift_amplitude;
float stairs_A_shift_amplitude;
float stairs_Y_spread_amplitude;
float stairs_X_shift_body;
// internal offset variables
float stairs_Hip_Pitch_Offset;
float stairs_X_Offset;
float stairs_Y_Offset;
float stairs_Z_Offset;
float stairs_R_Offset;
float stairs_P_Offset;
float stairs_A_Offset;
// internal time variables
float stairs_Time;
......@@ -32,7 +39,6 @@ float stairs_rise_left_foot_time;
float stairs_advance_left_foot_time;
float stairs_contact_left_foot_time;
float stairs_center_weight_time;
float stairs_walk_ready_time;
// control variables
uint8_t stairs_Ctrl_Running;
......@@ -73,16 +79,22 @@ void stairs_start(void)
stairs_advance_left_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE7_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE7_TIME_H]<<8));
stairs_contact_left_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE8_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE8_TIME_H]<<8));
stairs_center_weight_time=((float)ram_data[DARWIN_STAIRS_PHASE9_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE9_TIME_H]<<8));
stairs_walk_ready_time=((float)ram_data[DARWIN_STAIRS_PHASE10_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE10_TIME_H]<<8));
stairs_Z_stair_height=((float)ram_data[DARWIN_STAIRS_Z_HEIGHT]);
stairs_Z_overshoot=((float)ram_data[DARWIN_STAIRS_Z_OVERSHOOT]);
stairs_Y_shift_amplitude=((float)ram_data[DARWIN_STAIRS_Y_SHIFT]);
stairs_X_left_shift_amplitude=((float)ram_data[DARWIN_STAIRS_X_LEFT_SHIFT]);
stairs_X_right_shift_amplitude=((float)ram_data[DARWIN_STAIRS_X_RIGHT_SHIFT]);
stairs_Z_stair_height=((float)((int8_t)ram_data[DARWIN_STAIRS_Z_HEIGHT]));
stairs_Z_overshoot=((float)((int8_t)ram_data[DARWIN_STAIRS_Z_OVERSHOOT]));
stairs_Y_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_SHIFT]));
stairs_X_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_X_SHIFT]));
stairs_Hip_Pitch_Offset=((float)((int16_t)(ram_data[DARWIN_WALK_HIP_PITCH_OFF_L]+(ram_data[DARWIN_WALK_HIP_PITCH_OFF_H]<<8))))/1024.0;
stairs_R_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_R_SHIFT]))*PI/1440.0;// (r_shift/8)*(pi/180)
stairs_P_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_P_SHIFT]))*PI/1440.0;// (p_shift/8)*(pi/180)
stairs_A_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_A_SHIFT]))*PI/720.0;// (a_shift/4)*(pi/180)
stairs_Y_spread_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_SPREAD]));
stairs_X_shift_body=((float)((int8_t)ram_data[DARWIN_STAIRS_X_SHIFT_BODY]));
stairs_X_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_X_OFFSET]));
stairs_Y_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_OFFSET]));
stairs_Z_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_Z_OFFSET]));
stairs_R_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_X_OFFSET]))*PI/1440.0;
stairs_P_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_OFFSET]))*PI/1440.0;
stairs_A_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_Z_OFFSET]))*PI/1440.0;
// start operation
ram_data[DARWIN_STAIRS_CNTRL]|=STAIRS_STATUS;
stairs_Ctrl_Running=0x01;
......@@ -105,6 +117,7 @@ uint8_t is_climbing_stairs(void)
void stairs_process(void)
{
float angle[14]={0},ep[12]={0};
float delta;
if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS)
{
......@@ -112,156 +125,165 @@ void stairs_process(void)
if(stairs_Time>=0 && stairs_Time<=stairs_shift_weight_left_time)
{
//1
delta=stairs_Time/stairs_shift_weight_left_time;
ep[0]=stairs_X_Offset;
ep[1]=stairs_Y_Offset/2.0+(stairs_Y_shift_amplitude/stairs_shift_weight_left_time)*stairs_Time;
ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta;
ep[2]=stairs_Z_Offset;
ep[3]=stairs_R_Offset;
ep[4]=stairs_P_Offset;
ep[5]=stairs_A_Offset;
ep[6]=stairs_X_Offset;
ep[7]=stairs_Y_Offset/2.0+(stairs_Y_shift_amplitude/stairs_shift_weight_left_time)*stairs_Time;
ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta;
ep[8]=stairs_Z_Offset;
ep[9]=stairs_R_Offset;
ep[10]=stairs_P_Offset;
ep[11]=stairs_A_Offset;
ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_LEFT;
}
else if(stairs_Time>stairs_shift_weight_left_time && stairs_Time<=stairs_rise_right_foot_time)
{
//2
delta=(stairs_Time-stairs_shift_weight_left_time)/(stairs_rise_right_foot_time-stairs_shift_weight_left_time);
ep[0]=stairs_X_Offset;
ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude;
ep[2]=stairs_Z_Offset;
ep[3]=-0.05/(stairs_rise_right_foot_time-stairs_shift_weight_left_time)*(stairs_Time-stairs_shift_weight_left_time);
ep[3]=stairs_R_Offset-stairs_R_shift_amplitude*delta;
ep[4]=stairs_P_Offset;
ep[5]=stairs_A_Offset;
ep[6]=stairs_X_Offset;
ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude;
ep[8]=stairs_Z_Offset+(stairs_Z_overshoot+stairs_Z_stair_height)/(stairs_rise_right_foot_time-stairs_shift_weight_left_time)*(stairs_Time-stairs_shift_weight_left_time);
ep[9]=-0.05/(stairs_rise_right_foot_time-stairs_shift_weight_left_time)*(stairs_Time-stairs_shift_weight_left_time);
ep[8]=stairs_Z_Offset+(stairs_Z_overshoot+stairs_Z_stair_height)*delta;
ep[9]=stairs_R_Offset-stairs_R_shift_amplitude*delta;
ep[10]=stairs_P_Offset;
ep[11]=stairs_A_Offset;
ram_data[DARWIN_STAIRS_CNTRL]|=RISE_RIGHT_FOOT;
}
else if(stairs_Time>stairs_rise_right_foot_time && stairs_Time<=stairs_advance_right_foot_time)
{
//3
delta=(stairs_Time-stairs_rise_right_foot_time)/(stairs_advance_right_foot_time-stairs_rise_right_foot_time);
ep[0]=stairs_X_Offset;
ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-10.0/(stairs_advance_right_foot_time-stairs_rise_right_foot_time)*(stairs_Time-stairs_rise_right_foot_time);
ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)*delta;
ep[2]=stairs_Z_Offset;
ep[3]=-0.05;
ep[4]=0.1/(stairs_advance_right_foot_time-stairs_rise_right_foot_time)*(stairs_Time-stairs_rise_right_foot_time);
ep[5]=0.3/(stairs_advance_right_foot_time-stairs_rise_right_foot_time)*(stairs_Time-stairs_rise_right_foot_time);
ep[6]=stairs_X_Offset+stairs_X_right_shift_amplitude/(stairs_advance_right_foot_time-stairs_rise_right_foot_time)*(stairs_Time-stairs_rise_right_foot_time);
ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+10.0/(stairs_advance_right_foot_time-stairs_rise_right_foot_time)*(stairs_Time-stairs_rise_right_foot_time);
ep[3]=stairs_R_Offset-stairs_R_shift_amplitude;
ep[4]=stairs_P_Offset+stairs_P_shift_amplitude*delta;
ep[5]=stairs_A_Offset+stairs_A_shift_amplitude*delta;
ep[6]=stairs_X_Offset+stairs_X_shift_amplitude*delta;
ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)*delta;
ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height;
ep[9]=-0.05;
ep[10]=0.1/(stairs_advance_right_foot_time-stairs_rise_right_foot_time)*(stairs_Time-stairs_rise_right_foot_time);
ep[11]=0.3/(stairs_advance_right_foot_time-stairs_rise_right_foot_time)*(stairs_Time-stairs_rise_right_foot_time);
ep[9]=stairs_R_Offset-stairs_R_shift_amplitude;
ep[10]=stairs_P_Offset+stairs_P_shift_amplitude*delta;
ep[11]=stairs_A_Offset+stairs_A_shift_amplitude*delta;
ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_RIGHT_FOOT;
}
else if(stairs_Time>stairs_advance_right_foot_time && stairs_Time<=stairs_contact_right_foot_time)
{
//4
delta=(stairs_Time-stairs_advance_right_foot_time)/(stairs_contact_right_foot_time-stairs_advance_right_foot_time);
ep[0]=stairs_X_Offset;
ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-10.0;
ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0);
ep[2]=stairs_Z_Offset;
ep[3]=-0.05;
ep[4]=0.1;
ep[5]=0.3;
ep[6]=stairs_X_Offset+stairs_X_right_shift_amplitude;
ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+10.0;
ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_overshoot/(stairs_contact_right_foot_time-stairs_advance_right_foot_time)*(stairs_Time-stairs_advance_right_foot_time);
ep[9]=-0.05;
ep[10]=0.1;
ep[11]=0.3;
ep[3]=stairs_R_Offset-stairs_R_shift_amplitude;
ep[4]=stairs_P_Offset+stairs_P_shift_amplitude;
ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
ep[6]=stairs_X_Offset+stairs_X_shift_amplitude;
ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0);
ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_overshoot*delta;
ep[9]=stairs_R_Offset-stairs_R_shift_amplitude;
ep[10]=stairs_P_Offset+stairs_P_shift_amplitude;
ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_RIGHT_FOOT;
}
else if(stairs_Time>stairs_contact_right_foot_time && stairs_Time<=stairs_shift_weight_right_time)
{
//5
ep[0]=stairs_X_Offset-35.0/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time);
ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-10.0-(2*stairs_Y_shift_amplitude)/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time);
delta=(stairs_Time-stairs_contact_right_foot_time)/(stairs_shift_weight_right_time-stairs_contact_right_foot_time);
ep[0]=stairs_X_Offset-stairs_X_shift_body*delta;
ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta;
ep[2]=stairs_Z_Offset;
ep[3]=-0.05+0.05/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time);
ep[4]=0.1-0.2/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time);
ep[5]=0.3;
ep[6]=stairs_X_Offset+stairs_X_right_shift_amplitude-35.0/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time);
ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+10.0-(2*stairs_Y_shift_amplitude)/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time);
ep[3]=stairs_R_Offset-stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta;
ep[4]=stairs_P_Offset+stairs_P_shift_amplitude-(2.0*stairs_P_shift_amplitude)*delta;
ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body*delta;
ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta;
ep[8]=stairs_Z_Offset+stairs_Z_stair_height;
ep[9]=-0.05+0.05/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time);
ep[10]=0.1-0.2/(stairs_shift_weight_right_time-stairs_contact_right_foot_time)*(stairs_Time-stairs_contact_right_foot_time);
ep[11]=0.3;
ep[9]=stairs_R_Offset-stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta;
ep[10]=stairs_P_Offset+stairs_P_shift_amplitude-(2.0*stairs_P_shift_amplitude)*delta;
ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_RIGHT;
}
else if(stairs_Time>stairs_shift_weight_right_time && stairs_Time<=stairs_rise_left_foot_time)
{
//6
ep[0]=stairs_X_Offset-35.0;
ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-10.0;
ep[2]=stairs_Z_Offset+(stairs_Z_overshoot+stairs_Z_stair_height)/(stairs_rise_left_foot_time-stairs_shift_weight_right_time)*(stairs_Time-stairs_shift_weight_right_time);
ep[3]=0.05/(stairs_rise_left_foot_time-stairs_shift_weight_right_time)*(stairs_Time-stairs_shift_weight_right_time);
ep[4]=-0.1;
ep[5]=0.3;
ep[6]=stairs_X_Offset+stairs_X_right_shift_amplitude-35.0;
ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+10.0;
delta=(stairs_Time-stairs_shift_weight_right_time)/(stairs_rise_left_foot_time-stairs_shift_weight_right_time);
ep[0]=stairs_X_Offset-stairs_X_shift_body;
ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0);
ep[2]=stairs_Z_Offset+(stairs_Z_overshoot+stairs_Z_stair_height)*delta;
ep[3]=stairs_R_Offset+stairs_R_shift_amplitude*delta;
ep[4]=stairs_P_Offset-stairs_P_shift_amplitude;
ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body;
ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0);
ep[8]=stairs_Z_Offset+stairs_Z_stair_height;
ep[9]=0.05/(stairs_rise_left_foot_time-stairs_shift_weight_right_time)*(stairs_Time-stairs_shift_weight_right_time);
ep[10]=-0.1;
ep[11]=0.3;
ep[9]=stairs_R_Offset+stairs_R_shift_amplitude*delta;
ep[10]=stairs_P_Offset-stairs_P_shift_amplitude;
ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
ram_data[DARWIN_STAIRS_CNTRL]|=RISE_LEFT_FOOT;
}
else if(stairs_Time>stairs_rise_left_foot_time && stairs_Time<=stairs_advance_left_foot_time)
{
//7
ep[0]=stairs_X_Offset-35.0+35.0/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time);
ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-10.0+10.0/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time);
delta=(stairs_Time-stairs_rise_left_foot_time)/(stairs_advance_left_foot_time-stairs_rise_left_foot_time);
ep[0]=stairs_X_Offset-stairs_X_shift_body+stairs_X_shift_body*delta;
ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)+(stairs_Y_spread_amplitude/2.0)*delta;
ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height;
ep[3]=0.05;
ep[4]=-0.1+0.1/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time);
ep[5]=0.3-0.3/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time);
ep[6]=stairs_X_Offset+stairs_X_right_shift_amplitude-35.0-(-35.0+stairs_X_right_shift_amplitude)/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time);
ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+10.0-10.0/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time);
ep[3]=stairs_R_Offset+stairs_R_shift_amplitude;
ep[4]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta;
ep[5]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta;
ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body-(stairs_X_shift_amplitude-stairs_X_shift_body)*delta;
ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(stairs_Y_spread_amplitude/2.0)*delta;
ep[8]=stairs_Z_Offset+stairs_Z_stair_height;
ep[9]=0.05;
ep[10]=-0.1+0.1/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time);
ep[11]=0.3-0.3/(stairs_advance_left_foot_time-stairs_rise_left_foot_time)*(stairs_Time-stairs_rise_left_foot_time);
ep[9]=stairs_R_Offset+stairs_R_shift_amplitude;
ep[10]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta;
ep[11]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta;
ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_LEFT_FOOT;
}
else if(stairs_Time>stairs_advance_left_foot_time && stairs_Time<=stairs_contact_left_foot_time)
{
//8
delta=(stairs_Time-stairs_advance_left_foot_time)/(stairs_contact_left_foot_time-stairs_advance_left_foot_time);
ep[0]=stairs_X_Offset;
ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude;
ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_overshoot/(stairs_contact_left_foot_time-stairs_advance_left_foot_time)*(stairs_Time-stairs_advance_left_foot_time);
ep[3]=0.05-0.05/(stairs_contact_left_foot_time-stairs_advance_left_foot_time)*(stairs_Time-stairs_advance_left_foot_time);
ep[4]=0.0;
ep[5]=0.0;
ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_overshoot*delta;
ep[3]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
ep[4]=stairs_P_Offset;
ep[5]=stairs_A_Offset;
ep[6]=stairs_X_Offset;
ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude;
ep[8]=stairs_Z_Offset+stairs_Z_stair_height;
ep[9]=0.05-0.05/(stairs_contact_left_foot_time-stairs_advance_left_foot_time)*(stairs_Time-stairs_advance_left_foot_time);
ep[10]=0.0;
ep[11]=0.0;
ep[9]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
ep[10]=stairs_P_Offset;
ep[11]=stairs_A_Offset;
ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_LEFT_FOOT;
}
else if(stairs_Time>stairs_contact_left_foot_time && stairs_Time<=stairs_center_weight_time)
{
//9
delta=(stairs_Time-stairs_contact_left_foot_time)/(stairs_center_weight_time-stairs_contact_left_foot_time);
ep[0]=stairs_X_Offset;
ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude/(stairs_center_weight_time-stairs_contact_left_foot_time)*(stairs_Time-stairs_contact_left_foot_time);
ep[2]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height/(stairs_center_weight_time-stairs_contact_left_foot_time)*(stairs_Time-stairs_contact_left_foot_time);
ep[3]=0.0;
ep[4]=0.0;
ep[5]=0.0;
ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta;
ep[2]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height*delta;
ep[3]=stairs_R_Offset;
ep[4]=stairs_P_Offset;
ep[5]=stairs_A_Offset;
ep[6]=stairs_X_Offset;
ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude/(stairs_center_weight_time-stairs_contact_left_foot_time)*(stairs_Time-stairs_contact_left_foot_time);
ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height/(stairs_center_weight_time-stairs_contact_left_foot_time)*(stairs_Time-stairs_contact_left_foot_time);
ep[9]=0.0;
ep[10]=0.0;
ep[11]=0.0;
ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta;
ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height*delta;
ep[9]=stairs_R_Offset;
ep[10]=stairs_P_Offset;
ep[11]=stairs_A_Offset;
ram_data[DARWIN_STAIRS_CNTRL]|=CENTER_WEIGHT;
}
else if(stairs_Time>stairs_center_weight_time && stairs_Time<=stairs_walk_ready_time)
{
ep[0]=stairs_X_Offset;
ep[1]=stairs_Y_Offset/2.0;
ep[2]=stairs_Z_Offset;
ep[6]=stairs_X_Offset;
ep[7]=stairs_Y_Offset/2.0;
ep[8]=stairs_Z_Offset;
ram_data[DARWIN_STAIRS_CNTRL]|=STAND_UP;
}
else
{
ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_STATUS);
......
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