diff --git a/Makefile b/Makefile
index c448792639103ebb9b6d61828c9f1ce867092dce..b40c7b58328c9719c15036efcb108742f6729c8e 100755
--- a/Makefile
+++ b/Makefile
@@ -14,6 +14,13 @@ TARGET_FILES+=src/imu.c
TARGET_FILES+=src/darwin_dyn_slave.c
TARGET_FILES+=src/darwin_dyn_master.c
TARGET_FILES+=src/stm32f1xx_hal_msp.c
+TARGET_FILES+=src/motion_manager.c
+TARGET_FILES+=src/action.c
+TARGET_FILES+=src/motion_pages.c
+TARGET_FILES+=src/walking.c
+TARGET_FILES+=src/darwin_math.c
+TARGET_FILES+=src/darwin_kinematics.c
+
TARGET_PROCESSOR=STM32F103RE
HAL_PATH=../../STM32_processor/hal/f1
@@ -95,7 +102,7 @@ $(BUILD_PATH)/%.o: $(USART_PATH)/src/%.c
$(CC) -c $(CFLAGS) -o $@ $<
$(MAIN_OUT_ELF): mkdir_build $(DARWIN_OBJS) $(BUID_PATH)/$(STARTUP_FILE:.s=.o)
- $(LD) $(LDFLAGS) $(DARWIN_OBJS) $(BUILD_PATH)/$(STARTUP_FILE:.s=.o) $(EXT_LIB) --output $@
+ $(LD) $(LDFLAGS) $(DARWIN_OBJS) $(BUILD_PATH)/$(STARTUP_FILE:.s=.o) $(EXT_LIB) -lm --output $@
$(MAIN_OUT_HEX): $(MAIN_OUT_ELF)
$(OBJCP) $(OBJCPFLAGS_HEX) $< $@
diff --git a/include/action.h b/include/action.h
index afaa86108ce32495aee2d520c819f092ff71221e..98626f1bbc5bd44544809bb9a1e6b935ecdd1e55 100755
--- a/include/action.h
+++ b/include/action.h
@@ -1,18 +1,34 @@
#ifndef _ACTION_H
#define _ACTION_H
-#include "stm32f10x.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f1xx.h"
+#include "motion_pages.h"
// public functions
-void action_init(uint16_t period);
-inline void action_set_period(uint16_t period);
+void action_init(uint16_t period_us);
+inline void action_set_period(uint16_t period_us);
inline 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/action_id.h b/include/action_id.h
new file mode 100644
index 0000000000000000000000000000000000000000..10c36280ec7962672eb397fab14b03b0e67b370f
--- /dev/null
+++ b/include/action_id.h
@@ -0,0 +1,148 @@
+#ifndef _ACTION_ID_H
+#define _ACTION_ID_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define BOW 1
+#define BRAVO 2
+#define RAP_CHEST 5
+#define SCRATCH_HEAD 8
+#define HAND_STANDING 11
+#define R_BLOCKING 14
+#define L_BLOCKING 16
+#define L_KICK 18
+#define R_KICK 19
+#define R_ATTACK 20
+#define L_ATTACK 21
+#define F_ATTACK 22
+#define DEFENCE 23
+#define SIT_DOWN 25
+#define STAND_UP 26
+#define F_GET_UP 27
+#define B_GET_UP 28
+#define CLAP_READY 29
+#define CLAPPING 30
+#define WALK_READY 31
+/* forward step */
+#define F_S_L 32
+#define F_S_R 34
+#define F_M_L 36
+#define F_M_R 38
+#define F_E_L 40
+#define F_E_R 42
+/* backward step */
+#define B_S_L 44
+#define B_S_R 46
+#define B_M_L 48
+#define B_M_R 50
+#define B_E_L 52
+#define B_E_R 54
+/* left turn step */
+#define LT_S_L 56
+#define LT_S_R 58
+#define LT_M_L 60
+#define LT_M_R 62
+#define LT_E_L 64
+#define LT_E_R 66
+/* right turn step */
+#define RT_S_L 68
+#define RT_S_R 70
+#define RT_M_L 72
+#define RT_M_R 74
+#define RT_E_L 76
+#define RT_E_R 78
+/* left step */
+#define L_S_L 80
+#define L_S_R 82
+#define L_M_L 84
+#define L_M_R 86
+#define L_E_L 88
+#define L_E_R 90
+/* right step */
+#define R_S_L 92
+#define R_S_R 94
+#define R_M_L 96
+#define R_M_R 98
+#define R_E_L 100
+#define R_E_R 102
+/* forward left turn step */
+#define FLT_S_L 104
+#define FLT_S_R 106
+#define FLT_M_L 108
+#define FLT_M_R 110
+#define FLT_E_L 112
+#define FLT_E_R 114
+/* forward right turn step */
+#define FRT_S_L 116
+#define FRT_S_R 118
+#define FRT_M_L 120
+#define FRT_M_R 122
+#define FRT_E_L 124
+#define FRT_E_R 126
+/* backward left turn step */
+#define BLT_S_L 128
+#define BLT_S_R 130
+#define BLT_M_L 132
+#define BLT_M_R 134
+#define BLT_E_L 136
+#define BLT_E_R 138
+/* backward right turn step */
+#define BRT_S_L 140
+#define BRT_S_R 142
+#define BRT_M_L 144
+#define BRT_M_R 146
+#define BRT_E_L 148
+#define BRT_E_R 150
+/* left turn A??? step */
+#define LTA_S_L 152
+#define LTA_S_R 154
+#define LTA_M_L 156
+#define LTA_M_R 158
+#define LTA_E_L 160
+#define LTA_E_R 162
+/* right turn A?? step */
+#define RTA_S_L 164
+#define RTA_S_R 166
+#define RTA_M_L 168
+#define RTA_M_R 170
+#define RTA_E_L 172
+#define RTA_E_R 174
+/* forward left step */
+#define FL_S_L 176
+#define FL_S_R 178
+#define FL_M_L 180
+#define FL_M_R 182
+#define FL_E_L 184
+#define FL_E_R 186
+/* forward right step */
+#define FR_S_L 188
+#define FR_S_R 190
+#define FR_M_L 192
+#define FR_M_R 194
+#define FR_E_L 196
+#define FR_E_R 198
+/* backward left step */
+#define BL_S_L 200
+#define BL_S_R 202
+#define BL_M_L 204
+#define BL_M_R 206
+#define BL_E_L 208
+#define BL_E_R 210
+/* backward right step */
+#define BR_S_L 212
+#define BR_S_R 214
+#define BR_M_L 216
+#define BR_M_R 218
+#define BR_E_L 220
+#define BR_E_R 222
+
+#define BALANCE 224
+#define CLAP_END 225
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/include/darwin_dyn_master.h b/include/darwin_dyn_master.h
index 47f387a0fc9f10f2c926cdb6d3fc5ab8f1b43ddf..8031f12a3c6a3b3ec81f4cea5d40c54a19bdcfab 100644
--- a/include/darwin_dyn_master.h
+++ b/include/darwin_dyn_master.h
@@ -1,8 +1,11 @@
#ifndef _DARWIN_DYN_MASTER_H
#define _DARWIN_DYN_MASTER_H
-#include "stm32f1xx_hal.h"
-#include "stm32_time.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f1xx.h"
#include "comm.h"
#include "dynamixel_master.h"
@@ -12,5 +15,9 @@ void darwin_dyn_master_init(void);
inline void darwin_dyn_master_enable_power(void);
inline void darwin_dyn_master_disable_power(void);
+#ifdef __cplusplus
+}
+#endif
+
#endif
diff --git a/include/darwin_kinematics.h b/include/darwin_kinematics.h
index 40c2dabe808bff6ce3ddb4b82f591f49488d8b7d..7f67ffa092131a028b2f3ca17c120f1c7bb24d2e 100755
--- a/include/darwin_kinematics.h
+++ b/include/darwin_kinematics.h
@@ -1,15 +1,25 @@
#ifndef _DARWIN_KINEMATICS_H
#define _DARWIN_KINEMATICS_H
-#include "stm32f10x.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f1xx.h"
-extern const float LEG_SIDE_OFFSET; //mm
-extern const float THIGH_LENGTH; //mm
-extern const float CALF_LENGTH; //mm
-extern const float ANKLE_LENGTH; //mm
-extern const float LEG_LENGTH; //mm (THIGH_LENGTH + CALF_LENGTH + ANKLE_LENGTH)
+#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_PITCH_OFFSET 0.1910 //rad
+#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 computeIK(float *out, float x, float y, float z, float a, float b, float c);
+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
index 6fd0f81afa2ad7ff0a408fcb4b8c2d41537c965d..82ef927c3d8c6a225cfe6c18086c37e10ea5d4a6 100755
--- a/include/darwin_math.h
+++ b/include/darwin_math.h
@@ -1,8 +1,11 @@
#ifndef _DARWIN_MATH_H
#define _DARWIN_MATH_H
-#include "stm32f10x.h"
-#include <math.h>
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f1xx.h"
#define PI 3.14159
@@ -60,4 +63,8 @@ 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/darwin_registers.h b/include/darwin_registers.h
index 3a579afe35a99f43d5dc7118db5ec11027c3fc1f..a9b994e3cd5ba18ac01f6612fffebf927098724e 100644
--- a/include/darwin_registers.h
+++ b/include/darwin_registers.h
@@ -331,22 +331,14 @@ typedef enum {
DARWIN_MM_SERVO31_CUR_POS_L = 0x0178, // angle in fixed point format 9|7
DARWIN_MM_SERVO31_CUR_POS_H = 0x0179,
DARWIN_ACTION_PAGE = 0x017A,
- DARWIN_ACTION_CNTRL = 0x017B, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | | | page running | interrupt flag | enable interrupt | stop page | start page
- DARWIN_GYRO_CNTRL = 0x017C, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | | | | enable fall int | enable fall det | enable calibrate int | calibrate
- DARWIN_GYRO_STATUS = 0x017D, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- //
- DARWIN_GYRO_FWD_FALL_THD = 0x017E,
- DARWIN_GYRO_BWD_FALL_THD = 0x017F,
- DARWIN_GYRO_LEFT_FALL_THD = 0x0180,
- DARWIN_GYRO_RIGHT_FALL_THD = 0x0181,
- DARWIN_WALK_CNTRL = 0x0182, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ DARWIN_ACTION_CNTRL = 0x017B, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | page running | | | stop page | start page
+ DARWIN_WALK_CNTRL = 0x017C, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// current phase walking stop walking start walking
- DARWIN_WALK_STEP_FW_BW = 0x0183,
- DARWIN_WALK_STEP_LEFT_RIGHT = 0x0184,
- DARWIN_WALK_STEP_DIRECTION = 0x0185
-} bioloid_registers;
+ DARWIN_WALK_STEP_FW_BW = 0x017D,
+ DARWIN_WALK_STEP_LEFT_RIGHT = 0x017E,
+ DARWIN_WALK_STEP_DIRECTION = 0x017F
+} darwin_registers;
#define GPIO_BASE_ADDRESS 0x0100
#define GPIO_MEM_LENGTH 23
diff --git a/include/motion_manager.h b/include/motion_manager.h
index 0d6c86fee6bc0b32ac3de4635d26007d11063a61..04caa19e1c496f5fe60de50f61349fdbeec819ce 100755
--- a/include/motion_manager.h
+++ b/include/motion_manager.h
@@ -1,19 +1,13 @@
#ifndef _MOTION_MANAGER_H
#define _MOTION_MANAGER_H
-#include "stm32f10x.h"
-
-#define MANAGER_MAX_NUM_SERVOS 31
+#ifdef __cplusplus
+extern "C" {
+#endif
-typedef enum{MM_NONE = 0,
- MM_ACTION = 1,
- MM_WALKING = 2,
- MM_JOINTS = 3,
- MM_HEAD = 4,
- MM_GRIPPER = 5} TModules;
+#include "stm32f1xx.h"
-// default joint identifiers
-enum{
+typedef enum {
R_SHOULDER_PITCH = 1,
L_SHOULDER_PITCH = 2,
R_SHOULDER_ROLL = 3,
@@ -32,13 +26,23 @@ enum{
L_ANKLE_PITCH = 16,
R_ANKLE_ROLL = 17,
L_ANKLE_ROLL = 18,
- HEAD_PAN = 19,
- HEAD_TILT = 20,
- R_WRIST = 23,
- L_WRIST = 22,
- R_GRIPPER = 21,
- L_GRIPPER = 24
-};
+ L_PAN = 19,
+ L_TILT = 20} servo_id_t;
+
+
+typedef enum {MM_NONE = 0,
+ MM_ACTION = 1,
+ MM_WALKING = 2,
+ MM_JOINTS = 3} TModules;
+
+typedef struct
+{
+ void (*process_fnct)(void);
+ void (*init_fnct)(void);
+ uint8_t name[16];
+}TMotionModule;
+
+#define MAX_MOTION_MODULES 8
// servo information structure
typedef struct{
@@ -50,6 +54,8 @@ typedef struct{
uint16_t center_angle;
uint16_t max_speed;
int16_t current_angle;
+ uint8_t cw_comp;
+ uint8_t ccw_comp;
uint16_t current_value;
TModules module;
uint8_t enabled;
@@ -58,12 +64,16 @@ typedef struct{
uint8_t dyn_version;
}TServoInfo;
+#define MANAGER_MAX_NUM_SERVOS 31
+
// public variables
-extern int64_t current_angles[MANAGER_MAX_NUM_SERVOS];
+extern int64_t manager_current_angles[MANAGER_MAX_NUM_SERVOS];
+extern int8_t manager_current_slopes[MANAGER_MAX_NUM_SERVOS];
// public functions
void manager_init(uint16_t period_us);
inline uint16_t manager_get_period(void);
+inline uint16_t manager_get_period_us(void);
inline void manager_set_period(uint16_t period_us);
inline void manager_enable(void);
inline void manager_disable(void);
@@ -71,18 +81,21 @@ inline uint8_t manager_is_enabled(void);
void manager_enable_balance(void);
void manager_disable_balance(void);
inline uint8_t manager_get_num_servos(void);
-inline void manager_set_module(uint8_t servo_id,TModules module);
+void manager_set_module(uint8_t servo_id,TModules module);
inline TModules manager_get_module(uint8_t servo_id);
inline void manager_enable_servo(uint8_t servo_id);
inline void manager_disable_servo(uint8_t servo_id);
-void manager_disable_servos(void);
inline uint8_t manager_is_servo_enabled(uint8_t servo_id);
+void manager_set_offset(uint8_t servo_id,int8_t offset);
inline int16_t manager_get_cw_angle_limit(uint8_t servo_id);
inline int16_t manager_get_ccw_angle_limit(uint8_t servo_id);
-void manager_set_knee_gain(uint8_t gain);
-void manager_set_ankle_pitch_gain(uint8_t gain);
-void manager_set_hip_roll_gain(uint8_t gain);
-void manager_set_ankle_roll_gain(uint8_t gain);
-uint8_t manager_get_dyn_version(uint8_t servo_id);
+// motion modules handling
+// operation functions
+uint8_t manager_in_range(unsigned short int address, unsigned short int length);
+void manager_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
+
+#ifdef __cplusplus
+}
+#endif
#endif
diff --git a/include/motion_pages.h b/include/motion_pages.h
index 5d3f3fea6f051266bda506fceefd68c115963d88..b4530de6acd85223c6c8763ca0786b75338b113a 100755
--- a/include/motion_pages.h
+++ b/include/motion_pages.h
@@ -1,7 +1,11 @@
#ifndef _MOTION_PAGES_H
#define _MOTION_PAGES_H
-#include "stm32f10x.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f1xx.h"
#define MAX_PAGES 256
#define PAGE_MAX_NUM_SERVOS 31
@@ -48,4 +52,8 @@ uint8_t pages_check_checksum(TPage *page);
void pages_clear_page(TPage *page);
void pages_copy_page(TPage *src,TPage *dst);
+#ifdef __cplusplus
+}
+#endif
+
#endif
diff --git a/include/walking.h b/include/walking.h
index da933a5efbd30306b01a35ca7e868b4c01d75b6d..4e0975562df333d8a93448f32ea4aadc18299535 100755
--- a/include/walking.h
+++ b/include/walking.h
@@ -1,18 +1,30 @@
#ifndef _WALKING_H
#define _WALKING_H
-#include "stm32f10x.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f1xx.h"
+#include "motion_manager.h"
// public functions
-void walking_init(uint16_t period);
-inline void walking_set_period(uint16_t period);
+void walking_init(uint16_t period_us);
+inline void walking_set_period(uint16_t period_us);
inline uint16_t walking_get_period(void);
-void walking_set_param(uint8_t param_id,int8_t param_value);
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
index 67ff5ba94832bf05191203d3cb5495eec66b18f2..b7e2c653799b90a2cfccf940a0b62fa5d7514705 100755
--- a/src/action.c
+++ b/src/action.c
@@ -12,6 +12,7 @@ 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
@@ -32,12 +33,12 @@ 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 uint8_t current_index=0;
static int8_t num_repetitions=0;
// angle variables
int16_t next_angle;// information from the flash memory (fixed point 9|7 format)
@@ -68,7 +69,7 @@ void action_load_next_step(void)
{
num_repetitions--;
if(num_repetitions>0)
- action_next_index=current_index;
+ action_next_index=action_current_page_index;
else
action_next_index=action_current_page.header.next;
}
@@ -76,9 +77,10 @@ void action_load_next_step(void)
action_end=0x01;
else
{
- if(action_next_index!=current_index)
+ 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;
}
@@ -87,10 +89,10 @@ void action_load_next_step(void)
}
}
pages_copy_page(&action_next_page,&action_current_page);// make the next page active
- if(current_index!=action_next_index)
+ if(action_current_page_index!=action_next_index)
num_repetitions=action_current_page.header.repeat;
action_current_step_index=0;
- current_index=action_next_index;
+ action_current_page_index=action_next_index;
}
else if(action_current_step_index==action_current_page.header.stepnum-1)// it is the last step
{
@@ -100,7 +102,7 @@ void action_load_next_step(void)
{
num_repetitions--;
if(num_repetitions>0)
- action_next_index=current_index;
+ action_next_index=action_current_page_index;
else
action_next_index=action_current_page.header.next;
}
@@ -108,9 +110,10 @@ void action_load_next_step(void)
action_end=0x01;
else
{
- if(action_next_index!=current_index)
+ 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;
}
@@ -119,8 +122,10 @@ void action_load_next_step(void)
}
}
// compute trajectory
- action_pause_time=((action_current_page.steps[action_current_step_index].pause<<14)/action_current_page.header.speed);
- action_step_time=((action_current_page.steps[action_current_step_index].time<<14)/action_current_page.header.speed);
+ 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;
@@ -130,10 +135,10 @@ void action_load_next_step(void)
{
angle=action_current_page.steps[action_current_step_index].position[i];
if(angle==0x5A00)// bigger than 180
- target_angles=current_angles[i];
+ target_angles=manager_current_angles[i];
else
target_angles=angle<<9;
- action_moving_angles[i]=target_angles-current_angles[i];
+ action_moving_angles[i]=target_angles-manager_current_angles[i];
if(action_end)
next_angles=target_angles;
else
@@ -148,8 +153,8 @@ void action_load_next_step(void)
next_angles=next_angle<<9;
}
// check changes in direction
- if(((current_angles[i] < target_angles) && (target_angles < next_angles)) ||
- ((current_angles[i] > target_angles) && (target_angles > next_angles)))
+ 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;
@@ -169,6 +174,7 @@ void action_load_next_step(void)
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)
@@ -213,17 +219,29 @@ void action_load_next_step(void)
}
}
+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)
+void action_init(uint16_t period_us)
{
unsigned char i;
- // init current_angles with the current position of the servos
+ // 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]=current_angles[i];
+ 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
@@ -233,6 +251,8 @@ void action_init(uint16_t period)
// 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;
@@ -247,17 +267,19 @@ void action_init(uint16_t period)
action_current_time=0;// fixed point 48|16 format
action_section_time=0;// fixed point 48|16 format
- action_period=period;
+ action_period=(period_us<<16)/1000000;
+ action_period_us=period_us;
}
-inline void action_set_period(uint16_t period)
+inline void action_set_period(uint16_t period_us)
{
- action_period=period;
+ action_period=(period_us<<16)/1000000;
+ action_period_us=period_us;
}
inline uint16_t action_get_period(void)
{
- return action_period;
+ return action_period_us;
}
uint8_t action_load_page(uint8_t page_id)
@@ -271,7 +293,7 @@ uint8_t action_load_page(uint8_t page_id)
return 0x00;
if(!pages_check_checksum(&action_next_page))
return 0x00;
- ram_write_byte(DARWIN_ACTION_PAGE,page_id);
+ ram_data[DARWIN_ACTION_PAGE]=page_id;
return 0x01;
}
@@ -281,20 +303,20 @@ void action_start_page(void)
uint8_t i;
for(i=0;i<PAGE_MAX_NUM_SERVOS;i++)
- action_start_angles[i]=current_angles[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_set_bit(DARWIN_ACTION_STATUS,0);
+ ram_data[DARWIN_ACTION_CNTRL]|=ACTION_STATUS;
+ /* clear the interrupt flag */
+ ram_data[DARWIN_ACTION_CNTRL]&=(~ACTION_INT_FLAG);
action_running=0x01;
- ram_set_bit(DARWIN_ACTION_CONTROL,0);
}
void action_stop_page(void)
{
action_stop=0x01;
- ram_set_bit(DARWIN_ACTION_CONTROL,1);
}
uint8_t action_is_running(void)
@@ -302,6 +324,34 @@ 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)
{
@@ -330,7 +380,7 @@ void action_process(void)
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;
- current_angles[i]=action_start_angles[i]+accel_angles[i];
+ manager_current_angles[i]=action_start_angles[i]+accel_angles[i];
/* update of the state */
if(!action_zero_speed_finish[i])
{
@@ -338,15 +388,15 @@ void action_process(void)
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]=current_angles[i];
+ 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]=current_angles[i];
+ action_start_angles[i]=manager_current_angles[i];
}
/* the first step of the main section */
- current_angles[i]=action_start_angles[i]+(main_angles[i]*(action_current_time-action_section_time))/(action_step_time-(action_pre_time<<1));
+ 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];
}
}
@@ -363,7 +413,7 @@ void action_process(void)
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;
- current_angles[i]=action_start_angles[i]+accel_angles[i];
+ manager_current_angles[i]=action_start_angles[i]+accel_angles[i];
}
}
state=ACTION_PRE;
@@ -376,21 +426,21 @@ void action_process(void)
if(manager_get_module(i)==MM_ACTION)
{
/* last segment of the main section */
- current_angles[i]=action_start_angles[i]+main_angles[i];
+ manager_current_angles[i]=action_start_angles[i]+main_angles[i];
current_speed[i]=action_main_speed[i];
/* update state */
- action_start_angles[i]=current_angles[i];
+ 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;
- 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);
+ 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
{
- current_angles[i]=action_start_angles[i]+((main_angles[i]*(action_current_time-action_section_time))/action_pre_time);
+ 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];
}
}
@@ -405,7 +455,7 @@ void action_process(void)
{
if(manager_get_module(i)==MM_ACTION)
{
- current_angles[i]=action_start_angles[i]+(main_angles[i]*action_current_time)/action_section_time;
+ manager_current_angles[i]=action_start_angles[i]+(main_angles[i]*action_current_time)/action_section_time;
current_speed[i]=action_main_speed[i];
}
}
@@ -423,15 +473,15 @@ void action_process(void)
{
delta_speed=-action_main_speed[i];
current_speed[i]=action_main_speed[i]+delta_speed;
- current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*action_section_time)+((delta_speed*action_section_time)>>1))>>16);
+ manager_current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*action_section_time)+((delta_speed*action_section_time)>>1))>>16);
}
else
{
- current_angles[i]=action_start_angles[i]+main_angles[i];
+ manager_current_angles[i]=action_start_angles[i]+main_angles[i];
current_speed[i]=action_main_speed[i];
}
/* update state */
- action_start_angles[i]=current_angles[i];
+ action_start_angles[i]=manager_current_angles[i];
action_start_speed[i]=current_speed[i];
}
}
@@ -440,11 +490,8 @@ void action_process(void)
{
if(action_end)
{
- action_load_next_step();
state=ACTION_PAUSE;
- action_end=0x00;
- ram_clear_bit(DARWIN_ACTION_STATUS,0);
- action_running=0x00;
+ action_finish();
}
else
{
@@ -457,7 +504,7 @@ void action_process(void)
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);
- current_angles[i]=action_start_angles[i]+accel_angles[i];
+ manager_current_angles[i]=action_start_angles[i]+accel_angles[i];
}
}
action_current_time=action_current_time-action_section_time;
@@ -482,11 +529,11 @@ void action_process(void)
{
delta_speed=((0.0-action_main_speed[i])*action_current_time)/action_section_time;
current_speed[i]=action_main_speed[i]+delta_speed;
- current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*action_current_time)+((delta_speed*action_current_time)>>1))>>16);
+ manager_current_angles[i]=action_start_angles[i]+(((action_main_speed[i]*action_current_time)+((delta_speed*action_current_time)>>1))>>16);
}
else
{
- current_angles[i]=action_start_angles[i]+(main_angles[i]*action_current_time)/action_section_time;
+ manager_current_angles[i]=action_start_angles[i]+(main_angles[i]*action_current_time)/action_section_time;
current_speed[i]=action_main_speed[i];
}
}
@@ -498,12 +545,8 @@ void action_process(void)
{
if(action_end)
{
- // find next page to execute
- action_load_next_step();
state=ACTION_PAUSE;
- action_end=0x00;
- ram_clear_bit(DARWIN_ACTION_STATUS,0);
- action_running=0x00;
+ action_finish();
}
else
{
@@ -515,7 +558,7 @@ void action_process(void)
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);
- current_angles[i]=action_start_angles[i]+accel_angles[i];
+ 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;
@@ -523,10 +566,35 @@ void action_process(void)
}
}
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/cm730_fw.c b/src/cm730_fw.c
index aa8df078d76384414ce592bdd32e52b34325b638..5679db03820b8b854ea451224f6aebedf1258039 100755
--- a/src/cm730_fw.c
+++ b/src/cm730_fw.c
@@ -6,10 +6,12 @@
#include "imu.h"
#include "darwin_time.h"
#include "darwin_dyn_slave.h"
-#include "darwin_dyn_master.h"
+#include "motion_manager.h"
int main(void)
{
+ uint16_t eeprom_data,period;
+
/* initialize the HAL module */
HAL_Init();
/* initialize the GPIO module */
@@ -27,17 +29,12 @@ int main(void)
/* initialize the dynamixel slave interface */
darwin_dyn_slave_init();
darwin_dyn_slave_start();
- /* initialize the dynamixel master interface */
- darwin_dyn_master_init();
-
-/* darwin_dyn_master_enable_power();
- HAL_Delay(1000);
- gpio_set_led(LED_3);
- if(dyn_master_ping(&darwin_dyn_master,0x01)==DYN_SUCCESS)
- gpio_set_led(LED_2);
- else
- gpio_clear_led(LED_2);
- darwin_dyn_master_disable_power();*/
+ /* initialize motion manager module */
+ EE_ReadVariable(MM_PERIOD_OFFSET,&eeprom_data);
+ period=eeprom_data&0x00FF;
+ EE_ReadVariable(MM_PERIOD_OFFSET+1,&eeprom_data);
+ period+=((eeprom_data&0x00FF)<<8);
+ manager_init(period);
while(1);/* main function does not return */
}
diff --git a/src/darwin_dyn_master.c b/src/darwin_dyn_master.c
index 13ef77880b64f8a8620fe75d33d5bce350c2b525..d55b13c8a87d614617273bd9aaf3bdad3af9671d 100755
--- a/src/darwin_dyn_master.c
+++ b/src/darwin_dyn_master.c
@@ -85,6 +85,10 @@ void darwin_dyn_master_init(void)
dyn_master_init(&darwin_dyn_master,&darwin_dyn_master_comm,DYN_VER1);
darwin_dyn_master.set_rx_mode=darwin_dyn_master_set_rx_mode;
darwin_dyn_master.set_tx_mode=darwin_dyn_master_set_tx_mode;
+
+ /* configure dynamixel master module */
+ dyn_master_set_rx_timeout(&darwin_dyn_master,50);
+ dyn_master_set_return_level(&darwin_dyn_master,return_all);
}
inline void darwin_dyn_master_enable_power(void)
diff --git a/src/darwin_dyn_slave.c b/src/darwin_dyn_slave.c
index e00f1952cf84b9a0e1579d0f4e43883c983eb08e..2970d520007dd8c45459d4fa3b01aa7f586e8ffd 100755
--- a/src/darwin_dyn_slave.c
+++ b/src/darwin_dyn_slave.c
@@ -6,6 +6,9 @@
#include "gpio.h"
#include "adc_dma.h"
#include "imu.h"
+#include "motion_manager.h"
+#include "action.h"
+#include "walking.h"
/* timer for the execution of the dynamixel slave loop */
#define DYN_SLAVE_TIMER TIM7
@@ -68,6 +71,15 @@ unsigned char darwin_on_write(unsigned short int address,unsigned short int leng
// IMU commands
if(imu_in_range(address,length))
imu_process_write_cmd(address,length,data);
+ // Manager commands
+ if(manager_in_range(address,length))
+ manager_process_write_cmd(address,length,data);
+ // action commands
+ if(action_in_range(address,length))
+ action_process_write_cmd(address,length,data);
+ // walk commands
+ if(walking_in_range(address,length))
+ walking_process_write_cmd(address,length,data);
// write eeprom
for(i=address,j=0;i<LAST_EEPROM_OFFSET && i<(address+length);i++,j++)
EE_WriteVariable(i,data[j]);
diff --git a/src/darwin_kinematics.c b/src/darwin_kinematics.c
index a7791fb1931fc7c328288863e0e5fdaffab7c2eb..a60ca577c8c7529d72d3d5d012f44eede9e65e10 100755
--- a/src/darwin_kinematics.c
+++ b/src/darwin_kinematics.c
@@ -1,13 +1,8 @@
#include "darwin_kinematics.h"
#include "darwin_math.h"
+#include <math.h>
-const float LEG_SIDE_OFFSET = 37.0; //mm
-const float THIGH_LENGTH = 93.0; //mm
-const float CALF_LENGTH = 93.0; //mm
-const float ANKLE_LENGTH = 33.5; //mm
-const float LEG_LENGTH = 219.5; //mm (THIGH_LENGTH + CALF_LENGTH + ANKLE_LENGTH)
-
-uint8_t computeIK(float *out, float x, float y, float z, float a, float b, float c)
+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;
@@ -15,16 +10,16 @@ uint8_t computeIK(float *out, float x, float y, float z, float a, float b, float
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 - LEG_LENGTH);
+ point3d_load(&position,x, y, z - DARWIN_LEG_LENGTH);
matrix3d_set_transform(&Tad,&position,&orientation);
- vec.x = x + Tad.coef[2] * ANKLE_LENGTH;
- vec.y = y + Tad.coef[6] * ANKLE_LENGTH;
- vec.z = (z - LEG_LENGTH) + Tad.coef[10] * ANKLE_LENGTH;
+ 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 - THIGH_LENGTH * THIGH_LENGTH - CALF_LENGTH * CALF_LENGTH) / (2 * THIGH_LENGTH * CALF_LENGTH));
+ _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;
@@ -34,8 +29,8 @@ uint8_t computeIK(float *out, float x, float y, float z, float a, float b, float
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] - ANKLE_LENGTH) * (Tda.coef[11] - ANKLE_LENGTH));
- _m = (_k * _k - _l * _l - ANKLE_LENGTH * ANKLE_LENGTH) / (2 * _l * ANKLE_LENGTH);
+ _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)
@@ -49,7 +44,7 @@ uint8_t computeIK(float *out, float x, float y, float z, float a, float b, float
*(out + 5) = _Acos;
// Get Hip Yaw
vector3d_load(&orientation,*(out + 5) * 180.0 / PI, 0, 0);
- point3d_load(&position,0, 0, -ANKLE_LENGTH);
+ point3d_load(&position,0, 0, -DARWIN_ANKLE_LENGTH);
matrix3d_set_transform(&Tcd,&position,&orientation);
Tdc = Tcd;
if(matrix3d_inverse(&Tdc,&Tdc) == 0x00)
@@ -71,8 +66,8 @@ uint8_t computeIK(float *out, float x, float y, float z, float a, float b, float
if(isinf(_Atan) == 1)
return 0x00;
_theta = _Atan;
- _k = sin(*(out + 3)) * CALF_LENGTH;
- _l = -THIGH_LENGTH - cos(*(out + 3)) * CALF_LENGTH;
+ _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);
diff --git a/src/darwin_math.c b/src/darwin_math.c
index 5a4b80cd4df6e3afd6c6ac3e9835c491c5abc0d4..e76646357603c96cf91e2dc5994c9fea00f1971d 100755
--- a/src/darwin_math.c
+++ b/src/darwin_math.c
@@ -1,5 +1,4 @@
#include "darwin_math.h"
-
#include <math.h>
// point functions
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 93323f66eafcc7cf74aae31ea49d693a15c1b348..4857ef925fab6881334dbfbf3eabb408ad5d7c35 100755
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -1,73 +1,54 @@
+#include "darwin_dyn_master.h"
#include "motion_manager.h"
-#include "dynamixel_master_uart_dma.h"
-#include "dynamixel_slave_uart_dma.h"
#include "dyn_servos.h"
#include "ram.h"
#include "action.h"
#include "walking.h"
-#include "joint_motion.h"
-#include "head_tracking.h"
-#include "time.h"
-#include "imu.h"
-#include "gpio.h"
-#define MOTION_TIMER TIM5
-#define MOTION_TIMER_IRQn TIM5_IRQn
-#define MOTION_TIMER_IRQHandler TIM5_IRQHandler
-#define MOTION_TIMER_CLK RCC_APB1Periph_TIM5
+#define MANAGER_TIMER TIM5
+#define ENABLE_MANAGER_TIMER_CLK __HAL_RCC_TIM5_CLK_ENABLE()
+#define MANAGER_TIMER_IRQn TIM5_IRQn
+#define MANAGER_TIMER_IRQHandler TIM5_IRQHandler
// private variables
-// manager period
-__IO uint16_t manager_motion_period;
-// manager number of servos (both version 1 and 2)
+TIM_HandleTypeDef MANAGER_TIM_Handle;
+
+uint16_t manager_motion_period;
+uint16_t manager_motion_period_us;
uint8_t manager_num_servos;
-// servo information structures
TServoInfo manager_servos[MANAGER_MAX_NUM_SERVOS];
// current angles used for all motion modules
-int64_t current_angles[MANAGER_MAX_NUM_SERVOS];
+int64_t manager_current_angles[MANAGER_MAX_NUM_SERVOS];
+int8_t manager_current_slopes[MANAGER_MAX_NUM_SERVOS];
// balance values and configuration
-int64_t balance_offset[MANAGER_MAX_NUM_SERVOS];
-uint8_t balance_enabled;
-// package variables
-uint8_t servo_ids[MANAGER_MAX_NUM_SERVOS];
-TWriteData write_data[MANAGER_MAX_NUM_SERVOS];
-uint8_t length[MANAGER_MAX_NUM_SERVOS];
-uint8_t address[MANAGER_MAX_NUM_SERVOS];
-
-// balane variables
-float knee_gain=0.3;
-float ankle_pitch_gain=0.9;
-float hip_roll_gain=0.5;
-float ankle_roll_gain=1.0;
+int64_t manager_balance_offset[MANAGER_MAX_NUM_SERVOS];
+uint8_t manager_balance_enabled;
+// manager offsets
+int16_t manager_offset[MANAGER_MAX_NUM_SERVOS];
// private functions
void manager_send_motion_command(void)
{
+ static uint8_t servo_ids[MANAGER_MAX_NUM_SERVOS];
+ static TWriteData write_data[MANAGER_MAX_NUM_SERVOS];
uint8_t i,num=0;
-
- for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
- {
- if(manager_servos[i].enabled && manager_servos[i].model!=0x0000 && manager_servos[i].dyn_version==0x02)
- {
- servo_ids[num]=manager_servos[i].id;
- write_data[num].data_addr=(uint8_t *)&(manager_servos[i].current_value);
- num++;
- }
- }
- if(num>0)
- dyn2_master_sync_write(num,servo_ids,XL_GOAL_POSITION_L,2,write_data);
- num=0;
+
for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
{
- if(manager_servos[i].enabled && manager_servos[i].model!=0x0000 && manager_servos[i].dyn_version==0x01)
- {
- servo_ids[num]=manager_servos[i].id;
- write_data[num].data_addr=(uint8_t *)&(manager_servos[i].current_value);
- num++;
+ if(manager_servos[i].enabled)
+ {
+ if(manager_servos[i].model==SERVO_MX28)
+ {
+ servo_ids[num]=manager_servos[i].id;
+ manager_servos[i].cw_comp=(1<<(manager_current_slopes[i]&0x0F));
+ manager_servos[i].ccw_comp=(1<<((manager_current_slopes[i]&0xF0)>>4));
+ write_data[num].data_addr=(uint8_t *)&(manager_servos[i].cw_comp);
+ num++;
+ }
}
}
if(num>0)
- dyn_master_sync_write(num,servo_ids,P_GOAL_POSITION_L,2,write_data);
+ dyn_master_sync_write(&darwin_dyn_master,num,servo_ids,P_CW_COMPLIANCE_SLOPE,4,write_data);
}
inline uint16_t manager_angle_to_value(uint8_t servo_id,int16_t angle)
@@ -94,38 +75,18 @@ inline uint16_t manager_value_to_speed(uint8_t servo_id,uint16_t value)
void manager_get_current_position(void)
{
- uint8_t i,num=0,id;
+ uint8_t i;
for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
{
- if((!manager_servos[i].enabled && manager_servos[i].model!=0x0000 && manager_servos[i].dyn_version==0x01) || manager_servos[i].module==MM_HEAD)// servo is disabled
- {
- servo_ids[num]=manager_servos[i].id;
- length[num]=2;
- address[num]=P_PRESENT_POSITION_L;
- write_data[num].data_addr=(uint8_t *)&(manager_servos[i].current_value);
- num++;
- }
- else
+ if(!manager_servos[i].enabled && manager_servos[i].model!=0x0000)// servo is not enabled but it is present
{
+ dyn_master_read_word(&darwin_dyn_master,manager_servos[i].id,P_PRESENT_POSITION_L,&manager_servos[i].current_value);
+ manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value);
ram_data[DARWIN_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
ram_data[DARWIN_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8);
}
}
- if(num>0)
- {
- if(dyn_master_bulk_read(num,servo_ids,address,length,write_data)==DYN_SUCCESS)
- {
- for(i=0;i<num;i++)
- {
- id=servo_ids[i];
- manager_servos[id].current_angle=manager_value_to_angle(id,manager_servos[id].current_value);
- current_angles[id]=manager_servos[id].current_angle<<9;
- ram_data[DARWIN_MM_SERVO0_CUR_POS_L+id*2]=(manager_servos[id].current_angle&0xFF);
- ram_data[DARWIN_MM_SERVO0_CUR_POS_H+id*2]=(manager_servos[id].current_angle>>8);
- }
- }
- }
}
void manager_get_target_position(void)
@@ -134,96 +95,99 @@ void manager_get_target_position(void)
for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
{
- if(manager_servos[i].enabled)// servo is enabled
+ if(manager_servos[i].enabled && manager_servos[i].model!=0x0000)// servo is enabled and present
{
- manager_servos[i].current_angle=((current_angles[i]+balance_offset[i])>>9);
+ manager_servos[i].current_angle=((manager_current_angles[i]>>9)+manager_balance_offset[i]+manager_offset[i]);
//>>16 from the action codification, <<7 from the manager codification
manager_servos[i].current_value=manager_angle_to_value(i,manager_servos[i].current_angle);
+ ram_data[DARWIN_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
+ ram_data[DARWIN_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8);
}
}
}
void manager_balance(void)
{
- int32_t gyro_x,gyro_y,gyro_z;
-
- if(balance_enabled)
+ //adc_ch_t fb_ch,lr_ch;
+ int16_t knee_gain,ankle_roll_gain,ankle_pitch_gain,hip_roll_gain;
+ //uint16_t fb_offset,lr_offset;
+ int16_t fb_value,lr_value;
+
+ if(manager_balance_enabled==0x01)// balance is enabled
{
- imu_get_gyro_data(&gyro_x,&gyro_y,&gyro_z);
- // filter out some noise
- if(gyro_x>0 && gyro_x<4096)
- gyro_x=0;
- else if(gyro_x<0 && gyro_x>-4096)
- gyro_x=0;
- if(gyro_y>0 && gyro_y<4096)
- gyro_y=0;
- else if(gyro_y<0 && gyro_y>-4096)
- gyro_y=0;
-
- balance_offset[R_HIP_ROLL]=(int32_t)(gyro_x*hip_roll_gain*6.428571); // R_HIP_ROLL
- balance_offset[L_HIP_ROLL]=(int32_t)(gyro_x*hip_roll_gain*6.428571); // L_HIP_ROLL
- balance_offset[R_KNEE]=(int32_t)(gyro_y*knee_gain*-6.428571); // R_KNEE
- balance_offset[L_KNEE]=(int32_t)(gyro_y*knee_gain*6.428571); // L_KNEE
-
- balance_offset[R_ANKLE_PITCH]=(int32_t)(gyro_y*ankle_pitch_gain*6.428571); // R_ANKLE_PITCH
- balance_offset[L_ANKLE_PITCH]=(int32_t)(gyro_y*ankle_pitch_gain*-6.428571); // L_ANKLE_PITCH
- balance_offset[R_ANKLE_ROLL]=(int32_t)(gyro_x*ankle_roll_gain*6.428571); // R_ANKLE_ROLL
- balance_offset[L_ANKLE_ROLL]=(int32_t)(gyro_x*ankle_roll_gain*6.428571); // L_ANKLE_ROLL
+ //fb_ch=gyro_get_fb_adc_channel();
+ //lr_ch=gyro_get_lr_adc_channel();
+ // get the balance gains
+ knee_gain=ram_data[DARWIN_MM_BAL_KNEE_GAIN_L]+(ram_data[DARWIN_MM_BAL_KNEE_GAIN_H]<<8);
+ ankle_roll_gain=ram_data[DARWIN_MM_BAL_ANKLE_ROLL_GAIN_L]+(ram_data[DARWIN_MM_BAL_ANKLE_ROLL_GAIN_H]<<8);
+ ankle_pitch_gain=ram_data[DARWIN_MM_BAL_ANKLE_PITCH_GAIN_L]+(ram_data[DARWIN_MM_BAL_ANKLE_PITCH_GAIN_H]<<8);
+ hip_roll_gain=ram_data[DARWIN_MM_BAL_HIP_ROLL_GAIN_L]+(ram_data[DARWIN_MM_BAL_HIP_ROLL_GAIN_H]<<8);
+ // get the offsets of the gyroscope calibration
+ //gyro_get_offsets(&fb_offset,&lr_offset);
+ // get the values of the gyroscope
+ //fb_value=adc_get_channel_raw(fb_ch)-fb_offset;
+ //lr_value=adc_get_channel_raw(lr_ch)-lr_offset;
+ fb_value=0;
+ lr_value=0;
+ // compensate the servo angle values
+ manager_balance_offset[R_KNEE]=(((fb_value*manager_servos[R_KNEE].max_angle)/manager_servos[R_KNEE].encoder_resolution)*knee_gain)>>16;
+ manager_balance_offset[R_ANKLE_PITCH]=(((fb_value*manager_servos[R_ANKLE_PITCH].max_angle)/manager_servos[R_ANKLE_PITCH].encoder_resolution)*ankle_pitch_gain)>>16;
+ manager_balance_offset[L_KNEE]=-(((fb_value*manager_servos[L_KNEE].max_angle)/manager_servos[L_KNEE].encoder_resolution)*knee_gain)>>16;
+ manager_balance_offset[L_ANKLE_PITCH]=-(((fb_value*manager_servos[L_ANKLE_PITCH].max_angle)/manager_servos[L_ANKLE_PITCH].encoder_resolution)*ankle_pitch_gain)>>16;
+ manager_balance_offset[R_HIP_ROLL]=(((lr_value*manager_servos[R_HIP_ROLL].max_angle)/manager_servos[R_HIP_ROLL].encoder_resolution)*hip_roll_gain)>>16;
+ manager_balance_offset[L_HIP_ROLL]=(((lr_value*manager_servos[L_HIP_ROLL].max_angle)/manager_servos[L_HIP_ROLL].encoder_resolution)*hip_roll_gain)>>16;
+ manager_balance_offset[R_ANKLE_ROLL]=-(((lr_value*manager_servos[R_ANKLE_ROLL].max_angle)/manager_servos[R_ANKLE_ROLL].encoder_resolution)*ankle_roll_gain)>>16;
+ manager_balance_offset[L_ANKLE_ROLL]=-(((lr_value*manager_servos[L_ANKLE_ROLL].max_angle)/manager_servos[L_ANKLE_ROLL].encoder_resolution)*ankle_roll_gain)>>16;
}
}
// interrupt handlers
-void MOTION_TIMER_IRQHandler(void)
+void MANAGER_TIMER_IRQHandler(void)
{
- uint16_t capture_start,capture_end;
- int16_t elapsed;
+ uint16_t capture;
- if(TIM_GetITStatus(MOTION_TIMER, TIM_IT_CC1)!=RESET)
+ if(__HAL_TIM_GET_FLAG(&MANAGER_TIM_Handle, TIM_FLAG_CC1) != RESET)
{
- TIM_ClearITPendingBit(MOTION_TIMER,TIM_IT_CC1);
- capture_start = TIM_GetCapture1(MOTION_TIMER);
- TIM_SetCompare1(MOTION_TIMER, capture_start + manager_motion_period);
- // get the disabled servos position
- manager_get_current_position();
- // call the action process
- action_process();
- // call the joint motion process
- joint_motion_process();
- // call the walking process
- walking_process();
- // head tracking process
- head_tracking_process();
- // balance the robot
- manager_balance();
- // get the target angles from all modules
- manager_get_target_position();
- // send the motion commands to the servos
- manager_send_motion_command();
- capture_end = TIM_GetCounter(MOTION_TIMER);
- elapsed=capture_end-capture_start;
- if(elapsed<0) elapsed+=0xFFFFF;
- ram_write_word(DARWIN_MM_CUR_PERIOD,(uint16_t)elapsed);
+ if(__HAL_TIM_GET_IT_SOURCE(&MANAGER_TIM_Handle, TIM_IT_CC1) !=RESET)
+ {
+ __HAL_TIM_CLEAR_IT(&MANAGER_TIM_Handle, TIM_IT_CC1);
+ capture = HAL_TIM_ReadCapturedValue(&MANAGER_TIM_Handle, TIM_CHANNEL_1);
+ __HAL_TIM_SET_COMPARE(&MANAGER_TIM_Handle, TIM_CHANNEL_1, (capture + manager_motion_period_us));
+ // call the action process
+ action_process();
+ // call the joint motion process
+ // joint_motion_process();
+ // call the walking process
+ walking_process();
+ // balance the robot
+ manager_balance();
+ // get the target angles from all modules
+ manager_get_target_position();
+ // send the motion commands to the servos
+ manager_send_motion_command();
+ // get the disabled servos position
+ // manager_get_current_position();
+ }
}
}
// public functions
void manager_init(uint16_t period_us)
{
- TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
- TIM_OCInitTypeDef TIM_OCInitStructure;
- NVIC_InitTypeDef NVIC_InitStructure;
+ TIM_ClockConfigTypeDef sClockSourceConfig;
+ TIM_MasterConfigTypeDef sMasterConfig;
uint8_t servo_ids[MANAGER_MAX_NUM_SERVOS];
uint16_t model,value;
uint8_t i,num=0;
- RCC_APB1PeriphClockCmd(MOTION_TIMER_CLK,ENABLE);
+ /* initialize the dynamixel master module for the servos */
+ darwin_dyn_master_init();
// enable power to the servos
- dyn_master_enable_power();
- delay_ms(1000);
-
+ darwin_dyn_master_enable_power();
+ HAL_Delay(1000);
// detect the servos connected
- dyn_master_scan(&num,servo_ids);
+ dyn_master_scan(&darwin_dyn_master,&num,servo_ids);
ram_data[DARWIN_MM_NUM_SERVOS]=num;
manager_num_servos=0;
for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
@@ -231,7 +195,7 @@ void manager_init(uint16_t period_us)
if(i==servo_ids[manager_num_servos])
{
// read the model of the i-th device
- dyn_master_read_word(servo_ids[manager_num_servos],P_MODEL_NUMBER_L,&model);
+ dyn_master_read_word(&darwin_dyn_master,servo_ids[manager_num_servos],P_MODEL_NUMBER_L,&model);
switch(model)
{
case SERVO_AX12A: manager_servos[i].encoder_resolution=1023;
@@ -300,19 +264,18 @@ void manager_init(uint16_t period_us)
manager_servos[i].model=model;
manager_servos[i].module=MM_NONE;
manager_servos[i].enabled=0x00;
- manager_servos[i].dyn_version=0x01;
// get the servo's current position
- dyn_master_read_word(manager_servos[i].id,P_PRESENT_POSITION_L,&manager_servos[i].current_value);
+ dyn_master_read_word(&darwin_dyn_master,manager_servos[i].id,P_PRESENT_POSITION_L,&manager_servos[i].current_value);
manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value);
ram_data[DARWIN_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
ram_data[DARWIN_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8);
// read the servo limits
- dyn_master_read_word(manager_servos[i].id,P_CW_ANGLE_LIMIT_L,&value);
+ dyn_master_read_word(&darwin_dyn_master,manager_servos[i].id,P_CW_ANGLE_LIMIT_L,&value);
manager_servos[i].cw_angle_limit=manager_value_to_angle(i,value);
- dyn_master_read_word(manager_servos[i].id,P_CCW_ANGLE_LIMIT_L,&value);
+ dyn_master_read_word(&darwin_dyn_master,manager_servos[i].id,P_CCW_ANGLE_LIMIT_L,&value);
manager_servos[i].ccw_angle_limit=manager_value_to_angle(i,value);
// set the action current angles
- current_angles[i]=manager_servos[i].current_angle<<9;
+ manager_current_angles[i]=manager_servos[i].current_angle<<9;
manager_num_servos++;
}
else
@@ -330,92 +293,43 @@ void manager_init(uint16_t period_us)
manager_servos[i].enabled=0x00;
manager_servos[i].cw_angle_limit=0;
manager_servos[i].ccw_angle_limit=0;
- manager_servos[i].dyn_version=0x00;
}
}
- // scan for version 2 servos
- dyn2_master_scan(&num,servo_ids);
- ram_data[DARWIN_MM_NUM_SERVOS]=manager_num_servos;
- manager_num_servos=0;
+ darwin_dyn_master_disable_power();
+
+ /* configure timer */
+ ENABLE_MANAGER_TIMER_CLK;
+ MANAGER_TIM_Handle.Instance=MANAGER_TIMER;
+ MANAGER_TIM_Handle.Init.Period = 0xFFFF;
+ MANAGER_TIM_Handle.Init.Prescaler = 72;
+ MANAGER_TIM_Handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+ MANAGER_TIM_Handle.Init.CounterMode = TIM_COUNTERMODE_UP;
+ HAL_NVIC_SetPriority(MANAGER_TIMER_IRQn, 2, 1);
+ HAL_NVIC_EnableIRQ(MANAGER_TIMER_IRQn);
+ /* use the internal clock */
+ sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+ HAL_TIM_ConfigClockSource(&MANAGER_TIM_Handle, &sClockSourceConfig);
+ HAL_TIM_OC_Init(&MANAGER_TIM_Handle);
+ /* disable master/slave mode */
+ sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+ sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+ HAL_TIMEx_MasterConfigSynchronization(&MANAGER_TIM_Handle, &sMasterConfig);
+ /* configure ouptut counter channel 4 */
+ manager_motion_period=(period_us<<16)/1000000;
+ manager_motion_period_us=period_us;
+
+ /* initialize balance parameters */
+ /* initialize the manager offsets from EEPROM */
for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
{
- if(i==servo_ids[manager_num_servos])
- {
- // read the model of the i-th device
- dyn2_master_read_word(servo_ids[manager_num_servos],XL_MODEL_NUMBER_L,&model);
- switch(model)
- {
- case SERVO_XL320: manager_servos[i].encoder_resolution=1023;
- manager_servos[i].gear_ratio=238;
- manager_servos[i].max_angle=300<<7;
- manager_servos[i].center_angle=150<<7;
- manager_servos[i].max_speed=690;
- break;
- default: break;
- }
- manager_servos[i].id=servo_ids[manager_num_servos];
- manager_servos[i].model=model;
- manager_servos[i].module=MM_NONE;
- manager_servos[i].enabled=0x00;
- manager_servos[i].dyn_version=0x02;
- // get the servo's current position
- dyn2_master_read_word(manager_servos[i].id,XL_PRESENT_POSITION_L,&manager_servos[i].current_value);
- manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value);
- ram_data[DARWIN_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
- ram_data[DARWIN_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8);
- // read the servo limits
- dyn2_master_read_word(manager_servos[i].id,XL_CW_ANGLE_LIMIT_L,&value);
- manager_servos[i].cw_angle_limit=manager_value_to_angle(i,value);
- dyn2_master_read_word(manager_servos[i].id,XL_CCW_ANGLE_LIMIT_L,&value);
- manager_servos[i].ccw_angle_limit=manager_value_to_angle(i,value);
- // set the action current angles
- current_angles[i]=manager_servos[i].current_angle<<9;
- manager_num_servos++;
- }
+ manager_balance_offset[i]=0;
+ manager_set_offset(i,(signed char)ram_data[DARWIN_MM_SERVO0_OFFSET+i]);
}
- dyn_master_disable_power();
- manager_num_servos=num+ram_data[DARWIN_MM_NUM_SERVOS];
- ram_data[DARWIN_MM_NUM_SERVOS]=((num&0x07)<<5)+(ram_data[DARWIN_MM_NUM_SERVOS]&0x1F);
-
- // initialize the timer interrupts
- NVIC_InitStructure.NVIC_IRQChannel = MOTION_TIMER_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
-
- /* motion timer configuration */
- TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
- TIM_TimeBaseStructure.TIM_Prescaler = 72;
- TIM_TimeBaseStructure.TIM_ClockDivision = 0;
- TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
- TIM_TimeBaseInit(MOTION_TIMER,&TIM_TimeBaseStructure);
-
- TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Timing;
- TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Disable;
- TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
- manager_motion_period=(period_us*1000000)>>16;
- TIM_OCInitStructure.TIM_Pulse = manager_motion_period;
- TIM_OC1Init(MOTION_TIMER, &TIM_OCInitStructure);
- TIM_OC1PreloadConfig(MOTION_TIMER, TIM_OCPreload_Disable);
-
- ram_clear_bit(DARWIN_MM_STATUS,0);
-
- /* initialize motion modules */
- action_init(period_us);
- walking_init(manager_motion_period);
- joint_motion_init(period_us);
- head_tracking_init(period_us);
-
- ram_write_byte(DARWIN_MM_KNEE_GAIN,(uint8_t)(knee_gain*64.0));
- ram_write_byte(DARWIN_MM_ANKLE_PITCH_GAIN,(uint8_t)(ankle_pitch_gain*64.0));
- ram_write_byte(DARWIN_MM_HIP_ROLL_GAIN,(uint8_t)(hip_roll_gain*64.0));
- ram_write_byte(DARWIN_MM_ANKLE_ROLL_GAIN,(uint8_t)(ankle_roll_gain*64.0));
+ manager_balance_enabled=0x00;
- for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++)
- balance_offset[i]=0;
- balance_enabled=0x01;
- ram_data[DARWIN_MM_CONTROL]=0x02;
+ /* initialize action module */
+ action_init(period_us);
+ walking_init(period_us);
}
uint16_t manager_get_period(void)
@@ -423,53 +337,57 @@ uint16_t manager_get_period(void)
return manager_motion_period;
}
+uint16_t manager_get_period_us(void)
+{
+ return manager_motion_period_us;
+}
+
void manager_set_period(uint16_t period_us)
{
- manager_motion_period=(period_us*1000000)>>16;
- ram_write_word(DARWIN_MM_PERIOD_H,period_us);
+ manager_motion_period=(period_us<<16)/1000000;
+ manager_motion_period_us=period_us;
+ ram_data[DARWIN_MM_PERIOD_L]=period_us&0x00FF;
+ ram_data[DARWIN_MM_PERIOD_H]=(period_us&0xFF00)>>8;
action_set_period(period_us);
- walking_set_period(manager_motion_period);
- joint_motion_set_period(period_us);
- head_tracking_set_period(period_us);
+ walking_set_period(period_us);
}
inline void manager_enable(void)
{
+ TIM_OC_InitTypeDef TIM_OCInitStructure;
uint16_t capture;
- capture = TIM_GetCapture1(MOTION_TIMER);
- TIM_SetCompare1(MOTION_TIMER, capture + manager_motion_period);
- TIM_Cmd(MOTION_TIMER, ENABLE);
- TIM_ITConfig(MOTION_TIMER, TIM_IT_CC1, ENABLE);
- ram_set_bit(DARWIN_MM_STATUS,0);
- ram_set_bit(DARWIN_MM_CONTROL,0);
+ TIM_OCInitStructure.OCMode = TIM_OCMODE_TIMING;
+ TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH;
+ TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE;
+ capture = HAL_TIM_ReadCapturedValue(&MANAGER_TIM_Handle, TIM_CHANNEL_1);
+ TIM_OCInitStructure.Pulse = capture+manager_motion_period_us;
+ HAL_TIM_OC_ConfigChannel(&MANAGER_TIM_Handle, &TIM_OCInitStructure,TIM_CHANNEL_1);
+ HAL_TIM_OC_Start_IT(&MANAGER_TIM_Handle, TIM_CHANNEL_1);
+ ram_data[DARWIN_MM_CNTRL]|=MANAGER_ENABLE;
}
inline void manager_disable(void)
{
- TIM_Cmd(MOTION_TIMER, DISABLE);
- TIM_ITConfig(MOTION_TIMER, TIM_IT_CC1, DISABLE);
- ram_clear_bit(DARWIN_MM_STATUS,0);
- ram_clear_bit(DARWIN_MM_CONTROL,0);
+ HAL_TIM_OC_Stop_IT(&MANAGER_TIM_Handle, TIM_CHANNEL_1);
+ ram_data[DARWIN_MM_CNTRL]&=(~MANAGER_ENABLE);
}
inline uint8_t manager_is_enabled(void)
{
- return ram_data[DARWIN_MM_STATUS]&0x01;
+ return ram_data[DARWIN_MM_CNTRL]&MANAGER_ENABLE;
}
void manager_enable_balance(void)
{
- ram_set_bit(DARWIN_MM_STATUS,1);
- ram_set_bit(DARWIN_MM_CONTROL,1);
- balance_enabled=0x01;
+ manager_balance_enabled=0x01;
+ ram_data[DARWIN_MM_CNTRL]|=MANAGER_EN_BAL;
}
void manager_disable_balance(void)
{
- ram_clear_bit(DARWIN_MM_STATUS,1);
- ram_clear_bit(DARWIN_MM_CONTROL,1);
- balance_enabled=0x00;
+ manager_balance_enabled=0x00;
+ ram_data[DARWIN_MM_CNTRL]&=(~MANAGER_EN_BAL);
}
inline uint8_t manager_get_num_servos(void)
@@ -484,12 +402,12 @@ void manager_set_module(uint8_t servo_id,TModules module)
if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
{
manager_servos[servo_id].module=module;
- ram_read_byte(DARWIN_MM_MODULE_EN0+servo_id/2,&byte);
- if(servo_id%2)
- byte=(byte&0xF8)+((uint8_t)module);
- else
- byte=(byte&0x8F)+(((uint8_t)module)<<4);
- ram_write_byte(DARWIN_MM_MODULE_EN0+servo_id/2,byte);
+ byte=ram_data[DARWIN_MM_MODULE_EN0+servo_id/2];
+ if(servo_id%2)// odd servo
+ byte=(byte&(~MANAGER_ODD_SER_MOD))+((uint8_t)module);
+ else// even servo
+ byte=(byte&(~MANAGER_EVEN_SER_MOD))+(((uint8_t)module)<<4);
+ ram_data[DARWIN_MM_MODULE_EN0+servo_id/2]=byte;
}
}
@@ -508,12 +426,12 @@ inline void manager_enable_servo(uint8_t servo_id)
if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
{
manager_servos[servo_id].enabled=0x01;
- ram_read_byte(DARWIN_MM_MODULE_EN0+servo_id/2,&byte);
- if(servo_id%2)
- byte|=0x08;
- else
- byte|=0x80;
- ram_write_byte(DARWIN_MM_MODULE_EN0+servo_id/2,byte);
+ byte=ram_data[DARWIN_MM_MODULE_EN0+servo_id/2];;
+ if(servo_id%2)// odd servo
+ byte|=MANAGER_ODD_SER_EN;
+ else// even servo
+ byte|=MANAGER_EVEN_SER_EN;
+ ram_data[DARWIN_MM_MODULE_EN0+servo_id/2]=byte;
}
}
@@ -524,32 +442,15 @@ inline void manager_disable_servo(uint8_t servo_id)
if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
{
manager_servos[servo_id].enabled=0x00;
- ram_read_byte(DARWIN_MM_MODULE_EN0+servo_id/2,&byte);
+ byte=ram_data[DARWIN_MM_MODULE_EN0+servo_id/2];
if(servo_id%2)
- byte&=0xF7;
+ byte&=(~MANAGER_ODD_SER_EN);
else
- byte&=0x7F;
- ram_write_byte(DARWIN_MM_MODULE_EN0+servo_id/2,byte);
+ byte&=(~MANAGER_EVEN_SER_EN);
+ ram_data[DARWIN_MM_MODULE_EN0+servo_id/2]=byte;
}
}
-void manager_disable_servos(void)
-{
- uint8_t i,num=0,data=0x00;
-
- for(i=0;i<manager_num_servos;i++)
- {
- if(!manager_servos[i].enabled && manager_servos[i].dyn_version==0x01)
- {
- servo_ids[num]=manager_servos[i].id;
- write_data[num].data_addr=&data;
- num++;
- }
- }
- if(num>0)
- dyn_master_sync_write(num,servo_ids,P_TORQUE_ENABLE,1,write_data);
-}
-
inline uint8_t manager_is_servo_enabled(uint8_t servo_id)
{
if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
@@ -558,6 +459,15 @@ inline uint8_t manager_is_servo_enabled(uint8_t servo_id)
return 0x00;
}
+void manager_set_offset(uint8_t servo_id, int8_t offset)
+{
+ if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
+ {
+ manager_offset[servo_id]=(int16_t)(offset*8);
+ ram_data[DARWIN_MM_SERVO0_OFFSET+servo_id]=offset;
+ }
+}
+
inline int16_t manager_get_cw_angle_limit(uint8_t servo_id)
{
return manager_servos[servo_id].cw_angle_limit;
@@ -568,32 +478,64 @@ inline int16_t manager_get_ccw_angle_limit(uint8_t servo_id)
return manager_servos[servo_id].ccw_angle_limit;
}
-void manager_set_knee_gain(uint8_t gain)
+// operation functions
+uint8_t manager_in_range(unsigned short int address, unsigned short int length)
{
- ram_data[DARWIN_MM_KNEE_GAIN]=gain;
- knee_gain=gain/64.0;
-}
-
-void manager_set_ankle_pitch_gain(uint8_t gain)
-{
- ram_data[DARWIN_MM_ANKLE_PITCH_GAIN]=gain;
- ankle_pitch_gain=gain/64.0;
-}
-
-void manager_set_hip_roll_gain(uint8_t gain)
-{
- ram_data[DARWIN_MM_HIP_ROLL_GAIN]=gain;
- hip_roll_gain=gain/64.0;
+ if(ram_in_window(MANAGER_BASE_ADDRESS,MANAGER_MEM_LENGTH,address,length) ||
+ ram_in_window(MANAGER_EEPROM_BASE1,MANAGER_EEPROM_LENGTH1,address,length) ||
+ ram_in_window(MANAGER_EEPROM_BASE2,MANAGER_EEPROM_LENGTH2,address,length))
+ return 0x01;
+ else
+ return 0x00;
}
-void manager_set_ankle_roll_gain(uint8_t gain)
+void manager_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
{
- ram_data[DARWIN_MM_ANKLE_ROLL_GAIN]=gain;
- ankle_roll_gain=gain/64.0;
-}
+ uint16_t word_value,i,j;
+ uint8_t byte_value,module;
-uint8_t manager_get_dyn_version(uint8_t servo_id)
-{
- return manager_servos[servo_id].dyn_version;
+ if(ram_in_range(DARWIN_MM_PERIOD_L,address,length) && ram_in_range(DARWIN_MM_PERIOD_H,address,length))
+ {
+ word_value=data[DARWIN_MM_PERIOD_L-address]+(data[DARWIN_MM_PERIOD_H-address]<<8);
+ manager_set_period(word_value);
+ }
+ for(i=DARWIN_MM_SERVO0_OFFSET,j=0;i<=DARWIN_MM_SERVO31_OFFSET;i++,j++)
+ {
+ if(ram_in_range(i,address,length))
+ manager_set_offset(j,(signed char)(data[i-address]));
+ }
+ for(i=DARWIN_MM_MODULE_EN0,j=0;i<=DARWIN_MM_MODULE_EN15;i++,j+=2)
+ {
+ if(ram_in_range(i,address,length))
+ {
+ byte_value=data[i-address];
+ if(byte_value&MANAGER_EVEN_SER_EN) manager_enable_servo(j);
+ else manager_disable_servo(j);
+ module=(byte_value&MANAGER_EVEN_SER_MOD)>>4;
+ manager_set_module(j,module);
+ if(byte_value&MANAGER_ODD_SER_EN) manager_enable_servo(j+1);
+ else manager_disable_servo(j+1);
+ module=byte_value&MANAGER_ODD_SER_MOD;
+ manager_set_module(j+1,module);
+ }
+ }
+ if(ram_in_range(DARWIN_MM_CNTRL,address,length))
+ {
+ if(data[DARWIN_MM_CNTRL-address]&MANAGER_ENABLE)
+ manager_enable();
+ else
+ manager_disable();
+ if(data[DARWIN_MM_CNTRL-address]&MANAGER_EN_BAL)
+ manager_enable_balance();
+ else
+ manager_disable_balance();
+ if(data[DARWIN_MM_CNTRL-address]&MANAGER_EN_PWR)
+ darwin_dyn_master_enable_power();
+ else
+ darwin_dyn_master_disable_power();
+ }
+ // balance gains
+ for(i=MM_BAL_KNEE_GAIN_OFFSET;i<=MM_BAL_HIP_ROLL_GAIN_OFFSET+1;i++)
+ if(ram_in_range(i,address,length))
+ ram_data[i]=data[i-address];
}
-
diff --git a/src/walking.c b/src/walking.c
index 13d03c12bb971f658d568c638f3fe2de9ccd2fac..192efbdad77bc1766f0469ad5fdd15fefb575182 100755
--- a/src/walking.c
+++ b/src/walking.c
@@ -1,79 +1,15 @@
#include "walking.h"
-#include "motion_manager.h"
-#include "ram.h"
#include "darwin_kinematics.h"
#include "darwin_math.h"
-#include "time.h"
-
-enum {PHASE0 = 0,PHASE1 = 1,PHASE2 = 2, PHASE3 = 3};
-
-// leg kinematics
-const uint8_t servo_seq[14]={R_HIP_YAW,R_HIP_ROLL,R_HIP_PITCH,R_KNEE,R_ANKLE_PITCH,R_ANKLE_ROLL,L_HIP_YAW,L_HIP_ROLL,L_HIP_PITCH,L_KNEE,L_ANKLE_PITCH,L_ANKLE_ROLL,R_SHOULDER_PITCH,L_SHOULDER_PITCH};
-
-const int8_t dir[14]={-1,-1,1,1,-1,1,-1,-1,-1,-1,1,1,1,-1};
-
-// walking configuration variables
-float Y_SWAP_AMPLITUDE=20.0;//mm
-float Z_SWAP_AMPLITUDE=5;//mm
-float ARM_SWING_GAIN=1.5;
-float PELVIS_OFFSET=3.0;//degrees
-float HIP_PITCH_OFFSET=0;//23.0;//degrees
-
-uint8_t P_GAIN=32;
-uint8_t I_GAIN=0;
-uint8_t D_GAIN=0;
+#include "ram.h"
+#include <math.h>
-// Walking initial pose
-float X_OFFSET=-10;//mm
-float Y_OFFSET=5;//mm
-float Z_OFFSET=20;//mm
-float A_OFFSET=0;
-float P_OFFSET=0;
-float R_OFFSET=0;
+enum {PHASE0=0x00,PHASE1=0x40,PHASE2=0x80,PHASE3=0xC0};
// Walking control
-float PERIOD_TIME=600;//milliseconds
-float DSP_RATIO=0.1;
-float STEP_FB_RATIO=0.3;
-float X_MOVE_AMPLITUDE=0;//mm
-float current_x_amplitude=0;
-float Y_MOVE_AMPLITUDE=0;//mm
-float current_y_amplitude=0;
-float Z_MOVE_AMPLITUDE=40;//mm
-float A_MOVE_AMPLITUDE=0;//degrees
-float current_a_amplitude=0;
uint8_t A_MOVE_AIM_ON=0x00;
-float max_acceleration=0.02;// m/s/s
-float max_rot_accel=0.02;// rad/s/s
-
-// private variables
-float m_PeriodTime;
-float m_DSP_Ratio;
-float m_SSP_Ratio;
-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;
-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;
-
-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 motion variables
float m_X_Swap_Phase_Shift;
float m_X_Swap_Amplitude;
float m_X_Swap_Amplitude_Shift;
@@ -96,170 +32,179 @@ 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_Hip_Pitch_Offset;
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;
-volatile uint8_t m_Ctrl_Running;
+// internal time variables
float m_Time;
-float walking_period;
+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;
-uint8_t m_Phase;
-float m_Body_Swing_Y;
-float m_Body_Swing_Z;
+// control variables
+uint8_t m_Ctrl_Running;
+float walking_period;
// private functions
inline 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;
+ return mag*sin(((2.0*PI*time)/period)-period_shift)+mag_shift;
}
void update_param_time()
{
- m_PeriodTime = PERIOD_TIME;
- m_DSP_Ratio = DSP_RATIO;
- m_SSP_Ratio = 1 - DSP_RATIO;
+ float m_SSP_Ratio;
+ float m_SSP_Time;
- m_SSP_Time = m_PeriodTime * m_SSP_Ratio;
+ 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_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=m_PeriodTime*m_SSP_Ratio;
- 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_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_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_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 = PELVIS_OFFSET;
- m_Pelvis_Swing = m_Pelvis_Offset * 0.35;
- m_Arm_Swing_Gain = ARM_SWING_GAIN;
+ 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<X_MOVE_AMPLITUDE)
+ if(current_x_amplitude<target_x_amplitude)
{
- current_x_amplitude+=max_acceleration*PERIOD_TIME;
- if(current_x_amplitude>X_MOVE_AMPLITUDE)
- current_x_amplitude=X_MOVE_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>X_MOVE_AMPLITUDE)
+ else if(current_x_amplitude>target_x_amplitude)
{
- current_x_amplitude-=max_acceleration*PERIOD_TIME;
- if(current_x_amplitude<X_MOVE_AMPLITUDE)
- current_x_amplitude=X_MOVE_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 * STEP_FB_RATIO;
+ 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
- if(current_y_amplitude<Y_MOVE_AMPLITUDE)
+ target_y_amplitude=((float)((int8_t)ram_data[DARWIN_WALK_STEP_LEFT_RIGHT]));
+ if(current_y_amplitude<target_y_amplitude)
{
- current_y_amplitude+=max_acceleration*PERIOD_TIME;
- if(current_y_amplitude>Y_MOVE_AMPLITUDE)
- current_y_amplitude=Y_MOVE_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>Y_MOVE_AMPLITUDE)
+ else if(current_y_amplitude>target_y_amplitude)
{
- current_y_amplitude-=max_acceleration*PERIOD_TIME;
- if(current_y_amplitude<Y_MOVE_AMPLITUDE)
- current_y_amplitude=Y_MOVE_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;
+ 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 = Y_SWAP_AMPLITUDE + m_Y_Move_Amplitude_Shift * 0.04;
+ 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 = Z_MOVE_AMPLITUDE / 2.0;
- m_Z_Move_Amplitude_Shift = m_Z_Move_Amplitude / 2.0;
- m_Z_Swap_Amplitude = Z_SWAP_AMPLITUDE;
- m_Z_Swap_Amplitude_Shift = m_Z_Swap_Amplitude;
+ 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
- if(current_a_amplitude<A_MOVE_AMPLITUDE)
+ target_a_amplitude=((float)((int8_t)ram_data[DARWIN_WALK_STEP_DIRECTION]))/8.0;
+ if(current_a_amplitude<target_a_amplitude)
{
- current_a_amplitude+=max_acceleration*PERIOD_TIME;
- if(current_a_amplitude>A_MOVE_AMPLITUDE)
- current_a_amplitude=A_MOVE_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>A_MOVE_AMPLITUDE)
+ else if(current_a_amplitude>target_a_amplitude)
{
- current_a_amplitude-=max_acceleration*PERIOD_TIME;
- if(current_a_amplitude<A_MOVE_AMPLITUDE)
- current_a_amplitude=A_MOVE_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)
+ if(A_MOVE_AIM_ON==0x00)
{
- m_A_Move_Amplitude = A_MOVE_AMPLITUDE * PI / 180.0 / 2.0;
- if(m_A_Move_Amplitude > 0)
- m_A_Move_Amplitude_Shift = m_A_Move_Amplitude;
+ 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;
+ m_A_Move_Amplitude_Shift=-m_A_Move_Amplitude;
}
else
{
- m_A_Move_Amplitude = -A_MOVE_AMPLITUDE * PI / 180.0 / 2.0;
- if(m_A_Move_Amplitude > 0)
- m_A_Move_Amplitude_Shift = -m_A_Move_Amplitude;
+ 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;
+ m_A_Move_Amplitude_Shift=m_A_Move_Amplitude;
}
}
void update_param_balance()
{
- m_X_Offset = X_OFFSET;
- m_Y_Offset = Y_OFFSET;
- m_Z_Offset = Z_OFFSET;
- m_R_Offset = R_OFFSET * PI / 180.0;
- m_P_Offset = P_OFFSET * PI / 180.0;
- m_A_Offset = A_OFFSET * PI / 180.0;
- m_Hip_Pitch_Offset = HIP_PITCH_OFFSET;
+ 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)
+void walking_init(uint16_t period_us)
{
- // initialize RAM
- ram_write_byte(DARWIN_X_OFFSET,(uint8_t)X_OFFSET);//mm
- ram_write_byte(DARWIN_Y_OFFSET,(uint8_t)Y_OFFSET);//mm
- ram_write_byte(DARWIN_Z_OFFSET,(uint8_t)Z_OFFSET);//mm
- ram_write_byte(DARWIN_ROLL,(uint8_t)(A_OFFSET*8));//degrees format 5|3
- ram_write_byte(DARWIN_PITCH,(uint8_t)(P_OFFSET*8));//degrees format 5|3
- ram_write_byte(DARWIN_YAW,(uint8_t)(R_OFFSET*8));//degrees format 5|3
- ram_write_word(DARWIN_HIP_PITCH_OFF_L,(uint16_t)(HIP_PITCH_OFFSET*1024));//degrees format 6|10
- ram_write_word(DARWIN_PERIOD_TIME_L,(uint16_t)PERIOD_TIME);// milliseconds
- ram_write_byte(DARWIN_DSP_RATIO,(uint8_t)(DSP_RATIO*128));//ratio [0,1] format 1|7
- ram_write_byte(DARWIN_STEP_FW_BW_RATIO,(uint8_t)(STEP_FB_RATIO*128));//ratio [0,1] format 1|7
- ram_write_byte(DARWIN_STEP_FW_BW,(uint8_t)(X_MOVE_AMPLITUDE));//mm
- ram_write_byte(DARWIN_STEP_LEFT_RIGHT,(uint8_t)(Y_MOVE_AMPLITUDE));//mm
- ram_write_byte(DARWIN_STEP_DIRECTION,(uint8_t)(A_MOVE_AMPLITUDE*8));//degrees 5|3
- ram_write_byte(DARWIN_FOOT_HEIGHT,(uint8_t)(Z_MOVE_AMPLITUDE));//mm
- ram_write_byte(DARWIN_SWING_RIGHT_LEFT,(uint8_t)(Y_SWAP_AMPLITUDE));//mm
- ram_write_byte(DARWIN_SWING_TOP_DOWN,(uint8_t)(Z_SWAP_AMPLITUDE));//mm
- ram_write_byte(DARWIN_PELVIS_OFFSET,(uint8_t)(PELVIS_OFFSET*8));//degrees 5|3
- ram_write_byte(DARWIN_ARM_SWING_GAIN,(uint8_t)(ARM_SWING_GAIN*32));// gain 3|5
- ram_write_byte(DARWIN_P_GAIN,(uint8_t)(P_GAIN));// servo gain
- ram_write_byte(DARWIN_I_GAIN,(uint8_t)(I_GAIN));// servo gain
- ram_write_byte(DARWIN_D_GAIN,(uint8_t)(D_GAIN));// servo gain
- ram_write_byte(DARWIN_MAX_ACC,(uint8_t)(max_acceleration*1024));// maximum linear acceleration
- ram_write_byte(DARWIN_MAX_ROT_ACC,(uint8_t)(max_rot_accel*1024));// maximum rotational acceleration
- // initialize the internal variables
+ // 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;
@@ -272,12 +217,9 @@ void walking_init(uint16_t period)
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=((double)period)/1000.0;
-
- m_Body_Swing_Y=0.0;
- m_Body_Swing_Z=0.0;
-
+ walking_period=((float)period_us)/1000.0;
m_Ctrl_Running=0x00;
update_param_time();
@@ -286,9 +228,9 @@ void walking_init(uint16_t period)
walking_process();
}
-inline void walking_set_period(uint16_t period)
+inline void walking_set_period(uint16_t period_us)
{
- walking_period=((double)period)/1000.0;
+ walking_period=((float)period_us)/1000.0;
}
inline uint16_t walking_get_period(void)
@@ -296,166 +238,92 @@ inline uint16_t walking_get_period(void)
return (uint16_t)(walking_period*1000);
}
-void walking_set_param(uint8_t param_id,int8_t param_value)
-{
- uint16_t value;
-
- switch(param_id)
- {
- case DARWIN_X_OFFSET: X_OFFSET=param_value;
- break;
- case DARWIN_Y_OFFSET: Y_OFFSET=param_value;
- break;
- case DARWIN_Z_OFFSET: Z_OFFSET=param_value;
- break;
- case DARWIN_ROLL: A_OFFSET=((float)param_value)/8.0;
- break;
- case DARWIN_PITCH: P_OFFSET=((float)param_value)/8.0;
- break;
- case DARWIN_YAW: R_OFFSET=((float)param_value)/8.0;
- break;
- case DARWIN_HIP_PITCH_OFF_L: value=(uint16_t)(HIP_PITCH_OFFSET*1024);
- value&=0xFF00;
- value+=param_value;
- HIP_PITCH_OFFSET=((float)value)/1024.0;
- break;
- case DARWIN_HIP_PITCH_OFF_H: value=(uint16_t)(HIP_PITCH_OFFSET*1024);
- value&=0x00FF;
- value+=param_value*256;
- HIP_PITCH_OFFSET=((float)value)/1024.0;
- break;
- case DARWIN_PERIOD_TIME_L: value=(uint16_t)PERIOD_TIME;
- value&=0xFF00;
- value+=param_value;
- PERIOD_TIME=value;
- break;
- case DARWIN_PERIOD_TIME_H: value=(uint16_t)PERIOD_TIME;
- value&=0x00FF;
- value+=param_value*256;
- PERIOD_TIME=value;
- break;
- case DARWIN_DSP_RATIO: DSP_RATIO=((float)param_value)/128.0;
- break;
- case DARWIN_STEP_FW_BW_RATIO: STEP_FB_RATIO=((float)param_value)/128.0;
- break;
- case DARWIN_STEP_FW_BW: X_MOVE_AMPLITUDE=param_value;
- break;
- case DARWIN_STEP_LEFT_RIGHT: Y_MOVE_AMPLITUDE=param_value;
- break;
- case DARWIN_STEP_DIRECTION: A_MOVE_AMPLITUDE=((float)param_value)/8.0;
- break;
- case DARWIN_FOOT_HEIGHT: Z_MOVE_AMPLITUDE=param_value;
- break;
- case DARWIN_SWING_RIGHT_LEFT: Y_SWAP_AMPLITUDE=param_value;
- break;
- case DARWIN_SWING_TOP_DOWN: Z_SWAP_AMPLITUDE=param_value;
- break;
- case DARWIN_PELVIS_OFFSET: PELVIS_OFFSET=((float)param_value)/8.0;
- break;
- case DARWIN_ARM_SWING_GAIN: ARM_SWING_GAIN=((float)param_value)/32.0;
- break;
- case DARWIN_P_GAIN: P_GAIN=param_value;
- break;
- case DARWIN_I_GAIN: I_GAIN=param_value;
- break;
- case DARWIN_D_GAIN: D_GAIN=param_value;
- break;
- case DARWIN_MAX_ACC: max_acceleration=param_value/1024.0;
- break;
- case DARWIN_MAX_ROT_ACC: max_rot_accel=param_value/1024.0;
- break;
- }
- ram_write_byte(param_id,param_value);
-}
-
void walking_start(void)
{
- ram_set_bit(DARWIN_WALK_STATUS,0);
- ram_set_bit(DARWIN_WALK_CONTROL,0);
+ ram_data[DARWIN_WALK_CNTRL]|=WALK_STATUS;
m_Ctrl_Running=0x01;
}
void walking_stop(void)
{
- ram_set_bit(DARWIN_WALK_CONTROL,1);
m_Ctrl_Running=0x00;
}
uint8_t is_walking(void)
{
- uint8_t walking;
-
- ram_read_byte(DARWIN_WALK_STATUS,&walking);
-
- return walking;
+ 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 angle[14], ep[12];
- uint8_t walking,i;
-
- //R_HIP_YAW, R_HIP_ROLL, R_HIP_PITCH, R_KNEE, R_ANKLE_PITCH, R_ANKLE_ROLL, L_HIP_YAW, L_HIP_ROLL, L_HIP_PITCH, L_KNEE, L_ANKLE_PITCH, L_ANKLE_ROLL, R_ARM_SWING, L_ARM_SWING
- float initAngle[14] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-48.345,41.313};
-
- if(m_Time == 0)
+ 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();
- m_Phase = PHASE0;
- if(m_Ctrl_Running == 0x00)
+ 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_write_byte(DARWIN_WALK_STATUS,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
{
- X_MOVE_AMPLITUDE = 0;
- Y_MOVE_AMPLITUDE = 0;
- A_MOVE_AMPLITUDE = 0;
+ 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))
+ else if(m_Time>=(m_Phase_Time1-walking_period/2.0) && m_Time<(m_Phase_Time1+walking_period/2.0))
{
update_param_move();
- m_Phase = PHASE1;
+ 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))
+ 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;
- m_Phase = PHASE2;
- if(m_Ctrl_Running == 0x00)
+ 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_write_byte(DARWIN_WALK_STATUS,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
{
- X_MOVE_AMPLITUDE = 0;
- Y_MOVE_AMPLITUDE = 0;
- A_MOVE_AMPLITUDE = 0;
+ 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))
+ else if(m_Time>=(m_Phase_Time3-walking_period/2.0) && m_Time<(m_Phase_Time3+walking_period/2.0))
{
update_param_move();
- m_Phase = PHASE3;
+ 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_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);
@@ -465,10 +333,10 @@ void walking_process(void)
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;
+ pelvis_offset_l=0;
+ pelvis_offset_r=0;
}
- else if(m_Time <= m_SSP_Time_End_L)
+ 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);
@@ -481,7 +349,7 @@ void walking_process(void)
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)
+ 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);
@@ -491,10 +359,10 @@ void walking_process(void)
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;
- pelvis_offset_r = 0;
+ pelvis_offset_l=0.0;
+ pelvis_offset_r=0.0;
}
- else if(m_Time <= m_SSP_Time_End_R)
+ 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);
@@ -517,81 +385,125 @@ void walking_process(void)
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;
- pelvis_offset_r = 0;
+ pelvis_offset_l=0.0;
+ pelvis_offset_r=0.0;
}
- a_move_l = 0;
- b_move_l = 0;
- a_move_r = 0;
- b_move_r = 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;
+ 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)
+/* if(m_Time<=m_SSP_Time_End_L)
{
- m_Body_Swing_Y = -ep[7];
- m_Body_Swing_Z = ep[8];
+ 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_Y=-ep[1];
+ m_Body_Swing_Z=ep[2];
}
- m_Body_Swing_Z -= LEG_LENGTH;
+ m_Body_Swing_Z-=DARWIN_LEG_LENGTH;*/
// Compute arm swing
- if(m_X_Move_Amplitude == 0)
+ if(m_X_Move_Amplitude==0)
{
- angle[12] = 0; // Right
- angle[13] = 0; // Left
+ angle[12]=0; // Right
+ angle[13]=0; // Left
}
else
{
- angle[12] = wsin(m_Time, m_PeriodTime, PI * 1.5, -m_X_Move_Amplitude * m_Arm_Swing_Gain, 0);
- angle[13] = wsin(m_Time, m_PeriodTime, PI * 1.5, m_X_Move_Amplitude * m_Arm_Swing_Gain, 0);
+ 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);
}
- ram_read_byte(DARWIN_WALK_STATUS,&walking);
- if(walking == 0x01)
+
+ if(ram_data[DARWIN_WALK_CNTRL]&WALK_STATUS)
{
m_Time += walking_period;
if(m_Time >= m_PeriodTime)
m_Time = 0;
}
- // Compute angles
- if((computeIK(&angle[0], ep[0], ep[1], ep[2], ep[3], ep[4], ep[5]) == 1)
- && (computeIK(&angle[6], ep[6], ep[7], ep[8], ep[9], ep[10], ep[11]) == 1))
- {
- for(i=0; i<12; i++)
- angle[i] *= 180.0 / PI;
- }
- else
- return;// Do not use angle;
+
+ // 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
- for(i=0; i<14; i++)
+ if(manager_get_module(R_HIP_YAW)==MM_WALKING)
+ manager_current_angles[R_HIP_YAW]=((180.0*(angle[0]-PI/4.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]-DARWIN_PITCH_OFFSET))/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]+DARWIN_PITCH_OFFSET))/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/4.0))/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]-DARWIN_PITCH_OFFSET))/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]+DARWIN_PITCH_OFFSET))/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]-90.0)*65536.0;
+ if(manager_get_module(L_SHOULDER_PITCH)==MM_WALKING)
+ manager_current_angles[L_SHOULDER_PITCH]=(-angle[13]+90.0)*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(manager_get_module(i)==MM_WALKING)
- {
- if(i==1)
- current_angles[servo_seq[i]]=(initAngle[i]+(float)dir[i]*(angle[i] + pelvis_offset_r))*65536;// format 9|7
- else if(i==7)
- current_angles[servo_seq[i]]=(initAngle[i]+(float)dir[i]*(angle[i] + pelvis_offset_l))*65536;// format 9|7
- else if(i==2 || i==8)
- current_angles[servo_seq[i]]=(initAngle[i]+(float)dir[i]*(angle[i] - HIP_PITCH_OFFSET))*65536;// format 9|7
- else
- current_angles[servo_seq[i]]=(initAngle[i]+(float)dir[i] * angle[i])*65536;//format 9|7
- }
+ if(data[DARWIN_WALK_CNTRL-address]&WALK_START)
+ walking_start();
+ if(data[DARWIN_WALK_CNTRL-address]&WALK_STOP)
+ walking_stop();
}
+ // 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];
}