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

Added the algorithm to go down stairs. 

Added the necessary changes in the dynamixel interface to execute it.
parent 20897b8d
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
+ 5
− 4
View file @ 52a03466
......@@ -643,8 +643,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_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 down | start stairs up
}darwin_registers;
#define GPIO_BASE_ADDRESS 0x0100
......@@ -753,8 +753,9 @@ typedef enum {
#define STAIRS_MEM_LENGTH 1
#define STAIRS_EEPROM_ADDRESS 0x0066
#define STAIRS_EEPROM_LENGTH 30
#define STAIRS_START 0x01
#define STAIRS_STOP 0x02
#define STAIRS_START_UP 0x01
#define STAIRS_START_DOWN 0x02
#define STAIRS_STOP 0x04
#define STAIRS_STATUS 0x08
#define STAIRS_PHASE 0xF0
......
include/stairs.h
+ 1
− 1
View file @ 52a03466
......@@ -12,7 +12,7 @@ extern "C" {
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_start(uint8_t up);
void stairs_stop(void);
uint8_t is_climbing_stairs(void);
uint8_t stairs_get_phase(void);
......
src/stairs.c
+ 344
− 166
View file @ 52a03466
......@@ -41,6 +41,7 @@ float stairs_center_weight_time;
// control variables
uint8_t stairs_Ctrl_Running;
uint8_t stairs_up;
// private functions
......@@ -54,6 +55,7 @@ void stairs_init(uint16_t period_us)
stairs_Time=0;
stairs_period=((float)period_us)/1000.0;
stairs_Ctrl_Running=0x00;
stairs_up=0x00;
}
inline void stairs_set_period(uint16_t period_us)
......@@ -66,7 +68,7 @@ inline uint16_t stairs_get_period(void)
return (uint16_t)(stairs_period*1000);
}
void stairs_start(void)
void stairs_start(uint8_t up)
{
// load all parameters
stairs_shift_weight_left_time=((float)ram_data[DARWIN_STAIRS_PHASE1_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE1_TIME_H]<<8));
......@@ -97,6 +99,7 @@ void stairs_start(void)
// start operation
ram_data[DARWIN_STAIRS_CNTRL]|=STAIRS_STATUS;
stairs_Ctrl_Running=0x01;
stairs_up=up;
}
void stairs_stop(void)
......@@ -126,172 +129,345 @@ void stairs_process(void)
if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS)
{
ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_PHASE);
if(stairs_Time>=0 && stairs_Time<=stairs_shift_weight_left_time)
if(stairs_up)
{
//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*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*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]=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)*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-(stairs_Y_spread_amplitude/2.0)*delta;
ep[2]=stairs_Z_Offset;
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]=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-(stairs_Y_spread_amplitude/2.0);
ep[2]=stairs_Z_Offset;
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
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]=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]=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
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-stairs_Z_overshoot*delta;
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
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]=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-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-stairs_Z_overshoot;
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-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*delta;
ep[3]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
ep[4]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta;
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-stairs_Z_overshoot+stairs_Z_overshoot*delta;
ep[9]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
ep[10]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta;
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*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*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;
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*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*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]=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)*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-(stairs_Y_spread_amplitude/2.0)*delta;
ep[2]=stairs_Z_Offset;
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]=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-(stairs_Y_spread_amplitude/2.0);
ep[2]=stairs_Z_Offset;
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
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]=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]=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
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-stairs_Z_overshoot*delta;
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
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]=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-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-stairs_Z_overshoot;
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-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*delta;
ep[3]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
ep[4]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta;
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-stairs_Z_overshoot+stairs_Z_overshoot*delta;
ep[9]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
ep[10]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta;
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*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*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
{
ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_STATUS);
stairs_Ctrl_Running=0x00;
}
}
else
{
ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_STATUS);
stairs_Ctrl_Running=0x00;
{
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*delta;
ep[2]=stairs_Z_Offset+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*delta;
ep[8]=stairs_Z_Offset+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]|=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+stairs_Z_stair_height;
ep[3]=stairs_R_Offset-stairs_R_shift_amplitude;
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+stairs_Z_overshoot*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-(stairs_Y_spread_amplitude/2.0)*delta;
ep[2]=stairs_Z_Offset+stairs_Z_stair_height;
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]=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-(stairs_Y_spread_amplitude/2.0);
ep[2]=stairs_Z_Offset+stairs_Z_stair_height+stairs_Z_overshoot*delta;
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_stair_height*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
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+stairs_Z_stair_height+stairs_Z_overshoot;
ep[3]=stairs_R_Offset-stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta;
ep[4]=stairs_P_Offset+stairs_P_shift_amplitude-(3.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_overshoot;
ep[9]=stairs_R_Offset-stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta;
ep[10]=stairs_P_Offset+stairs_P_shift_amplitude-(3.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
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;
ep[3]=stairs_R_Offset+stairs_R_shift_amplitude*delta;
ep[4]=stairs_P_Offset-2.0*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_overshoot-stairs_Z_overshoot*delta;
ep[9]=stairs_R_Offset+stairs_R_shift_amplitude*delta;
ep[10]=stairs_P_Offset-2.0*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
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]=stairs_R_Offset+stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta;
ep[4]=stairs_P_Offset-2.0*stairs_P_shift_amplitude+2.0*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;
ep[9]=stairs_R_Offset+stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta;
ep[10]=stairs_P_Offset-2.0*stairs_P_shift_amplitude+2.0*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_stair_height+stairs_Z_overshoot)*delta;
ep[3]=stairs_R_Offset+2.0*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;
ep[9]=stairs_R_Offset+2.0*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*delta;
ep[2]=stairs_Z_Offset;
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+stairs_Y_shift_amplitude*delta;
ep[8]=stairs_Z_Offset;
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]|=CENTER_WEIGHT;
}
else
{
ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_STATUS);
stairs_Ctrl_Running=0x00;
}
}
if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS)
......@@ -358,8 +534,10 @@ void stairs_process_write_cmd(unsigned short int address,unsigned short int leng
// walk control
if(ram_in_range(DARWIN_STAIRS_CNTRL,address,length))
{
if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_START)
stairs_start();
if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_START_UP)
stairs_start(0x01);
if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_START_DOWN)
stairs_start(0x00);
if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_STOP)
stairs_stop();
}
......
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