diff --git a/include/action.h b/include/action.h deleted file mode 100755 index 6a3817af3a1509331ce20e4953bbf37f25c20e96..0000000000000000000000000000000000000000 --- a/include/action.h +++ /dev/null @@ -1,34 +0,0 @@ -#ifndef _ACTION_H -#define _ACTION_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include "stm32f1xx.h" -#include "motion_pages.h" - -// public functions -void action_init(uint16_t period_us); -void action_set_period(uint16_t period_us); -uint16_t action_get_period(void); -uint8_t action_load_page(uint8_t page_id); -void action_start_page(void); -void action_stop_page(void); -uint8_t action_is_running(void); -uint8_t action_get_current_page(void); -uint8_t action_get_current_step(void); -void action_enable_int(void); -void action_disable_int(void); -uint8_t action_is_int_enabled(void); -// operation functions -uint8_t action_in_range(unsigned short int address, unsigned short int length); -void action_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data); -// motion manager interface -void action_process(void); - -#ifdef __cplusplus -} -#endif - -#endif diff --git a/include/darwin_kinematics.h b/include/darwin_kinematics.h deleted file mode 100755 index 783781e8a49a95ab0aecd55691071913b5096959..0000000000000000000000000000000000000000 --- a/include/darwin_kinematics.h +++ /dev/null @@ -1,24 +0,0 @@ -#ifndef _DARWIN_KINEMATICS_H -#define _DARWIN_KINEMATICS_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include "stm32f1xx.h" - -#define DARWIN_PELVIS_LENGTH 29.0 //mm -#define DARWIN_LEG_SIDE_OFFSET 37.0 //mm -#define DARWIN_THIGH_LENGTH 93.0 //mm -#define DARWIN_CALF_LENGTH 93.0 //mm -#define DARWIN_ANKLE_LENGTH 33.5 //mm -#define DARWIN_LEG_LENGTH (DARWIN_THIGH_LENGTH + DARWIN_CALF_LENGTH + DARWIN_ANKLE_LENGTH) //mm (THIGH_LENGTH + CALF_LENGTH + ANKLE_LENGTH) - -// public functions -uint8_t darwin_leg_ik(float *out, float x, float y, float z, float a, float b, float c); - -#ifdef __cplusplus -} -#endif - -#endif diff --git a/include/darwin_math.h b/include/darwin_math.h deleted file mode 100755 index 82ef927c3d8c6a225cfe6c18086c37e10ea5d4a6..0000000000000000000000000000000000000000 --- a/include/darwin_math.h +++ /dev/null @@ -1,70 +0,0 @@ -#ifndef _DARWIN_MATH_H -#define _DARWIN_MATH_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include "stm32f1xx.h" - -#define PI 3.14159 - -typedef struct{ - float x; - float y; - float z; -}TPoint3D; - -// point public functions -void point3d_init(TPoint3D *point); -void point3d_load(TPoint3D *point, float x, float y, float z); -void point3d_copy(TPoint3D *point_dst, TPoint3D *point_src); - -typedef struct { - float x; - float y; - float z; -}TVector3D; - -// vector public functions -void vector3d_init(TVector3D *vector); -void vector3d_load(TVector3D *vector, float x, float y, float z); -void vector3d_copy(TVector3D *vector_dst, TVector3D *vector_src); -float vector3d_length(TVector3D *vector); - -enum{ - m00=0, - m01, - m02, - m03, - m10, - m11, - m12, - m13, - m20, - m21, - m22, - m23, - m30, - m31, - m32, - m33, -}; - -typedef struct{ - float coef[16]; -}TMatrix3D; - -// matrix public functions -void matrix3d_init(TMatrix3D *matrix); -void matrix3d_copy(TMatrix3D *matrix_dst, TMatrix3D *matrix_src); -void matrix3d_identity(TMatrix3D *matrix); -uint8_t matrix3d_inverse(TMatrix3D *org, TMatrix3D *inv); -void matrix3d_set_transform(TMatrix3D *matrix, TPoint3D *point, TVector3D *vector); -void matrix3d_mult(TMatrix3D *dst,TMatrix3D *a,TMatrix3D *b); - -#ifdef __cplusplus -} -#endif - -#endif diff --git a/include/grippers.h b/include/grippers.h deleted file mode 100644 index b8818bd90bffa713d8cdb8b80bdaaa3769a4eedd..0000000000000000000000000000000000000000 --- a/include/grippers.h +++ /dev/null @@ -1,34 +0,0 @@ -#ifndef _GRIPPERS_H -#define _GRIPPERS_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include "stm32f1xx.h" -#include "motion_manager.h" - -typedef enum {GRIPPER_LEFT,GRIPPER_RIGHT} grippers_t; - -// public functions -void grippers_init(uint16_t period_us); -void grippers_set_period(uint16_t period_us); -uint16_t grippers_get_period(void); -void grippers_open(grippers_t gripper); -void grippers_close(grippers_t gripper); -void grippers_stop(grippers_t gripper); -uint8_t gripper_is_open(grippers_t gripper); -uint8_t gripper_is_close(grippers_t gripper); -uint8_t gripper_is_moving(grippers_t gripper); - -// operation functions -uint8_t grippers_in_range(unsigned short int address, unsigned short int length); -void grippers_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data); -void grippers_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data); -void grippers_process(void); - -#ifdef __cplusplus -} -#endif - -#endif diff --git a/include/head_tracking.h b/include/head_tracking.h deleted file mode 100644 index f5562649aa1bc791cf19d7e402df00d8e5f18c62..0000000000000000000000000000000000000000 --- a/include/head_tracking.h +++ /dev/null @@ -1,41 +0,0 @@ -#ifndef _HEAD_TRACKING_H -#define _HEAD_TRACKINH_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include "stm32f1xx.h" -#include "motion_manager.h" - -// public functions -void head_tracking_init(uint16_t period_us); -void head_tracking_set_period(uint16_t period_us); -uint16_t head_tracking_get_period(void); -void head_tracking_start(void); -void head_tracking_stop(void); -uint8_t head_is_tracking(void); -void head_tracking_set_pan_range(int16_t max_angle,int16_t min_angle); -void head_tracking_get_pan_range(int16_t *max_angle,int16_t *min_angle); -void head_tracking_set_pan_pid(uint16_t kp,uint16_t ki,uint16_t kd, uint16_t i_clamp); -void head_tracking_get_pan_pid(uint16_t *kp,uint16_t *ki,uint16_t *kd, uint16_t *i_clamp); -void head_tracking_set_tilt_range(int16_t max_angle,int16_t min_angle); -void head_tracking_get_tilt_range(int16_t *max_angle,int16_t *min_angle); -void head_tracking_set_tilt_pid(uint16_t kp,uint16_t ki,uint16_t kd, uint16_t i_clamp); -void head_tracking_get_tilt_pid(uint16_t *kp,uint16_t *ki,uint16_t *kd, uint16_t *i_clamp); -void head_tracking_set_target_pan(int16_t target); -void head_tracking_set_target_tilt(int16_t target); - -// operation functions -uint8_t head_tracking_in_range(unsigned short int address, unsigned short int length); -void head_tracking_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data); -void head_tracking_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data); - -// motion manager process function -void head_tracking_process(void); - -#ifdef __cplusplus -} -#endif - -#endif diff --git a/include/joint_motion.h b/include/joint_motion.h deleted file mode 100644 index f2f0e1621409487cc494c876ab7d90b55a343028..0000000000000000000000000000000000000000 --- a/include/joint_motion.h +++ /dev/null @@ -1,34 +0,0 @@ -#ifndef _JOINT_MOTION_H -#define _JOINT_MOTION_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include "stm32f1xx.h" -#include "motion_manager.h" - -#define NUM_JOINT_GROUPS 4 - -// public functions -void joint_motion_init(uint16_t period_us); -void joint_motion_set_period(uint16_t period_us); -uint16_t joint_motion_get_period(void); -void joint_motion_set_group_servos(uint8_t group_id,uint32_t servos); -uint32_t joint_motion_get_group_servos(uint8_t group_id); -void joint_motion_start(uint8_t group_id); -void joint_motion_stop(uint8_t group_id); -uint8_t are_joints_moving(uint8_t group_id); - -// operation functions -uint8_t joint_motion_in_range(unsigned short int address, unsigned short int length); -void joint_motion_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data); -void joint_motion_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data); -// motion manager interface functions -void joint_motion_process(void); - -#ifdef __cplusplus -} -#endif - -#endif diff --git a/include/stairs.h b/include/stairs.h deleted file mode 100755 index a34b4dc37a37d8474c7183b785637bfe81d82d04..0000000000000000000000000000000000000000 --- a/include/stairs.h +++ /dev/null @@ -1,31 +0,0 @@ -#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); -void stairs_set_period(uint16_t period_us); -uint16_t stairs_get_period(void); -void stairs_start(uint8_t up); -void stairs_stop(void); -uint8_t is_climbing_stairs(void); -uint8_t stairs_get_phase(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 diff --git a/include/walking.h b/include/walking.h deleted file mode 100755 index 30bc04e06e2d59fcf8dc4be369adcad289250b66..0000000000000000000000000000000000000000 --- a/include/walking.h +++ /dev/null @@ -1,30 +0,0 @@ -#ifndef _WALKING_H -#define _WALKING_H - -#ifdef __cplusplus -extern "C" { -#endif - -#include "stm32f1xx.h" -#include "motion_manager.h" - -// public functions -void walking_init(uint16_t period_us); -void walking_set_period(uint16_t period_us); -uint16_t walking_get_period(void); -void walking_start(void); -void walking_stop(void); -uint8_t is_walking(void); - -// operation functions -uint8_t walking_in_range(unsigned short int address, unsigned short int length); -void walking_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data); -void walking_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data); -// motion manager interface functions -void walking_process(void); - -#ifdef __cplusplus -} -#endif - -#endif diff --git a/src/action.c b/src/action.c deleted file mode 100755 index bfe36ed583094b592b765389de7c77598a6e456a..0000000000000000000000000000000000000000 --- a/src/action.c +++ /dev/null @@ -1,602 +0,0 @@ -#include "action.h" -#include "motion_pages.h" -#include "motion_manager.h" -#include "ram.h" - -#define SPEED_BASE_SCHEDULE 0x00 -#define TIME_BASE_SCHEDULE 0x0A - -// private variables -typedef enum {ACTION_PRE,ACTION_MAIN,ACTION_POST,ACTION_PAUSE} action_states; - -TPage action_next_page; -TPage action_current_page; -uint8_t action_current_step_index; -uint8_t action_current_page_index; -// angle variables -int64_t action_moving_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format -int64_t action_start_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format -// speed variables -int64_t action_start_speed[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format -int64_t action_main_speed[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format -// control variables -uint8_t action_end,action_stop; -uint8_t action_zero_speed_finish[PAGE_MAX_NUM_SERVOS]; -uint8_t action_next_index; -uint8_t action_running; -// time variables (in time units (7.8 ms each time unit)) -int64_t action_total_time;// fixed point 48|16 format -int64_t action_pre_time;// fixed point 48|16 format -int64_t action_main_time;// fixed point 48|16 format -int64_t action_step_time;// fixed point 48|16 format -int64_t action_pause_time;// fixed point 48|16 format -int64_t action_current_time;// fixed point 48|16 format -int64_t action_section_time;// fixed point 48|16 format -int64_t action_period; -int16_t action_period_us; - -// private functions -void action_load_next_step(void) -{ - // page and step information - static int8_t num_repetitions=0; - // angle variables - int16_t next_angle;// information from the flash memory (fixed point 9|7 format) - int64_t max_angle;// fixed point format 48|16 format - int64_t tmp_angle;// fixed point format 48|16 format - int64_t angle;// fixed point format 48|16 format - int64_t target_angles;// fixed point 48|16 format - int64_t next_angles;// fixed point 48|16 format - int64_t action_pre_time_initial_speed_angle;// fixed point 48|16 format - int64_t moving_angle;// fixed point 48|16 format - // time variables - int64_t accel_time_num;// fixed point 48|16 format - int64_t zero_speed_divider;// fixed point 48|16 format - int64_t non_zero_speed_divider;// fixed point 48|16 format - // control variables - uint8_t dir_change; - // control information - uint8_t i=0; - - action_current_step_index++; - if(action_current_step_index>=action_current_page.header.stepnum)// the last step has been executed - { - if(action_current_page.header.stepnum==1) - { - if(action_stop) - action_next_index=action_current_page.header.exit; - else - { - num_repetitions--; - if(num_repetitions>0) - action_next_index=action_current_page_index; - else - action_next_index=action_current_page.header.next; - } - if(action_next_index==0) - action_end=0x01; - else - { - if(action_next_index!=action_current_page_index) - { - pages_get_page(action_next_index,&action_next_page); - ram_data[DARWIN_ACTION_PAGE]=action_next_index; - if(!pages_check_checksum(&action_next_page)) - action_end=0x01; - } - if(action_next_page.header.repeat==0 || action_next_page.header.stepnum==0) - action_end=0x01; - } - } - pages_copy_page(&action_next_page,&action_current_page);// make the next page active - if(action_current_page_index!=action_next_index) - num_repetitions=action_current_page.header.repeat; - action_current_step_index=0; - action_current_page_index=action_next_index; - } - else if(action_current_step_index==action_current_page.header.stepnum-1)// it is the last step - { - if(action_stop) - action_next_index=action_current_page.header.exit; - else - { - num_repetitions--; - if(num_repetitions>0) - action_next_index=action_current_page_index; - else - action_next_index=action_current_page.header.next; - } - if(action_next_index==0) - action_end=0x01; - else - { - if(action_next_index!=action_current_page_index) - { - pages_get_page(action_next_index,&action_next_page); - ram_data[DARWIN_ACTION_PAGE]=action_next_index; - if(!pages_check_checksum(&action_next_page)) - action_end=0x01; - } - if(action_next_page.header.repeat==0 || action_next_page.header.stepnum==0) - action_end=0x01; - } - } - ram_data[DARWIN_ACTION_CNTRL]&=0x1F; - ram_data[DARWIN_ACTION_CNTRL]|=(action_current_step_index<<5); - // compute trajectory - action_pause_time=(action_current_page.steps[action_current_step_index].pause*action_current_page.header.speed)>>5; - action_pause_time=action_pause_time<<9; - action_step_time=(action_current_page.steps[action_current_step_index].time*action_current_page.header.speed)>>5; - action_step_time=action_step_time<<9; - if(action_step_time<action_period) - action_step_time=action_period;// 0.078 in 16|16 format - max_angle=0; - for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) - { - if(manager_get_module(i)==MM_ACTION) - { - angle=action_current_page.steps[action_current_step_index].position[i]; - if(angle==0x5A00)// bigger than 180 - target_angles=manager_current_angles[i]; - else - target_angles=angle<<9; - action_moving_angles[i]=target_angles-manager_current_angles[i]; - if(action_end) - next_angles=target_angles; - else - { - if(action_current_step_index==action_current_page.header.stepnum-1) - next_angle=action_next_page.steps[0].position[i]; - else - next_angle=action_current_page.steps[action_current_step_index+1].position[i]; - if(next_angle==0x5A00) - next_angles=target_angles; - else - next_angles=next_angle<<9; - } - // check changes in direction - if(((manager_current_angles[i] < target_angles) && (target_angles < next_angles)) || - ((manager_current_angles[i] > target_angles) && (target_angles > next_angles))) - dir_change=0x00; - else - dir_change=0x01; - // check the type of ending - if(dir_change || action_pause_time>0 || action_end) - action_zero_speed_finish[i]=0x01; - else - action_zero_speed_finish[i]=0x00; - // find the maximum angle of motion in speed base schedule - if(action_current_page.header.schedule==SPEED_BASE_SCHEDULE)// speed base schedule - { - // fer el valor absolut - if(action_moving_angles[i]<0) - tmp_angle=-action_moving_angles[i]; - else - tmp_angle=action_moving_angles[i]; - if(tmp_angle>max_angle) - max_angle=tmp_angle; - } - manager_current_slopes[i]=action_current_page.header.slope[i]; - } - } - if(action_current_page.header.schedule==SPEED_BASE_SCHEDULE) - action_step_time=(max_angle*256)/(action_current_page.steps[action_current_step_index].time*720); - accel_time_num=action_current_page.header.accel; - accel_time_num=accel_time_num<<9; - if(action_step_time<=(accel_time_num<<1)) - { - if(action_step_time==0) - accel_time_num=0; - else - { - accel_time_num=(action_step_time-action_period)>>1; - if(accel_time_num==0) - action_step_time=0; - } - } - action_total_time=action_step_time; - action_pre_time=accel_time_num; - action_main_time=action_total_time-action_pre_time; - non_zero_speed_divider=(action_pre_time>>1)+action_main_time; - if(non_zero_speed_divider<action_period) - non_zero_speed_divider=action_period; - zero_speed_divider=action_main_time; - if(zero_speed_divider<action_period) - zero_speed_divider=action_period; - for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) - { - if(manager_get_module(i)==MM_ACTION) - { - action_pre_time_initial_speed_angle=(action_start_speed[i]*action_pre_time)>>1; - moving_angle=action_moving_angles[i]<<16; - if(action_zero_speed_finish[i]) - action_main_speed[i]=(moving_angle-action_pre_time_initial_speed_angle)/zero_speed_divider; - else - action_main_speed[i]=(moving_angle-action_pre_time_initial_speed_angle)/non_zero_speed_divider; -// if((action_main_speed[i]>>16)>info.max_speed) -// action_main_speed[i]=(info.max_speed*65536); -// else if((action_main_speed[i]>>16)<-info.max_speed) -// action_main_speed[i]=-(info.max_speed*65536); - } - } -} - -void action_finish(void) -{ - // set the internal state machine to the idle state - action_end=0x00; - // clear the status falg for the action module - ram_data[DARWIN_ACTION_CNTRL]&=(~ACTION_STATUS); - // set the interrupt falg for the action module - ram_data[DARWIN_ACTION_CNTRL]|=ACTION_INT_FLAG; - // change the internal state - action_running=0x00; -} - -// public functions -void action_init(uint16_t period_us) -{ - unsigned char i; - - // init manager_current_angles with the current position of the servos - for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) - { - // angle variables - action_moving_angles[i]=0;// fixed point 48|16 format - action_start_angles[i]=manager_current_angles[i]; - // speed variables - action_start_speed[i]=0;// fixed point 48|16 format - action_main_speed[i]=0;// fixed point 48|16 format - // control variables - action_zero_speed_finish[i]=0x00; - } - // clear the contents of the next page - pages_clear_page(&action_next_page); - pages_clear_page(&action_current_page); - action_current_page_index=0; - action_current_step_index=-1; - // control variables - action_end=0x00; - action_stop=0x00; - action_running=0x00; - action_next_index=0; - // time variables (in time units (7.8 ms each time unit)) - action_total_time=0;// fixed point 48|16 format - action_pre_time=0;// fixed point 48|16 format - action_main_time=0;// fixed point 48|16 format - action_step_time=0;// fixed point 48|16 format - action_pause_time=0;// fixed point 48|16 format - action_current_time=0;// fixed point 48|16 format - action_section_time=0;// fixed point 48|16 format - - action_period=(period_us<<16)/1000000; - action_period_us=period_us; -} - -void action_set_period(uint16_t period_us) -{ - action_period=(period_us<<16)/1000000; - action_period_us=period_us; -} - -uint16_t action_get_period(void) -{ - return action_period_us; -} - -uint8_t action_load_page(uint8_t page_id) -{ - action_next_index=page_id; - if(action_next_index<0 || action_next_index>255) - return 0x00; - pages_get_page(action_next_index,&action_current_page); - pages_get_page(action_current_page.header.next,&action_next_page); - if(!pages_check_checksum(&action_current_page)) - return 0x00; - if(!pages_check_checksum(&action_next_page)) - return 0x00; - ram_data[DARWIN_ACTION_PAGE]=page_id; - - return 0x01; -} - -void action_start_page(void) -{ - uint8_t i; - - for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) - action_start_angles[i]=manager_current_angles[i]; - action_stop=0x00; - action_current_time=0; - action_section_time=0; - action_current_step_index=-1; - ram_data[DARWIN_ACTION_CNTRL]|=ACTION_STATUS; - /* clear the interrupt flag */ - ram_data[DARWIN_ACTION_CNTRL]&=(~ACTION_INT_FLAG); - action_running=0x01; -} - -void action_stop_page(void) -{ - action_stop=0x01; -} - -uint8_t action_is_running(void) -{ - return action_running; -} - -uint8_t action_get_current_page(void) -{ - return action_current_page_index; -} - -uint8_t action_get_current_step(void) -{ - return action_current_step_index; -} - -void action_enable_int(void) -{ - ram_data[DARWIN_ACTION_CNTRL]|=ACTION_INT_EN; -} - -void action_disable_int(void) -{ - ram_data[DARWIN_ACTION_CNTRL]&=(~ACTION_INT_EN); -} - -uint8_t action_is_int_enabled(void) -{ - if(ram_data[DARWIN_ACTION_CNTRL]&ACTION_INT_EN) - return 0x01; - else - return 0x00; -} - -// motion manager interface -void action_process(void) -{ - uint8_t i; - // angle variables - static int64_t accel_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format - static int64_t main_angles[PAGE_MAX_NUM_SERVOS];// fixed point 48|16 format - // speed variables (in controller units) - static int64_t current_speed[PAGE_MAX_NUM_SERVOS]={0};// fixed point 48|16 format - int64_t delta_speed; - // control variables - static action_states state=ACTION_PAUSE; - - if(action_running==0x01) - { - action_current_time+=action_period; - switch(state) - { - case ACTION_PRE: if(action_current_time>action_section_time) - { - for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) - { - if(manager_get_module(i)==MM_ACTION) - { - /* the last segment of the pre section */ - delta_speed=(action_main_speed[i]-action_start_speed[i]); - current_speed[i]=action_start_speed[i]+delta_speed; - accel_angles[i]=(action_start_speed[i]*action_section_time+((delta_speed*action_section_time)>>1))>>16; - manager_current_angles[i]=action_start_angles[i]+accel_angles[i]; - /* update of the state */ - if(!action_zero_speed_finish[i]) - { - if((action_step_time-action_pre_time)==0) - main_angles[i]=0; - else - main_angles[i]=((action_moving_angles[i]-accel_angles[i])*(action_step_time-(action_pre_time<<1)))/(action_step_time-action_pre_time); - action_start_angles[i]=manager_current_angles[i]; - } - else - { - main_angles[i]=action_moving_angles[i]-accel_angles[i]-(((action_main_speed[i]*action_pre_time)>>1)>>16); - action_start_angles[i]=manager_current_angles[i]; - } - /* the first step of the main section */ - manager_current_angles[i]=action_start_angles[i]+(main_angles[i]*(action_current_time-action_section_time))/(action_step_time-(action_pre_time<<1)); - current_speed[i]=action_main_speed[i]; - } - } - action_current_time=action_current_time-action_section_time; - action_section_time=action_step_time-(action_pre_time<<1); - state=ACTION_MAIN; - } - else - { - for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) - { - if(manager_get_module(i)==MM_ACTION) - { - delta_speed=((action_main_speed[i]-action_start_speed[i])*action_current_time)/action_section_time; - current_speed[i]=action_start_speed[i]+delta_speed; - accel_angles[i]=((action_start_speed[i]*action_current_time)+((delta_speed*action_current_time)>>1))>>16; - manager_current_angles[i]=action_start_angles[i]+accel_angles[i]; - } - } - state=ACTION_PRE; - } - break; - case ACTION_MAIN: if(action_current_time>action_section_time) - { - for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) - { - if(manager_get_module(i)==MM_ACTION) - { - /* last segment of the main section */ - manager_current_angles[i]=action_start_angles[i]+main_angles[i]; - current_speed[i]=action_main_speed[i]; - /* update state */ - action_start_angles[i]=manager_current_angles[i]; - main_angles[i]=action_moving_angles[i]-main_angles[i]-accel_angles[i]; - /* first segment of the post section */ - if(action_zero_speed_finish[i]) - { - delta_speed=((0.0-action_main_speed[i])*(action_current_time-action_section_time))/action_pre_time; - current_speed[i]=action_main_speed[i]+delta_speed; - manager_current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16); - } - else - { - manager_current_angles[i]=action_start_angles[i]+((main_angles[i]*(action_current_time-action_section_time))/action_pre_time); - current_speed[i]=action_main_speed[i]; - } - } - } - action_current_time=action_current_time-action_section_time; - action_section_time=action_pre_time; - state=ACTION_POST; - } - else - { - for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) - { - if(manager_get_module(i)==MM_ACTION) - { - manager_current_angles[i]=action_start_angles[i]+(main_angles[i]*action_current_time)/action_section_time; - current_speed[i]=action_main_speed[i]; - } - } - state=ACTION_MAIN; - } - break; - case ACTION_POST: if(action_current_time>action_section_time) - { - for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) - { - if(manager_get_module(i)==MM_ACTION) - { - /* last segment of the post section */ - if(action_zero_speed_finish[i]) - { - delta_speed=-action_main_speed[i]; - current_speed[i]=action_main_speed[i]+delta_speed; - manager_current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*action_section_time)+((delta_speed*action_section_time)>>1))>>16); - } - else - { - manager_current_angles[i]=action_start_angles[i]+main_angles[i]; - current_speed[i]=action_main_speed[i]; - } - /* update state */ - action_start_angles[i]=manager_current_angles[i]; - action_start_speed[i]=current_speed[i]; - } - } - /* load the next step */ - if(action_pause_time==0) - { - if(action_end) - { - state=ACTION_PAUSE; - action_finish(); - } - else - { - action_load_next_step(); - /* first step of the next section */ - for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) - { - if(manager_get_module(i)==MM_ACTION) - { - delta_speed=((action_main_speed[i]-action_start_speed[i])*(action_current_time-action_section_time))/action_pre_time; - current_speed[i]=action_start_speed[i]+delta_speed; - accel_angles[i]=(((action_start_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16); - manager_current_angles[i]=action_start_angles[i]+accel_angles[i]; - } - } - action_current_time=action_current_time-action_section_time; - action_section_time=action_pre_time; - state=ACTION_PRE; - } - } - else - { - action_current_time=action_current_time-action_section_time; - action_section_time=action_pause_time; - state=ACTION_PAUSE; - } - } - else - { - for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) - { - if(manager_get_module(i)==MM_ACTION) - { - if(action_zero_speed_finish[i]) - { - delta_speed=((0.0-action_main_speed[i])*action_current_time)/action_section_time; - current_speed[i]=action_main_speed[i]+delta_speed; - manager_current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*action_current_time)+((delta_speed*action_current_time)>>1))>>16); - } - else - { - manager_current_angles[i]=action_start_angles[i]+(main_angles[i]*action_current_time)/action_section_time; - current_speed[i]=action_main_speed[i]; - } - } - } - state=ACTION_POST; - } - break; - case ACTION_PAUSE: if(action_current_time>action_section_time) - { - if(action_end) - { - state=ACTION_PAUSE; - action_finish(); - } - else - { - // find next page to execute - action_load_next_step(); - /* first step of the next section */ - for(i=0;i<PAGE_MAX_NUM_SERVOS;i++) - { - delta_speed=((action_main_speed[i]-action_start_speed[i])*(action_current_time-action_section_time))/action_pre_time; - current_speed[i]=action_start_speed[i]+delta_speed; - accel_angles[i]=(((action_start_speed[i]*(action_current_time-action_section_time))+((delta_speed*(action_current_time-action_section_time))>>1))>>16); - manager_current_angles[i]=action_start_angles[i]+accel_angles[i]; - } - action_current_time=action_current_time-action_section_time; - action_section_time=action_pre_time; - state=ACTION_PRE; - } - } - else// pause section still active - { - state=ACTION_PAUSE; - } - break; - default: break; - } - } -} - -// operation functions -uint8_t action_in_range(unsigned short int address, unsigned short int length) -{ - return ram_in_window(ACTION_BASE_ADDRESS,ACTION_MEM_LENGTH,address,length); -} - -void action_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data) -{ - if(ram_in_range(DARWIN_ACTION_CNTRL,address,length)) - { - if(data[DARWIN_ACTION_CNTRL-address]&ACTION_START) - action_start_page(); - if(data[DARWIN_ACTION_CNTRL-address]&ACTION_STOP) - action_stop_page(); - if(data[DARWIN_ACTION_CNTRL-address]&ACTION_INT_EN) - action_enable_int(); - else - action_disable_int(); - } - if(ram_in_range(DARWIN_ACTION_PAGE,address,length))// load the page identifier - action_load_page(data[DARWIN_ACTION_PAGE-address]); -} - diff --git a/src/darwin_kinematics.c b/src/darwin_kinematics.c deleted file mode 100755 index a60ca577c8c7529d72d3d5d012f44eede9e65e10..0000000000000000000000000000000000000000 --- a/src/darwin_kinematics.c +++ /dev/null @@ -1,82 +0,0 @@ -#include "darwin_kinematics.h" -#include "darwin_math.h" -#include <math.h> - -uint8_t darwin_leg_ik(float *out, float x, float y, float z, float a, float b, float c) -{ - TMatrix3D Tad, Tda, Tcd, Tdc, Tac; - TVector3D vec,orientation; - TPoint3D position; - float _Rac, _Acos, _Atan, _k, _l, _m, _n, _s, _c, _theta; - - vector3d_load(&orientation,a * 180.0 / PI, b * 180.0 / PI, c * 180.0 / PI); - point3d_load(&position,x, y, z - DARWIN_LEG_LENGTH); - matrix3d_set_transform(&Tad,&position,&orientation); - - vec.x = x + Tad.coef[2] * DARWIN_ANKLE_LENGTH; - vec.y = y + Tad.coef[6] * DARWIN_ANKLE_LENGTH; - vec.z = (z - DARWIN_LEG_LENGTH) + Tad.coef[10] * DARWIN_ANKLE_LENGTH; - - // Get Knee - _Rac = vector3d_length(&vec); - _Acos = acos((_Rac * _Rac - DARWIN_THIGH_LENGTH * DARWIN_THIGH_LENGTH - DARWIN_CALF_LENGTH * DARWIN_CALF_LENGTH) / (2 * DARWIN_THIGH_LENGTH * DARWIN_CALF_LENGTH)); - if(isnan(_Acos) == 1) - return 0x00;; - *(out + 3) = _Acos; - - // Get Ankle Roll - Tda = Tad; - if(matrix3d_inverse(&Tda,&Tda) == 0x00) - return 0x00; - _k = sqrt(Tda.coef[7] * Tda.coef[7] + Tda.coef[11] * Tda.coef[11]); - _l = sqrt(Tda.coef[7] * Tda.coef[7] + (Tda.coef[11] - DARWIN_ANKLE_LENGTH) * (Tda.coef[11] - DARWIN_ANKLE_LENGTH)); - _m = (_k * _k - _l * _l - DARWIN_ANKLE_LENGTH * DARWIN_ANKLE_LENGTH) / (2 * _l * DARWIN_ANKLE_LENGTH); - if(_m > 1.0) - _m = 1.0; - else if(_m < -1.0) - _m = -1.0; - _Acos = acos(_m); - if(isnan(_Acos) == 1) - return 0x00; - if(Tda.coef[7] < 0.0) - *(out + 5) = -_Acos; - else - *(out + 5) = _Acos; - // Get Hip Yaw - vector3d_load(&orientation,*(out + 5) * 180.0 / PI, 0, 0); - point3d_load(&position,0, 0, -DARWIN_ANKLE_LENGTH); - matrix3d_set_transform(&Tcd,&position,&orientation); - Tdc = Tcd; - if(matrix3d_inverse(&Tdc,&Tdc) == 0x00) - return 0x00; - matrix3d_mult(&Tac,&Tad,&Tdc); - _Atan = atan2(-Tac.coef[1] , Tac.coef[5]); - if(isinf(_Atan) == 1) - return 0x00; - *(out) = _Atan; - - // Get Hip Roll - _Atan = atan2(Tac.coef[9], -Tac.coef[1] * sin(*(out)) + Tac.coef[5] * cos(*(out))); - if(isinf(_Atan) == 1) - return 0x00; - *(out + 1) = _Atan; - - // Get Hip Pitch and Ankle Pitch - _Atan = atan2(Tac.coef[2] * cos(*(out)) + Tac.coef[6] * sin(*(out)), Tac.coef[0] * cos(*(out)) + Tac.coef[4] * sin(*(out))); - if(isinf(_Atan) == 1) - return 0x00; - _theta = _Atan; - _k = sin(*(out + 3)) * DARWIN_CALF_LENGTH; - _l = -DARWIN_THIGH_LENGTH - cos(*(out + 3)) * DARWIN_CALF_LENGTH; - _m = cos(*(out)) * vec.x + sin(*(out)) * vec.y; - _n = cos(*(out + 1)) * vec.z + sin(*(out)) * sin(*(out + 1)) * vec.x - cos(*(out)) * sin(*(out + 1)) * vec.y; - _s = (_k * _n + _l * _m) / (_k * _k + _l * _l); - _c = (_n - _k * _s) / _l; - _Atan = atan2(_s, _c); - if(isinf(_Atan) == 1) - return 0x00; - *(out + 2) = _Atan; - *(out + 4) = _theta - *(out + 3) - *(out + 2); - - return 0x01; -} diff --git a/src/darwin_math.c b/src/darwin_math.c deleted file mode 100755 index e76646357603c96cf91e2dc5994c9fea00f1971d..0000000000000000000000000000000000000000 --- a/src/darwin_math.c +++ /dev/null @@ -1,237 +0,0 @@ -#include "darwin_math.h" -#include <math.h> - -// point functions -void point3d_init(TPoint3D *point) -{ - point->x=0.0; - point->y=0.0; - point->z=0.0; -} - -void point3d_load(TPoint3D *point, float x, float y,float z) -{ - point->x=x; - point->y=y; - point->z=z; -} - -void point3d_copy(TPoint3D *point_dst, TPoint3D *point_src) -{ - point_dst->x=point_src->x; - point_dst->y=point_src->y; - point_dst->z=point_src->z; -} - -// vector functions -void vector3d_init(TVector3D *vector) -{ - vector->x=0.0; - vector->y=0.0; - vector->z=0.0; -} - -void vector3d_load(TVector3D *vector, float x, float y, float z) -{ - vector->x=x; - vector->y=y; - vector->z=z; -} - -void vector3d_copy(TVector3D *vector_dst, TVector3D *vector_src) -{ - vector_dst->x=vector_src->x; - vector_dst->y=vector_src->y; - vector_dst->z=vector_src->z; -} - -float vector3d_length(TVector3D *vector) -{ - return sqrt(vector->x*vector->x + vector->y*vector->y + vector->z*vector->z); -} - -// matrix functions -void matrix3d_init(TMatrix3D *matrix) -{ - matrix3d_identity(matrix); -} - -void matrix3d_copy(TMatrix3D *matrix_dst, TMatrix3D *matrix_src) -{ - matrix_dst->coef[m00]=matrix_src->coef[m00]; - matrix_dst->coef[m01]=matrix_src->coef[m01]; - matrix_dst->coef[m02]=matrix_src->coef[m02]; - matrix_dst->coef[m03]=matrix_src->coef[m03]; - matrix_dst->coef[m10]=matrix_src->coef[m10]; - matrix_dst->coef[m11]=matrix_src->coef[m11]; - matrix_dst->coef[m12]=matrix_src->coef[m12]; - matrix_dst->coef[m13]=matrix_src->coef[m13]; - matrix_dst->coef[m20]=matrix_src->coef[m20]; - matrix_dst->coef[m21]=matrix_src->coef[m21]; - matrix_dst->coef[m22]=matrix_src->coef[m22]; - matrix_dst->coef[m23]=matrix_src->coef[m23]; - matrix_dst->coef[m30]=matrix_src->coef[m30]; - matrix_dst->coef[m31]=matrix_src->coef[m31]; - matrix_dst->coef[m32]=matrix_src->coef[m32]; - matrix_dst->coef[m33]=matrix_src->coef[m33]; -} - -void matrix3d_zero(TMatrix3D *matrix) -{ - matrix->coef[m00]=0.0; - matrix->coef[m01]=0.0; - matrix->coef[m02]=0.0; - matrix->coef[m03]=0.0; - matrix->coef[m10]=0.0; - matrix->coef[m11]=0.0; - matrix->coef[m12]=0.0; - matrix->coef[m13]=0.0; - matrix->coef[m20]=0.0; - matrix->coef[m21]=0.0; - matrix->coef[m22]=0.0; - matrix->coef[m23]=0.0; - matrix->coef[m30]=0.0; - matrix->coef[m31]=0.0; - matrix->coef[m32]=0.0; - matrix->coef[m33]=0.0; -} - -void matrix3d_identity(TMatrix3D *matrix) -{ - matrix->coef[m00]=1.0; - matrix->coef[m01]=0.0; - matrix->coef[m02]=0.0; - matrix->coef[m03]=0.0; - matrix->coef[m10]=0.0; - matrix->coef[m11]=1.0; - matrix->coef[m12]=0.0; - matrix->coef[m13]=0.0; - matrix->coef[m20]=0.0; - matrix->coef[m21]=0.0; - matrix->coef[m22]=1.0; - matrix->coef[m23]=0.0; - matrix->coef[m30]=0.0; - matrix->coef[m31]=0.0; - matrix->coef[m32]=0.0; - matrix->coef[m33]=1.0; -} - -uint8_t matrix3d_inverse(TMatrix3D *org, TMatrix3D *inv) -{ - TMatrix3D src, tmp; - float det; - uint8_t i; - - /* transpose matrix */ - for (i = 0; i < 4; i++) - { - src.coef[i] = org->coef[i*4]; - src.coef[i + 4] = org->coef[i*4 + 1]; - src.coef[i + 8] = org->coef[i*4 + 2]; - src.coef[i + 12] = org->coef[i*4 + 3]; - } - - /* calculate pairs for first 8 elements (cofactors) */ - tmp.coef[0] = src.coef[10] * src.coef[15]; - tmp.coef[1] = src.coef[11] * src.coef[14]; - tmp.coef[2] = src.coef[9] * src.coef[15]; - tmp.coef[3] = src.coef[11] * src.coef[13]; - tmp.coef[4] = src.coef[9] * src.coef[14]; - tmp.coef[5] = src.coef[10] * src.coef[13]; - tmp.coef[6] = src.coef[8] * src.coef[15]; - tmp.coef[7] = src.coef[11] * src.coef[12]; - tmp.coef[8] = src.coef[8] * src.coef[14]; - tmp.coef[9] = src.coef[10] * src.coef[12]; - tmp.coef[10] = src.coef[8] * src.coef[13]; - tmp.coef[11] = src.coef[9] * src.coef[12]; - /* calculate first 8 elements (cofactors) */ - inv->coef[0] = (tmp.coef[0]*src.coef[5] + tmp.coef[3]*src.coef[6] + tmp.coef[4]*src.coef[7]) - (tmp.coef[1]*src.coef[5] + tmp.coef[2]*src.coef[6] + tmp.coef[5]*src.coef[7]); - inv->coef[1] = (tmp.coef[1]*src.coef[4] + tmp.coef[6]*src.coef[6] + tmp.coef[9]*src.coef[7]) - (tmp.coef[0]*src.coef[4] + tmp.coef[7]*src.coef[6] + tmp.coef[8]*src.coef[7]); - inv->coef[2] = (tmp.coef[2]*src.coef[4] + tmp.coef[7]*src.coef[5] + tmp.coef[10]*src.coef[7]) - (tmp.coef[3]*src.coef[4] + tmp.coef[6]*src.coef[5] + tmp.coef[11]*src.coef[7]); - inv->coef[3] = (tmp.coef[5]*src.coef[4] + tmp.coef[8]*src.coef[5] + tmp.coef[11]*src.coef[6]) - (tmp.coef[4]*src.coef[4] + tmp.coef[9]*src.coef[5] + tmp.coef[10]*src.coef[6]); - inv->coef[4] = (tmp.coef[1]*src.coef[1] + tmp.coef[2]*src.coef[2] + tmp.coef[5]*src.coef[3]) - (tmp.coef[0]*src.coef[1] + tmp.coef[3]*src.coef[2] + tmp.coef[4]*src.coef[3]); - inv->coef[5] = (tmp.coef[0]*src.coef[0] + tmp.coef[7]*src.coef[2] + tmp.coef[8]*src.coef[3]) - (tmp.coef[1]*src.coef[0] + tmp.coef[6]*src.coef[2] + tmp.coef[9]*src.coef[3]); - inv->coef[6] = (tmp.coef[3]*src.coef[0] + tmp.coef[6]*src.coef[1] + tmp.coef[11]*src.coef[3]) - (tmp.coef[2]*src.coef[0] + tmp.coef[7]*src.coef[1] + tmp.coef[10]*src.coef[3]); - inv->coef[7] = (tmp.coef[4]*src.coef[0] + tmp.coef[9]*src.coef[1] + tmp.coef[10]*src.coef[2]) - (tmp.coef[5]*src.coef[0] + tmp.coef[8]*src.coef[1] + tmp.coef[11]*src.coef[2]); - /* calculate pairs for second 8 elements (cofactors) */ - tmp.coef[0] = src.coef[2]*src.coef[7]; - tmp.coef[1] = src.coef[3]*src.coef[6]; - tmp.coef[2] = src.coef[1]*src.coef[7]; - tmp.coef[3] = src.coef[3]*src.coef[5]; - tmp.coef[4] = src.coef[1]*src.coef[6]; - tmp.coef[5] = src.coef[2]*src.coef[5]; - //Streaming SIMD Extensions - Inverse of 4x4 Matrix 8 - tmp.coef[6] = src.coef[0]*src.coef[7]; - tmp.coef[7] = src.coef[3]*src.coef[4]; - tmp.coef[8] = src.coef[0]*src.coef[6]; - tmp.coef[9] = src.coef[2]*src.coef[4]; - tmp.coef[10] = src.coef[0]*src.coef[5]; - tmp.coef[11] = src.coef[1]*src.coef[4]; - /* calculate second 8 elements (cofactors) */ - inv->coef[8] = (tmp.coef[0]*src.coef[13] + tmp.coef[3]*src.coef[14] + tmp.coef[4]*src.coef[15]) - (tmp.coef[1]*src.coef[13] + tmp.coef[2]*src.coef[14] + tmp.coef[5]*src.coef[15]); - inv->coef[9] = (tmp.coef[1]*src.coef[12] + tmp.coef[6]*src.coef[14] + tmp.coef[9]*src.coef[15]) - (tmp.coef[0]*src.coef[12] + tmp.coef[7]*src.coef[14] + tmp.coef[8]*src.coef[15]); - inv->coef[10] = (tmp.coef[2]*src.coef[12] + tmp.coef[7]*src.coef[13] + tmp.coef[10]*src.coef[15]) - (tmp.coef[3]*src.coef[12] + tmp.coef[6]*src.coef[13] + tmp.coef[11]*src.coef[15]); - inv->coef[11] = (tmp.coef[5]*src.coef[12] + tmp.coef[8]*src.coef[13] + tmp.coef[11]*src.coef[14]) - (tmp.coef[4]*src.coef[12] + tmp.coef[9]*src.coef[13] + tmp.coef[10]*src.coef[14]); - inv->coef[12] = (tmp.coef[2]*src.coef[10] + tmp.coef[5]*src.coef[11] + tmp.coef[1]*src.coef[9]) - (tmp.coef[4]*src.coef[11] + tmp.coef[0]*src.coef[9] + tmp.coef[3]*src.coef[10]); - inv->coef[13] = (tmp.coef[8]*src.coef[11] + tmp.coef[0]*src.coef[8] + tmp.coef[7]*src.coef[10]) - (tmp.coef[6]*src.coef[10] + tmp.coef[9]*src.coef[11] + tmp.coef[1]*src.coef[8]); - inv->coef[14] = (tmp.coef[6]*src.coef[9] + tmp.coef[11]*src.coef[11] + tmp.coef[3]*src.coef[8]) - (tmp.coef[10]*src.coef[11] + tmp.coef[2]*src.coef[8] + tmp.coef[7]*src.coef[9]); - inv->coef[15] = (tmp.coef[10]*src.coef[10] + tmp.coef[4]*src.coef[8] + tmp.coef[9]*src.coef[9]) - (tmp.coef[8]*src.coef[9] + tmp.coef[11]*src.coef[10] + tmp.coef[5]*src.coef[8]); - /* calculate determinant */ - det = src.coef[0]*inv->coef[0] + src.coef[1]*inv->coef[1] + src.coef[2]*inv->coef[2] + src.coef[3]*inv->coef[3]; - /* calculate matrix inverse */ - if (det == 0) - { - det = 0; - return 0x00; - } - else - { - det = 1 / det; - } - - for (i = 0; i < 16; i++) - inv->coef[i]*=det; - - return 0x01; -} - -void matrix3d_set_transform(TMatrix3D *matrix, TPoint3D *point, TVector3D *vector) -{ - float Cx = cos(vector->x * 3.141592 / 180.0); - float Cy = cos(vector->y * 3.141592 / 180.0); - float Cz = cos(vector->z * 3.141592 / 180.0); - float Sx = sin(vector->x * 3.141592 / 180.0); - float Sy = sin(vector->y * 3.141592 / 180.0); - float Sz = sin(vector->z * 3.141592 / 180.0); - - matrix3d_identity(matrix); - matrix->coef[0] = Cz * Cy; - matrix->coef[1] = Cz * Sy * Sx - Sz * Cx; - matrix->coef[2] = Cz * Sy * Cx + Sz * Sx; - matrix->coef[3] = point->x; - matrix->coef[4] = Sz * Cy; - matrix->coef[5] = Sz * Sy * Sx + Cz * Cx; - matrix->coef[6] = Sz * Sy * Cx - Cz * Sx; - matrix->coef[7] = point->y; - matrix->coef[8] = -Sy; - matrix->coef[9] = Cy * Sx; - matrix->coef[10] = Cy * Cx; - matrix->coef[11] = point->z; -} - -void matrix3d_mult(TMatrix3D *dst,TMatrix3D *a,TMatrix3D *b) -{ - uint8_t i,j,c; - - matrix3d_zero(dst); - for(j = 0; j < 4; j++) - { - for(i = 0; i < 4; i++) - { - for(c = 0; c < 4; c++) - { - dst->coef[j*4+i] += a->coef[j*4+c] * b->coef[c*4+i]; - } - } - } -} diff --git a/src/grippers.c b/src/grippers.c deleted file mode 100644 index 14ab402f9e7c8597bb42e27e0d09349dda066e7f..0000000000000000000000000000000000000000 --- a/src/grippers.c +++ /dev/null @@ -1,218 +0,0 @@ -#include "darwin_dyn_master_v2.h" -#include "dyn_servos.h" -#include "grippers.h" -#include "ram.h" -#include "joint_motion.h" - -// private variables -int64_t grippers_period; -int16_t grippers_period_us; -uint8_t gripper_left_target; -uint8_t gripper_right_target; - -// public functions -void grippers_init(uint16_t period_us) -{ - uint16_t force; - uint32_t servo_mask=0x00000000; - - /* initialize private variables */ - gripper_left_target=0x00;//close; - gripper_right_target=0x00;//close; - grippers_period=(period_us<<16)/1000000; - grippers_period_us=period_us; - /* configure the maximum force of the servos */ - servo_mask=0x00000000; - if(ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]!=0xFF && ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]!=0xFF) - { - force=ram_data[DARWIN_GRIPPER_LEFT_MAX_FORCE_L]+(ram_data[DARWIN_GRIPPER_LEFT_MAX_FORCE_H]<<8); - dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_LEFT_TOP_ID],XL_TORQUE_LIMIT_L,force); - dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_LEFT_BOT_ID],XL_TORQUE_LIMIT_L,force); - servo_mask|=1<<ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]; - servo_mask|=1<<ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]; - joint_motion_set_group_servos(1,servo_mask); - } - servo_mask=0x00000000; - if(ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]!=0xFF && ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]!=0xFF) - { - force=ram_data[DARWIN_GRIPPER_RIGHT_MAX_FORCE_L]+(ram_data[DARWIN_GRIPPER_RIGHT_MAX_FORCE_H]<<8); - dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID],XL_TORQUE_LIMIT_L,force); - dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID],XL_TORQUE_LIMIT_L,force); - servo_mask|=1<<ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]; - servo_mask|=1<<ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]; - joint_motion_set_group_servos(2,servo_mask); - } - grippers_close(GRIPPER_LEFT); - grippers_close(GRIPPER_RIGHT); -} - -void grippers_set_period(uint16_t period_us) -{ - grippers_period=(period_us<<16)/1000000; - grippers_period_us=period_us; -} - -uint16_t grippers_get_period(void) -{ - return grippers_period_us; -} - -void grippers_open(grippers_t gripper) -{ - if(gripper==GRIPPER_LEFT) - { - ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L]; - ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H]; - ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]]=60; - ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L]; - ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H]; - ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]]=60; - joint_motion_start(1); - } - else if(gripper==GRIPPER_RIGHT) - { - ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L]; - ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H]; - ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]]=60; - ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L]; - ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H]; - ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]]=60; - joint_motion_start(2); - } -} - -void grippers_close(grippers_t gripper) -{ - if(gripper==GRIPPER_LEFT) - { - ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L]; - ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H]; - ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]]=60; - ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L]; - ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H]; - ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]]=60; - joint_motion_start(1); - } - else if(gripper==GRIPPER_RIGHT) - { - ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L]; - ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H]; - ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]]=60; - ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L]; - ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H]; - ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]]=60; - joint_motion_start(2); - } -} - -void grippers_stop(grippers_t gripper) -{ - if(gripper==GRIPPER_LEFT) - joint_motion_stop(1); - else - joint_motion_stop(2); -} - -uint8_t gripper_is_open(grippers_t gripper) -{ - if(gripper==GRIPPER_LEFT) - { - if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_LEFT) - return 0x01; - else - return 0x00; - } - else - { - if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_RIGHT) - return 0x01; - else - return 0x00; - } -} - -uint8_t gripper_is_close(grippers_t gripper) -{ - if(gripper==GRIPPER_LEFT) - { - if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_LEFT) - return 0x00; - else - return 0x01; - } - else - { - if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_RIGHT) - return 0x00; - else - return 0x01; - } -} - -uint8_t gripper_is_moving(grippers_t gripper) -{ - if(gripper==GRIPPER_LEFT) - return are_joints_moving(1); - else - return are_joints_moving(2); -} - -// operation functions -uint8_t grippers_in_range(unsigned short int address, unsigned short int length) -{ - if(ram_in_window(GRIPPER_BASE_ADDRESS,GRIPPER_MEM_LENGTH,address,length) || - ram_in_window(GRIPPER_EEPROM_ADDRESS,GRIPPER_EEPROM_LENGTH,address,length)) - return 0x01; - else - return 0x00; -} - -void grippers_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data) -{ - -} - -void grippers_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data) -{ - if(ram_in_range(DARWIN_GRIPPER_CNTRL,address,length)) - { - if(data[DARWIN_GRIPPER_CNTRL-address]&GRIPPER_OPEN_RIGHT) - grippers_open(GRIPPER_RIGHT); - else if(data[DARWIN_GRIPPER_CNTRL-address]&GRIPPER_CLOSE_RIGHT) - grippers_close(GRIPPER_RIGHT); - if(data[DARWIN_GRIPPER_CNTRL-address]&GRIPPER_OPEN_LEFT) - grippers_open(GRIPPER_LEFT); - else if(data[DARWIN_GRIPPER_CNTRL-address]&GRIPPER_CLOSE_LEFT) - grippers_close(GRIPPER_LEFT); - } -} - -void grippers_process(void) -{ - if(!are_joints_moving(1)) - { - ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_MOVING_LEFT; - if(gripper_left_target==0x01) - ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_OPENED_LEFT; - else - ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_LEFT; - } - else - { - ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_LEFT; - ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_MOVING_LEFT; - } - if(!are_joints_moving(2)) - { - ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_MOVING_RIGHT; - if(gripper_right_target==0x01) - ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_OPENED_RIGHT; - else - ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_RIGHT; - } - else - { - ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_RIGHT; - ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_MOVING_RIGHT; - } -} diff --git a/src/head_tracking.c b/src/head_tracking.c deleted file mode 100644 index 12ee27c4d4485534af863f89d724864df4104f16..0000000000000000000000000000000000000000 --- a/src/head_tracking.c +++ /dev/null @@ -1,333 +0,0 @@ -#include "head_tracking.h" -#include "ram.h" - -// private variables -typedef struct{ - uint16_t kp; - uint16_t ki; - uint16_t kd; - int64_t prev_error;// 48|16 - int64_t integral_part; - int64_t integral_clamp; - int64_t max_angle; - int64_t min_angle; - int64_t target_angle; -}TJointControl; - -TJointControl head_tracking_pan; -TJointControl head_tracking_tilt; -int64_t head_tracking_period; -int16_t head_tracking_period_us; -uint8_t head_tracking_enabled; - -// public functions -void head_tracking_init(uint16_t period_us) -{ - /* initialize private variables */ - head_tracking_pan.kp=ram_data[DARWIN_HEAD_PAN_P_L]+(ram_data[DARWIN_HEAD_PAN_P_H]<<8); - head_tracking_pan.ki=ram_data[DARWIN_HEAD_PAN_I_L]+(ram_data[DARWIN_HEAD_PAN_I_H]<<8); - head_tracking_pan.kd=ram_data[DARWIN_HEAD_PAN_D_L]+(ram_data[DARWIN_HEAD_PAN_D_H]<<8); - head_tracking_pan.prev_error=0; - head_tracking_pan.integral_part=0; - head_tracking_pan.integral_clamp=(ram_data[DARWIN_HEAD_PAN_I_CLAMP_L]+(ram_data[DARWIN_HEAD_PAN_I_CLAMP_H]<<8))<<9; - head_tracking_pan.min_angle=manager_get_cw_angle_limit(L_PAN)<<9; - ram_data[DARWIN_HEAD_MIN_PAN_L]=manager_get_cw_angle_limit(L_PAN)%256; - ram_data[DARWIN_HEAD_MIN_PAN_H]=manager_get_cw_angle_limit(L_PAN)/256; - head_tracking_pan.max_angle=manager_get_ccw_angle_limit(L_PAN)<<9; - ram_data[DARWIN_HEAD_MAX_PAN_L]=manager_get_ccw_angle_limit(L_PAN)%256; - ram_data[DARWIN_HEAD_MAX_PAN_H]=manager_get_ccw_angle_limit(L_PAN)/256; - head_tracking_pan.target_angle=0; - head_tracking_tilt.kp=ram_data[DARWIN_HEAD_TILT_P_L]+(ram_data[DARWIN_HEAD_TILT_P_H]<<8); - head_tracking_tilt.ki=ram_data[DARWIN_HEAD_TILT_I_L]+(ram_data[DARWIN_HEAD_TILT_I_H]<<8); - head_tracking_tilt.kd=ram_data[DARWIN_HEAD_TILT_D_L]+(ram_data[DARWIN_HEAD_TILT_D_H]<<8); - head_tracking_tilt.prev_error=0; - head_tracking_tilt.integral_part=0; - head_tracking_tilt.integral_clamp=(ram_data[DARWIN_HEAD_TILT_I_CLAMP_L]+(ram_data[DARWIN_HEAD_TILT_I_CLAMP_H]<<8))<<9; - head_tracking_tilt.min_angle=manager_get_cw_angle_limit(L_TILT)<<9; - ram_data[DARWIN_HEAD_MIN_TILT_L]=manager_get_cw_angle_limit(L_TILT)%256; - ram_data[DARWIN_HEAD_MIN_TILT_H]=manager_get_cw_angle_limit(L_TILT)/256; - head_tracking_tilt.max_angle=manager_get_ccw_angle_limit(L_TILT)<<9; - ram_data[DARWIN_HEAD_MAX_TILT_L]=manager_get_ccw_angle_limit(L_TILT)%256; - ram_data[DARWIN_HEAD_MAX_TILT_H]=manager_get_ccw_angle_limit(L_TILT)/256; - head_tracking_tilt.target_angle=0; - head_tracking_period=(period_us<<16)/1000000; - head_tracking_period_us=period_us; - head_tracking_enabled=0x00; -} - -void head_tracking_set_period(uint16_t period_us) -{ - head_tracking_period=(period_us<<16)/1000000; - head_tracking_period_us=period_us; -} - -uint16_t head_tracking_get_period(void) -{ - return head_tracking_period_us; -} - -void head_tracking_start(void) -{ - head_tracking_enabled=0x01; - ram_data[DARWIN_HEAD_CNTRL]|=HEAD_STATUS; -} - -void head_tracking_stop(void) -{ - head_tracking_enabled=0x00; - ram_data[DARWIN_HEAD_CNTRL]&=(~HEAD_STATUS); -} - -uint8_t head_is_tracking(void) -{ - return head_tracking_enabled; -} - -void head_tracking_set_pan_range(int16_t max_angle,int16_t min_angle) -{ - head_tracking_pan.max_angle=max_angle<<9; - ram_data[DARWIN_HEAD_MAX_PAN_L]=max_angle%256; - ram_data[DARWIN_HEAD_MAX_PAN_H]=max_angle/256; - head_tracking_pan.min_angle=min_angle<<9; - ram_data[DARWIN_HEAD_MIN_PAN_L]=min_angle%256; - ram_data[DARWIN_HEAD_MIN_PAN_H]=min_angle/256; -} - -void head_tracking_get_pan_range(int16_t *max_angle,int16_t *min_angle) -{ - *max_angle=head_tracking_pan.max_angle>>9; - *min_angle=head_tracking_pan.min_angle>>9; -} - -void head_tracking_set_pan_pid(uint16_t kp,uint16_t ki,uint16_t kd, uint16_t i_clamp) -{ - head_tracking_pan.kp=kp; - ram_data[DARWIN_HEAD_PAN_P_L]=kp%256; - ram_data[DARWIN_HEAD_PAN_P_H]=kp/256; - head_tracking_pan.ki=ki; - ram_data[DARWIN_HEAD_PAN_I_L]=ki%256; - ram_data[DARWIN_HEAD_PAN_I_H]=ki/256; - head_tracking_pan.kd=kd; - ram_data[DARWIN_HEAD_PAN_D_L]=kd%256; - ram_data[DARWIN_HEAD_PAN_D_H]=kd/256; - head_tracking_pan.integral_clamp=i_clamp<<9; - ram_data[DARWIN_HEAD_PAN_I_CLAMP_L]=i_clamp%256; - ram_data[DARWIN_HEAD_PAN_I_CLAMP_H]=i_clamp/256; -} - -void head_tracking_get_pan_pid(uint16_t *kp,uint16_t *ki,uint16_t *kd, uint16_t *i_clamp) -{ - *kp=head_tracking_pan.kp; - *ki=head_tracking_pan.ki; - *kd=head_tracking_pan.kd; - *i_clamp=head_tracking_pan.integral_clamp>>9; -} - -void head_tracking_set_tilt_range(int16_t max_angle,int16_t min_angle) -{ - head_tracking_tilt.max_angle=max_angle<<9; - ram_data[DARWIN_HEAD_MAX_TILT_L]=max_angle%256; - ram_data[DARWIN_HEAD_MAX_TILT_H]=max_angle/256; - head_tracking_tilt.min_angle=min_angle<<9; - ram_data[DARWIN_HEAD_MIN_TILT_L]=min_angle%256; - ram_data[DARWIN_HEAD_MIN_TILT_H]=min_angle/256; -} - -void head_tracking_get_tilt_range(int16_t *max_angle,int16_t *min_angle) -{ - *max_angle=head_tracking_tilt.max_angle>>9; - *min_angle=head_tracking_tilt.min_angle>>9; -} - -void head_tracking_set_tilt_pid(uint16_t kp,uint16_t ki,uint16_t kd, uint16_t i_clamp) -{ - head_tracking_tilt.kp=kp; - ram_data[DARWIN_HEAD_TILT_P_L]=kp%256; - ram_data[DARWIN_HEAD_TILT_P_H]=kp/256; - head_tracking_tilt.ki=ki; - ram_data[DARWIN_HEAD_TILT_I_L]=ki%256; - ram_data[DARWIN_HEAD_TILT_I_H]=ki/256; - head_tracking_tilt.kd=kd; - ram_data[DARWIN_HEAD_TILT_D_L]=kd%256; - ram_data[DARWIN_HEAD_TILT_D_H]=kd/256; - head_tracking_tilt.integral_clamp=i_clamp<<9; - ram_data[DARWIN_HEAD_TILT_I_CLAMP_L]=i_clamp%256; - ram_data[DARWIN_HEAD_TILT_I_CLAMP_H]=i_clamp/256; -} - -void head_tracking_get_tilt_pid(uint16_t *kp,uint16_t *ki,uint16_t *kd, uint16_t *i_clamp) -{ - *kp=head_tracking_tilt.kp; - *ki=head_tracking_tilt.ki; - *kd=head_tracking_tilt.kd; - *i_clamp=head_tracking_tilt.integral_clamp>>9; -} - -void head_tracking_set_target_pan(int16_t target) -{ - head_tracking_pan.target_angle=target<<9; - ram_data[DARWIN_HEAD_PAN_TARGET_L]=target%256; - ram_data[DARWIN_HEAD_PAN_TARGET_H]=target/256; -} - -void head_tracking_set_target_tilt(int16_t target) -{ - head_tracking_tilt.target_angle=target<<9; - ram_data[DARWIN_HEAD_TILT_TARGET_L]=target%256; - ram_data[DARWIN_HEAD_TILT_TARGET_H]=target/256; -} - -// operation functions -uint8_t head_tracking_in_range(unsigned short int address, unsigned short int length) -{ - if(ram_in_window(HEAD_BASE_ADDRESS,HEAD_MEM_LENGTH,address,length) || - ram_in_window(HEAD_EEPROM_ADDRESS,HEAD_EEPROM_LENGTH,address,length)) - return 0x01; - else - return 0x00; -} - -void head_tracking_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data) -{ - -} - -void head_tracking_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data) -{ - uint16_t kp,kd,ki,i_clamp; - int16_t max,min,target; - - // head tracking control - if(ram_in_range(DARWIN_HEAD_CNTRL,address,length)) - { - if(data[DARWIN_HEAD_CNTRL-address]&HEAD_START) - head_tracking_start(); - if(data[DARWIN_HEAD_CNTRL-address]&HEAD_STOP) - head_tracking_stop(); - } - if(ram_in_window(DARWIN_HEAD_PAN_P_L,8,address,length)) - { - head_tracking_get_pan_pid(&kp,&kd,&ki,&i_clamp); - if(ram_in_range(DARWIN_HEAD_PAN_P_L,address,length)) kp=(kp&0xFF00)|data[DARWIN_HEAD_PAN_P_L-address]; - if(ram_in_range(DARWIN_HEAD_PAN_P_H,address,length)) kp=(kp&0x00FF)|(data[DARWIN_HEAD_PAN_P_H-address]<<8); - if(ram_in_range(DARWIN_HEAD_PAN_I_L,address,length)) ki=(ki&0xFF00)|data[DARWIN_HEAD_PAN_I_L-address]; - if(ram_in_range(DARWIN_HEAD_PAN_I_H,address,length)) ki=(ki&0x00FF)|(data[DARWIN_HEAD_PAN_I_H-address]<<8); - if(ram_in_range(DARWIN_HEAD_PAN_D_L,address,length)) kd=(kd&0xFF00)|data[DARWIN_HEAD_PAN_D_L-address]; - if(ram_in_range(DARWIN_HEAD_PAN_D_H,address,length)) kd=(kd&0x00FF)|(data[DARWIN_HEAD_PAN_D_H-address]<<8); - if(ram_in_range(DARWIN_HEAD_PAN_I_CLAMP_L,address,length)) i_clamp=(i_clamp&0xFF00)|data[DARWIN_HEAD_PAN_I_CLAMP_L-address]; - if(ram_in_range(DARWIN_HEAD_PAN_I_CLAMP_H,address,length)) i_clamp=(i_clamp&0x00FF)|(data[DARWIN_HEAD_PAN_I_CLAMP_H-address]<<8); - head_tracking_set_pan_pid(kp,kd,ki,i_clamp); - } - if(ram_in_window(DARWIN_HEAD_TILT_P_L,8,address,length)) - { - head_tracking_get_tilt_pid(&kp,&kd,&ki,&i_clamp); - if(ram_in_range(DARWIN_HEAD_TILT_P_L,address,length)) kp=(kp&0xFF00)|data[DARWIN_HEAD_TILT_P_L-address]; - if(ram_in_range(DARWIN_HEAD_TILT_P_H,address,length)) kp=(kp&0x00FF)|(data[DARWIN_HEAD_TILT_P_H-address]<<8); - if(ram_in_range(DARWIN_HEAD_TILT_I_L,address,length)) ki=(ki&0xFF00)|data[DARWIN_HEAD_TILT_I_L-address]; - if(ram_in_range(DARWIN_HEAD_TILT_I_H,address,length)) ki=(ki&0x00FF)|(data[DARWIN_HEAD_TILT_I_H-address]<<8); - if(ram_in_range(DARWIN_HEAD_TILT_D_L,address,length)) kd=(kd&0xFF00)|data[DARWIN_HEAD_TILT_D_L-address]; - if(ram_in_range(DARWIN_HEAD_TILT_D_H,address,length)) kd=(kd&0x00FF)|(data[DARWIN_HEAD_TILT_D_H-address]<<8); - if(ram_in_range(DARWIN_HEAD_TILT_I_CLAMP_L,address,length)) i_clamp=(i_clamp&0xFF00)|data[DARWIN_HEAD_TILT_I_CLAMP_L-address]; - if(ram_in_range(DARWIN_HEAD_TILT_I_CLAMP_H,address,length)) i_clamp=(i_clamp&0x00FF)|(data[DARWIN_HEAD_TILT_I_CLAMP_H-address]<<8); - head_tracking_set_tilt_pid(kp,kd,ki,i_clamp); - } - if(ram_in_window(DARWIN_HEAD_MAX_PAN_L,4,address,length)) - { - head_tracking_get_pan_range(&max,&min); - if(ram_in_range(DARWIN_HEAD_MAX_PAN_L,address,length)) max=(max&0xFF00)|data[DARWIN_HEAD_MAX_PAN_L-address]; - if(ram_in_range(DARWIN_HEAD_MAX_PAN_H,address,length)) max=(max&0x00FF)|(data[DARWIN_HEAD_MAX_PAN_H-address]<<8); - if(ram_in_range(DARWIN_HEAD_MIN_PAN_L,address,length)) min=(min&0xFF00)|data[DARWIN_HEAD_MIN_PAN_L-address]; - if(ram_in_range(DARWIN_HEAD_MIN_PAN_H,address,length)) min=(min&0x00FF)|(data[DARWIN_HEAD_MIN_PAN_H-address]<<8); - head_tracking_set_pan_range(max,min); - } - if(ram_in_window(DARWIN_HEAD_MAX_TILT_L,4,address,length)) - { - head_tracking_get_tilt_range(&max,&min); - if(ram_in_range(DARWIN_HEAD_MAX_TILT_L,address,length)) max=(max&0xFF00)|data[DARWIN_HEAD_MAX_TILT_L-address]; - if(ram_in_range(DARWIN_HEAD_MAX_TILT_H,address,length)) max=(max&0x00FF)|(data[DARWIN_HEAD_MAX_TILT_H-address]<<8); - if(ram_in_range(DARWIN_HEAD_MIN_TILT_L,address,length)) min=(min&0xFF00)|data[DARWIN_HEAD_MIN_TILT_L-address]; - if(ram_in_range(DARWIN_HEAD_MIN_TILT_H,address,length)) min=(min&0x00FF)|(data[DARWIN_HEAD_MIN_TILT_H-address]<<8); - head_tracking_set_tilt_range(max,min); - } - if(ram_in_window(DARWIN_HEAD_PAN_TARGET_L,2,address,length)) - { - target=(head_tracking_pan.target_angle>>9); - if(ram_in_range(DARWIN_HEAD_PAN_TARGET_L,address,length)) - { - target=(target&0xFF00)|data[DARWIN_HEAD_PAN_TARGET_L-address]; - ram_data[DARWIN_HEAD_PAN_TARGET_L]=data[DARWIN_HEAD_PAN_TARGET_L-address]; - } - if(ram_in_range(DARWIN_HEAD_PAN_TARGET_H,address,length)) - { - target=(target&0x00FF)|(data[DARWIN_HEAD_PAN_TARGET_H-address]<<8); - ram_data[DARWIN_HEAD_PAN_TARGET_H]=data[DARWIN_HEAD_PAN_TARGET_H-address]; - } - head_tracking_pan.target_angle=(target<<9); - } - if(ram_in_window(DARWIN_HEAD_TILT_TARGET_L,2,address,length)) - { - target=(head_tracking_tilt.target_angle>>9); - if(ram_in_range(DARWIN_HEAD_TILT_TARGET_L,address,length)) - { - target=(target&0xFF00)|data[DARWIN_HEAD_TILT_TARGET_L-address]; - ram_data[DARWIN_HEAD_TILT_TARGET_L]=data[DARWIN_HEAD_TILT_TARGET_L-address]; - } - if(ram_in_range(DARWIN_HEAD_TILT_TARGET_H,address,length)) - { - target=(target&0x00FF)|(data[DARWIN_HEAD_TILT_TARGET_H-address]<<8); - ram_data[DARWIN_HEAD_TILT_TARGET_H]=data[DARWIN_HEAD_TILT_TARGET_H-address]; - } - head_tracking_tilt.target_angle=(target<<9); - } -} - -// motion manager process function -void head_tracking_process(void) -{ - int64_t pan_error, tilt_error; - int64_t derivative_pan=0, derivative_tilt=0; - - if(head_tracking_enabled) - { - ram_data[DARWIN_HEAD_CNTRL]|=HEAD_STATUS; - if(manager_get_module(L_PAN)==MM_HEAD) - { - pan_error = head_tracking_pan.target_angle-manager_current_angles[L_PAN]; - head_tracking_pan.integral_part+=(pan_error*head_tracking_period)>>16; - if(head_tracking_pan.integral_part>head_tracking_pan.integral_clamp) - head_tracking_pan.integral_part=head_tracking_pan.integral_clamp; - else if(head_tracking_pan.integral_part<-head_tracking_pan.integral_clamp) - head_tracking_pan.integral_part=-head_tracking_pan.integral_clamp; - derivative_pan = ((pan_error - head_tracking_pan.prev_error)<<16)/head_tracking_period; - manager_current_angles[L_PAN]+=((head_tracking_pan.kp*pan_error)>>16); - manager_current_angles[L_PAN]+=((head_tracking_pan.ki*head_tracking_pan.integral_part)>>16); - manager_current_angles[L_PAN]+=((head_tracking_pan.kd*derivative_pan)>>16); - if(manager_current_angles[L_PAN]>head_tracking_pan.max_angle) - manager_current_angles[L_PAN]=head_tracking_pan.max_angle; - else if(manager_current_angles[L_PAN]<head_tracking_pan.min_angle) - manager_current_angles[L_PAN]=head_tracking_pan.min_angle; - head_tracking_pan.prev_error = pan_error; - } - if(manager_get_module(L_TILT)==MM_HEAD) - { - tilt_error = head_tracking_tilt.target_angle-manager_current_angles[L_TILT]; - head_tracking_tilt.integral_part+=(tilt_error*head_tracking_period)>>16; - if(head_tracking_tilt.integral_part>head_tracking_tilt.integral_clamp) - head_tracking_tilt.integral_part=head_tracking_tilt.integral_clamp; - else if(head_tracking_tilt.integral_part<-head_tracking_tilt.integral_clamp) - head_tracking_tilt.integral_part=-head_tracking_tilt.integral_clamp; - derivative_tilt = ((tilt_error - head_tracking_tilt.prev_error)<<16)/head_tracking_period; - manager_current_angles[L_TILT]+=((head_tracking_tilt.kp*tilt_error)>>16); - manager_current_angles[L_TILT]+=((head_tracking_tilt.ki*head_tracking_tilt.integral_part)>>16); - manager_current_angles[L_TILT]+=((head_tracking_tilt.kd*derivative_tilt)>>16); - if(manager_current_angles[L_TILT]>head_tracking_tilt.max_angle) - manager_current_angles[L_TILT]=head_tracking_tilt.max_angle; - else if(manager_current_angles[L_TILT]<head_tracking_tilt.min_angle) - manager_current_angles[L_TILT]=head_tracking_tilt.min_angle; - head_tracking_tilt.prev_error = tilt_error; - } - } - else - ram_data[DARWIN_HEAD_CNTRL]&=(~HEAD_STATUS); -} - diff --git a/src/joint_motion.c b/src/joint_motion.c deleted file mode 100644 index bd537d08b82d5557a02bbcbcc8535aae354557ba..0000000000000000000000000000000000000000 --- a/src/joint_motion.c +++ /dev/null @@ -1,285 +0,0 @@ -#include "joint_motion.h" -#include "ram.h" -#include <stdlib.h> - -// private variables -int64_t joint_target_angles[MANAGER_MAX_NUM_SERVOS];//48|16 -int64_t joint_target_speeds[MANAGER_MAX_NUM_SERVOS];//48|16 -int64_t joint_target_accels[MANAGER_MAX_NUM_SERVOS];//48|16 -int64_t joint_target_stop_angles[MANAGER_MAX_NUM_SERVOS];//48|16 - -int64_t joint_current_angles[MANAGER_MAX_NUM_SERVOS];//48|16 -int64_t joint_current_speeds[MANAGER_MAX_NUM_SERVOS];//48|16 - -int8_t joint_dir[MANAGER_MAX_NUM_SERVOS];// joint_direction - -int64_t joint_motion_period; -int16_t joint_motion_period_us; - -uint8_t joint_stop[NUM_JOINT_GROUPS]; -uint8_t joint_moving[NUM_JOINT_GROUPS]; -uint32_t joint_group_servos[NUM_JOINT_GROUPS]; - -// public functions -void joint_motion_init(uint16_t period_us) -{ - uint8_t i; - - /* initialize the internal variables */ - for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++) - { - // target values - joint_target_angles[i]=manager_current_angles[i]; - joint_target_speeds[i]=0; - joint_target_accels[i]=0; - // current values - joint_current_angles[i]=manager_current_angles[i]; - joint_current_speeds[i]=0; - // the current angles, speeds and accelerations are in RAM - joint_dir[i]=0; - } - for(i=0;i<NUM_JOINT_GROUPS;i++) - { - joint_stop[i]=0x00; - joint_moving[i]=0x00; - joint_group_servos[i]=0x00000000; - } - joint_motion_period=(period_us<<16)/1000000; - joint_motion_period_us=period_us; -} - -void joint_motion_set_period(uint16_t period_us) -{ - joint_motion_period=(period_us<<16)/1000000; - joint_motion_period_us=period_us; -} - -uint16_t joint_motion_get_period(void) -{ - return joint_motion_period_us; -} - -void joint_motion_set_group_servos(uint8_t group_id,uint32_t servos) -{ - joint_group_servos[group_id]=servos; - ram_data[DARWIN_JOINT_GRP0_SERVOS0+group_id*5]=servos&0x000000FF; - ram_data[DARWIN_JOINT_GRP0_SERVOS1+group_id*5]=(servos>>8)&0x000000FF; - ram_data[DARWIN_JOINT_GRP0_SERVOS2+group_id*5]=(servos>>16)&0x000000FF; - ram_data[DARWIN_JOINT_GRP0_SERVOS3+group_id*5]=(servos>>24)&0x000000FF; -} - -uint32_t joint_motion_get_group_servos(uint8_t group_id) -{ - return joint_group_servos[group_id]; -} - -void joint_motion_start(uint8_t group_id) -{ - uint8_t i; - int16_t angle; - uint32_t servo_index; - - /* initialize the internal variables */ - for(i=0,servo_index=0x00000001;i<MANAGER_MAX_NUM_SERVOS;i++,servo_index=servo_index<<1) - { - if(joint_group_servos[group_id]&servo_index) - { - if(ram_data[DARWIN_JOINT_SERVO0_SPEED+i]==0) - { - joint_target_angles[i]=manager_current_angles[i]; - joint_current_angles[i]=manager_current_angles[i]; - angle=manager_current_angles[i]>>9; - ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+i*2]=angle%256; - ram_data[DARWIN_JOINT_SERVO0_ANGLE_H+i*2]=angle/256; - } - else - { - // current values - joint_current_angles[i]=manager_current_angles[i]; - // target angle - angle=ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+i*2]+(ram_data[DARWIN_JOINT_SERVO0_ANGLE_H+i*2]<<8); - if(angle>manager_get_ccw_angle_limit(i)) - angle=manager_get_ccw_angle_limit(i); - else if(angle<manager_get_cw_angle_limit(i)) - angle=manager_get_cw_angle_limit(i); - joint_target_angles[i]=angle<<9; - // target speed - joint_target_speeds[i]=ram_data[DARWIN_JOINT_SERVO0_SPEED+i]<<16; - // target speed - joint_target_accels[i]=ram_data[DARWIN_JOINT_SERVO0_ACCEL+i]<<16; - if(joint_target_accels[i]==0) - { - joint_target_accels[i]=1<<16; - ram_data[DARWIN_JOINT_SERVO0_ACCEL+i]=1; - } - // check the parameters - if(abs(joint_target_angles[i]-joint_current_angles[i])>=65) - { - if((joint_target_speeds[i]*joint_target_speeds[i])/joint_target_accels[i]>abs(joint_target_angles[i]-joint_current_angles[i])) - { - joint_target_accels[i]=(joint_target_speeds[i]*joint_target_speeds[i])/abs(joint_target_angles[i]-joint_current_angles[i]); - ram_data[DARWIN_JOINT_SERVO0_ACCEL+i]=joint_target_accels[i]>>16; - } - } - // stop angles - joint_target_stop_angles[i]=joint_target_speeds[i]*joint_target_speeds[i]/(2*joint_target_accels[i]); - // the current angles, speeds and accelerations are in RAM - if(joint_target_angles[i]>=joint_current_angles[i]) - joint_dir[i]=1; - else - joint_dir[i]=-1; - } - } - } - joint_moving[group_id]=0x01; - joint_stop[group_id]=0x00; - ram_data[DARWIN_JOINT_GRP0_CNTRL+group_id*5]|=JOINT_STATUS; -} - -void joint_motion_stop(uint8_t group_id) -{ - joint_stop[group_id]=0x01; -} - -uint8_t are_joints_moving(uint8_t group_id) -{ - return joint_moving[group_id]; -} - -// operation functions -uint8_t joint_motion_in_range(unsigned short int address, unsigned short int length) -{ - if(ram_in_window(JOINT_BASE_ADDRESS,JOINT_MEM_LENGTH,address,length)) - return 0x01; - else - return 0x00; -} - -void joint_motion_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data) -{ - -} - -void joint_motion_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data) -{ - uint8_t j,k; - uint16_t i; - uint32_t servos; - - // load motion information - for(i=DARWIN_JOINT_SERVO0_ANGLE_L;i<=DARWIN_JOINT_SERVO31_ACCEL;i++) - if(ram_in_range(i,address,length)) - ram_data[i]=data[i-address]; - // group servos - for(j=0;j<4;j++) - { - if(ram_in_window(DARWIN_JOINT_GRP0_SERVOS0+j*5,4,address,length)) - { - servos=joint_motion_get_group_servos(j); - for(i=DARWIN_JOINT_GRP0_SERVOS0+j*5,k=0;i<=DARWIN_JOINT_GRP0_SERVOS3+j*5;i++,k++) - if(ram_in_range(i,address,length)) - servos|=(data[i-address]<<(k*8)); - joint_motion_set_group_servos(j,servos); - } - if(ram_in_range(DARWIN_JOINT_GRP0_CNTRL+j*5,address,length)) - { - if(data[DARWIN_JOINT_GRP0_CNTRL+j*5-address]&JOINT_START) - joint_motion_start(j); - if(data[DARWIN_JOINT_GRP0_CNTRL+j*5-address]&JOINT_STOP) - joint_motion_stop(j); - } - } -} - -// motion manager interface functions -void joint_motion_process(void) -{ - uint8_t moving,i,j; - uint32_t servo_index; - - for(j=0;j<4;j++) - { - moving=0x00; - if(joint_moving[j]==0x01) - { - for(i=0,servo_index=0x00000001;i<MANAGER_MAX_NUM_SERVOS;i++,servo_index=servo_index<<1) - { - if(manager_get_module(i)==MM_JOINTS && joint_group_servos[j]&servo_index) - { - if(joint_stop[j]==0x01) - { - joint_target_angles[i]=manager_current_angles[i]; - joint_target_speeds[i]=0; - joint_target_accels[i]=0; - joint_current_speeds[i]=0; - } - else - { - if(abs(joint_target_angles[i]-joint_current_angles[i])>=65) - { - moving=0x01; - if(joint_current_speeds[i]!=joint_dir[i]*joint_target_speeds[i])// it is necessary to change the current speed - { - joint_current_angles[i]+=((joint_current_speeds[i]*joint_motion_period)>>17); - if(joint_current_speeds[i]>joint_dir[i]*joint_target_speeds[i]) - { - joint_current_speeds[i]-=((joint_target_accels[i]*joint_motion_period)>>16);// reduce speed - if(joint_current_speeds[i]<joint_dir[i]*joint_target_speeds[i]) - joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i]; - } - else - { - joint_current_speeds[i]+=((joint_target_accels[i]*joint_motion_period)>>16);// increase speed - if(joint_current_speeds[i]>joint_dir[i]*joint_target_speeds[i]) - joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i]; - } - joint_current_angles[i]+=((joint_current_speeds[i]*joint_motion_period)>>17); - if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i]) - joint_current_angles[i]=joint_target_angles[i]; - if(joint_dir[i]==-1 && joint_current_angles[i]<joint_target_angles[i]) - joint_current_angles[i]=joint_target_angles[i]; - } - else - { - if(abs(joint_target_angles[i]-joint_current_angles[i])>joint_target_stop_angles[i])// constant speed phase - { - joint_current_angles[i]+=((joint_current_speeds[i]*joint_motion_period)>>16); - if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i]) - joint_current_angles[i]=joint_target_angles[i]; - if(joint_dir[i]==-1 && joint_current_angles[i]<joint_target_angles[i]) - joint_current_angles[i]=joint_target_angles[i]; - } - else// deceleration phase - { - joint_current_angles[i]=joint_current_angles[i]+((joint_current_speeds[i]*joint_motion_period)>>17); - joint_target_speeds[i]=joint_target_speeds[i]-((joint_target_accels[i]*joint_motion_period)>>16); - if(joint_target_speeds[i]<0) - joint_target_speeds[i]=0; - joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i]; - joint_current_angles[i]=joint_current_angles[i]+((joint_current_speeds[i]*joint_motion_period)>>17); - if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i]) - joint_current_angles[i]=joint_target_angles[i]; - if(joint_dir[i]==-1 && joint_current_angles[i]<joint_target_angles[i]) - joint_current_angles[i]=joint_target_angles[i]; - } - } - } - else - { - joint_current_speeds[i]=0; - } - manager_current_angles[i]=joint_current_angles[i]; - manager_current_slopes[i]=5; - } - } - } - if(moving==0) - { - joint_moving[j]=0x00; - joint_stop[j]=0x00; - ram_data[DARWIN_JOINT_GRP0_CNTRL+j*5]&=(~JOINT_STATUS); - } - } - } -} - diff --git a/src/stairs.c b/src/stairs.c deleted file mode 100755 index 42555871998c5a3e25655b333e53ae049db7e11f..0000000000000000000000000000000000000000 --- a/src/stairs.c +++ /dev/null @@ -1,580 +0,0 @@ -#include "stairs.h" -#include "darwin_kinematics.h" -#include "darwin_math.h" -#include "ram.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} stairs_phase_t; - -// internal motion variables -float stairs_Z_stair_height; -float stairs_Z_overshoot; -float stairs_Y_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; -float stairs_period; -float stairs_shift_weight_left_time; -float stairs_rise_right_foot_time; -float stairs_advance_right_foot_time; -float stairs_contact_right_foot_time; -float stairs_shift_weight_right_time; -float stairs_rise_left_foot_time; -float stairs_advance_left_foot_time; -float stairs_contact_left_foot_time; -float stairs_center_weight_time; - -// control variables -uint8_t stairs_Ctrl_Running; -uint8_t stairs_up; - -// private functions - -// public functions -void stairs_init(uint16_t period_us) -{ - // initialize the internal motion variables - - - // initialize internal control variables - stairs_Time=0; - stairs_period=((float)period_us)/1000.0; - stairs_Ctrl_Running=0x00; - stairs_up=0x00; -} - -void stairs_set_period(uint16_t period_us) -{ - stairs_period=((float)period_us)/1000.0; -} - -uint16_t stairs_get_period(void) -{ - return (uint16_t)(stairs_period*1000); -} - -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)); - stairs_rise_right_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE2_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE2_TIME_H]<<8)); - stairs_advance_right_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE3_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE3_TIME_H]<<8)); - stairs_contact_right_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE4_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE4_TIME_H]<<8)); - stairs_shift_weight_right_time=((float)ram_data[DARWIN_STAIRS_PHASE5_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE5_TIME_H]<<8)); - stairs_rise_left_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE6_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE6_TIME_H]<<8)); - 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_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/720.0;// (r_shift/4)*(pi/180) - stairs_P_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_P_SHIFT]))*PI/720.0;// (p_shift/4)*(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; - stairs_up=up; -} - -void stairs_stop(void) -{ - stairs_Ctrl_Running=0x00; -} - -uint8_t is_climbing_stairs(void) -{ - if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS) - return 0x01; - else - return 0x00; -} - -uint8_t stairs_get_phase(void) -{ - return (int8_t)ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_PHASE; -} - -// motion manager interface functions -void stairs_process(void) -{ - float angle[14]={0},ep[12]={0}; - float delta; - - if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS) - { - ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_PHASE); - if(stairs_up) - { - 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+2.0*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_stair_height; - ep[9]=stairs_R_Offset-stairs_R_shift_amplitude+2.0*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)*delta; - ep[3]=stairs_R_Offset+stairs_R_shift_amplitude; - 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_stair_height; - ep[9]=stairs_R_Offset+stairs_R_shift_amplitude; - 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; - ep[4]=stairs_P_Offset-2.0*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]=stairs_R_Offset+stairs_R_shift_amplitude; - ep[10]=stairs_P_Offset-2.0*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*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; - 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 - { - 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+2.0*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_overshoot; - ep[9]=stairs_R_Offset-stairs_R_shift_amplitude+2.0*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; - ep[3]=stairs_R_Offset+stairs_R_shift_amplitude; - ep[4]=stairs_P_Offset-1.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; - ep[10]=stairs_P_Offset-1.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-1.0*stairs_P_shift_amplitude+1.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-1.0*stairs_P_shift_amplitude+1.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+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+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) - stairs_Time += stairs_period; - else - stairs_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; - manager_current_slopes[R_HIP_YAW]=5; - } - if(manager_get_module(R_HIP_ROLL)==MM_WALKING) - { - manager_current_angles[R_HIP_ROLL]=((-180.0*angle[1])/PI)*65536.0; - manager_current_slopes[R_HIP_ROLL]=5; - } - if(manager_get_module(R_HIP_PITCH)==MM_WALKING) - { - manager_current_angles[R_HIP_PITCH]=((180.0*angle[2])/PI-stairs_Hip_Pitch_Offset)*65536.0; - manager_current_slopes[R_HIP_PITCH]=5; - } - if(manager_get_module(R_KNEE)==MM_WALKING) - { - manager_current_angles[R_KNEE]=((180.0*angle[3])/PI)*65536.0; - manager_current_slopes[R_KNEE]=5; - } - if(manager_get_module(R_ANKLE_PITCH)==MM_WALKING) - { - manager_current_angles[R_ANKLE_PITCH]=((-180.0*angle[4])/PI)*65536.0; - manager_current_slopes[R_ANKLE_PITCH]=5; - } - if(manager_get_module(R_ANKLE_ROLL)==MM_WALKING) - { - manager_current_angles[R_ANKLE_ROLL]=((180.0*angle[5])/PI)*65536.0; - manager_current_slopes[R_ANKLE_ROLL]=5; - } - if(manager_get_module(L_HIP_YAW)==MM_WALKING) - { - manager_current_angles[L_HIP_YAW]=((-180.0*angle[6])/PI)*65536.0; - manager_current_slopes[L_HIP_YAW]=5; - } - if(manager_get_module(L_HIP_ROLL)==MM_WALKING) - { - manager_current_angles[L_HIP_ROLL]=((-180.0*angle[7])/PI)*65536.0; - manager_current_slopes[L_HIP_ROLL]=5; - } - if(manager_get_module(L_HIP_PITCH)==MM_WALKING) - { - manager_current_angles[L_HIP_PITCH]=((-180.0*angle[8])/PI+stairs_Hip_Pitch_Offset)*65536.0; - manager_current_slopes[L_HIP_PITCH]=5; - } - if(manager_get_module(L_KNEE)==MM_WALKING) - { - manager_current_angles[L_KNEE]=((-180.0*angle[9])/PI)*65536.0; - manager_current_slopes[L_KNEE]=5; - } - if(manager_get_module(L_ANKLE_PITCH)==MM_WALKING) - { - manager_current_angles[L_ANKLE_PITCH]=((180.0*angle[10])/PI)*65536.0; - manager_current_slopes[L_ANKLE_PITCH]=5; - } - if(manager_get_module(L_ANKLE_ROLL)==MM_WALKING) - { - manager_current_angles[L_ANKLE_ROLL]=((180.0*angle[11])/PI)*65536.0; - manager_current_slopes[L_ANKLE_ROLL]=5; - } - } -} - -// operation functions -uint8_t stairs_in_range(unsigned short int address, unsigned short int length) -{ - if(ram_in_window(STAIRS_BASE_ADDRESS,STAIRS_MEM_LENGTH,address,length) || - ram_in_window(STAIRS_EEPROM_ADDRESS,STAIRS_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_STAIRS_CNTRL,address,length)) - { - 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(); - } - for(i=STAIRS_EEPROM_ADDRESS;i<=STAIRS_EEPROM_ADDRESS+STAIRS_EEPROM_LENGTH;i++) - if(ram_in_range(i,address,length)) - ram_data[i]=data[i-address]; -} - diff --git a/src/walking.c b/src/walking.c deleted file mode 100755 index 57277dec02844b3cb7f8eb400228833f35e35f30..0000000000000000000000000000000000000000 --- a/src/walking.c +++ /dev/null @@ -1,515 +0,0 @@ -#include "walking.h" -#include "darwin_kinematics.h" -#include "darwin_math.h" -#include "ram.h" -#include <math.h> - -enum {PHASE0=0x00,PHASE1=0x40,PHASE2=0x80,PHASE3=0xC0}; - -// Walking control -uint8_t A_MOVE_AIM_ON=0x00; - -// internal motion variables -float m_X_Swap_Phase_Shift; -float m_X_Swap_Amplitude; -float m_X_Swap_Amplitude_Shift; -float m_X_Move_Phase_Shift; -float m_X_Move_Amplitude; -float m_X_Move_Amplitude_Shift; -float m_Y_Swap_Phase_Shift; -float m_Y_Swap_Amplitude; -float m_Y_Swap_Amplitude_Shift; -float m_Y_Move_Phase_Shift; -float m_Y_Move_Amplitude; -float m_Y_Move_Amplitude_Shift; -float m_Z_Swap_Phase_Shift; -float m_Z_Swap_Amplitude; -float m_Z_Swap_Amplitude_Shift; -float m_Z_Move_Phase_Shift; -float m_Z_Move_Amplitude; -float m_Z_Move_Amplitude_Shift; -float m_A_Move_Phase_Shift; -float m_A_Move_Amplitude; -float m_A_Move_Amplitude_Shift; - -// internal offset variables -float m_Hip_Pitch_Offset; -float m_Pelvis_Offset; -float m_Pelvis_Swing; -float m_Arm_Swing_Gain; -float m_X_Offset; -float m_Y_Offset; -float m_Z_Offset; -float m_R_Offset; -float m_P_Offset; -float m_A_Offset; - -// internal time variables -float m_Time; -float m_PeriodTime; -float m_X_Swap_PeriodTime; -float m_X_Move_PeriodTime; -float m_Y_Swap_PeriodTime; -float m_Y_Move_PeriodTime; -float m_Z_Swap_PeriodTime; -float m_Z_Move_PeriodTime; -float m_A_Move_PeriodTime; -float m_SSP_Time_Start_L; -float m_SSP_Time_End_L; -float m_SSP_Time_Start_R; -float m_SSP_Time_End_R; -float m_Phase_Time1; -float m_Phase_Time2; -float m_Phase_Time3; - -// control variables -uint8_t m_Ctrl_Running; -float walking_period; - -// private functions -float wsin(float time, float period, float period_shift, float mag, float mag_shift) -{ - return mag*sin(((2.0*PI*time)/period)-period_shift)+mag_shift; -} - -void update_param_time() -{ - float m_SSP_Ratio; - float m_SSP_Time; - - m_PeriodTime=((float)((int16_t)(ram_data[DARWIN_WALK_PERIOD_TIME_L]+(ram_data[DARWIN_WALK_PERIOD_TIME_H]<<8)))); - m_SSP_Ratio=1.0-((float)ram_data[DARWIN_WALK_DSP_RATIO])/256.0; - - m_SSP_Time=m_PeriodTime*m_SSP_Ratio; - - m_X_Swap_PeriodTime=m_PeriodTime/2.0; - m_X_Move_PeriodTime=m_SSP_Time; - m_Y_Swap_PeriodTime=m_PeriodTime; - m_Y_Move_PeriodTime=m_SSP_Time; - m_Z_Swap_PeriodTime=m_PeriodTime/2.0; - m_Z_Move_PeriodTime=m_SSP_Time/2.0; - m_A_Move_PeriodTime=m_SSP_Time; - - m_SSP_Time_Start_L=(1-m_SSP_Ratio)*m_PeriodTime/4.0; - m_SSP_Time_End_L=(1+m_SSP_Ratio)*m_PeriodTime/4.0; - m_SSP_Time_Start_R=(3-m_SSP_Ratio)*m_PeriodTime/4.0; - m_SSP_Time_End_R=(3+m_SSP_Ratio)*m_PeriodTime/4.0; - - m_Phase_Time1=(m_SSP_Time_End_L+m_SSP_Time_Start_L)/2.0; - m_Phase_Time2=(m_SSP_Time_Start_R+m_SSP_Time_End_L)/2.0; - m_Phase_Time3=(m_SSP_Time_End_R+m_SSP_Time_Start_R)/2.0; - - // m_pelvis_offset and m_pelvis_Swing in degrees - m_Pelvis_Offset=((float)ram_data[DARWIN_WALK_PELVIS_OFFSET])*PI/1440.0; - m_Pelvis_Swing=m_Pelvis_Offset*0.35; - m_Arm_Swing_Gain=((float)ram_data[DARWIN_WALK_ARM_SWING_GAIN])/32.0; -} - -void update_param_move() -{ - float target_x_amplitude; - float target_y_amplitude; - float target_a_amplitude; - static float current_x_amplitude=0; - static float current_y_amplitude=0; - static float current_a_amplitude=0; - - target_x_amplitude=((float)((int8_t)ram_data[DARWIN_WALK_STEP_FW_BW])); - // change longitudinal and transversal velocities to get to the target ones - if(current_x_amplitude<target_x_amplitude) - { - current_x_amplitude+=(((float)ram_data[DARWIN_WALK_MAX_VEL])*m_PeriodTime)/1000.0; - if(current_x_amplitude>target_x_amplitude) - current_x_amplitude=target_x_amplitude; - } - else if(current_x_amplitude>target_x_amplitude) - { - current_x_amplitude-=(((float)ram_data[DARWIN_WALK_MAX_VEL])*m_PeriodTime)/1000.0; - if(current_x_amplitude<target_x_amplitude) - current_x_amplitude=target_x_amplitude; - } - m_X_Move_Amplitude=current_x_amplitude; - m_X_Swap_Amplitude=current_x_amplitude*((float)ram_data[DARWIN_WALK_STEP_FW_BW_RATIO])/256.0; - - // Right/Left - target_y_amplitude=((float)((int8_t)ram_data[DARWIN_WALK_STEP_LEFT_RIGHT])); - if(current_y_amplitude<target_y_amplitude) - { - current_y_amplitude+=(((float)ram_data[DARWIN_WALK_MAX_VEL])*m_PeriodTime)/1000.0; - if(current_y_amplitude>target_y_amplitude) - current_y_amplitude=target_y_amplitude; - } - else if(current_y_amplitude>target_y_amplitude) - { - current_y_amplitude-=(((float)ram_data[DARWIN_WALK_MAX_VEL])*m_PeriodTime)/1000.0; - if(current_y_amplitude<target_y_amplitude) - current_y_amplitude=target_y_amplitude; - } - m_Y_Move_Amplitude=current_y_amplitude/2.0; - if(m_Y_Move_Amplitude>0) - m_Y_Move_Amplitude_Shift=m_Y_Move_Amplitude; - else - m_Y_Move_Amplitude_Shift=-m_Y_Move_Amplitude; - m_Y_Swap_Amplitude=((float)ram_data[DARWIN_WALK_SWING_RIGHT_LEFT])+m_Y_Move_Amplitude_Shift*0.04; - - m_Z_Move_Amplitude=((float)ram_data[DARWIN_WALK_FOOT_HEIGHT])/2.0; - m_Z_Move_Amplitude_Shift=m_Z_Move_Amplitude / 2.0; - m_Z_Swap_Amplitude=((float)ram_data[DARWIN_WALK_SWING_TOP_DOWN]); - m_Z_Swap_Amplitude_Shift=m_Z_Swap_Amplitude; - - // Direction - target_a_amplitude=((float)((int8_t)ram_data[DARWIN_WALK_STEP_DIRECTION]))/8.0; - if(current_a_amplitude<target_a_amplitude) - { - current_a_amplitude+=(((float)ram_data[DARWIN_WALK_MAX_ROT_VEL])*m_PeriodTime)/8000.0; - if(current_a_amplitude>target_a_amplitude) - current_a_amplitude=target_a_amplitude; - } - else if(current_a_amplitude>target_a_amplitude) - { - current_a_amplitude-=(((float)ram_data[DARWIN_WALK_MAX_ROT_VEL])*m_PeriodTime)/8000.0; - if(current_a_amplitude<target_a_amplitude) - current_a_amplitude=target_a_amplitude; - } - - if(A_MOVE_AIM_ON==0x00) - { - m_A_Move_Amplitude=current_a_amplitude*PI/180.0/2.0; - if(m_A_Move_Amplitude>0) - m_A_Move_Amplitude_Shift=m_A_Move_Amplitude; - else - m_A_Move_Amplitude_Shift=-m_A_Move_Amplitude; - } - else - { - m_A_Move_Amplitude=-current_a_amplitude*PI/180.0/2.0; - if(m_A_Move_Amplitude>0) - m_A_Move_Amplitude_Shift=-m_A_Move_Amplitude; - else - m_A_Move_Amplitude_Shift=m_A_Move_Amplitude; - } -} - -void update_param_balance() -{ - m_X_Offset = ((float)((int8_t)ram_data[DARWIN_WALK_X_OFFSET])); - m_Y_Offset = ((float)((int8_t)ram_data[DARWIN_WALK_Y_OFFSET])); - m_Z_Offset = ((float)((int8_t)ram_data[DARWIN_WALK_Z_OFFSET])); - m_R_Offset = ((float)((int8_t)ram_data[DARWIN_WALK_ROLL_OFFSET]))*PI/1440.0;// (r_offset/8)*(pi/180) - m_P_Offset = ((float)((int8_t)ram_data[DARWIN_WALK_PITCH_OFFSET]))*PI/1440.0;// (p_offset/8)*(pi/180) - m_A_Offset = ((float)((int8_t)ram_data[DARWIN_WALK_YAW_OFFSET]))*PI/1440.0;// (a_offset/8)*(pi/180) - m_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; -} - -// public functions -void walking_init(uint16_t period_us) -{ - // initialize the internal motion variables - m_X_Swap_Phase_Shift=PI; - m_X_Swap_Amplitude_Shift=0.0; - m_X_Move_Phase_Shift=PI/2.0; - m_X_Move_Amplitude=0.0; - m_X_Move_Amplitude_Shift=0.0; - m_Y_Swap_Phase_Shift=0.0; - m_Y_Swap_Amplitude_Shift=0.0; - m_Y_Move_Phase_Shift=PI/2.0; - m_Z_Swap_Phase_Shift=PI*3.0/2.0; - m_Z_Move_Phase_Shift=PI/2.0; - m_A_Move_Phase_Shift=PI/2.0; - - // initialize internal control variables - m_Time=0; - walking_period=((float)period_us)/1000.0; - m_Ctrl_Running=0x00; - - update_param_time(); - update_param_move(); - - walking_process(); -} - -void walking_set_period(uint16_t period_us) -{ - walking_period=((float)period_us)/1000.0; -} - -uint16_t walking_get_period(void) -{ - return (uint16_t)(walking_period*1000); -} - -void walking_start(void) -{ - ram_data[DARWIN_WALK_CNTRL]|=WALK_STATUS; - m_Ctrl_Running=0x01; -} - -void walking_stop(void) -{ - m_Ctrl_Running=0x00; -} - -uint8_t is_walking(void) -{ - if(ram_data[DARWIN_WALK_CNTRL]&WALK_STATUS) - return 0x01; - else - return 0x00; -} - -// motion manager interface functions -void walking_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-walking_period/2.0) && m_Time<(m_Phase_Time1+walking_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-walking_period/2.0) && m_Time<(m_Phase_Time2+walking_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-walking_period/2.0) && m_Time<(m_Phase_Time3+walking_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 += walking_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 walking_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 walking_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data) -{ - -} - -void walking_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) - walking_start(); - if(data[DARWIN_WALK_CNTRL-address]&WALK_STOP) - walking_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]; -} -