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
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
+ 5
− 4
View file @ 52a03466
...@@ -643,8 +643,8 @@ typedef enum { ...@@ -643,8 +643,8 @@ typedef enum {
// | detected | enable // | detected | enable
DARWIN_GRIPPER_CNTRL = 0x0251, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 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 // 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 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 // current phase | climbing | stop stairs | start stairs down | start stairs up
}darwin_registers; }darwin_registers;
#define GPIO_BASE_ADDRESS 0x0100 #define GPIO_BASE_ADDRESS 0x0100
...@@ -753,8 +753,9 @@ typedef enum { ...@@ -753,8 +753,9 @@ typedef enum {
#define STAIRS_MEM_LENGTH 1 #define STAIRS_MEM_LENGTH 1
#define STAIRS_EEPROM_ADDRESS 0x0066 #define STAIRS_EEPROM_ADDRESS 0x0066
#define STAIRS_EEPROM_LENGTH 30 #define STAIRS_EEPROM_LENGTH 30
#define STAIRS_START 0x01 #define STAIRS_START_UP 0x01
#define STAIRS_STOP 0x02 #define STAIRS_START_DOWN 0x02
#define STAIRS_STOP 0x04
#define STAIRS_STATUS 0x08 #define STAIRS_STATUS 0x08
#define STAIRS_PHASE 0xF0 #define STAIRS_PHASE 0xF0
......
include/stairs.h
+ 1
− 1
View file @ 52a03466
...@@ -12,7 +12,7 @@ extern "C" { ...@@ -12,7 +12,7 @@ extern "C" {
void stairs_init(uint16_t period_us); void stairs_init(uint16_t period_us);
inline void stairs_set_period(uint16_t period_us); inline void stairs_set_period(uint16_t period_us);
inline uint16_t stairs_get_period(void); inline uint16_t stairs_get_period(void);
void stairs_start(void); void stairs_start(uint8_t up);
void stairs_stop(void); void stairs_stop(void);
uint8_t is_climbing_stairs(void); uint8_t is_climbing_stairs(void);
uint8_t stairs_get_phase(void); uint8_t stairs_get_phase(void);
......
src/stairs.c
+ 344
− 166
View file @ 52a03466
...@@ -41,6 +41,7 @@ float stairs_center_weight_time; ...@@ -41,6 +41,7 @@ float stairs_center_weight_time;
// control variables // control variables
uint8_t stairs_Ctrl_Running; uint8_t stairs_Ctrl_Running;
uint8_t stairs_up;
// private functions // private functions
...@@ -54,6 +55,7 @@ void stairs_init(uint16_t period_us) ...@@ -54,6 +55,7 @@ void stairs_init(uint16_t period_us)
stairs_Time=0; stairs_Time=0;
stairs_period=((float)period_us)/1000.0; stairs_period=((float)period_us)/1000.0;
stairs_Ctrl_Running=0x00; stairs_Ctrl_Running=0x00;
stairs_up=0x00;
} }
inline void stairs_set_period(uint16_t period_us) inline void stairs_set_period(uint16_t period_us)
...@@ -66,7 +68,7 @@ inline uint16_t stairs_get_period(void) ...@@ -66,7 +68,7 @@ inline uint16_t stairs_get_period(void)
return (uint16_t)(stairs_period*1000); return (uint16_t)(stairs_period*1000);
} }
void stairs_start(void) void stairs_start(uint8_t up)
{ {
// load all parameters // load all parameters
stairs_shift_weight_left_time=((float)ram_data[DARWIN_STAIRS_PHASE1_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE1_TIME_H]<<8)); 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) ...@@ -97,6 +99,7 @@ void stairs_start(void)
// start operation // start operation
ram_data[DARWIN_STAIRS_CNTRL]|=STAIRS_STATUS; ram_data[DARWIN_STAIRS_CNTRL]|=STAIRS_STATUS;
stairs_Ctrl_Running=0x01; stairs_Ctrl_Running=0x01;
stairs_up=up;
} }
void stairs_stop(void) void stairs_stop(void)
...@@ -126,172 +129,345 @@ void stairs_process(void) ...@@ -126,172 +129,345 @@ void stairs_process(void)
if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS) if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS)
{ {
ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_PHASE); ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_PHASE);
if(stairs_Time>=0 && stairs_Time<=stairs_shift_weight_left_time) if(stairs_up)
{ {
//1 if(stairs_Time>=0 && stairs_Time<=stairs_shift_weight_left_time)
delta=stairs_Time/stairs_shift_weight_left_time; {
ep[0]=stairs_X_Offset; //1
ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta; delta=stairs_Time/stairs_shift_weight_left_time;
ep[2]=stairs_Z_Offset; ep[0]=stairs_X_Offset;
ep[3]=stairs_R_Offset; ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta;
ep[4]=stairs_P_Offset; ep[2]=stairs_Z_Offset;
ep[5]=stairs_A_Offset; ep[3]=stairs_R_Offset;
ep[6]=stairs_X_Offset; ep[4]=stairs_P_Offset;
ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta; ep[5]=stairs_A_Offset;
ep[8]=stairs_Z_Offset; ep[6]=stairs_X_Offset;
ep[9]=stairs_R_Offset; ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta;
ep[10]=stairs_P_Offset; ep[8]=stairs_Z_Offset;
ep[11]=stairs_A_Offset; ep[9]=stairs_R_Offset;
ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_LEFT; ep[10]=stairs_P_Offset;
} ep[11]=stairs_A_Offset;
else if(stairs_Time>stairs_shift_weight_left_time && stairs_Time<=stairs_rise_right_foot_time) ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_LEFT;
{ }
//2 else if(stairs_Time>stairs_shift_weight_left_time && stairs_Time<=stairs_rise_right_foot_time)
delta=(stairs_Time-stairs_shift_weight_left_time)/(stairs_rise_right_foot_time-stairs_shift_weight_left_time); {
ep[0]=stairs_X_Offset; //2
ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude; delta=(stairs_Time-stairs_shift_weight_left_time)/(stairs_rise_right_foot_time-stairs_shift_weight_left_time);
ep[2]=stairs_Z_Offset; ep[0]=stairs_X_Offset;
ep[3]=stairs_R_Offset-stairs_R_shift_amplitude*delta; ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude;
ep[4]=stairs_P_Offset; ep[2]=stairs_Z_Offset;
ep[5]=stairs_A_Offset; ep[3]=stairs_R_Offset-stairs_R_shift_amplitude*delta;
ep[6]=stairs_X_Offset; ep[4]=stairs_P_Offset;
ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude; ep[5]=stairs_A_Offset;
ep[8]=stairs_Z_Offset+(stairs_Z_overshoot+stairs_Z_stair_height)*delta; ep[6]=stairs_X_Offset;
ep[9]=stairs_R_Offset-stairs_R_shift_amplitude*delta; ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude;
ep[10]=stairs_P_Offset; ep[8]=stairs_Z_Offset+(stairs_Z_overshoot+stairs_Z_stair_height)*delta;
ep[11]=stairs_A_Offset; ep[9]=stairs_R_Offset-stairs_R_shift_amplitude*delta;
ram_data[DARWIN_STAIRS_CNTRL]|=RISE_RIGHT_FOOT; ep[10]=stairs_P_Offset;
} ep[11]=stairs_A_Offset;
else if(stairs_Time>stairs_rise_right_foot_time && stairs_Time<=stairs_advance_right_foot_time) ram_data[DARWIN_STAIRS_CNTRL]|=RISE_RIGHT_FOOT;
{ }
//3 else if(stairs_Time>stairs_rise_right_foot_time && stairs_Time<=stairs_advance_right_foot_time)
delta=(stairs_Time-stairs_rise_right_foot_time)/(stairs_advance_right_foot_time-stairs_rise_right_foot_time); {
ep[0]=stairs_X_Offset; //3
ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)*delta; delta=(stairs_Time-stairs_rise_right_foot_time)/(stairs_advance_right_foot_time-stairs_rise_right_foot_time);
ep[2]=stairs_Z_Offset; ep[0]=stairs_X_Offset;
ep[3]=stairs_R_Offset-stairs_R_shift_amplitude; ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)*delta;
ep[4]=stairs_P_Offset+stairs_P_shift_amplitude*delta; ep[2]=stairs_Z_Offset;
ep[5]=stairs_A_Offset+stairs_A_shift_amplitude*delta; ep[3]=stairs_R_Offset-stairs_R_shift_amplitude;
ep[6]=stairs_X_Offset+stairs_X_shift_amplitude*delta; ep[4]=stairs_P_Offset+stairs_P_shift_amplitude*delta;
ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)*delta; ep[5]=stairs_A_Offset+stairs_A_shift_amplitude*delta;
ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height; ep[6]=stairs_X_Offset+stairs_X_shift_amplitude*delta;
ep[9]=stairs_R_Offset-stairs_R_shift_amplitude; ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)*delta;
ep[10]=stairs_P_Offset+stairs_P_shift_amplitude*delta; ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height;
ep[11]=stairs_A_Offset+stairs_A_shift_amplitude*delta; ep[9]=stairs_R_Offset-stairs_R_shift_amplitude;
ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_RIGHT_FOOT; ep[10]=stairs_P_Offset+stairs_P_shift_amplitude*delta;
} ep[11]=stairs_A_Offset+stairs_A_shift_amplitude*delta;
else if(stairs_Time>stairs_advance_right_foot_time && stairs_Time<=stairs_contact_right_foot_time) ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_RIGHT_FOOT;
{ }
//4 else if(stairs_Time>stairs_advance_right_foot_time && stairs_Time<=stairs_contact_right_foot_time)
delta=(stairs_Time-stairs_advance_right_foot_time)/(stairs_contact_right_foot_time-stairs_advance_right_foot_time); {
ep[0]=stairs_X_Offset; //4
ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0); delta=(stairs_Time-stairs_advance_right_foot_time)/(stairs_contact_right_foot_time-stairs_advance_right_foot_time);
ep[2]=stairs_Z_Offset; ep[0]=stairs_X_Offset;
ep[3]=stairs_R_Offset-stairs_R_shift_amplitude; ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0);
ep[4]=stairs_P_Offset+stairs_P_shift_amplitude; ep[2]=stairs_Z_Offset;
ep[5]=stairs_A_Offset+stairs_A_shift_amplitude; ep[3]=stairs_R_Offset-stairs_R_shift_amplitude;
ep[6]=stairs_X_Offset+stairs_X_shift_amplitude; ep[4]=stairs_P_Offset+stairs_P_shift_amplitude;
ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0); ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_overshoot*delta; ep[6]=stairs_X_Offset+stairs_X_shift_amplitude;
ep[9]=stairs_R_Offset-stairs_R_shift_amplitude; ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0);
ep[10]=stairs_P_Offset+stairs_P_shift_amplitude; ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_overshoot*delta;
ep[11]=stairs_A_Offset+stairs_A_shift_amplitude; ep[9]=stairs_R_Offset-stairs_R_shift_amplitude;
ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_RIGHT_FOOT; ep[10]=stairs_P_Offset+stairs_P_shift_amplitude;
} ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
else if(stairs_Time>stairs_contact_right_foot_time && stairs_Time<=stairs_shift_weight_right_time) ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_RIGHT_FOOT;
{ }
//5 else if(stairs_Time>stairs_contact_right_foot_time && stairs_Time<=stairs_shift_weight_right_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; //5
ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta; delta=(stairs_Time-stairs_contact_right_foot_time)/(stairs_shift_weight_right_time-stairs_contact_right_foot_time);
ep[2]=stairs_Z_Offset; ep[0]=stairs_X_Offset-stairs_X_shift_body*delta;
ep[3]=stairs_R_Offset-stairs_R_shift_amplitude+stairs_R_shift_amplitude*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[4]=stairs_P_Offset+stairs_P_shift_amplitude-(2.0*stairs_P_shift_amplitude)*delta; ep[2]=stairs_Z_Offset;
ep[5]=stairs_A_Offset+stairs_A_shift_amplitude; ep[3]=stairs_R_Offset-stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta;
ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body*delta; ep[4]=stairs_P_Offset+stairs_P_shift_amplitude-(2.0*stairs_P_shift_amplitude)*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[5]=stairs_A_Offset+stairs_A_shift_amplitude;
ep[8]=stairs_Z_Offset+stairs_Z_stair_height; ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body*delta;
ep[9]=stairs_R_Offset-stairs_R_shift_amplitude+stairs_R_shift_amplitude*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[10]=stairs_P_Offset+stairs_P_shift_amplitude-(2.0*stairs_P_shift_amplitude)*delta; ep[8]=stairs_Z_Offset+stairs_Z_stair_height;
ep[11]=stairs_A_Offset+stairs_A_shift_amplitude; ep[9]=stairs_R_Offset-stairs_R_shift_amplitude+stairs_R_shift_amplitude*delta;
ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_RIGHT; 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;
else if(stairs_Time>stairs_shift_weight_right_time && stairs_Time<=stairs_rise_left_foot_time) ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_RIGHT;
{ }
//6 else if(stairs_Time>stairs_shift_weight_right_time && stairs_Time<=stairs_rise_left_foot_time)
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; //6
ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0); delta=(stairs_Time-stairs_shift_weight_right_time)/(stairs_rise_left_foot_time-stairs_shift_weight_right_time);
ep[2]=stairs_Z_Offset+(stairs_Z_overshoot+stairs_Z_stair_height)*delta; ep[0]=stairs_X_Offset-stairs_X_shift_body;
ep[3]=stairs_R_Offset+stairs_R_shift_amplitude*delta; ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0);
ep[4]=stairs_P_Offset-stairs_P_shift_amplitude; ep[2]=stairs_Z_Offset+(stairs_Z_overshoot+stairs_Z_stair_height)*delta;
ep[5]=stairs_A_Offset+stairs_A_shift_amplitude; ep[3]=stairs_R_Offset+stairs_R_shift_amplitude*delta;
ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body; ep[4]=stairs_P_Offset-stairs_P_shift_amplitude;
ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0); ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_overshoot*delta; ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body;
ep[9]=stairs_R_Offset+stairs_R_shift_amplitude*delta; ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0);
ep[10]=stairs_P_Offset-stairs_P_shift_amplitude; ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_overshoot*delta;
ep[11]=stairs_A_Offset+stairs_A_shift_amplitude; ep[9]=stairs_R_Offset+stairs_R_shift_amplitude*delta;
ram_data[DARWIN_STAIRS_CNTRL]|=RISE_LEFT_FOOT; ep[10]=stairs_P_Offset-stairs_P_shift_amplitude;
} ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
else if(stairs_Time>stairs_rise_left_foot_time && stairs_Time<=stairs_advance_left_foot_time) ram_data[DARWIN_STAIRS_CNTRL]|=RISE_LEFT_FOOT;
{ }
//7 else if(stairs_Time>stairs_rise_left_foot_time && stairs_Time<=stairs_advance_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; //7
ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)+(stairs_Y_spread_amplitude/2.0)*delta; delta=(stairs_Time-stairs_rise_left_foot_time)/(stairs_advance_left_foot_time-stairs_rise_left_foot_time);
ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height; ep[0]=stairs_X_Offset-stairs_X_shift_body+stairs_X_shift_body*delta;
ep[3]=stairs_R_Offset+stairs_R_shift_amplitude; 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[4]=stairs_P_Offset-stairs_P_shift_amplitude; ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height;
ep[5]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta; ep[3]=stairs_R_Offset+stairs_R_shift_amplitude;
ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body-(stairs_X_shift_amplitude-stairs_X_shift_body)*delta; ep[4]=stairs_P_Offset-stairs_P_shift_amplitude;
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[5]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta;
ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_overshoot; ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body-(stairs_X_shift_amplitude-stairs_X_shift_body)*delta;
ep[9]=stairs_R_Offset+stairs_R_shift_amplitude; 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[10]=stairs_P_Offset-stairs_P_shift_amplitude; ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_overshoot;
ep[11]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta; ep[9]=stairs_R_Offset+stairs_R_shift_amplitude;
ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_LEFT_FOOT; ep[10]=stairs_P_Offset-stairs_P_shift_amplitude;
} ep[11]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta;
else if(stairs_Time>stairs_advance_left_foot_time && stairs_Time<=stairs_contact_left_foot_time) ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_LEFT_FOOT;
{ }
//8 else if(stairs_Time>stairs_advance_left_foot_time && stairs_Time<=stairs_contact_left_foot_time)
delta=(stairs_Time-stairs_advance_left_foot_time)/(stairs_contact_left_foot_time-stairs_advance_left_foot_time); {
ep[0]=stairs_X_Offset; //8
ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude; delta=(stairs_Time-stairs_advance_left_foot_time)/(stairs_contact_left_foot_time-stairs_advance_left_foot_time);
ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_overshoot*delta; ep[0]=stairs_X_Offset;
ep[3]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta; ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude;
ep[4]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta; ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_overshoot*delta;
ep[5]=stairs_A_Offset; ep[3]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
ep[6]=stairs_X_Offset; ep[4]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta;
ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude; ep[5]=stairs_A_Offset;
ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_overshoot+stairs_Z_overshoot*delta; ep[6]=stairs_X_Offset;
ep[9]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta; ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude;
ep[10]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta; ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_overshoot+stairs_Z_overshoot*delta;
ep[11]=stairs_A_Offset; ep[9]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_LEFT_FOOT; ep[10]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta;
} ep[11]=stairs_A_Offset;
else if(stairs_Time>stairs_contact_left_foot_time && stairs_Time<=stairs_center_weight_time) ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_LEFT_FOOT;
{ }
//9 else if(stairs_Time>stairs_contact_left_foot_time && stairs_Time<=stairs_center_weight_time)
delta=(stairs_Time-stairs_contact_left_foot_time)/(stairs_center_weight_time-stairs_contact_left_foot_time); {
ep[0]=stairs_X_Offset; //9
ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta; delta=(stairs_Time-stairs_contact_left_foot_time)/(stairs_center_weight_time-stairs_contact_left_foot_time);
ep[2]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height*delta; ep[0]=stairs_X_Offset;
ep[3]=stairs_R_Offset; ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta;
ep[4]=stairs_P_Offset; ep[2]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height*delta;
ep[5]=stairs_A_Offset; ep[3]=stairs_R_Offset;
ep[6]=stairs_X_Offset; ep[4]=stairs_P_Offset;
ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta; ep[5]=stairs_A_Offset;
ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height*delta; ep[6]=stairs_X_Offset;
ep[9]=stairs_R_Offset; ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta;
ep[10]=stairs_P_Offset; ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height*delta;
ep[11]=stairs_A_Offset; ep[9]=stairs_R_Offset;
ram_data[DARWIN_STAIRS_CNTRL]|=CENTER_WEIGHT; 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 else
{ {
ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_STATUS); if(stairs_Time>=0 && stairs_Time<=stairs_shift_weight_left_time)
stairs_Ctrl_Running=0x00; {
//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) 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 ...@@ -358,8 +534,10 @@ void stairs_process_write_cmd(unsigned short int address,unsigned short int leng
// walk control // walk control
if(ram_in_range(DARWIN_STAIRS_CNTRL,address,length)) if(ram_in_range(DARWIN_STAIRS_CNTRL,address,length))
{ {
if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_START) if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_START_UP)
stairs_start(); stairs_start(0x01);
if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_START_DOWN)
stairs_start(0x00);
if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_STOP) if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_STOP)
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