diff --git a/Makefile b/Makefile index b45c2be89f203cee089eeae8b20f08e8abae2ec6..62d4f0ce3d6e6b5d297e25a0dfbb363f1dc157c3 100755 --- a/Makefile +++ b/Makefile @@ -1,8 +1,8 @@ # setup # modified by zerom for WinARM 8/2010 -STM32_HAL_PATH=/home/shernand/humanoids/stm32_hal -STM32_LIBRARIES_PATH=/home/shernand/humanoids/stm32_libraries +STM32_HAL_PATH=/home/sergi/humanoids/stm32_hal +STM32_LIBRARIES_PATH=/home/sergi/humanoids/stm32_libraries PROJECT_NAME=darwin_firmware #TARGET_FILES=$(wildcard src/*.c) @@ -23,6 +23,8 @@ TARGET_FILES+=src/motion_pages.c TARGET_FILES+=src/walking.c TARGET_FILES+=src/darwin_math.c TARGET_FILES+=src/darwin_kinematics.c +TARGET_FILES+=src/joint_motion.c +TARGET_FILES+=src/head_tracking.c TARGET_PROCESSOR=STM32F103RE diff --git a/include/darwin_registers.h b/include/darwin_registers.h index 9c19ae0680929e6c287d7ae6ab9c03829d2166a7..f9f515baeb8fb0232bd19df8f03050d9c5230cb2 100644 --- a/include/darwin_registers.h +++ b/include/darwin_registers.h @@ -65,6 +65,14 @@ extern "C" { #define WALK_ARM_SWING_GAIN ((unsigned short int)0x0041) #define WALK_MAX_VEL ((unsigned short int)0x0042) #define WALK_MAX_ROT_VEL ((unsigned short int)0x0043) +#define HEAD_PAN_P ((unsigned short int)0x0044) +#define HEAD_PAN_I ((unsigned short int)0x0046) +#define HEAD_PAN_D ((unsigned short int)0x0048) +#define HEAD_PAN_I_CLAMP ((unsigned short int)0x004A) +#define HEAD_TILT_P ((unsigned short int)0x004C) +#define HEAD_TILT_I ((unsigned short int)0x004E) +#define HEAD_TILT_D ((unsigned short int)0x0050) +#define HEAD_TILT_I_CLAMP ((unsigned short int)0x0052) #define LAST_EEPROM_OFFSET ((unsigned short int)0x00FF) @@ -137,6 +145,22 @@ typedef enum { DARWIN_WALK_ARM_SWING_GAIN = WALK_ARM_SWING_GAIN, DARWIN_WALK_MAX_VEL = WALK_MAX_VEL, DARWIN_WALK_MAX_ROT_VEL = WALK_MAX_ROT_VEL, + DARWIN_HEAD_PAN_P_L = HEAD_PAN_P,// constant in fixed point format 0|16 + DARWIN_HEAD_PAN_P_H = HEAD_PAN_P+1, + DARWIN_HEAD_PAN_I_L = HEAD_PAN_I,// constant in fixed point format 0|16 + DARWIN_HEAD_PAN_I_H = HEAD_PAN_I+1, + DARWIN_HEAD_PAN_D_L = HEAD_PAN_D,// constant in fixed point format 0|16 + DARWIN_HEAD_PAN_D_H = HEAD_PAN_D+1, + DARWIN_HEAD_PAN_I_CLAMP_L = HEAD_PAN_I_CLAMP,// max error in fixed point format 9|7 + DARWIN_HEAD_PAN_I_CLAMP_H = HEAD_PAN_I_CLAMP+1, + DARWIN_HEAD_TILT_P_L = HEAD_TILT_P,// constant in fixed point format 0|16 + DARWIN_HEAD_TILT_P_H = HEAD_TILT_P+1, + DARWIN_HEAD_TILT_I_L = HEAD_TILT_I,// constant in fixed point format 0|16 + DARWIN_HEAD_TILT_I_H = HEAD_TILT_I+1, + DARWIN_HEAD_TILT_D_L = HEAD_TILT_D,// constant in fixed point format 0|16 + DARWIN_HEAD_TILT_D_H = HEAD_TILT_D+1, + DARWIN_HEAD_TILT_I_CLAMP_L = HEAD_TILT_I_CLAMP,// max error in fixed point format 9|7 + DARWIN_HEAD_TILT_I_CLAMP_H = HEAD_TILT_I_CLAMP+1, DARWIN_TX_LED_CNTRL = 0x0100, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 // | | | | blink | toggle | value | internally used DARWIN_TX_LED_PERIOD_L = 0x0101, @@ -337,7 +361,189 @@ typedef enum { // current phase walking stop walking start walking DARWIN_WALK_STEP_FW_BW = 0x019F, DARWIN_WALK_STEP_LEFT_RIGHT = 0x01A0, - DARWIN_WALK_STEP_DIRECTION = 0x01A1 + DARWIN_WALK_STEP_DIRECTION = 0x01A1, + DARWIN_JOINT_GRP0_CNTRL = 0x01A2,// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // | | | joints moving | | | stop joint | start joint + DARWIN_JOINT_GRP0_SERVOS0 = 0x01A3,// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // servo7 | servo6 | servo5 | servo4 | servo3 | servo2 | servo1 | servo0 + DARWIN_JOINT_GRP0_SERVOS1 = 0x01A4,// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // servo15 | servo14 | servo13 | servo12 | servo11 | servo10 | servo9 | servo8 + DARWIN_JOINT_GRP0_SERVOS2 = 0x01A5,// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // servo23 | servo22 | servo21 | servo20 | servo19 | servo18 | servo17 | servo16 + DARWIN_JOINT_GRP0_SERVOS3 = 0x01A6,// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // servo31 | servo30 | servo29 | servo28 | servo27 | servo26 | servo25 | servo24 + DARWIN_JOINT_GRP1_CNTRL = 0x01A7,// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // | | | joints moving | | | stop joint | start joint + DARWIN_JOINT_GRP1_SERVOS0 = 0x01A8,// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // servo7 | servo6 | servo5 | servo4 | servo3 | servo2 | servo1 | servo0 + DARWIN_JOINT_GRP1_SERVOS1 = 0x01A9,// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // servo15 | servo14 | servo13 | servo12 | servo11 | servo10 | servo9 | servo8 + DARWIN_JOINT_GRP1_SERVOS2 = 0x01AA,// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // servo23 | servo22 | servo21 | servo20 | servo19 | servo18 | servo17 | servo16 + DARWIN_JOINT_GRP1_SERVOS3 = 0x01AB,// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // servo31 | servo30 | servo29 | servo28 | servo27 | servo26 | servo25 | servo24 + DARWIN_JOINT_GRP2_CNTRL = 0x01AC,// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // | | | joints moving | | | stop joint | start joint + DARWIN_JOINT_GRP2_SERVOS0 = 0x01AD,// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // servo7 | servo6 | servo5 | servo4 | servo3 | servo2 | servo1 | servo0 + DARWIN_JOINT_GRP2_SERVOS1 = 0x01AE,// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // servo15 | servo14 | servo13 | servo12 | servo11 | servo10 | servo9 | servo8 + DARWIN_JOINT_GRP2_SERVOS2 = 0x01AF,// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // servo23 | servo22 | servo21 | servo20 | servo19 | servo18 | servo17 | servo16 + DARWIN_JOINT_GRP2_SERVOS3 = 0x01B0,// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // servo31 | servo30 | servo29 | servo28 | servo27 | servo26 | servo25 | servo24 + DARWIN_JOINT_GRP3_CNTRL = 0x01B1,// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // | | | joints moving | | | stop joint | start joint + DARWIN_JOINT_GRP3_SERVOS0 = 0x01B2,// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // servo7 | servo6 | servo5 | servo4 | servo3 | servo2 | servo1 | servo0 + DARWIN_JOINT_GRP3_SERVOS1 = 0x01B3,// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // servo15 | servo14 | servo13 | servo12 | servo11 | servo10 | servo9 | servo8 + DARWIN_JOINT_GRP3_SERVOS2 = 0x01B4,// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // servo23 | servo22 | servo21 | servo20 | servo19 | servo18 | servo17 | servo16 + DARWIN_JOINT_GRP3_SERVOS3 = 0x01B5,// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // servo31 | servo30 | servo29 | servo28 | servo27 | servo26 | servo25 | servo24 + DARWIN_JOINT_SERVO0_ANGLE_L = 0x01B6, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO0_ANGLE_H = 0x01B7, + DARWIN_JOINT_SERVO1_ANGLE_L = 0x01B8, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO1_ANGLE_H = 0x01B9, + DARWIN_JOINT_SERVO2_ANGLE_L = 0x01BA, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO2_ANGLE_H = 0x01BB, + DARWIN_JOINT_SERVO3_ANGLE_L = 0x01BC, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO3_ANGLE_H = 0x01BD, + DARWIN_JOINT_SERVO4_ANGLE_L = 0x01BE, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO4_ANGLE_H = 0x01BF, + DARWIN_JOINT_SERVO5_ANGLE_L = 0x01C0, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO5_ANGLE_H = 0x01C1, + DARWIN_JOINT_SERVO6_ANGLE_L = 0x01C2, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO6_ANGLE_H = 0x01C3, + DARWIN_JOINT_SERVO7_ANGLE_L = 0x01C4, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO7_ANGLE_H = 0x01C5, + DARWIN_JOINT_SERVO8_ANGLE_L = 0x01C6, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO8_ANGLE_H = 0x01C7, + DARWIN_JOINT_SERVO9_ANGLE_L = 0x01C8, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO9_ANGLE_H = 0x01C9, + DARWIN_JOINT_SERVO10_ANGLE_L = 0x01CA, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO10_ANGLE_H = 0x01CB, + DARWIN_JOINT_SERVO11_ANGLE_L = 0x01CC, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO11_ANGLE_H = 0x01CD, + DARWIN_JOINT_SERVO12_ANGLE_L = 0x01CE, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO12_ANGLE_H = 0x01CF, + DARWIN_JOINT_SERVO13_ANGLE_L = 0x01D0, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO13_ANGLE_H = 0x01D1, + DARWIN_JOINT_SERVO14_ANGLE_L = 0x01D2, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO14_ANGLE_H = 0x01D3, + DARWIN_JOINT_SERVO15_ANGLE_L = 0x01D4, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO15_ANGLE_H = 0x01D5, + DARWIN_JOINT_SERVO16_ANGLE_L = 0x01D6, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO16_ANGLE_H = 0x01D7, + DARWIN_JOINT_SERVO17_ANGLE_L = 0x01D8, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO17_ANGLE_H = 0x01D9, + DARWIN_JOINT_SERVO18_ANGLE_L = 0x01DA, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO18_ANGLE_H = 0x01DB, + DARWIN_JOINT_SERVO19_ANGLE_L = 0x01DC, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO19_ANGLE_H = 0x01DD, + DARWIN_JOINT_SERVO20_ANGLE_L = 0x01DE, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO20_ANGLE_H = 0x01DF, + DARWIN_JOINT_SERVO21_ANGLE_L = 0x01E0, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO21_ANGLE_H = 0x01E1, + DARWIN_JOINT_SERVO22_ANGLE_L = 0x01E2, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO22_ANGLE_H = 0x01E3, + DARWIN_JOINT_SERVO23_ANGLE_L = 0x01E4, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO23_ANGLE_H = 0x01E5, + DARWIN_JOINT_SERVO24_ANGLE_L = 0x01E6, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO24_ANGLE_H = 0x01E7, + DARWIN_JOINT_SERVO25_ANGLE_L = 0x01E8, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO25_ANGLE_H = 0x01E9, + DARWIN_JOINT_SERVO26_ANGLE_L = 0x01EA, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO26_ANGLE_H = 0x01EB, + DARWIN_JOINT_SERVO27_ANGLE_L = 0x01EC, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO27_ANGLE_H = 0x01ED, + DARWIN_JOINT_SERVO28_ANGLE_L = 0x01EE, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO28_ANGLE_H = 0x01EF, + DARWIN_JOINT_SERVO29_ANGLE_L = 0x01F0, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO29_ANGLE_H = 0x01F1, + DARWIN_JOINT_SERVO30_ANGLE_L = 0x01F2, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO30_ANGLE_H = 0x01F3, + DARWIN_JOINT_SERVO31_ANGLE_L = 0x01F4, // angle in fixed point format 9|7 + DARWIN_JOINT_SERVO31_ANGLE_H = 0x01F5, + DARWIN_JOINT_SERVO0_SPEED = 0x01F6, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO1_SPEED = 0x01F7, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO2_SPEED = 0x01F8, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO3_SPEED = 0x01F9, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO4_SPEED = 0x01FA, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO5_SPEED = 0x01FB, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO6_SPEED = 0x01FC, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO7_SPEED = 0x01FD, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO8_SPEED = 0x01FE, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO9_SPEED = 0x01FF, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO10_SPEED = 0x0200, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO11_SPEED = 0x0201, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO12_SPEED = 0x0202, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO13_SPEED = 0x0203, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO14_SPEED = 0x0204, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO15_SPEED = 0x0205, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO16_SPEED = 0x0206, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO17_SPEED = 0x0207, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO18_SPEED = 0x0208, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO19_SPEED = 0x0209, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO20_SPEED = 0x020A, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO21_SPEED = 0x020B, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO22_SPEED = 0x020C, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO23_SPEED = 0x020D, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO24_SPEED = 0x020E, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO25_SPEED = 0x020F, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO26_SPEED = 0x0210, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO27_SPEED = 0x0211, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO28_SPEED = 0x0212, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO29_SPEED = 0x0213, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO30_SPEED = 0x0214, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO31_SPEED = 0x0215, // speed in fixed point format 8|0 + DARWIN_JOINT_SERVO0_ACCEL = 0x0216, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO1_ACCEL = 0x0217, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO2_ACCEL = 0x0218, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO3_ACCEL = 0x0219, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO4_ACCEL = 0x021A, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO5_ACCEL = 0x021B, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO6_ACCEL = 0x021C, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO7_ACCEL = 0x021D, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO8_ACCEL = 0x021E, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO9_ACCEL = 0x021F, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO10_ACCEL = 0x0220, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO11_ACCEL = 0x0221, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO12_ACCEL = 0x0222, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO13_ACCEL = 0x0223, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO14_ACCEL = 0x0224, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO15_ACCEL = 0x0225, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO16_ACCEL = 0x0226, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO17_ACCEL = 0x0227, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO18_ACCEL = 0x0228, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO19_ACCEL = 0x0229, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO20_ACCEL = 0x022A, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO21_ACCEL = 0x022B, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO22_ACCEL = 0x022C, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO23_ACCEL = 0x022D, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO24_ACCEL = 0x022E, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO25_ACCEL = 0x022F, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO26_ACCEL = 0x0230, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO27_ACCEL = 0x0231, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO28_ACCEL = 0x0232, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO29_ACCEL = 0x0233, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO30_ACCEL = 0x0234, // acceleration in fixed point format 8|0 + DARWIN_JOINT_SERVO31_ACCEL = 0x0235, // acceleration in fixed point format 8|0 + DARWIN_HEAD_CNTRL = 0x0236, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // | | | tracking | | | stop tracking | start tracking + DARWIN_HEAD_MAX_PAN_L = 0x0237, // angle in fixed point format 9|7 + DARWIN_HEAD_MAX_PAN_H = 0x0238, + DARWIN_HEAD_MIN_PAN_L = 0x0239, // angle in fixed point format 9|7 + DARWIN_HEAD_MIN_PAN_H = 0x023A, + DARWIN_HEAD_PAN_TARGET_L = 0x023B, // angle in fixed point format 9|7 + DARWIN_HEAD_PAN_TARGET_H = 0x023C, + DARWIN_HEAD_MAX_TILT_L = 0x023D, // angle in fixed point format 9|7 + DARWIN_HEAD_MAX_TILT_H = 0x023E, + DARWIN_HEAD_MIN_TILT_L = 0x023F, // angle in fixed point format 9|7 + DARWIN_HEAD_MIN_TILT_H = 0x0240, + DARWIN_HEAD_TILT_TARGET_L = 0x0241, // angle in fixed point format 9|7 + DARWIN_HEAD_TILT_TARGET_H = 0x0242 } darwin_registers; #define GPIO_BASE_ADDRESS 0x0100 @@ -405,6 +611,20 @@ typedef enum { #define WALK_STATUS 0x10 #define WALK_PHASE 0xC0 +#define JOINT_BASE_ADDRESS 0x01A2 +#define JOINT_MEM_LENGTH 148 +#define JOINT_START 0x01 +#define JOINT_STOP 0x02 +#define JOINT_STATUS 0x10 + +#define HEAD_BASE_ADDRESS 0x0236 +#define HEAD_MEM_LENGTH 13 +#define HEAD_EEPROM_ADDRESS 0x0044 +#define HEAD_EEPROM_LENGTH 16 +#define HEAD_START 0x01 +#define HEAD_STOP 0x02 +#define HEAD_STATUS 0x10 + #ifdef __cplusplus } #endif diff --git a/include/eeprom.h b/include/eeprom.h index e0b5189c7d4359c097a839dbe341410ee6d71762..6ae5309ae699919c016e15447869d4d9707d2c9d 100755 --- a/include/eeprom.h +++ b/include/eeprom.h @@ -83,7 +83,7 @@ extern "C" { #define PAGE_FULL ((uint8_t)0x80) /* Variables' number */ -#define NB_OF_VAR ((uint8_t)0x44) +#define NB_OF_VAR ((uint8_t)0x54) /* Exported types ------------------------------------------------------------*/ /* Exported macro ------------------------------------------------------------*/ diff --git a/include/eeprom_init.h b/include/eeprom_init.h index 1917b78d07d036ff3168576ca92d7718d5bfd9bd..329b4ef37c86090a36cb3594c316f459ec0a88f6 100644 --- a/include/eeprom_init.h +++ b/include/eeprom_init.h @@ -65,6 +65,14 @@ extern "C" { #define DEFAULT_WALK_ARM_SWING_GAIN 0x0030 // 1.5 in fixed point format 3 | 5 #define DEFAULT_WALK_MAX_VEL 0x0016 // 20 mm/s #define DEFAULT_WALK_MAX_ROT_VEL 0x0008 // 8 degrees/s in fixed point format 5 | 3 +#define DEFAULT_HEAD_PAN_P 0x028F // 0.01 in fixed point format 0|16 +#define DEFAULT_HEAD_PAN_I 0x0000 // 0.0 in fixed point format 0|16 +#define DEFAULT_HEAD_PAN_D 0x0000 // 0.0 in fixed point format 0|16 +#define DEFAULT_HEAD_PAN_I_CLAMP 0x0000 // 0.0 in fixed point format 9|7 +#define DEFAULT_HEAD_TILT_P 0x028F // 0.01 in fixed point format 0|16 +#define DEFAULT_HEAD_TILT_I 0x0000 // 0.0 in fixed point format 0|16 +#define DEFAULT_HEAD_TILT_D 0x0000 // 0.0 in fixed point format 0|16 +#define DEFAULT_HEAD_TILT_I_CLAMP 0x0000 // 0.0 in fixed point format 9|7 #ifdef __cplusplus diff --git a/include/head_tracking.h b/include/head_tracking.h index ddba99bcf5aed0eb8862c827514aa58039072480..31d3a53c3c1a6bd0db61046305b257beef4ccee5 100644 --- a/include/head_tracking.h +++ b/include/head_tracking.h @@ -1,23 +1,41 @@ #ifndef _HEAD_TRACKING_H #define _HEAD_TRACKINH_H -#include "stm32f10x.h" +#ifdef __cplusplus +extern "C" { +#endif + +#include "stm32f1xx.h" +#include "motion_manager.h" // public functions -void head_tracking_init(uint16_t period); +void head_tracking_init(uint16_t period_us); +inline void head_tracking_set_period(uint16_t period_us); +inline uint16_t head_tracking_get_period(void); void head_tracking_start(void); void head_tracking_stop(void); -void head_tracking_set_period(int16_t period); -void head_tracking_set_pan_p(uint8_t value); -void head_tracking_set_pan_i(uint8_t value); -void head_tracking_set_pan_d(uint8_t value); -void head_tracking_set_tilt_p(uint8_t value); -void head_tracking_set_tilt_i(uint8_t value); -void head_tracking_set_tilt_d(uint8_t value); +uint8_t head_is_tracking(void); +void head_tracking_set_pan_range(int16_t max_angle,int16_t min_angle); +void head_tracking_get_pan_range(int16_t *max_angle,int16_t *min_angle); +void head_tracking_set_pan_pid(uint16_t kp,uint16_t ki,uint16_t kd, uint16_t i_clamp); +void head_tracking_get_pan_pid(uint16_t *kp,uint16_t *ki,uint16_t *kd, uint16_t *i_clamp); +void head_tracking_set_tilt_range(int16_t max_angle,int16_t min_angle); +void head_tracking_get_tilt_range(int16_t *max_angle,int16_t *min_angle); +void head_tracking_set_tilt_pid(uint16_t kp,uint16_t ki,uint16_t kd, uint16_t i_clamp); +void head_tracking_get_tilt_pid(uint16_t *kp,uint16_t *ki,uint16_t *kd, uint16_t *i_clamp); void head_tracking_set_target_pan(int16_t target); void head_tracking_set_target_tilt(int16_t target); +// operation functions +uint8_t head_tracking_in_range(unsigned short int address, unsigned short int length); +void head_tracking_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data); +void head_tracking_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data); + // motion manager process function void head_tracking_process(void); +#ifdef __cplusplus +} +#endif + #endif diff --git a/include/imu.h b/include/imu.h index 0a72629d94a0214c03a2cabc1aec74e3ec4ece91..079304bf1b235660acb081ab1186a926366633c0 100755 --- a/include/imu.h +++ b/include/imu.h @@ -14,6 +14,7 @@ void imu_stop(void); void imu_set_calibration_samples(uint16_t num_samples); void imu_start_gyro_cal(void); void imu_get_gyro_data(int32_t *x, int32_t *y, int32_t *z); +uint8_t imu_is_gyro_calibrated(void); // operation functions uint8_t imu_in_range(unsigned short int address,unsigned short int length); void imu_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data); diff --git a/include/joint_motion.h b/include/joint_motion.h index 654f9d9df99b2592984781b34de134550fa21828..70587b38aa8d0caad2dc7aaf26318d7360ee1525 100644 --- a/include/joint_motion.h +++ b/include/joint_motion.h @@ -1,20 +1,34 @@ #ifndef _JOINT_MOTION_H #define _JOINT_MOTION_H -#include "stm32f10x.h" +#ifdef __cplusplus +extern "C" { +#endif + +#include "stm32f1xx.h" +#include "motion_manager.h" + +#define NUM_JOINT_GROUPS 4 // public functions -void joint_motion_init(uint16_t period); -void joint_motion_set_period(uint16_t period); -uint16_t joint_motion_get_period(void); -inline void joint_motion_load_target_angle(uint8_t servo_id, int16_t angle); -inline void joint_motion_load_target_speed(uint8_t servo_id, uint8_t speed); -inline void joint_motion_load_target_accel(uint8_t servo_id, uint8_t accel); -void joint_motion_start(void); -void joint_motion_stop(void); -uint8_t are_joints_moving(void); +void joint_motion_init(uint16_t period_us); +inline void joint_motion_set_period(uint16_t period_us); +inline uint16_t joint_motion_get_period(void); +void joint_motion_set_group_servos(uint8_t group_id,uint32_t servos); +uint32_t joint_motion_get_group_servos(uint8_t group_id); +void joint_motion_start(uint8_t group_id); +void joint_motion_stop(uint8_t group_id); +uint8_t are_joints_moving(uint8_t group_id); +// operation functions +uint8_t joint_motion_in_range(unsigned short int address, unsigned short int length); +void joint_motion_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data); +void joint_motion_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data); // motion manager interface functions void joint_motion_process(void); +#ifdef __cplusplus +} +#endif + #endif diff --git a/include/motion_manager.h b/include/motion_manager.h index 04caa19e1c496f5fe60de50f61349fdbeec819ce..c3987f5af395e597c677889d9083b498dc207529 100755 --- a/include/motion_manager.h +++ b/include/motion_manager.h @@ -33,7 +33,8 @@ typedef enum { typedef enum {MM_NONE = 0, MM_ACTION = 1, MM_WALKING = 2, - MM_JOINTS = 3} TModules; + MM_JOINTS = 3, + MM_HEAD = 4} TModules; typedef struct { diff --git a/include/ram.h b/include/ram.h index b096a64789613b6a9e627e74b5095a24768341d6..d920367d293c06bb7aa4e8a4ce2fe2cd1a47bd0b 100755 --- a/include/ram.h +++ b/include/ram.h @@ -14,7 +14,7 @@ extern "C" { #define RAM_BAD_BIT -3 #define RAM_BAD_ACCESS -4 -#define RAM_SIZE 512 +#define RAM_SIZE 1024 extern uint8_t ram_data[RAM_SIZE]; diff --git a/src/darwin_dyn_slave.c b/src/darwin_dyn_slave.c index 2970d520007dd8c45459d4fa3b01aa7f586e8ffd..952c846091e9a0eda5826d57628796939e5416a5 100755 --- a/src/darwin_dyn_slave.c +++ b/src/darwin_dyn_slave.c @@ -9,6 +9,8 @@ #include "motion_manager.h" #include "action.h" #include "walking.h" +#include "joint_motion.h" +#include "head_tracking.h" /* timer for the execution of the dynamixel slave loop */ #define DYN_SLAVE_TIMER TIM7 @@ -80,6 +82,12 @@ unsigned char darwin_on_write(unsigned short int address,unsigned short int leng // walk commands if(walking_in_range(address,length)) walking_process_write_cmd(address,length,data); + // joint motion commands + if(joint_motion_in_range(address,length)) + joint_motion_process_write_cmd(address,length,data); + // head_tracking commands + if(head_tracking_in_range(address,length)) + head_tracking_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/eeprom.c b/src/eeprom.c index 20b740e3926f02b19f8478d5f67e4372c761d885..701a3ad081d42f9a51233fbb249ead6ff147ced6 100755 --- a/src/eeprom.c +++ b/src/eeprom.c @@ -121,7 +121,24 @@ uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE, DEFAULT_WALK_PELVIS_OFFSET, WALK_PELVIS_OFFSET, DEFAULT_WALK_ARM_SWING_GAIN, WALK_ARM_SWING_GAIN, DEFAULT_WALK_MAX_VEL, WALK_MAX_VEL, - DEFAULT_WALK_MAX_ROT_VEL, WALK_MAX_ROT_VEL}; + DEFAULT_WALK_MAX_ROT_VEL, WALK_MAX_ROT_VEL, + DEFAULT_HEAD_PAN_P&0xFF, HEAD_PAN_P, + DEFAULT_HEAD_PAN_P>>8, HEAD_PAN_P+1, + DEFAULT_HEAD_PAN_I&0xFF, HEAD_PAN_I, + DEFAULT_HEAD_PAN_I>>8, HEAD_PAN_I+1, + DEFAULT_HEAD_PAN_D&0xFF, HEAD_PAN_D, + DEFAULT_HEAD_PAN_D>>8, HEAD_PAN_D+1, + DEFAULT_HEAD_PAN_I_CLAMP&0xFF, HEAD_PAN_I_CLAMP, + DEFAULT_HEAD_PAN_I_CLAMP>>8, HEAD_PAN_I_CLAMP+1, + DEFAULT_HEAD_PAN_D>>8, HEAD_PAN_D+1, + DEFAULT_HEAD_TILT_P&0xFF, HEAD_TILT_P, + DEFAULT_HEAD_TILT_P>>8, HEAD_TILT_P+1, + DEFAULT_HEAD_TILT_I&0xFF, HEAD_TILT_I, + DEFAULT_HEAD_TILT_I>>8, HEAD_TILT_I+1, + DEFAULT_HEAD_TILT_D&0xFF, HEAD_TILT_D, + DEFAULT_HEAD_TILT_D>>8, HEAD_TILT_D+1, + DEFAULT_HEAD_TILT_I_CLAMP&0xFF, HEAD_TILT_I_CLAMP, + DEFAULT_HEAD_TILT_I_CLAMP>>8, HEAD_TILT_I_CLAMP+1}; /* Private function prototypes -----------------------------------------------*/ /* Private functions ---------------------------------------------------------*/ diff --git a/src/head_tracking.c b/src/head_tracking.c index 8acf565fc1e0bb66b9485d1fc3a7116e0228f105..d996ad9ec8c7454d70a1e3334de59af01cf3d260 100644 --- a/src/head_tracking.c +++ b/src/head_tracking.c @@ -1,57 +1,71 @@ #include "head_tracking.h" #include "ram.h" -#include "motion_manager.h" - -#define DEFAULT_PAN_P 40// -#define DEFAULT_PAN_I 0// -#define DEFAULT_PAN_D 0// - -#define DEFAULT_TILT_P 40// -#define DEFAULT_TILT_I 0// -#define DEFAULT_TILT_D 0// // private variables -int64_t prev_error_pan;// 48|16 -int64_t prev_error_tilt;// 48|16 -int64_t integral_pan; -int64_t integral_tilt; -uint8_t pan_p; -uint8_t pan_i; -uint8_t pan_d; -uint8_t tilt_p; -uint8_t tilt_i; -uint8_t tilt_d; -int64_t pan_target;// 48|16 -int64_t tilt_target;// 48|16 +typedef struct{ + uint16_t kp; + uint16_t ki; + uint16_t kd; + int64_t prev_error;// 48|16 + int64_t integral_part; + int64_t integral_clamp; + int64_t max_angle; + int64_t min_angle; + int64_t target_angle; +}TJointControl; + +TJointControl head_tracking_pan; +TJointControl head_tracking_tilt; int64_t head_tracking_period; +int16_t head_tracking_period_us; uint8_t head_tracking_enabled; // public functions -void head_tracking_init(uint16_t period) +void head_tracking_init(uint16_t period_us) { /* initialize private variables */ - prev_error_pan=0; - prev_error_tilt=0; - integral_pan=0; - integral_tilt=0; - pan_p=DEFAULT_PAN_P; - ram_write_byte(DARWIN_HEAD_PAN_P,DEFAULT_PAN_P); - pan_i=DEFAULT_PAN_I; - ram_write_byte(DARWIN_HEAD_PAN_I,DEFAULT_PAN_I); - pan_d=DEFAULT_PAN_D; - ram_write_byte(DARWIN_HEAD_PAN_D,DEFAULT_PAN_D); - tilt_p=DEFAULT_TILT_P; - ram_write_byte(DARWIN_HEAD_TILT_P,DEFAULT_TILT_P); - tilt_i=DEFAULT_TILT_I; - ram_write_byte(DARWIN_HEAD_TILT_I,DEFAULT_TILT_I); - tilt_d=DEFAULT_TILT_D; - ram_write_byte(DARWIN_HEAD_TILT_D,DEFAULT_TILT_D); - pan_target=0; - tilt_target=0; - head_tracking_period=period; + head_tracking_pan.kp=ram_data[DARWIN_HEAD_PAN_P_L]+(ram_data[DARWIN_HEAD_PAN_P_H]<<8); + head_tracking_pan.ki=ram_data[DARWIN_HEAD_PAN_I_L]+(ram_data[DARWIN_HEAD_PAN_I_H]<<8); + head_tracking_pan.kd=ram_data[DARWIN_HEAD_PAN_D_L]+(ram_data[DARWIN_HEAD_PAN_D_H]<<8); + head_tracking_pan.prev_error=0; + head_tracking_pan.integral_part=0; + head_tracking_pan.integral_clamp=(ram_data[DARWIN_HEAD_PAN_I_CLAMP_L]+(ram_data[DARWIN_HEAD_PAN_I_CLAMP_H]<<8))<<9; + head_tracking_pan.min_angle=manager_get_cw_angle_limit(L_PAN)<<9; + ram_data[DARWIN_HEAD_MIN_PAN_L]=manager_get_cw_angle_limit(L_PAN)%256; + ram_data[DARWIN_HEAD_MIN_PAN_H]=manager_get_cw_angle_limit(L_PAN)/256; + head_tracking_pan.max_angle=manager_get_ccw_angle_limit(L_PAN)<<9; + ram_data[DARWIN_HEAD_MAX_PAN_L]=manager_get_ccw_angle_limit(L_PAN)%256; + ram_data[DARWIN_HEAD_MAX_PAN_H]=manager_get_ccw_angle_limit(L_PAN)/256; + head_tracking_pan.target_angle=0; + head_tracking_tilt.kp=ram_data[DARWIN_HEAD_TILT_P_L]+(ram_data[DARWIN_HEAD_TILT_P_H]<<8); + head_tracking_tilt.ki=ram_data[DARWIN_HEAD_TILT_I_L]+(ram_data[DARWIN_HEAD_TILT_I_H]<<8); + head_tracking_tilt.kd=ram_data[DARWIN_HEAD_TILT_D_L]+(ram_data[DARWIN_HEAD_TILT_D_H]<<8); + head_tracking_tilt.prev_error=0; + head_tracking_tilt.integral_part=0; + head_tracking_tilt.integral_clamp=(ram_data[DARWIN_HEAD_TILT_I_CLAMP_L]+(ram_data[DARWIN_HEAD_TILT_I_CLAMP_H]<<8))<<9; + head_tracking_tilt.min_angle=manager_get_cw_angle_limit(L_TILT)<<9; + ram_data[DARWIN_HEAD_MIN_TILT_L]=manager_get_cw_angle_limit(L_TILT)%256; + ram_data[DARWIN_HEAD_MIN_TILT_H]=manager_get_cw_angle_limit(L_TILT)/256; + head_tracking_tilt.max_angle=manager_get_ccw_angle_limit(L_TILT)<<9; + ram_data[DARWIN_HEAD_MAX_TILT_L]=manager_get_ccw_angle_limit(L_TILT)%256; + ram_data[DARWIN_HEAD_MAX_TILT_H]=manager_get_ccw_angle_limit(L_TILT)/256; + head_tracking_tilt.target_angle=0; + head_tracking_period=(period_us<<16)/1000000; + head_tracking_period_us=period_us; head_tracking_enabled=0x00; } +inline void head_tracking_set_period(uint16_t period_us) +{ + head_tracking_period=(period_us<<16)/1000000; + head_tracking_period_us=period_us; +} + +inline uint16_t head_tracking_get_period(void) +{ + return head_tracking_period_us; +} + void head_tracking_start(void) { head_tracking_enabled=0x01; @@ -62,57 +76,191 @@ void head_tracking_stop(void) head_tracking_enabled=0x00; } -void head_tracking_set_period(int16_t period) +uint8_t head_is_tracking(void) { - head_tracking_period=period; + return head_tracking_enabled; } -void head_tracking_set_pan_p(uint8_t value) +void head_tracking_set_pan_range(int16_t max_angle,int16_t min_angle) { - pan_p=value; - ram_write_byte(DARWIN_HEAD_PAN_P,value); + head_tracking_pan.max_angle=max_angle<<9; + ram_data[DARWIN_HEAD_MAX_PAN_L]=max_angle%256; + ram_data[DARWIN_HEAD_MAX_PAN_H]=max_angle/256; + head_tracking_pan.min_angle=min_angle<<9; + ram_data[DARWIN_HEAD_MIN_PAN_L]=min_angle%256; + ram_data[DARWIN_HEAD_MIN_PAN_H]=min_angle/256; } -void head_tracking_set_pan_i(uint8_t value) +void head_tracking_get_pan_range(int16_t *max_angle,int16_t *min_angle) { - pan_i=value; - ram_write_byte(DARWIN_HEAD_PAN_I,value); + *max_angle=head_tracking_pan.max_angle>>9; + *min_angle=head_tracking_pan.min_angle>>9; } -void head_tracking_set_pan_d(uint8_t value) +void head_tracking_set_pan_pid(uint16_t kp,uint16_t ki,uint16_t kd, uint16_t i_clamp) { - pan_d=value; - ram_write_byte(DARWIN_HEAD_PAN_D,value); + head_tracking_pan.kp=kp; + ram_data[DARWIN_HEAD_PAN_P_L]=kp%256; + ram_data[DARWIN_HEAD_PAN_P_H]=kp/256; + head_tracking_pan.ki=ki; + ram_data[DARWIN_HEAD_PAN_I_L]=ki%256; + ram_data[DARWIN_HEAD_PAN_I_H]=ki/256; + head_tracking_pan.kd=kd; + ram_data[DARWIN_HEAD_PAN_D_L]=kd%256; + ram_data[DARWIN_HEAD_PAN_D_H]=kd/256; + head_tracking_pan.integral_clamp=i_clamp<<9; + ram_data[DARWIN_HEAD_PAN_I_CLAMP_L]=i_clamp%256; + ram_data[DARWIN_HEAD_PAN_I_CLAMP_H]=i_clamp/256; } -void head_tracking_set_tilt_p(uint8_t value) +void head_tracking_get_pan_pid(uint16_t *kp,uint16_t *ki,uint16_t *kd, uint16_t *i_clamp) { - tilt_p=value; - ram_write_byte(DARWIN_HEAD_TILT_P,value); + *kp=head_tracking_pan.kp; + *ki=head_tracking_pan.ki; + *kd=head_tracking_pan.kd; + *i_clamp=head_tracking_pan.integral_clamp>>9; } -void head_tracking_set_tilt_i(uint8_t value) +void head_tracking_set_tilt_range(int16_t max_angle,int16_t min_angle) { - tilt_i=value; - ram_write_byte(DARWIN_HEAD_TILT_I,value); + head_tracking_tilt.max_angle=max_angle<<9; + ram_data[DARWIN_HEAD_MAX_TILT_L]=max_angle%256; + ram_data[DARWIN_HEAD_MAX_TILT_H]=max_angle/256; + head_tracking_tilt.min_angle=min_angle<<9; + ram_data[DARWIN_HEAD_MIN_TILT_L]=min_angle%256; + ram_data[DARWIN_HEAD_MIN_TILT_H]=min_angle/256; } -void head_tracking_set_tilt_d(uint8_t value) +void head_tracking_get_tilt_range(int16_t *max_angle,int16_t *min_angle) { - tilt_d=value; - ram_write_byte(DARWIN_HEAD_TILT_D,value); + *max_angle=head_tracking_tilt.max_angle>>9; + *min_angle=head_tracking_tilt.min_angle>>9; +} + +void head_tracking_set_tilt_pid(uint16_t kp,uint16_t ki,uint16_t kd, uint16_t i_clamp) +{ + head_tracking_tilt.kp=kp; + ram_data[DARWIN_HEAD_TILT_P_L]=kp%256; + ram_data[DARWIN_HEAD_TILT_P_H]=kp/256; + head_tracking_tilt.ki=ki; + ram_data[DARWIN_HEAD_TILT_I_L]=ki%256; + ram_data[DARWIN_HEAD_TILT_I_H]=ki/256; + head_tracking_tilt.kd=kd; + ram_data[DARWIN_HEAD_TILT_D_L]=kd%256; + ram_data[DARWIN_HEAD_TILT_D_H]=kd/256; + head_tracking_tilt.integral_clamp=i_clamp<<9; + ram_data[DARWIN_HEAD_TILT_I_CLAMP_L]=i_clamp%256; + ram_data[DARWIN_HEAD_TILT_I_CLAMP_H]=i_clamp/256; +} + +void head_tracking_get_tilt_pid(uint16_t *kp,uint16_t *ki,uint16_t *kd, uint16_t *i_clamp) +{ + *kp=head_tracking_tilt.kp; + *ki=head_tracking_tilt.ki; + *kd=head_tracking_tilt.kd; + *i_clamp=head_tracking_tilt.integral_clamp>>9; } void head_tracking_set_target_pan(int16_t target) { - pan_target=target<<9; - ram_write_word(DARWIN_HEAD_PAN_L,target); + head_tracking_pan.target_angle=target<<9; + ram_data[DARWIN_HEAD_PAN_TARGET_L]=target%256; + ram_data[DARWIN_HEAD_PAN_TARGET_H]=target/256; } void head_tracking_set_target_tilt(int16_t target) { - tilt_target=target<<9; - ram_write_word(DARWIN_HEAD_TILT_L,target); + head_tracking_tilt.target_angle=target<<9; + ram_data[DARWIN_HEAD_TILT_TARGET_L]=target%256; + ram_data[DARWIN_HEAD_TILT_TARGET_H]=target/256; +} + +// operation functions +uint8_t head_tracking_in_range(unsigned short int address, unsigned short int length) +{ + if(ram_in_window(HEAD_BASE_ADDRESS,HEAD_MEM_LENGTH,address,length) || + ram_in_window(HEAD_EEPROM_ADDRESS,HEAD_EEPROM_LENGTH,address,length)) + return 0x01; + else + return 0x00; +} + +void head_tracking_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data) +{ + +} + +void head_tracking_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data) +{ + uint16_t kp,kd,ki,i_clamp; + int16_t max,min,target; + + // head tracking control + if(ram_in_range(DARWIN_HEAD_CNTRL,address,length)) + { + if(data[DARWIN_HEAD_CNTRL-address]&HEAD_START) + head_tracking_start(); + if(data[DARWIN_HEAD_CNTRL-address]&HEAD_STOP) + head_tracking_stop(); + } + if(ram_in_window(DARWIN_HEAD_PAN_P_L,8,address,length)) + { + head_tracking_get_pan_pid(&kp,&kd,&ki,&i_clamp); + if(ram_in_range(DARWIN_HEAD_PAN_P_L,address,length)) kp=(kp&0xFF00)|data[DARWIN_HEAD_PAN_P_L-address]; + if(ram_in_range(DARWIN_HEAD_PAN_P_H,address,length)) kp=(kp&0x00FF)|(data[DARWIN_HEAD_PAN_P_H-address]<<8); + if(ram_in_range(DARWIN_HEAD_PAN_I_L,address,length)) ki=(ki&0xFF00)|data[DARWIN_HEAD_PAN_I_L-address]; + if(ram_in_range(DARWIN_HEAD_PAN_I_H,address,length)) ki=(ki&0x00FF)|(data[DARWIN_HEAD_PAN_I_H-address]<<8); + if(ram_in_range(DARWIN_HEAD_PAN_D_L,address,length)) kd=(kd&0xFF00)|data[DARWIN_HEAD_PAN_D_L-address]; + if(ram_in_range(DARWIN_HEAD_PAN_D_H,address,length)) kd=(kd&0x00FF)|(data[DARWIN_HEAD_PAN_D_H-address]<<8); + if(ram_in_range(DARWIN_HEAD_PAN_I_CLAMP_L,address,length)) i_clamp=(i_clamp&0xFF00)|data[DARWIN_HEAD_PAN_I_CLAMP_L-address]; + if(ram_in_range(DARWIN_HEAD_PAN_I_CLAMP_H,address,length)) i_clamp=(i_clamp&0x00FF)|(data[DARWIN_HEAD_PAN_I_CLAMP_H-address]<<8); + head_tracking_set_pan_pid(kp,kd,ki,i_clamp); + } + if(ram_in_window(DARWIN_HEAD_TILT_P_L,8,address,length)) + { + head_tracking_get_tilt_pid(&kp,&kd,&ki,&i_clamp); + if(ram_in_range(DARWIN_HEAD_TILT_P_L,address,length)) kp=(kp&0xFF00)|data[DARWIN_HEAD_TILT_P_L-address]; + if(ram_in_range(DARWIN_HEAD_TILT_P_H,address,length)) kp=(kp&0x00FF)|(data[DARWIN_HEAD_TILT_P_H-address]<<8); + if(ram_in_range(DARWIN_HEAD_TILT_I_L,address,length)) ki=(ki&0xFF00)|data[DARWIN_HEAD_TILT_I_L-address]; + if(ram_in_range(DARWIN_HEAD_TILT_I_H,address,length)) ki=(ki&0x00FF)|(data[DARWIN_HEAD_TILT_I_H-address]<<8); + if(ram_in_range(DARWIN_HEAD_TILT_D_L,address,length)) kd=(kd&0xFF00)|data[DARWIN_HEAD_TILT_D_L-address]; + if(ram_in_range(DARWIN_HEAD_TILT_D_H,address,length)) kd=(kd&0x00FF)|(data[DARWIN_HEAD_TILT_D_H-address]<<8); + if(ram_in_range(DARWIN_HEAD_TILT_I_CLAMP_L,address,length)) i_clamp=(i_clamp&0xFF00)|data[DARWIN_HEAD_TILT_I_CLAMP_L-address]; + if(ram_in_range(DARWIN_HEAD_TILT_I_CLAMP_H,address,length)) i_clamp=(i_clamp&0x00FF)|(data[DARWIN_HEAD_TILT_I_CLAMP_H-address]<<8); + head_tracking_set_tilt_pid(kp,kd,ki,i_clamp); + } + if(ram_in_window(DARWIN_HEAD_MAX_PAN_L,4,address,length)) + { + head_tracking_get_pan_range(&max,&min); + if(ram_in_range(DARWIN_HEAD_MAX_PAN_L,address,length)) max=(max&0xFF00)|data[DARWIN_HEAD_MAX_PAN_L-address]; + if(ram_in_range(DARWIN_HEAD_MAX_PAN_H,address,length)) max=(max&0x00FF)|(data[DARWIN_HEAD_MAX_PAN_H-address]<<8); + if(ram_in_range(DARWIN_HEAD_MIN_PAN_L,address,length)) min=(min&0xFF00)|data[DARWIN_HEAD_MIN_PAN_L-address]; + if(ram_in_range(DARWIN_HEAD_MIN_PAN_H,address,length)) min=(min&0x00FF)|(data[DARWIN_HEAD_MIN_PAN_H-address]<<8); + head_tracking_set_pan_range(max,min); + } + if(ram_in_window(DARWIN_HEAD_MAX_TILT_L,4,address,length)) + { + head_tracking_get_tilt_range(&max,&min); + if(ram_in_range(DARWIN_HEAD_MAX_TILT_L,address,length)) max=(max&0xFF00)|data[DARWIN_HEAD_MAX_TILT_L-address]; + if(ram_in_range(DARWIN_HEAD_MAX_TILT_H,address,length)) max=(max&0x00FF)|(data[DARWIN_HEAD_MAX_TILT_H-address]<<8); + if(ram_in_range(DARWIN_HEAD_MIN_TILT_L,address,length)) min=(min&0xFF00)|data[DARWIN_HEAD_MIN_TILT_L-address]; + if(ram_in_range(DARWIN_HEAD_MIN_TILT_H,address,length)) min=(min&0x00FF)|(data[DARWIN_HEAD_MIN_TILT_H-address]<<8); + head_tracking_set_tilt_range(max,min); + } + if(ram_in_window(DARWIN_HEAD_PAN_TARGET_L,2,address,length)) + { + target=head_tracking_pan.target_angle; + if(ram_in_range(DARWIN_HEAD_PAN_TARGET_L,address,length)) target=(target&0xFF00)|data[DARWIN_HEAD_PAN_TARGET_L-address]; + if(ram_in_range(DARWIN_HEAD_PAN_TARGET_H,address,length)) target=(target&0x00FF)|(data[DARWIN_HEAD_PAN_TARGET_H-address]<<8); + head_tracking_pan.target_angle=target; + } + if(ram_in_window(DARWIN_HEAD_TILT_TARGET_L,2,address,length)) + { + target=head_tracking_tilt.target_angle; + if(ram_in_range(DARWIN_HEAD_TILT_TARGET_L,address,length)) target=(target&0xFF00)|data[DARWIN_HEAD_TILT_TARGET_L-address]; + if(ram_in_range(DARWIN_HEAD_TILT_TARGET_H,address,length)) target=(target&0x00FF)|(data[DARWIN_HEAD_TILT_TARGET_H-address]<<8); + head_tracking_tilt.target_angle=target; + } } // motion manager process function @@ -123,25 +271,45 @@ void head_tracking_process(void) if(head_tracking_enabled) { - ram_set_bit(DARWIN_HEAD_STATUS,0x01); - if(manager_get_module(HEAD_PAN)==MM_HEAD) + ram_data[DARWIN_HEAD_CNTRL]|=HEAD_STATUS; + if(manager_get_module(L_PAN)==MM_HEAD) { - pan_error = pan_target-current_angles[HEAD_PAN]; - integral_pan += (pan_error*head_tracking_period)>>16; - derivative_pan = ((pan_error - prev_error_pan)<<16)/head_tracking_period; - current_angles[HEAD_PAN]+=(pan_p*pan_error+pan_i*integral_pan+pan_d*derivative_pan)>>10; - prev_error_pan = pan_error; + pan_error = head_tracking_pan.target_angle-manager_current_angles[L_PAN]; + head_tracking_pan.integral_part+=(pan_error*head_tracking_period)>>16; + if(head_tracking_pan.integral_part>head_tracking_pan.integral_clamp) + head_tracking_pan.integral_part=head_tracking_pan.integral_clamp; + else if(head_tracking_pan.integral_part<-head_tracking_pan.integral_clamp) + head_tracking_pan.integral_part=-head_tracking_pan.integral_clamp; + derivative_pan = ((pan_error - head_tracking_pan.prev_error)<<16)/head_tracking_period; + manager_current_angles[L_PAN]+=((head_tracking_pan.kp*pan_error)>>16); + manager_current_angles[L_PAN]+=((head_tracking_pan.ki*head_tracking_pan.integral_part)>>16); + manager_current_angles[L_PAN]+=((head_tracking_pan.kd*derivative_pan)>>16); + if(manager_current_angles[L_PAN]>head_tracking_pan.max_angle) + manager_current_angles[L_PAN]=head_tracking_pan.max_angle; + else if(manager_current_angles[L_PAN]<head_tracking_pan.min_angle) + manager_current_angles[L_PAN]=head_tracking_pan.min_angle; + head_tracking_pan.prev_error = pan_error; } - if(manager_get_module(HEAD_TILT)==MM_HEAD) + if(manager_get_module(L_TILT)==MM_HEAD) { - tilt_error = tilt_target-current_angles[HEAD_TILT]; - integral_tilt += (tilt_error*head_tracking_period)>>16; - derivative_tilt = ((tilt_error - prev_error_tilt)<<16)/head_tracking_period; - current_angles[HEAD_TILT]+=(tilt_p*tilt_error+tilt_i*integral_tilt+tilt_d*derivative_tilt)>>10; - prev_error_tilt = tilt_error; + tilt_error = head_tracking_tilt.target_angle-manager_current_angles[L_TILT]; + head_tracking_tilt.integral_part+=(tilt_error*head_tracking_period)>>16; + if(head_tracking_tilt.integral_part>head_tracking_tilt.integral_clamp) + head_tracking_tilt.integral_part=head_tracking_tilt.integral_clamp; + else if(head_tracking_tilt.integral_part<-head_tracking_tilt.integral_clamp) + head_tracking_tilt.integral_part=-head_tracking_tilt.integral_clamp; + derivative_tilt = ((tilt_error - head_tracking_tilt.prev_error)<<16)/head_tracking_period; + manager_current_angles[L_TILT]+=((head_tracking_tilt.kp*tilt_error)>>16); + manager_current_angles[L_TILT]+=((head_tracking_tilt.ki*head_tracking_tilt.integral_part)>>16); + manager_current_angles[L_TILT]+=((head_tracking_tilt.kd*derivative_tilt)>>16); + if(manager_current_angles[L_TILT]>head_tracking_tilt.max_angle) + manager_current_angles[L_TILT]=head_tracking_tilt.max_angle; + else if(manager_current_angles[L_TILT]<head_tracking_tilt.min_angle) + manager_current_angles[L_TILT]=head_tracking_tilt.min_angle; + head_tracking_tilt.prev_error = tilt_error; } } else - ram_clear_bit(DARWIN_HEAD_STATUS,0x01); + ram_data[DARWIN_HEAD_CNTRL]&=(~HEAD_STATUS); } diff --git a/src/imu.c b/src/imu.c index 8175e80efb25fd04a7ac6755ff812c12c918ae06..3d3f0478946591baac10d9cfa0017ce762f82be9 100755 --- a/src/imu.c +++ b/src/imu.c @@ -588,6 +588,14 @@ void imu_start_gyro_cal(void) gyro_calibration=0x01; } +uint8_t imu_is_gyro_calibrated(void) +{ + if(ram_data[DARWIN_IMU_CNTRL]) + return 0x00; + else + return 0x01; +} + void imu_get_gyro_data(int32_t *x, int32_t *y, int32_t *z) { *x=gyro_data[0]; diff --git a/src/joint_motion.c b/src/joint_motion.c index 0672858f8856f35ff03948b60824ac0433f4c637..e1d92efdcc78be09b72e108766bc7e5bf48cbf46 100644 --- a/src/joint_motion.c +++ b/src/joint_motion.c @@ -1,7 +1,5 @@ #include "joint_motion.h" -#include "motion_manager.h" #include "ram.h" -#include "gpio.h" #include <stdlib.h> // private variables @@ -14,12 +12,15 @@ int64_t joint_current_speeds[MANAGER_MAX_NUM_SERVOS];//48|16 int8_t joint_dir[MANAGER_MAX_NUM_SERVOS];// joint_direction -uint8_t joint_stop; -uint8_t joint_moving; int64_t joint_motion_period; +int16_t joint_motion_period_us; + +uint8_t joint_stop[NUM_JOINT_GROUPS]; +uint8_t joint_moving[NUM_JOINT_GROUPS]; +uint32_t joint_group_servos[NUM_JOINT_GROUPS]; // public functions -void joint_motion_init(uint16_t period) +void joint_motion_init(uint16_t period_us) { uint8_t i; @@ -27,153 +28,243 @@ void joint_motion_init(uint16_t period) for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++) { // target values - joint_target_angles[i]=current_angles[i]; + joint_target_angles[i]=manager_current_angles[i]; joint_target_speeds[i]=0; joint_target_accels[i]=0; // current values - joint_current_angles[i]=current_angles[i]; + joint_current_angles[i]=manager_current_angles[i]; joint_current_speeds[i]=0; // the current angles, speeds and accelerations are in RAM joint_dir[i]=0; } - joint_stop=0x00; - joint_motion_period=period; -} - -void joint_motion_set_period(uint16_t period) -{ - joint_motion_period=period; + for(i=0;i<NUM_JOINT_GROUPS;i++) + { + joint_stop[i]=0x00; + joint_moving[i]=0x00; + joint_group_servos[i]=0x00000000; + } + joint_motion_period=(period_us<<16)/1000000; + joint_motion_period_us=period_us; } -uint16_t joint_motion_get_period(void) +inline void joint_motion_set_period(uint16_t period_us) { - return joint_motion_period; + joint_motion_period=(period_us<<16)/1000000; + joint_motion_period_us=period_us; } -inline void joint_motion_load_target_angle(uint8_t servo_id, int16_t angle) +inline uint16_t joint_motion_get_period(void) { - if(angle>manager_get_ccw_angle_limit(servo_id)) - angle=manager_get_ccw_angle_limit(servo_id); - else if(angle<manager_get_cw_angle_limit(servo_id)) - angle=manager_get_cw_angle_limit(servo_id); - joint_target_angles[servo_id]=angle<<9; + return joint_motion_period_us; } -inline void joint_motion_load_target_speed(uint8_t servo_id, uint8_t speed) +void joint_motion_set_group_servos(uint8_t group_id,uint32_t servos) { - joint_target_speeds[servo_id]=speed<<16; + joint_group_servos[group_id]=servos; + ram_data[DARWIN_JOINT_GRP0_SERVOS0+group_id*5]=servos&0x000000FF; + ram_data[DARWIN_JOINT_GRP0_SERVOS1+group_id*5]=(servos>>8)&0x000000FF; + ram_data[DARWIN_JOINT_GRP0_SERVOS2+group_id*5]=(servos>>16)&0x000000FF; + ram_data[DARWIN_JOINT_GRP0_SERVOS3+group_id*5]=(servos>>24)&0x000000FF; } -inline void joint_motion_load_target_accel(uint8_t servo_id, uint8_t accel) +uint32_t joint_motion_get_group_servos(uint8_t group_id) { - joint_target_accels[servo_id]=accel<<16; + return joint_group_servos[group_id]; } -void joint_motion_start(void) +void joint_motion_start(uint8_t group_id) { uint8_t i; + int16_t angle; + uint32_t servo_index; /* initialize the internal variables */ - for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++) + for(i=0,servo_index=0x00000001;i<MANAGER_MAX_NUM_SERVOS;i++,servo_index=servo_index<<1) { - // current values - joint_current_angles[i]=current_angles[i]; - // the current angles, speeds and accelerations are in RAM - if(joint_target_angles[i]>=joint_current_angles[i]) - joint_dir[i]=1; - else - joint_dir[i]=-1; + if(joint_group_servos[group_id]&servo_index) + { + if(ram_data[DARWIN_JOINT_SERVO0_SPEED+i]==0) + { + joint_target_angles[i]=manager_current_angles[i]; + angle=manager_current_angles[i]>>9; + ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+i*2]=angle%256; + ram_data[DARWIN_JOINT_SERVO0_ANGLE_H+i*2]=angle/256; + } + else + { + // current values + joint_current_angles[i]=manager_current_angles[i]; + // target angle + angle=ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+i*2]+(ram_data[DARWIN_JOINT_SERVO0_ANGLE_H+i*2]<<8); + if(angle>manager_get_ccw_angle_limit(i)) + angle=manager_get_ccw_angle_limit(i); + else if(angle<manager_get_cw_angle_limit(i)) + angle=manager_get_cw_angle_limit(i); + joint_target_angles[i]=angle<<9; + // target speed + joint_target_speeds[i]=ram_data[DARWIN_JOINT_SERVO0_SPEED+i]<<16; + // target speed + joint_target_accels[i]=ram_data[DARWIN_JOINT_SERVO0_ACCEL+i]<<16; + if(joint_target_accels[i]==0) + { + joint_target_accels[i]=1<<16; + ram_data[DARWIN_JOINT_SERVO0_ACCEL+i]=1; + } + // check the parameters + if(abs(joint_target_angles[i]-joint_current_angles[i])>=65) + { + if((joint_target_speeds[i]*joint_target_speeds[i])/joint_target_accels[i]>abs(joint_target_angles[i]-joint_current_angles[i])) + { + joint_target_accels[i]=(joint_target_speeds[i]*joint_target_speeds[i])/abs(joint_target_angles[i]-joint_current_angles[i]); + ram_data[DARWIN_JOINT_SERVO0_ACCEL+i]=joint_target_accels[i]>>16; + } + } + // the current angles, speeds and accelerations are in RAM + if(joint_target_angles[i]>=joint_current_angles[i]) + joint_dir[i]=1; + else + joint_dir[i]=-1; + } + } } - joint_moving=0x01; - ram_set_bit(DARWIN_JOINTS_CONTROL,0); - ram_clear_bit(DARWIN_JOINTS_CONTROL,1); - ram_set_bit(DARWIN_JOINTS_STATUS,0); + joint_moving[group_id]=0x01; + ram_data[DARWIN_JOINT_GRP0_CNTRL+group_id*5]|=JOINT_STATUS; +} + +void joint_motion_stop(uint8_t group_id) +{ + joint_stop[group_id]=0x01; } -void joint_motion_stop(void) +uint8_t are_joints_moving(uint8_t group_id) { - joint_stop=0x01; - ram_set_bit(DARWIN_JOINTS_CONTROL,1); + return joint_moving[group_id]; } -uint8_t are_joints_moving(void) +// operation functions +uint8_t joint_motion_in_range(unsigned short int address, unsigned short int length) { - return joint_moving; + if(ram_in_window(JOINT_BASE_ADDRESS,JOINT_MEM_LENGTH,address,length)) + return 0x01; + else + return 0x00; +} + +void joint_motion_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data) +{ + +} + +void joint_motion_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data) +{ + uint8_t j; + uint16_t i; + uint32_t servos; + + // load motion information + for(i=DARWIN_JOINT_SERVO0_ANGLE_L;i<=DARWIN_JOINT_SERVO31_ACCEL;i++) + if(ram_in_range(i,address,length)) + ram_data[i]=data[i-address]; + // group servos + for(j=0;j<4;j++) + { + if(ram_in_window(DARWIN_JOINT_GRP0_SERVOS0+j*5,4,address,length)) + { + servos=joint_motion_get_group_servos(j); + for(i=DARWIN_JOINT_GRP0_SERVOS0+j*5;i<=DARWIN_JOINT_GRP0_SERVOS3+j*5;i++) + if(ram_in_range(i,address,length)) + servos|=data[i-address]<<(i*8); + joint_motion_set_group_servos(j,servos); + } + if(ram_in_range(DARWIN_JOINT_GRP0_CNTRL+j*5,address,length)) + { + if(data[DARWIN_JOINT_GRP0_CNTRL+j*5-address]&JOINT_START) + joint_motion_start(j); + if(data[DARWIN_JOINT_GRP0_CNTRL+j*5-address]&JOINT_STOP) + joint_motion_stop(j); + } + } } // motion manager interface functions void joint_motion_process(void) { - uint8_t moving=0x00,i; + uint8_t moving,i,j; + uint32_t servo_index; - if(joint_moving==0x01) + for(j=0;j<4;j++) { - for(i=0;i<MANAGER_MAX_NUM_SERVOS;i++) + moving=0x00; + if(joint_moving[j]==0x01) { - if(manager_get_module(i)==MM_JOINTS) + for(i=0,servo_index=0x00000001;i<MANAGER_MAX_NUM_SERVOS;i++,servo_index=servo_index<<1) { - if(joint_stop==0x01) + if(manager_get_module(i)==MM_JOINTS && joint_group_servos[j]&servo_index) { - joint_target_angles[i]=current_angles[i]; - joint_target_speeds[i]=0; - joint_target_accels[i]=0; - joint_current_speeds[i]=0; - } - else - { - if(abs(joint_target_angles[i]-joint_current_angles[i])>=6553)// 0.5 degrees - { - moving=0x01; - if(joint_current_speeds[i]!=joint_dir[i]*joint_target_speeds[i]) + if(joint_stop[j]==0x01) + { + joint_target_angles[i]=manager_current_angles[i]; + joint_target_speeds[i]=0; + joint_target_accels[i]=0; + joint_current_speeds[i]=0; + } + else + { + if(abs(joint_target_angles[i]-joint_current_angles[i])>=65) { - if(joint_current_speeds[i]>joint_dir[i]*joint_target_speeds[i]) - { - joint_current_speeds[i]=joint_current_speeds[i]-((joint_target_accels[i]*joint_motion_period)>>16); - if(joint_current_speeds[i]<joint_dir[i]*joint_target_speeds[i]) - joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i]; - } - else + moving=0x01; + if(joint_current_speeds[i]!=joint_dir[i]*joint_target_speeds[i])// it is necessary to change the current speed { - joint_current_speeds[i]=joint_current_speeds[i]+((joint_target_accels[i]*joint_motion_period)>>16); if(joint_current_speeds[i]>joint_dir[i]*joint_target_speeds[i]) - joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i]; - } - joint_current_angles[i]=joint_current_angles[i]+((joint_current_speeds[i]*joint_motion_period)>>16); - if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i]) - joint_current_angles[i]=joint_target_angles[i]; - if(joint_dir[i]==-1 && joint_current_angles[i]<joint_target_angles[i]) - joint_current_angles[i]=joint_target_angles[i]; - } - else - { - if(abs(joint_target_angles[i]-joint_current_angles[i])>(joint_target_speeds[i]*joint_target_speeds[i]/(2*joint_target_accels[i]))) - { - joint_current_angles[i]=joint_current_angles[i]+((joint_current_speeds[i]*joint_motion_period)>>16); + { + joint_current_speeds[i]-=((joint_target_accels[i]*joint_motion_period)>>16);// reduce speed + if(joint_current_speeds[i]<joint_dir[i]*joint_target_speeds[i]) + joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i]; + } + else + { + joint_current_speeds[i]+=((joint_target_accels[i]*joint_motion_period)>>16);// increase speed + if(joint_current_speeds[i]>joint_dir[i]*joint_target_speeds[i]) + joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i]; + } + joint_current_angles[i]+=((joint_current_speeds[i]*joint_motion_period)>>16); if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i]) joint_current_angles[i]=joint_target_angles[i]; if(joint_dir[i]==-1 && joint_current_angles[i]<joint_target_angles[i]) joint_current_angles[i]=joint_target_angles[i]; - } + } else { - joint_target_speeds[i]=joint_target_speeds[i]-((joint_target_accels[i]*joint_motion_period)>>16); - joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i]; - joint_current_angles[i]=joint_current_angles[i]+((joint_current_speeds[i]*joint_motion_period)>>16); - if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i]) - joint_current_angles[i]=joint_target_angles[i]; - if(joint_dir[i]==-1 && joint_current_angles[i]<joint_target_angles[i]) - joint_current_angles[i]=joint_target_angles[i]; + if(abs(joint_target_angles[i]-joint_current_angles[i])>(joint_target_speeds[i]*joint_target_speeds[i]/(2*joint_target_accels[i])))// constant speed phase + { + joint_current_angles[i]+=((joint_current_speeds[i]*joint_motion_period)>>16); + if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i]) + joint_current_angles[i]=joint_target_angles[i]; + if(joint_dir[i]==-1 && joint_current_angles[i]<joint_target_angles[i]) + joint_current_angles[i]=joint_target_angles[i]; + } + else// deceleration phase + { + joint_target_speeds[i]=joint_target_speeds[i]-((joint_target_accels[i]*joint_motion_period)>>16); + joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i]; + joint_current_angles[i]=joint_current_angles[i]+((joint_current_speeds[i]*joint_motion_period)>>16); + if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i]) + joint_current_angles[i]=joint_target_angles[i]; + if(joint_dir[i]==-1 && joint_current_angles[i]<joint_target_angles[i]) + joint_current_angles[i]=joint_target_angles[i]; + } } } + manager_current_angles[i]=joint_current_angles[i]; + manager_current_slopes[i]=5; } - current_angles[i]=joint_current_angles[i]; } } - } - if(moving==0) - { - joint_moving=0x00; - ram_clear_bit(DARWIN_JOINTS_STATUS,0); + if(moving==0) + { + joint_moving[j]=0x00; + ram_data[DARWIN_JOINT_GRP0_CNTRL+j*5]&=(~JOINT_STATUS); + } } } } diff --git a/src/motion_manager.c b/src/motion_manager.c index a19992252053b04f9698dadf168f10ba28d9e38c..e24c877a8d2ba947beedbf7e2852e470f25d1c36 100755 --- a/src/motion_manager.c +++ b/src/motion_manager.c @@ -4,6 +4,9 @@ #include "ram.h" #include "action.h" #include "walking.h" +#include "joint_motion.h" +#include "head_tracking.h" +#include "imu.h" #define MANAGER_TIMER TIM5 #define ENABLE_MANAGER_TIMER_CLK __HAL_RCC_TIM5_CLK_ENABLE() @@ -108,36 +111,27 @@ void manager_get_target_position(void) void manager_balance(void) { - //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; + int32_t gyro_x,gyro_y,gyro_z; if(manager_balance_enabled==0x01)// balance is enabled { - //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; + imu_get_gyro_data(&gyro_x,&gyro_y,&gyro_z); // 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; + manager_balance_offset[R_KNEE]=(((gyro_y*manager_servos[R_KNEE].max_angle)/manager_servos[R_KNEE].encoder_resolution)*knee_gain)>>16; + manager_balance_offset[R_ANKLE_PITCH]=(((gyro_y*manager_servos[R_ANKLE_PITCH].max_angle)/manager_servos[R_ANKLE_PITCH].encoder_resolution)*ankle_pitch_gain)>>16; + manager_balance_offset[L_KNEE]=-(((gyro_y*manager_servos[L_KNEE].max_angle)/manager_servos[L_KNEE].encoder_resolution)*knee_gain)>>16; + manager_balance_offset[L_ANKLE_PITCH]=-(((gyro_y*manager_servos[L_ANKLE_PITCH].max_angle)/manager_servos[L_ANKLE_PITCH].encoder_resolution)*ankle_pitch_gain)>>16; + manager_balance_offset[R_HIP_ROLL]=(((gyro_x*manager_servos[R_HIP_ROLL].max_angle)/manager_servos[R_HIP_ROLL].encoder_resolution)*hip_roll_gain)>>16; + manager_balance_offset[L_HIP_ROLL]=(((gyro_x*manager_servos[L_HIP_ROLL].max_angle)/manager_servos[L_HIP_ROLL].encoder_resolution)*hip_roll_gain)>>16; + manager_balance_offset[R_ANKLE_ROLL]=-(((gyro_x*manager_servos[R_ANKLE_ROLL].max_angle)/manager_servos[R_ANKLE_ROLL].encoder_resolution)*ankle_roll_gain)>>16; + manager_balance_offset[L_ANKLE_ROLL]=-(((gyro_x*manager_servos[L_ANKLE_ROLL].max_angle)/manager_servos[L_ANKLE_ROLL].encoder_resolution)*ankle_roll_gain)>>16; } } @@ -156,7 +150,9 @@ void MANAGER_TIMER_IRQHandler(void) // call the action process action_process(); // call the joint motion process - // joint_motion_process(); + joint_motion_process(); + // call the head_tracking process + head_tracking_process(); // call the walking process walking_process(); // balance the robot @@ -333,6 +329,8 @@ void manager_init(uint16_t period_us) /* initialize action module */ action_init(period_us); walking_init(period_us); + joint_motion_init(period_us); + head_tracking_init(period_us); } uint16_t manager_get_period(void) @@ -353,6 +351,8 @@ void manager_set_period(uint16_t period_us) ram_data[DARWIN_MM_PERIOD_H]=(period_us&0xFF00)>>8; action_set_period(period_us); walking_set_period(period_us); + joint_motion_set_period(period_us); + head_tracking_init(period_us); } inline void manager_enable(void) diff --git a/src/ram.c b/src/ram.c index fdec190780ca59ae34ba6e2c5123cef1ca127a86..cf821c30d488f1062058d3420b40dadda31b8efc 100755 --- a/src/ram.c +++ b/src/ram.c @@ -87,6 +87,38 @@ void ram_init(void) ram_data[WALK_MAX_VEL]=(uint8_t)(eeprom_data&0x00FF); if(EE_ReadVariable(WALK_MAX_ROT_VEL,&eeprom_data)==0) ram_data[WALK_MAX_ROT_VEL]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(HEAD_PAN_P,&eeprom_data)==0) + ram_data[HEAD_PAN_P]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(HEAD_PAN_P+1,&eeprom_data)==0) + ram_data[HEAD_PAN_P+1]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(HEAD_PAN_I,&eeprom_data)==0) + ram_data[HEAD_PAN_I]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(HEAD_PAN_I+1,&eeprom_data)==0) + ram_data[HEAD_PAN_I+1]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(HEAD_PAN_D,&eeprom_data)==0) + ram_data[HEAD_PAN_D]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(HEAD_PAN_D+1,&eeprom_data)==0) + ram_data[HEAD_PAN_D+1]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(HEAD_PAN_I_CLAMP,&eeprom_data)==0) + ram_data[HEAD_PAN_I_CLAMP]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(HEAD_PAN_I_CLAMP+1,&eeprom_data)==0) + ram_data[HEAD_PAN_I_CLAMP+1]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(HEAD_TILT_P,&eeprom_data)==0) + ram_data[HEAD_TILT_P]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(HEAD_TILT_P+1,&eeprom_data)==0) + ram_data[HEAD_TILT_P+1]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(HEAD_TILT_I,&eeprom_data)==0) + ram_data[HEAD_TILT_I]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(HEAD_TILT_I+1,&eeprom_data)==0) + ram_data[HEAD_TILT_I+1]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(HEAD_TILT_D,&eeprom_data)==0) + ram_data[HEAD_TILT_D]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(HEAD_TILT_D+1,&eeprom_data)==0) + ram_data[HEAD_TILT_D+1]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(HEAD_TILT_I_CLAMP,&eeprom_data)==0) + ram_data[HEAD_TILT_I_CLAMP]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(HEAD_TILT_I_CLAMP+1,&eeprom_data)==0) + ram_data[HEAD_TILT_I_CLAMP+1]=(uint8_t)(eeprom_data&0x00FF); } inline void ram_read_byte(uint16_t address,uint8_t *data)