diff --git a/Makefile b/Makefile index b649d633d05cc2c28bd80276995fa2057a508ef4..c448792639103ebb9b6d61828c9f1ce867092dce 100755 --- a/Makefile +++ b/Makefile @@ -29,7 +29,7 @@ DYNAMIXEL_PATH=../../STM32_processor/libraries/dynamixel_base BUILD_PATH=build COMPILE_OPTS = -mlittle-endian -mcpu=cortex-m3 -mthumb -mthumb-interwork -COMPILE_OPTS += -Wall -O2 -fno-common -msoft-float +COMPILE_OPTS += -Wall -O2 -fno-common -msoft-float -DUSE_HAL_DRIVER COMPILE_OPTS += -ffreestanding -nostdlib -D$(PROCESSOR_MACRO) INCLUDE_DIRS = -I$(HAL_PATH)/include -I$(HAL_PATH)/include/core -I$(HAL_PATH)/include/devices INCLUDE_DIRS += -I$(UTILS_PATH)/include -I$(COMM_PATH)/include -I$(DYNAMIXEL_PATH)/include -I$(USART_PATH)/include -I./include diff --git a/include/adc_dma.h b/include/adc_dma.h index b11e6776ee03e7c596ff6b7ee1a2de57704d4356..931594aae6dfb263af39a2e835041928ff4ba474 100755 --- a/include/adc_dma.h +++ b/include/adc_dma.h @@ -1,8 +1,28 @@ #ifndef _ADC_DMA_H #define _ADC_DMA_H +#ifdef __cplusplus +extern "C" { +#endif + #include "stm32f1xx_hal.h" +typedef enum {ADC_CH1,ADC_CH2,ADC_CH3,ADC_CH4,ADC_CH6,ADC_CH8} adc_ch_t; + void adc_init(void); +void adc_start(void); +void adc_set_period(uint8_t period_ms); +inline uint8_t adc_get_period(void); +unsigned short adc_get_channel(adc_ch_t channel); +unsigned short adc_get_channel_raw(adc_ch_t channel); +unsigned short adc_get_temperature(void); +void adc_stop(void); +// operation functions +uint8_t adc_in_range(unsigned short int address,unsigned short int length); +void adc_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data); + +#ifdef __cplusplus +} +#endif #endif diff --git a/include/darwin_comm.h b/include/darwin_comm.h deleted file mode 100644 index cb2d4e6c3d5d0c8fea722384ea6576652641d9fc..0000000000000000000000000000000000000000 --- a/include/darwin_comm.h +++ /dev/null @@ -1,10 +0,0 @@ -#ifndef _COMM_H -#define _COMM_H - -#include "stm32f1xx_hal.h" - -// public functions -void comm_init(void); -void comm_start(void); - -#endif diff --git a/include/darwin_dyn_slave.h b/include/darwin_dyn_slave.h index 6790437e59a306d7341fe5a0fbd100eaf5463e17..b3a38c9467c69f2be448e3c19c64fa92437468e4 100644 --- a/include/darwin_dyn_slave.h +++ b/include/darwin_dyn_slave.h @@ -1,14 +1,20 @@ #ifndef _DARWIN_DYN_SLAVE_H #define _DARWIN_DYN_SLAVE_H -#include "stm32f1xx_hal.h" -#include "stm32_time.h" -#include "comm.h" -#include "dynamixel_slave.h" +#ifdef __cplusplus +extern "C" { +#endif -extern TDynamixelSlave darwin_dyn_slave; +#include "stm32f1xx.h" +#include "dynamixel_slave.h" void darwin_dyn_slave_init(void); +void darwin_dyn_slave_start(void); +void darwin_dyn_slave_stop(void); + +#ifdef __cplusplus +} +#endif #endif diff --git a/include/darwin_registers.h b/include/darwin_registers.h new file mode 100644 index 0000000000000000000000000000000000000000..9ed633293ce3c82d4dc77af426e55cee20384ed2 --- /dev/null +++ b/include/darwin_registers.h @@ -0,0 +1,379 @@ +#ifndef _DARWIN_REGISTERS_H +#define _DARWIN_REGISTERS_H + +#ifdef __cplusplus +extern "C" { +#endif + +#define DEVICE_MODEL_OFFSET ((unsigned short int)0x0000) +#define FIRMWARE_VERSION_OFFSET ((unsigned short int)0x0002) +#define DEVICE_ID_OFFSET ((unsigned short int)0x0003) +#define BAUDRATE_OFFSET ((unsigned short int)0x0004) +#define RETURN_DELAY_OFFSET ((unsigned short int)0x0005) +#define MM_PERIOD_OFFSET ((unsigned short int)0x0006) +#define MM_BAL_KNEE_GAIN_OFFSET ((unsigned short int)0x0008) +#define MM_BAL_ANKLE_ROLL_GAIN_OFFSET ((unsigned short int)0x000A) +#define MM_BAL_ANKLE_PITCH_GAIN_OFFSET ((unsigned short int)0x000C) +#define MM_BAL_HIP_ROLL_GAIN_OFFSET ((unsigned short int)0x000E) +#define RETURN_LEVEL_OFFSET ((unsigned short int)0x0010) +#define MM_SERVO0_OFFSET ((unsigned short int)0x0011) +#define MM_SERVO1_OFFSET ((unsigned short int)0x0012) +#define MM_SERVO2_OFFSET ((unsigned short int)0x0013) +#define MM_SERVO3_OFFSET ((unsigned short int)0x0014) +#define MM_SERVO4_OFFSET ((unsigned short int)0x0015) +#define MM_SERVO5_OFFSET ((unsigned short int)0x0016) +#define MM_SERVO6_OFFSET ((unsigned short int)0x0017) +#define MM_SERVO7_OFFSET ((unsigned short int)0x0018) +#define MM_SERVO8_OFFSET ((unsigned short int)0x0019) +#define MM_SERVO9_OFFSET ((unsigned short int)0x001A) +#define MM_SERVO10_OFFSET ((unsigned short int)0x001B) +#define MM_SERVO11_OFFSET ((unsigned short int)0x001C) +#define MM_SERVO12_OFFSET ((unsigned short int)0x001D) +#define MM_SERVO13_OFFSET ((unsigned short int)0x001E) +#define MM_SERVO14_OFFSET ((unsigned short int)0x001F) +#define MM_SERVO15_OFFSET ((unsigned short int)0x0020) +#define MM_SERVO16_OFFSET ((unsigned short int)0x0021) +#define MM_SERVO17_OFFSET ((unsigned short int)0x0022) +#define MM_SERVO18_OFFSET ((unsigned short int)0x0023) +#define MM_SERVO19_OFFSET ((unsigned short int)0x0024) +#define MM_SERVO20_OFFSET ((unsigned short int)0x0025) +#define MM_SERVO21_OFFSET ((unsigned short int)0x0026) +#define MM_SERVO22_OFFSET ((unsigned short int)0x0027) +#define MM_SERVO23_OFFSET ((unsigned short int)0x0028) +#define MM_SERVO24_OFFSET ((unsigned short int)0x0029) +#define MM_SERVO25_OFFSET ((unsigned short int)0x002A) +#define MM_SERVO26_OFFSET ((unsigned short int)0x002B) +#define MM_SERVO27_OFFSET ((unsigned short int)0x002C) +#define MM_SERVO28_OFFSET ((unsigned short int)0x002D) +#define MM_SERVO29_OFFSET ((unsigned short int)0x002E) +#define MM_SERVO30_OFFSET ((unsigned short int)0x002F) +#define MM_SERVO31_OFFSET ((unsigned short int)0x0030) +#define WALK_X_OFFSET ((unsigned short int)0x0031) +#define WALK_Y_OFFSET ((unsigned short int)0x0032) +#define WALK_Z_OFFSET ((unsigned short int)0x0033) +#define WALK_ROLL_OFFSET ((unsigned short int)0x0034) +#define WALK_PITCH_OFFSET ((unsigned short int)0x0035) +#define WALK_YAW_OFFSET ((unsigned short int)0x0036) +#define WALK_HIP_PITCH_OFF ((unsigned short int)0x0037) +#define WALK_PERIOD_TIME ((unsigned short int)0x0039) +#define WALK_DSP_RATIO ((unsigned short int)0x003B) +#define WALK_STEP_FW_BW_RATIO ((unsigned short int)0x003C) +#define WALK_FOOT_HEIGHT ((unsigned short int)0x003D) +#define WALK_SWING_RIGHT_LEFT ((unsigned short int)0x003E) +#define WALK_SWING_TOP_DOWN ((unsigned short int)0x003F) +#define WALK_PELVIS_OFFSET ((unsigned short int)0x0040) +#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 LAST_EEPROM_OFFSET ((unsigned short int)0x00FF) + +typedef enum { + DARWIN_MODEL_NUMBER_L = DEVICE_MODEL_OFFSET, + DARWIN_MODEL_NUMBER_H = DEVICE_MODEL_OFFSET+1, + DARWIN_VERSION = FIRMWARE_VERSION_OFFSET, + DARWIN_ID = DEVICE_ID_OFFSET, + DARWIN_BAUD_RATE = BAUDRATE_OFFSET, + DARWIN_RETURN_DELAY_TIME = RETURN_DELAY_OFFSET, + DARWIN_MM_PERIOD_L = MM_PERIOD_OFFSET, + DARWIN_MM_PERIOD_H = MM_PERIOD_OFFSET+1, + DARWIN_MM_BAL_KNEE_GAIN_L = MM_BAL_KNEE_GAIN_OFFSET,// fixed point format 0|16 + DARWIN_MM_BAL_KNEE_GAIN_H = MM_BAL_KNEE_GAIN_OFFSET+1, + DARWIN_MM_BAL_ANKLE_ROLL_GAIN_L = MM_BAL_ANKLE_ROLL_GAIN_OFFSET,// fixed point format 0|16 + DARWIN_MM_BAL_ANKLE_ROLL_GAIN_H = MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1, + DARWIN_MM_BAL_ANKLE_PITCH_GAIN_L = MM_BAL_ANKLE_PITCH_GAIN_OFFSET,// fixed point format 0|16 + DARWIN_MM_BAL_ANKLE_PITCH_GAIN_H = MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1, + DARWIN_MM_BAL_HIP_ROLL_GAIN_L = MM_BAL_HIP_ROLL_GAIN_OFFSET,// fixed point format 0|16 + DARWIN_MM_BAL_HIP_ROLL_GAIN_H = MM_BAL_HIP_ROLL_GAIN_OFFSET+1, + DARWIN_RETURN_LEVEL = RETURN_LEVEL_OFFSET, + DARWIN_MM_SERVO0_OFFSET = MM_SERVO0_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO1_OFFSET = MM_SERVO1_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO2_OFFSET = MM_SERVO2_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO3_OFFSET = MM_SERVO3_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO4_OFFSET = MM_SERVO4_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO5_OFFSET = MM_SERVO5_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO6_OFFSET = MM_SERVO6_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO7_OFFSET = MM_SERVO7_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO8_OFFSET = MM_SERVO8_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO9_OFFSET = MM_SERVO9_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO10_OFFSET = MM_SERVO10_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO11_OFFSET = MM_SERVO11_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO12_OFFSET = MM_SERVO12_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO13_OFFSET = MM_SERVO13_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO14_OFFSET = MM_SERVO14_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO15_OFFSET = MM_SERVO15_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO16_OFFSET = MM_SERVO16_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO17_OFFSET = MM_SERVO17_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO18_OFFSET = MM_SERVO18_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO19_OFFSET = MM_SERVO19_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO20_OFFSET = MM_SERVO20_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO21_OFFSET = MM_SERVO21_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO22_OFFSET = MM_SERVO22_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO23_OFFSET = MM_SERVO23_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO24_OFFSET = MM_SERVO24_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO25_OFFSET = MM_SERVO25_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO26_OFFSET = MM_SERVO26_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO27_OFFSET = MM_SERVO27_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO28_OFFSET = MM_SERVO28_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO29_OFFSET = MM_SERVO29_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO30_OFFSET = MM_SERVO30_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_MM_SERVO31_OFFSET = MM_SERVO31_OFFSET, // angle offset in fixed point format 1-3|4 + DARWIN_WALK_X_OFFSET = WALK_X_OFFSET, + DARWIN_WALK_Y_OFFSET = WALK_Y_OFFSET, + DARWIN_WALK_Z_OFFSET = WALK_Z_OFFSET, + DARWIN_WALK_ROLL_OFFSET = WALK_ROLL_OFFSET, + DARWIN_WALK_PITCH_OFFSET = WALK_PITCH_OFFSET, + DARWIN_WALK_YAW_OFFSET = WALK_YAW_OFFSET, + DARWIN_WALK_HIP_PITCH_OFF_L = WALK_HIP_PITCH_OFF, + DARWIN_WALK_HIP_PITCH_OFF_H = WALK_HIP_PITCH_OFF+1, + DARWIN_WALK_PERIOD_TIME_L = WALK_PERIOD_TIME, + DARWIN_WALK_PERIOD_TIME_H = WALK_PERIOD_TIME+1, + DARWIN_WALK_DSP_RATIO = WALK_DSP_RATIO, + DARWIN_WALK_STEP_FW_BW_RATIO = WALK_STEP_FW_BW_RATIO, + DARWIN_WALK_FOOT_HEIGHT = WALK_FOOT_HEIGHT, + DARWIN_WALK_SWING_RIGHT_LEFT = WALK_SWING_RIGHT_LEFT, + DARWIN_WALK_SWING_TOP_DOWN = WALK_SWING_TOP_DOWN, + DARWIN_WALK_PELVIS_OFFSET = WALK_PELVIS_OFFSET, + 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_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, + DARWIN_TX_LED_PERIOD_H = 0x0102, + DARWIN_RX_LED_CNTRL = 0x0103, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // | | | | blink | toggle | value | internally used + DARWIN_RX_LED_PERIOD_L = 0x0104, + DARWIN_RX_LED_PERIOD_H = 0x0105, + DARWIN_PLAY_LED_CNTRL = 0x0106, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // | | | | blink | toggle | value | internally used + DARWIN_PLAY_LED_PERIOD_L = 0x0107, + DARWIN_PLAY_LED_PERIOD_H = 0x0108, + DARWIN_EDIT_LED_CNTRL = 0x0109, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // | | | | blink | toggle | value | internally used + DARWIN_EDIT_LED_PERIOD_L = 0x010A, + DARWIN_EDIT_LED_PERIOD_H = 0x010B, + DARWIN_MNG_LED_CNTRL = 0x010C, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // | | | | | toggle | value | internally used + DARWIN_AUX1_LED_CNTRL = 0x010D, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // | | | | color | toggle | value | internally used + DARWIN_AUX1_LED_COLOR_L = 0x010E, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // green value LSB | red value + DARWIN_AUX1_LED_COLOR_H = 0x010F, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // | blue value | green value MSB + DARWIN_AUX2_LED_CNTRL = 0x0110, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // | | | | color | toggle | value | internally used + DARWIN_AUX2_LED_COLOR_L = 0x0111, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // green value LSB | red value + DARWIN_AUX2_LED_COLOR_H = 0x0112, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // | blue value | green value MSB + DARWIN_START_PB_CNTRL = 0x0113, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // | | | | | | value | internally used + DARWIN_MODE_PB_CNTRL = 0x0114, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // | | | | | | value | internally used + DARWIN_ADC_CNTRL = 0x0115, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // | | | | | | | Enable + DARWIN_ADC_PERIOD = 0x0116, + + + DARWIN_ADC_CH1_L = 0x0112, + DARWIN_ADC_CH1_H = 0x0113, + DARWIN_ADC_CH2_L = 0x0114, + DARWIN_ADC_CH2_H = 0x0115, + DARWIN_ADC_CH3_L = 0x0116, + DARWIN_ADC_CH3_H = 0x0117, + DARWIN_ADC_CH4_L = 0x0118, + DARWIN_ADC_CH4_H = 0x0119, + DARWIN_ADC_TEMP_L = 0x011A, + DARWIN_ADC_TEMP_H = 0x011B, + DARWIN_ADC_CH6_L = 0x011C, + DARWIN_ADC_CH6_H = 0x011D, + DARWIN_ADC_VREF_L = 0x011E, + DARWIN_ADC_VREF_H = 0x011F, + DARWIN_ADC_CH8_L = 0x0120, + DARWIN_ADC_CH8_H = 0x0121, + DARWIN_MM_NUM_SERVOS = 0x0128, + DARWIN_MM_CNTRL = 0x0129, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // | | | | | Enable power | Enable balance | Enable manager + DARWIN_MM_MODULE_EN0 = 0x012A, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0i + // Enable servo 0 | assigned module | Enable servo 1 | assigned module + // | 000 -> none | + // | 001 -> action | + // | 010 -> walk | + // | 011 -> joints | + DARWIN_MM_MODULE_EN1 = 0x012B, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // Enable servo 2 | assigned module | Enable servo 3 | assigned module + DARWIN_MM_MODULE_EN2 = 0x012C, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // Enable servo 4 | assigned module | Enable servo 5 | assigned module + DARWIN_MM_MODULE_EN3 = 0x012D, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // Enable servo 6 | assigned module | Enable servo 7 | assigned module + DARWIN_MM_MODULE_EN4 = 0x012E, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // Enable servo 8 | assigned module | Enable servo 9 | assigned module + DARWIN_MM_MODULE_EN5 = 0x012F, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // Enable servo 10 | assigned module | Enable servo 11 | assigned module + DARWIN_MM_MODULE_EN6 = 0x0130, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // Enable servo 12 | assigned module | Enable servo 13 | assigned module + DARWIN_MM_MODULE_EN7 = 0x0131, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // Enable servo 14 | assigned module | Enable servo 15 | assigned module + DARWIN_MM_MODULE_EN8 = 0x0132, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // Enable servo 16 | assigned module | Enable servo 17 | assigned module + DARWIN_MM_MODULE_EN9 = 0x0133, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // Enable servo 18 | assigned module | Enable servo 19 | assigned module + DARWIN_MM_MODULE_EN10 = 0x0134, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // Enable servo 20 | assigned module | Enable servo 21 | assigned module + DARWIN_MM_MODULE_EN11 = 0x0135, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // Enable servo 22 | assigned module | Enable servo 23 | assigned module + DARWIN_MM_MODULE_EN12 = 0x0136, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // Enable servo 24 | assigned module | Enable servo 25 | assigned module + DARWIN_MM_MODULE_EN13 = 0x0137, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // Enable servo 26 | assigned module | Enable servo 27 | assigned module + DARWIN_MM_MODULE_EN14 = 0x0138, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // Enable servo 28 | assigned module | Enable servo 29 | assigned module + DARWIN_MM_MODULE_EN15 = 0x0139, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0 + // Enable servo 30 | assigned module | Enable servo 31 | assigned module + DARWIN_MM_SERVO0_CUR_POS_L = 0x013A, // angle in fixed point format 9|7 + DARWIN_MM_SERVO0_CUR_POS_H = 0x013B, + DARWIN_MM_SERVO1_CUR_POS_L = 0x013C, // angle in fixed point format 9|7 + DARWIN_MM_SERVO1_CUR_POS_H = 0x013D, + DARWIN_MM_SERVO2_CUR_POS_L = 0x013E, // angle in fixed point format 9|7 + DARWIN_MM_SERVO2_CUR_POS_H = 0x013F, + DARWIN_MM_SERVO3_CUR_POS_L = 0x0140, // angle in fixed point format 9|7 + DARWIN_MM_SERVO3_CUR_POS_H = 0x0141, + DARWIN_MM_SERVO4_CUR_POS_L = 0x0142, // angle in fixed point format 9|7 + DARWIN_MM_SERVO4_CUR_POS_H = 0x0143, + DARWIN_MM_SERVO5_CUR_POS_L = 0x0144, // angle in fixed point format 9|7 + DARWIN_MM_SERVO5_CUR_POS_H = 0x0145, + DARWIN_MM_SERVO6_CUR_POS_L = 0x0146, // angle in fixed point format 9|7 + DARWIN_MM_SERVO6_CUR_POS_H = 0x0147, + DARWIN_MM_SERVO7_CUR_POS_L = 0x0148, // angle in fixed point format 9|7 + DARWIN_MM_SERVO7_CUR_POS_H = 0x0149, + DARWIN_MM_SERVO8_CUR_POS_L = 0x014A, // angle in fixed point format 9|7 + DARWIN_MM_SERVO8_CUR_POS_H = 0x014B, + DARWIN_MM_SERVO9_CUR_POS_L = 0x014C, // angle in fixed point format 9|7 + DARWIN_MM_SERVO9_CUR_POS_H = 0x014D, + DARWIN_MM_SERVO10_CUR_POS_L = 0x014E, // angle in fixed point format 9|7 + DARWIN_MM_SERVO10_CUR_POS_H = 0x014F, + DARWIN_MM_SERVO11_CUR_POS_L = 0x0150, // angle in fixed point format 9|7 + DARWIN_MM_SERVO11_CUR_POS_H = 0x0151, + DARWIN_MM_SERVO12_CUR_POS_L = 0x0152, // angle in fixed point format 9|7 + DARWIN_MM_SERVO12_CUR_POS_H = 0x0153, + DARWIN_MM_SERVO13_CUR_POS_L = 0x0154, // angle in fixed point format 9|7 + DARWIN_MM_SERVO13_CUR_POS_H = 0x0155, + DARWIN_MM_SERVO14_CUR_POS_L = 0x0156, // angle in fixed point format 9|7 + DARWIN_MM_SERVO14_CUR_POS_H = 0x0157, + DARWIN_MM_SERVO15_CUR_POS_L = 0x0158, // angle in fixed point format 9|7 + DARWIN_MM_SERVO15_CUR_POS_H = 0x0159, + DARWIN_MM_SERVO16_CUR_POS_L = 0x015A, // angle in fixed point format 9|7 + DARWIN_MM_SERVO16_CUR_POS_H = 0x015B, + DARWIN_MM_SERVO17_CUR_POS_L = 0x015C, // angle in fixed point format 9|7 + DARWIN_MM_SERVO17_CUR_POS_H = 0x015D, + DARWIN_MM_SERVO18_CUR_POS_L = 0x015E, // angle in fixed point format 9|7 + DARWIN_MM_SERVO18_CUR_POS_H = 0x015F, + DARWIN_MM_SERVO19_CUR_POS_L = 0x0160, // angle in fixed point format 9|7 + DARWIN_MM_SERVO19_CUR_POS_H = 0x0161, + DARWIN_MM_SERVO20_CUR_POS_L = 0x0162, // angle in fixed point format 9|7 + DARWIN_MM_SERVO20_CUR_POS_H = 0x0163, + DARWIN_MM_SERVO21_CUR_POS_L = 0x0164, // angle in fixed point format 9|7 + DARWIN_MM_SERVO21_CUR_POS_H = 0x0165, + DARWIN_MM_SERVO22_CUR_POS_L = 0x0166, // angle in fixed point format 9|7 + DARWIN_MM_SERVO22_CUR_POS_H = 0x0167, + DARWIN_MM_SERVO23_CUR_POS_L = 0x0168, // angle in fixed point format 9|7 + DARWIN_MM_SERVO23_CUR_POS_H = 0x0169, + DARWIN_MM_SERVO24_CUR_POS_L = 0x016A, // angle in fixed point format 9|7 + DARWIN_MM_SERVO24_CUR_POS_H = 0x016B, + DARWIN_MM_SERVO25_CUR_POS_L = 0x016C, // angle in fixed point format 9|7 + DARWIN_MM_SERVO25_CUR_POS_H = 0x016D, + DARWIN_MM_SERVO26_CUR_POS_L = 0x016E, // angle in fixed point format 9|7 + DARWIN_MM_SERVO26_CUR_POS_H = 0x016F, + DARWIN_MM_SERVO27_CUR_POS_L = 0x0170, // angle in fixed point format 9|7 + DARWIN_MM_SERVO27_CUR_POS_H = 0x0171, + DARWIN_MM_SERVO28_CUR_POS_L = 0x0172, // angle in fixed point format 9|7 + DARWIN_MM_SERVO28_CUR_POS_H = 0x0173, + DARWIN_MM_SERVO29_CUR_POS_L = 0x0174, // angle in fixed point format 9|7 + DARWIN_MM_SERVO29_CUR_POS_H = 0x0175, + DARWIN_MM_SERVO30_CUR_POS_L = 0x0176, // angle in fixed point format 9|7 + DARWIN_MM_SERVO30_CUR_POS_H = 0x0177, + 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 + // 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; + +#define GPIO_BASE_ADDRESS 0x0100 +#define GPIO_MEM_LENGTH 23 +#define GPIO_INT_USED 0x01 +#define GPIO_VALUE 0x02 +#define GPIO_TOGGLE 0x04 +#define GPIO_BLINK 0x08 +#define GPIO_RED_COLOR 0x001F +#define GPIO_GREEN_COLOR 0x03E0 +#define GPIO_BLUE_COLOR 0x7C00 + +#define ADC_BASE_ADDRESS 0x0110 +#define ADC_MEM_LENGTH 18 +#define ADC_ENABLE 0x01 + +#define ZIGBEE_BASE_ADDRESS 0x0122 +#define ZIGBEE_MEM_LENGTH 6 +#define ZIGBEE_EN_PWR 0x01 +#define ZIGBEE_ENABLE 0x02 + +#define MANAGER_BASE_ADDRESS 0x0128 +#define MANAGER_MEM_LENGTH 82 +#define MANAGER_EEPROM_BASE1 0x0006 +#define MANAGER_EEPROM_LENGTH1 10 +#define MANAGER_EEPROM_BASE2 0x0011 +#define MANAGER_EEPROM_LENGTH2 32 +#define MANAGER_ENABLE 0x01 +#define MANAGER_EN_BAL 0x02 +#define MANAGER_EN_PWR 0x04 +#define MANAGER_EVEN_SER_EN 0x80 +#define MANAGER_EVEN_SER_MOD 0x70 +#define MANAGER_ODD_SER_EN 0x08 +#define MANAGER_ODD_SER_MOD 0x07 + +#define ACTION_BASE_ADDRESS 0x017A +#define ACTION_MEM_LENGTH 2 +#define ACTION_START 0x01 +#define ACTION_STOP 0x02 +#define ACTION_INT_EN 0x04 +#define ACTION_INT_FLAG 0x08 +#define ACTION_STATUS 0x10 + +#define GYRO_BASE_ADDRESS 0x017C +#define GYRO_MEM_LENGTH 6 +#define GYRO_EEPROM_ADDRESS 0x0031 +#define GYRO_EEPROM_LENGTH 2 +#define GYRO_CALIBRATE 0x01 +#define GYRO_EN_CAL_INT 0x02 +#define GYRO_EN_FALL_DET 0x04 +#define GYRO_EN_FALL_DET_INT 0x08 + +#define WALK_BASE_ADDRESS 0x0182 +#define WALK_MEM_LENGTH 4 +#define WALK_EEPROM_ADDRESS 0x0033 +#define WALK_EEPROM_LENGTH 19 +#define WALK_START 0x01 +#define WALK_STOP 0x02 +#define WALK_STATUS 0x10 +#define WALK_PHASE 0xC0 + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/include/darwin_time.h b/include/darwin_time.h index 6408b196c7d2cce7efd8aba326419713cc384f5b..b9614488cff4243b4671e9a5abc8c987199ece3f 100755 --- a/include/darwin_time.h +++ b/include/darwin_time.h @@ -1,11 +1,19 @@ #ifndef _DARWIN_TIME_H #define _DARWIN_TIME_H -#include "stm32f1xx_hal.h" +#ifdef __cplusplus +extern "C" { +#endif + +#include "stm32f1xx.h" void darwin_time_init(void); unsigned long long int darwin_time_get_counts(void); unsigned int darwin_time_get_counts_per_us(void); +#ifdef __cplusplus +} +#endif + #endif diff --git a/include/eeprom.h b/include/eeprom.h index b58e7c9a25826b79f99ac01161b001791d44cd66..e0b5189c7d4359c097a839dbe341410ee6d71762 100755 --- a/include/eeprom.h +++ b/include/eeprom.h @@ -40,8 +40,12 @@ #ifndef __EEPROM_H #define __EEPROM_H +#ifdef __cplusplus +extern "C" { +#endif + /* Includes ------------------------------------------------------------------*/ -#include "stm32f1xx_hal.h" +#include "stm32f1xx.h" /* Exported constants --------------------------------------------------------*/ /* Define the size of the sectors to be used */ @@ -78,19 +82,8 @@ /* Page full define */ #define PAGE_FULL ((uint8_t)0x80) -/* Virtual address defined by the user: 0xFFFF value is prohibited */ -#define DEVICE_MODEL_OFFSET ((uint16_t)0x0000) -#define FIRMWARE_VERSION_OFFSET ((uint16_t)0x0002) -#define DEVICE_ID_OFFSET ((uint16_t)0x0003) -#define BAUDRATE_OFFSET ((uint16_t)0x0004) -#define RETURN_DELAY_OFFSET ((uint16_t)0x0005) -#define MM_PERIOD_OFFSET ((uint16_t)0x0006) -#define RETURN_LEVEL_OFFSET ((uint16_t)0x0010) - -#define LAST_EEPROM_OFFSET ((uint16_t)0x0017) - /* Variables' number */ -#define NB_OF_VAR ((uint8_t)0x03) +#define NB_OF_VAR ((uint8_t)0x44) /* Exported types ------------------------------------------------------------*/ /* Exported macro ------------------------------------------------------------*/ @@ -99,6 +92,10 @@ uint16_t EE_Init(void); uint16_t EE_ReadVariable(uint16_t VirtAddress, uint16_t* Data); uint16_t EE_WriteVariable(uint16_t VirtAddress, uint16_t Data); +#ifdef __cplusplus +} +#endif + #endif /* __EEPROM_H */ /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/include/eeprom_init.h b/include/eeprom_init.h new file mode 100644 index 0000000000000000000000000000000000000000..1917b78d07d036ff3168576ca92d7718d5bfd9bd --- /dev/null +++ b/include/eeprom_init.h @@ -0,0 +1,75 @@ +#ifndef __EEPROM_INIT_H +#define __EEPROM_INIT_H + +#ifdef __cplusplus +extern "C" { +#endif + +#define DEFAULT_DEVICE_MODEL 0x7300 +#define DEFAULT_FIRMWARE_VERSION 0x0001 +#define DEFAULT_DEVICE_ID 0x0002 +#define DEFAULT_BAUDRATE 0x0001 +#define DEFAULT_RETURN_DELAY 0x0000 +#define DEFAULT_MM_PERIOD 0x1E78 +#define DEFAULT_BAL_KNEE_GAIN 0x04BE // 1/54 in fixed point format 0|16 +#define DEFAULT_BAL_ANKLE_ROLL_GAIN 0x0CCD // 1/20 +#define DEFAULT_BAL_ANKLE_PITCH_GAIN 0x0E39 // 1/18 +#define DEFAULT_BAL_HIP_ROLL_GAIN 0x0666 // 1/40 +#define DEFAULT_RETURN_LEVEL 0x0002 +#define DEFAULT_SERVO0_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO1_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO2_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO3_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO4_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO5_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO6_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO7_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO8_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO9_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO10_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO11_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO12_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO13_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO14_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO15_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO16_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO17_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO18_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO19_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO20_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO21_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO22_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO23_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO24_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO25_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO26_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO27_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO28_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO29_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO30_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_SERVO31_OFFSET 0x0000 // 0 in fixed point format 4 (1+3) | 4 +#define DEFAULT_WALK_X_OFFSET 0xFFF6 // -10 mm +#define DEFAULT_WALK_Y_OFFSET 0x0005 // 5 mm +#define DEFAULT_WALK_Z_OFFSET 0x0014 // 20 mm +#define DEFAULT_WALK_ROLL_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3 +#define DEFAULT_WALK_PITCH_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3 +#define DEFAULT_WALK_YAW_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3 +#define DEFAULT_WALK_HIP_PITCH_OFF 0x0000 // 0 degrees in fixed point format 6 (1+5) | 10 +#define DEFAULT_WALK_PERIOD_TIME 0x0258 // 600 ms +#define DEFAULT_WALK_DSP_RATIO 0x0019 // 0.1 in fixed point format 0 | 8 +#define DEFAULT_WALK_STEP_FW_BW_RATIO 0x004C // 0.3 in fixed point format 0 | 8 +#define DEFAULT_WALK_FOOT_HEIGHT 0x0028 // 40 mm +#define DEFAULT_WALK_SWING_RIGHT_LEFT 0x0014 // 20 mm +#define DEFAULT_WALK_SWING_TOP_DOWN 0x0005 // 5 mm +#define DEFAULT_WALK_PELVIS_OFFSET 0x0018 // 3 degrees in fixed point format 5 (1+4) | 3 +#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 + + +#ifdef __cplusplus +} +#endif + +#endif /* __EEPROM_INIT_H */ + diff --git a/include/gpio.h b/include/gpio.h index ae2b05b4f38e186f358ce604f3b450edbf31eca5..38aa2e98d178d72c3b57e8318e9bae7752765ec0 100755 --- a/include/gpio.h +++ b/include/gpio.h @@ -1,7 +1,11 @@ #ifndef _GPIO_H #define _GPIO_H -#include "stm32f1xx_hal.h" +#ifdef __cplusplus +extern "C" { +#endif + +#include "stm32f1xx.h" typedef enum {LED_TX,LED_RX,LED_2,LED_3,LED_4,LED_5_R,LED_5_G,LED_5_B,LED_6_R,LED_6_G,LED_6_B} led_t; @@ -13,8 +17,17 @@ void gpio_set_led(led_t led_id); void gpio_clear_led(led_t led_id); void gpio_toggle_led(led_t led_id); void gpio_blink_led(led_t led_id, int16_t period_ms); +void gpio_set_color(led_t led_id, uint8_t value); // Pushbuttons functions uint8_t gpio_is_pushbutton_pressed(pushbutton_t pb_id); void gpio_set_pushbutton_callback(pushbutton_t pb_id,void (*callback)(void)); +// operation functions +uint8_t gpio_in_range(unsigned short int address,unsigned short int length); +void gpio_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data); +void gpio_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data); + +#ifdef __cplusplus +} +#endif #endif diff --git a/include/ram.h b/include/ram.h index 2f300a33da6c35a9ce505788c57444cc626f8e13..b096a64789613b6a9e627e74b5095a24768341d6 100755 --- a/include/ram.h +++ b/include/ram.h @@ -1,8 +1,12 @@ #ifndef _RAM_H #define _RAM_H -#include "stm32f1xx_hal.h" -#include "eeprom.h" +#ifdef __cplusplus +extern "C" { +#endif + +#include "stm32f1xx.h" +#include "darwin_registers.h" #define RAM_SUCCESS 0 #define RAM_BAD_ADDRESS -1 @@ -10,263 +14,24 @@ #define RAM_BAD_BIT -3 #define RAM_BAD_ACCESS -4 -typedef enum { - DARWIN_MODEL_NUMBER_L = DEVICE_MODEL_OFFSET, - DARWIN_MODEL_NUMBER_H = DEVICE_MODEL_OFFSET+1, - DARWIN_VERSION = FIRMWARE_VERSION_OFFSET, - DARWIN_ID = DEVICE_ID_OFFSET, - DARWIN_BAUD_RATE = BAUDRATE_OFFSET, - DARWIN_RETURN_DELAY_TIME = RETURN_DELAY_OFFSET, - DARWIN_MM_PERIOD_L = MM_PERIOD_OFFSET, - DARWIN_MM_PERIOD_H = MM_PERIOD_OFFSET+1, - DARWIN_RETURN_LEVEL = RETURN_LEVEL_OFFSET, - DARWIN_DXL_POWER = 0x18, - DARWIN_LED_PANNEL = 0x19, - DARWIN_LED_5_L = 0x1A, - DARWIN_LED_5_H = 0x1B, - DARWIN_LED_6_L = 0x1C, - DARWIN_LED_6_H = 0x1D, - DARWIN_PUSHBUTTON = 0x1E, - DARWIN_MM_NUM_SERVOS = 0x1F, - DARWIN_MM_CONTROL = 0x20, - DARWIN_MM_STATUS = 0x21, - DARWIN_WALK_CONTROL = 0x22, - DARWIN_WALK_STATUS = 0x23, - DARWIN_IMU_CONTROL = 0x24, - DARWIN_IMU_STATUS = 0x25, - DARWIN_IMU_ACCEL_X_L = 0x26, - DARWIN_IMU_ACCEL_X_H = 0x27, - DARWIN_IMU_ACCEL_Y_L = 0x28, - DARWIN_IMU_ACCEL_Y_H = 0x29, - DARWIN_IMU_ACCEL_Z_L = 0x2A, - DARWIN_IMU_ACCEL_Z_H = 0x2B, - DARWIN_IMU_GYRO_X_L = 0x2C, - DARWIN_IMU_GYRO_X_H = 0x2D, - DARWIN_IMU_GYRO_Y_L = 0x2E, - DARWIN_IMU_GYRO_Y_H = 0x2F, - DARWIN_IMU_GYRO_Z_L = 0x30, - DARWIN_IMU_GYRO_Z_H = 0x31, - DARWIN_VOLTAGE = 0x32, - DARWIN_MIC1_L = 0x33, - DARWIN_MIC1_H = 0x34, - DARWIN_ADC2_L = 0x35, - DARWIN_ADC2_H = 0x36, - DARWIN_ADC3_L = 0x37, - DARWIN_ADC3_H = 0x38, - DARWIN_ADC4_L = 0x39, - DARWIN_ADC4_H = 0x3A, - DARWIN_ADC5_L = 0x3B, - DARWIN_ADC5_H = 0x3C, - DARWIN_ADC6_L = 0x3D, - DARWIN_ADC6_H = 0x3E, - DARWIN_ADC7_L = 0x3F, - DARWIN_ADC7_H = 0x40, - DARWIN_ADC8_L = 0x41, - DARWIN_ADC8_H = 0x42, - DARWIN_MIC2_L = 0x43, - DARWIN_MIC2_H = 0x44, - DARWIN_ADC10_L = 0x45, - DARWIN_ADC10_H = 0x46, - DARWIN_ADC11_L = 0x47, - DARWIN_ADC11_H = 0x48, - DARWIN_ADC12_L = 0x49, - DARWIN_ADC12_H = 0x4A, - DARWIN_ADC13_L = 0x4B, - DARWIN_ADC13_H = 0x4C, - DARWIN_ADC14_L = 0x4D, - DARWIN_ADC14_H = 0x4E, - DARWIN_ADC15_L = 0x4F, - DARWIN_ADC15_H = 0x50, - DARWIN_MM_MODULE_EN0 = 0x51, - DARWIN_MM_MODULE_EN1 = 0x52, - DARWIN_MM_MODULE_EN2 = 0x53, - DARWIN_MM_MODULE_EN3 = 0x54, - DARWIN_MM_MODULE_EN4 = 0x55, - DARWIN_MM_MODULE_EN5 = 0x56, - DARWIN_MM_MODULE_EN6 = 0x57, - DARWIN_MM_MODULE_EN7 = 0x58, - DARWIN_MM_MODULE_EN8 = 0x59, - DARWIN_MM_MODULE_EN9 = 0x5A, - DARWIN_MM_MODULE_EN10 = 0x5B, - DARWIN_MM_MODULE_EN11 = 0x5C, - DARWIN_MM_MODULE_EN12 = 0x5D, - DARWIN_MM_MODULE_EN13 = 0x5E, - DARWIN_MM_MODULE_EN14 = 0x5F, - DARWIN_MM_MODULE_EN15 = 0x60, - DARWIN_MM_SERVO0_CUR_POS_L = 0x61, - DARWIN_MM_SERVO0_CUR_POS_H = 0x62, - DARWIN_MM_SERVO1_CUR_POS_L = 0x63, - DARWIN_MM_SERVO1_CUR_POS_H = 0x64, - DARWIN_MM_SERVO2_CUR_POS_L = 0x65, - DARWIN_MM_SERVO2_CUR_POS_H = 0x66, - DARWIN_MM_SERVO3_CUR_POS_L = 0x67, - DARWIN_MM_SERVO3_CUR_POS_H = 0x68, - DARWIN_MM_SERVO4_CUR_POS_L = 0x69, - DARWIN_MM_SERVO4_CUR_POS_H = 0x6A, - DARWIN_MM_SERVO5_CUR_POS_L = 0x6B, - DARWIN_MM_SERVO5_CUR_POS_H = 0x6C, - DARWIN_MM_SERVO6_CUR_POS_L = 0x6D, - DARWIN_MM_SERVO6_CUR_POS_H = 0x6E, - DARWIN_MM_SERVO7_CUR_POS_L = 0x6F, - DARWIN_MM_SERVO7_CUR_POS_H = 0x70, - DARWIN_MM_SERVO8_CUR_POS_L = 0x71, - DARWIN_MM_SERVO8_CUR_POS_H = 0x72, - DARWIN_MM_SERVO9_CUR_POS_L = 0x73, - DARWIN_MM_SERVO9_CUR_POS_H = 0x74, - DARWIN_MM_SERVO10_CUR_POS_L = 0x75, - DARWIN_MM_SERVO10_CUR_POS_H = 0x76, - DARWIN_MM_SERVO11_CUR_POS_L = 0x77, - DARWIN_MM_SERVO11_CUR_POS_H = 0x78, - DARWIN_MM_SERVO12_CUR_POS_L = 0x79, - DARWIN_MM_SERVO12_CUR_POS_H = 0x7A, - DARWIN_MM_SERVO13_CUR_POS_L = 0x7B, - DARWIN_MM_SERVO13_CUR_POS_H = 0x7C, - DARWIN_MM_SERVO14_CUR_POS_L = 0x7D, - DARWIN_MM_SERVO14_CUR_POS_H = 0x7E, - DARWIN_MM_SERVO15_CUR_POS_L = 0x7F, - DARWIN_MM_SERVO15_CUR_POS_H = 0x80, - DARWIN_MM_SERVO16_CUR_POS_L = 0x81, - DARWIN_MM_SERVO16_CUR_POS_H = 0x82, - DARWIN_MM_SERVO17_CUR_POS_L = 0x83, - DARWIN_MM_SERVO17_CUR_POS_H = 0x84, - DARWIN_MM_SERVO18_CUR_POS_L = 0x85, - DARWIN_MM_SERVO18_CUR_POS_H = 0x86, - DARWIN_MM_SERVO19_CUR_POS_L = 0x87, - DARWIN_MM_SERVO19_CUR_POS_H = 0x88, - DARWIN_MM_SERVO20_CUR_POS_L = 0x89, - DARWIN_MM_SERVO20_CUR_POS_H = 0x8A, - DARWIN_MM_SERVO21_CUR_POS_L = 0x8B, - DARWIN_MM_SERVO21_CUR_POS_H = 0x8C, - DARWIN_MM_SERVO22_CUR_POS_L = 0x8D, - DARWIN_MM_SERVO22_CUR_POS_H = 0x8E, - DARWIN_MM_SERVO23_CUR_POS_L = 0x8F, - DARWIN_MM_SERVO23_CUR_POS_H = 0x90, - DARWIN_MM_SERVO24_CUR_POS_L = 0x91, - DARWIN_MM_SERVO24_CUR_POS_H = 0x92, - DARWIN_MM_SERVO25_CUR_POS_L = 0x93, - DARWIN_MM_SERVO25_CUR_POS_H = 0x94, - DARWIN_MM_SERVO26_CUR_POS_L = 0x95, - DARWIN_MM_SERVO26_CUR_POS_H = 0x96, - DARWIN_ACTION_PAGE = 0x97, - DARWIN_ACTION_CONTROL = 0x98, - DARWIN_ACTION_STATUS = 0x99, - DARWIN_X_OFFSET = 0x9A, - DARWIN_Y_OFFSET = 0x9B, - DARWIN_Z_OFFSET = 0x9C, - DARWIN_ROLL = 0x9D, - DARWIN_PITCH = 0x9E, - DARWIN_YAW = 0x9F, - DARWIN_HIP_PITCH_OFF_L = 0xA0, - DARWIN_HIP_PITCH_OFF_H = 0xA1, - DARWIN_PERIOD_TIME_L = 0xA2, - DARWIN_PERIOD_TIME_H = 0xA3, - DARWIN_DSP_RATIO = 0xA4, - DARWIN_STEP_FW_BW_RATIO = 0xA5, - DARWIN_STEP_FW_BW = 0xA6, - DARWIN_STEP_LEFT_RIGHT = 0xA7, - DARWIN_STEP_DIRECTION = 0xA8, - DARWIN_FOOT_HEIGHT = 0xA9, - DARWIN_SWING_RIGHT_LEFT = 0xAA, - DARWIN_SWING_TOP_DOWN = 0xAB, - DARWIN_PELVIS_OFFSET = 0xAC, - DARWIN_ARM_SWING_GAIN = 0xAD, - DARWIN_P_GAIN = 0xAE, - DARWIN_D_GAIN = 0xAF, - DARWIN_I_GAIN = 0xB0, - DARWIN_MAX_ACC = 0xB1, - DARWIN_MAX_ROT_ACC = 0xB2, - DARWIN_MM_KNEE_GAIN = 0xB3, - DARWIN_MM_ANKLE_PITCH_GAIN = 0xB4, - DARWIN_MM_HIP_ROLL_GAIN = 0xB5, - DARWIN_MM_ANKLE_ROLL_GAIN = 0xB6, - DARWIN_SERVO_0_SPEED = 0xB7, - DARWIN_SERVO_1_SPEED = 0xB8, - DARWIN_SERVO_2_SPEED = 0xB9, - DARWIN_SERVO_3_SPEED = 0xBA, - DARWIN_SERVO_4_SPEED = 0xBB, - DARWIN_SERVO_5_SPEED = 0xBC, - DARWIN_SERVO_6_SPEED = 0xBD, - DARWIN_SERVO_7_SPEED = 0xBE, - DARWIN_SERVO_8_SPEED = 0xBF, - DARWIN_SERVO_9_SPEED = 0xC0, - DARWIN_SERVO_10_SPEED = 0xC1, - DARWIN_SERVO_11_SPEED = 0xC2, - DARWIN_SERVO_12_SPEED = 0xC3, - DARWIN_SERVO_13_SPEED = 0xC4, - DARWIN_SERVO_14_SPEED = 0xC5, - DARWIN_SERVO_15_SPEED = 0xC6, - DARWIN_SERVO_16_SPEED = 0xC7, - DARWIN_SERVO_17_SPEED = 0xC8, - DARWIN_SERVO_18_SPEED = 0xC9, - DARWIN_SERVO_19_SPEED = 0xCA, - DARWIN_SERVO_20_SPEED = 0xCB, - DARWIN_SERVO_21_SPEED = 0xCC, - DARWIN_SERVO_22_SPEED = 0xCD, - DARWIN_SERVO_23_SPEED = 0xCE, - DARWIN_SERVO_24_SPEED = 0xCF, - DARWIN_SERVO_25_SPEED = 0xD0, - DARWIN_SERVO_26_SPEED = 0xD1, - DARWIN_SERVO_0_ACCEL = 0xD2, - DARWIN_SERVO_1_ACCEL = 0xD3, - DARWIN_SERVO_2_ACCEL = 0xD4, - DARWIN_SERVO_3_ACCEL = 0xD5, - DARWIN_SERVO_4_ACCEL = 0xD6, - DARWIN_SERVO_5_ACCEL = 0xD7, - DARWIN_SERVO_6_ACCEL = 0xD8, - DARWIN_SERVO_7_ACCEL = 0xD9, - DARWIN_SERVO_8_ACCEL = 0xDA, - DARWIN_SERVO_9_ACCEL = 0xDB, - DARWIN_SERVO_10_ACCEL = 0xDC, - DARWIN_SERVO_11_ACCEL = 0xDD, - DARWIN_SERVO_12_ACCEL = 0xDE, - DARWIN_SERVO_13_ACCEL = 0xDF, - DARWIN_SERVO_14_ACCEL = 0xE0, - DARWIN_SERVO_15_ACCEL = 0xE1, - DARWIN_SERVO_16_ACCEL = 0xE2, - DARWIN_SERVO_17_ACCEL = 0xE3, - DARWIN_SERVO_18_ACCEL = 0xE4, - DARWIN_SERVO_19_ACCEL = 0xE5, - DARWIN_SERVO_20_ACCEL = 0xE6, - DARWIN_SERVO_21_ACCEL = 0xE7, - DARWIN_SERVO_22_ACCEL = 0xE8, - DARWIN_SERVO_23_ACCEL = 0xE9, - DARWIN_SERVO_24_ACCEL = 0xEA, - DARWIN_SERVO_25_ACCEL = 0xEB, - DARWIN_SERVO_26_ACCEL = 0xEC, - DARWIN_JOINTS_CONTROL = 0xED, - DARWIN_JOINTS_STATUS = 0xEE, - DARWIN_HEAD_PAN_L = 0xEF, - DARWIN_HEAD_PAN_H = 0xF0, - DARWIN_HEAD_TILT_L = 0xF1, - DARWIN_HEAD_TILT_H = 0xF2, - DARWIN_HEAD_PAN_P = 0xF3, - DARWIN_HEAD_PAN_I = 0xF4, - DARWIN_HEAD_PAN_D = 0xF5, - DARWIN_HEAD_TILT_P = 0xF6, - DARWIN_HEAD_TILT_I = 0xF7, - DARWIN_HEAD_TILT_D = 0xF8, - DARWIN_HEAD_CONTROL = 0xF9, - DARWIN_HEAD_STATUS = 0xFA, - DARWIN_MM_CUR_PERIOD = 0xFB, - DARWIN_GRIPPER_CONTROL = 0xFC, - DARWIN_GRIPPER_STATUS = 0xFD, - DARWIN_GRIPPER_FORCE_LEFT = 0xFE, - DARWIN_GRIPPER_FORCE_RIGHT = 0xFF -} darwin_registers; - -#define RAM_SIZE ((uint16_t)0x100) +#define RAM_SIZE 512 extern uint8_t ram_data[RAM_SIZE]; void ram_init(void); -inline void ram_read_byte(uint8_t address, uint8_t *data); -inline void ram_read_word(uint8_t address, uint16_t *data); -uint8_t ram_read_table(uint8_t address, uint8_t length,uint8_t *data); -uint8_t ram_set_bit(uint8_t address, uint8_t bit); -uint8_t ram_clear_bit(uint8_t address, uint8_t bit); -uint8_t ram_write_byte(uint8_t address, uint8_t data); -uint8_t ram_write_word(uint8_t address, uint16_t data); -uint8_t ram_write_table(uint8_t address, uint8_t length,uint8_t *data); -inline uint8_t ram_in_range(uint8_t start_reg,uint8_t end_reg,uint8_t address,uint8_t length); +inline void ram_read_byte(uint16_t address, uint8_t *data); +inline void ram_read_word(uint16_t address, uint16_t *data); +uint8_t ram_read_table(uint16_t address, uint16_t length,uint8_t *data); +uint8_t ram_set_bit(uint16_t address, uint8_t bit); +uint8_t ram_clear_bit(uint16_t address, uint8_t bit); +uint8_t ram_write_byte(uint16_t address, uint8_t data); +uint8_t ram_write_word(uint16_t address, uint16_t data); +uint8_t ram_write_table(uint16_t address, uint16_t length,uint8_t *data); +inline uint8_t ram_in_range(uint16_t reg,uint16_t address,uint16_t length); +uint8_t ram_in_window(uint16_t start_reg,uint16_t reg_length,uint16_t start_address,uint16_t address_length); + +#ifdef __cplusplus +} +#endif #endif diff --git a/include/scheduler.h b/include/scheduler.h new file mode 100644 index 0000000000000000000000000000000000000000..9f0098e90d305d98d3e3d7df7a41217de24ff0d1 --- /dev/null +++ b/include/scheduler.h @@ -0,0 +1,16 @@ +#ifndef _SCHEDULER_H +#define _SCHEDULER_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "stm32f1xx.h" + +void scheduler_init(void); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/src/adc_dma.c b/src/adc_dma.c index e8fc3e7a7d15326128c67f00760e402fa56601db..48e249516a63ce904f2e064b859b55e0aa7f171f 100755 --- a/src/adc_dma.c +++ b/src/adc_dma.c @@ -76,10 +76,10 @@ #define ADC_DMA DMA1 #define ADC_ENABLE_DMA_CLK __HAL_RCC_DMA1_CLK_ENABLE() -#define ADC_DMA_CHANNEL DMA1_Channel1 +#define ADC_DMA_CHANNEL DMA1_Channel1 -#define ADC_DMA_IRQn DMA1_Channel1_IRQn -#define ADC_DMA_IRQHandler DMA1_Channel1_IRQHandler +#define ADC_DMA_IRQn DMA1_Channel1_IRQn +#define ADC_DMA_IRQHandler DMA1_Channel1_IRQHandler // private variables ADC_HandleTypeDef hadc1; diff --git a/src/cm730_fw.c b/src/cm730_fw.c index db059f17946e56f7b93523d883953b476f0e2769..eb2a5426efcec08344fa087302d32b5807adc6d3 100755 --- a/src/cm730_fw.c +++ b/src/cm730_fw.c @@ -6,46 +6,11 @@ #include "darwin_time.h" #include "darwin_dyn_slave.h" #include "darwin_dyn_master.h" -//#include "motion_manager.h" -//#include "action.h" -//#include "walking.h" -//#include "joint_motion.h" -//#include "head_tracking.h" -//#include "grippers.h" -//#include "dynamixel_master_uart_dma.h" -//#include "imu.h" -//#include <math.h> - -void SysTick_Handler(void) -{ - HAL_IncTick(); - HAL_SYSTICK_IRQHandler(); -} int main(void) { -// uint16_t eeprom_data; -// uint16_t period,count=0; - - /* initialize the dynamixel master interface */ -// dyn_master_init(); -// dyn_master_set_timeout(50); - // initialize motion manager -// 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); -// grippers_init(); - // initialize imu -// imu_init(); - -// dyn_master_enable_power(); -// delay_ms(1000); - /* initialize the HAL module */ HAL_Init(); - HAL_Delay(1000); /* initialize the GPIO module */ gpio_init(); /* initialize EEPROM */ @@ -58,18 +23,9 @@ int main(void) darwin_time_init(); /* initialize the dynamixel slave interface */ darwin_dyn_slave_init(); + darwin_dyn_slave_start(); /* initialize the dynamixel master interface */ darwin_dyn_master_init(); -// EE_ReadVariable(DEVICE_ID_OFFSET,&eeprom_data); -// dyn_slave_set_address((uint8_t)eeprom_data); -// EE_ReadVariable(RETURN_DELAY_OFFSET,&eeprom_data); -// dyn_slave_set_return_delay((uint8_t)eeprom_data); -// EE_ReadVariable(RETURN_LEVEL_OFFSET,&eeprom_data); -// dyn_slave_set_return_level((uint8_t)eeprom_data); - // initlaize the communication moduleKE - -// comm_init(); - darwin_dyn_master_enable_power(); HAL_Delay(1000); @@ -80,10 +36,6 @@ int main(void) gpio_clear_led(LED_2); darwin_dyn_master_disable_power(); -// comm_start(); - while(1)/* main function does not return */ - { - dyn_slave_loop(&darwin_dyn_slave); - } + while(1);/* main function does not return */ } diff --git a/src/comm.c b/src/comm.c deleted file mode 100644 index 0d0431ae5bd5c7ead5ca61a7e966b3e16966fa84..0000000000000000000000000000000000000000 --- a/src/comm.c +++ /dev/null @@ -1,363 +0,0 @@ -#include "comm.h" -#include "gpio.h" -#include "ram.h" -//#include "motion_manager.h" -//#include "action.h" -//#include "walking.h" -//#include "joint_motion.h" -//#include "head_tracking.h" -//#include "grippers.h" -#include "dynamixel_slave_uart_dma.h" -//#include "dynamixel_master_uart_dma.h" -//#include "imu.h" -//#include "adc_dma.h" - -#define COMM_TIMER TIM7 -#define COMM_TIMER_IRQn TIM7_IRQn -#define COMM_TIMER_IRQHandler TIM7_IRQHandler -#define COMM_TIMER_ENABLE_CLK __HAL_RCC_TIM7_CLK_ENABLE() - -// private variables -uint8_t inst_packet[MAX_DATA_LENGTH]; -uint8_t status_packet[MAX_DATA_LENGTH]; -uint8_t data[MAX_DATA_LENGTH]; -// registered write operation variables -uint8_t reg_data[MAX_DATA_LENGTH]; -uint8_t reg_address,reg_length,reg_op; -// Timer handle -TIM_HandleTypeDef COMM_TIMHandle; - -// private functions -uint8_t read_operation(uint8_t address, uint8_t length, uint8_t *data) -{ - uint8_t error; - - // pre-read operation - // read operation - error=ram_read_table(address,length,data); - // post-read operation - if(ram_in_range(DARWIN_PUSHBUTTON,DARWIN_PUSHBUTTON,address,length)) - ram_data[DARWIN_PUSHBUTTON]=0x00; - - return error; -} - -uint8_t write_operation(uint8_t address,uint8_t length, uint8_t *data) -{ - uint8_t error=RAM_SUCCESS,byte_value,i,j,do_op=0x00,module; - uint16_t word_value; - - // pre-write operation -/* if(ram_in_range(DARWIN_DXL_POWER,DARWIN_DXL_POWER,address,length)) - { - if(data[DARWIN_DXL_POWER-address]&0x01) dyn_master_enable_power(); - else dyn_master_disable_power(); - } - if(ram_in_range(DARWIN_LED_PANNEL,DARWIN_LED_PANNEL,address,length)) - { - byte_value=data[DARWIN_LED_PANNEL-address]; - if(byte_value&0x01) - gpio_set_led(LED_2); - else - gpio_clear_led(LED_2); - if(byte_value&0x02) - gpio_set_led(LED_3); - else - gpio_clear_led(LED_3); - if(byte_value&0x04) - gpio_set_led(LED_4); - else - gpio_clear_led(LED_4); - if(byte_value&0x08) - gpio_set_led(LED_TX); - else - gpio_clear_led(LED_TX); - if(byte_value&0x10) - gpio_set_led(LED_RX); - else - gpio_clear_led(LED_RX); - } - if(ram_in_range(DARWIN_MM_PERIOD_L,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_MODULE_EN0,j=0;i<=DARWIN_MM_MODULE_EN15;i++,j+=2) - { - do_op=0x01; - if(ram_in_range(i,i,address,length)) - { - byte_value=data[i-address]; - if(byte_value&0x80) manager_enable_servo(j); - else manager_disable_servo(j); - module=(byte_value&0x70)>>4; - manager_set_module(j,module); - if(module==MM_JOINTS) - joint_motion_load_target_angle(j,current_angles[j]>>9); - if(byte_value&0x08) manager_enable_servo(j+1); - else manager_disable_servo(j+1); - module=byte_value&0x07; - manager_set_module(j+1,module); - if(module==MM_JOINTS) - joint_motion_load_target_angle(j+1,current_angles[j+1]>>9); - } - } - if(do_op) - { - do_op=0x00; - manager_disable_servos(); - } - if(ram_in_range(DARWIN_MM_CONTROL,DARWIN_MM_CONTROL,address,length)) - { - if(data[DARWIN_MM_CONTROL-address]&0x01) - manager_enable(); - else - manager_disable(); - if(data[DARWIN_MM_CONTROL-address]&0x02) - manager_enable_balance(); - else - manager_disable_balance(); - } - if(ram_in_range(DARWIN_MM_KNEE_GAIN,DARWIN_MM_KNEE_GAIN,address,length)) - manager_set_knee_gain(data[DARWIN_MM_CONTROL-address]); - if(ram_in_range(DARWIN_MM_ANKLE_PITCH_GAIN,DARWIN_MM_ANKLE_PITCH_GAIN,address,length)) - manager_set_ankle_pitch_gain(data[DARWIN_MM_CONTROL-address]); - if(ram_in_range(DARWIN_MM_HIP_ROLL_GAIN,DARWIN_MM_HIP_ROLL_GAIN,address,length)) - manager_set_hip_roll_gain(data[DARWIN_MM_CONTROL-address]); - if(ram_in_range(DARWIN_MM_ANKLE_ROLL_GAIN,DARWIN_MM_ANKLE_ROLL_GAIN,address,length)) - manager_set_ankle_roll_gain(data[DARWIN_MM_CONTROL-address]); - if(ram_in_range(DARWIN_ACTION_CONTROL,DARWIN_ACTION_CONTROL,address,length)) - { - if(data[DARWIN_ACTION_CONTROL-address]&0x01) - action_start_page(); - if(data[DARWIN_ACTION_CONTROL-address]&0x02) - action_stop_page(); - } - if(ram_in_range(DARWIN_ACTION_PAGE,DARWIN_ACTION_PAGE,address,length))// load the page identifier - action_load_page(data[DARWIN_ACTION_PAGE-address]); - if(ram_in_range(DARWIN_WALK_CONTROL,DARWIN_WALK_CONTROL,address,length)) - { - if(data[DARWIN_WALK_CONTROL-address]&0x01) - walking_start(); - if(data[DARWIN_WALK_CONTROL-address]&0x02) - walking_stop(); - } - for(i=DARWIN_X_OFFSET;i<=DARWIN_MAX_ROT_ACC;i++) - if(ram_in_range(i,i,address,length)) - walking_set_param(i,data[i-address]); - if(ram_in_range(DARWIN_IMU_CONTROL,DARWIN_IMU_CONTROL,address,length)) - { - if(data[DARWIN_IMU_CONTROL-address]&0x01) - imu_start(); - else - imu_stop(); - if(data[DARWIN_IMU_CONTROL-address]&0x02) - imu_start_gyro_cal(); - } - for(i=DARWIN_MM_SERVO0_CUR_POS_L,j=0;i<=DARWIN_MM_SERVO26_CUR_POS_L;i+=2,j++) - if(ram_in_range(i,i+1,address,length)) - joint_motion_load_target_angle(j,data[i-address]+(data[i+1-address]<<8)); - for(i=DARWIN_SERVO_0_SPEED,j=0;i<=DARWIN_SERVO_26_SPEED;i++,j++) - if(ram_in_range(i,i,address,length)) - joint_motion_load_target_speed(j,data[i-address]); - for(i=DARWIN_SERVO_0_ACCEL,j=0;i<=DARWIN_SERVO_26_ACCEL;i++,j++) - if(ram_in_range(i,i,address,length)) - joint_motion_load_target_accel(j,data[i-address]); - if(ram_in_range(DARWIN_JOINTS_CONTROL,DARWIN_JOINTS_CONTROL,address,length)) - { - if(data[DARWIN_JOINTS_CONTROL-address]&0x01) - joint_motion_start(); - if(data[DARWIN_JOINTS_CONTROL-address]&0x02) - joint_motion_stop(); - } - if(ram_in_range(DARWIN_HEAD_PAN_L,DARWIN_HEAD_PAN_H,address,length)) - { - word_value=data[DARWIN_HEAD_PAN_L-address]+(data[DARWIN_HEAD_PAN_H-address]<<8); - head_tracking_set_target_pan(word_value); - } - if(ram_in_range(DARWIN_HEAD_TILT_L,DARWIN_HEAD_TILT_H,address,length)) - { - word_value=data[DARWIN_HEAD_TILT_L-address]+(data[DARWIN_HEAD_TILT_H-address]<<8); - head_tracking_set_target_tilt(word_value); - } - if(ram_in_range(DARWIN_HEAD_PAN_P,DARWIN_HEAD_PAN_P,address,length)) - head_tracking_set_pan_p(data[DARWIN_HEAD_PAN_P-address]); - if(ram_in_range(DARWIN_HEAD_PAN_I,DARWIN_HEAD_PAN_I,address,length)) - head_tracking_set_pan_i(data[DARWIN_HEAD_PAN_I-address]); - if(ram_in_range(DARWIN_HEAD_PAN_D,DARWIN_HEAD_PAN_D,address,length)) - head_tracking_set_pan_d(data[DARWIN_HEAD_PAN_D-address]); - if(ram_in_range(DARWIN_HEAD_TILT_P,DARWIN_HEAD_TILT_P,address,length)) - head_tracking_set_tilt_p(data[DARWIN_HEAD_TILT_P-address]); - if(ram_in_range(DARWIN_HEAD_TILT_I,DARWIN_HEAD_TILT_I,address,length)) - head_tracking_set_tilt_i(data[DARWIN_HEAD_TILT_I-address]); - if(ram_in_range(DARWIN_HEAD_TILT_D,DARWIN_HEAD_TILT_D,address,length)) - head_tracking_set_tilt_d(data[DARWIN_HEAD_TILT_D-address]); - if(ram_in_range(DARWIN_HEAD_CONTROL,DARWIN_HEAD_CONTROL,address,length)) - { - if(data[DARWIN_HEAD_CONTROL-address]&0x01) - head_tracking_start(); - else - head_tracking_stop(); - } - if(ram_in_range(DARWIN_GRIPPER_CONTROL,DARWIN_GRIPPER_CONTROL,address,length)) - { - if(data[DARWIN_GRIPPER_CONTROL-address]&0x01) - grippers_open(LEFT_GRIPPER); - else if(data[DARWIN_GRIPPER_CONTROL-address]&0x02) - grippers_close(LEFT_GRIPPER); - else if(data[DARWIN_GRIPPER_CONTROL-address]&0x04) - grippers_open(RIGHT_GRIPPER); - else if(data[DARWIN_GRIPPER_CONTROL-address]&0x08) - grippers_close(RIGHT_GRIPPER); - }*/ - // write eeprom - for(i=address,j=0;i<LAST_EEPROM_OFFSET && i<(address+length);i++,j++) - EE_WriteVariable(i,data[j]); - // write operation - // error=ram_write_table(address,length,data); - // post-write operation - - return error; -} - -// interrupt handlers -void COMM_TIMER_IRQHandler(void) -{ - uint8_t error,length,prev_id,id; - - if(__HAL_TIM_GET_FLAG(&COMM_TIMHandle, TIM_FLAG_UPDATE) != RESET) - { - if(__HAL_TIM_GET_IT_SOURCE(&COMM_TIMHandle, TIM_IT_UPDATE) !=RESET) - { - __HAL_TIM_CLEAR_IT(&COMM_TIMHandle, TIM_IT_UPDATE); - if(dyn_slave_is_packet_ready())// check if a new instruction packet has been received - { - dyn_slave_get_inst_packet(inst_packet);// get the received packet - // check address - id=dyn_get_id(inst_packet); - if(id==dyn_slave_get_address() || id==DYN_BROADCAST_ID)// the packet is addressed to this device or it is a broadcast - { - // check the packet checksum - if(dyn_check_checksum(inst_packet)==0xFF)// the incomming packet is okay - { - // process the packet - switch(dyn_get_instruction(inst_packet)) - { - case DYN_PING: if(id!=DYN_BROADCAST_ID) - dyn_slave_send_status_packet(DYN_NO_ERROR,0,data); - break; - case DYN_READ: error=read_operation(dyn_get_read_address(inst_packet),dyn_get_read_length(inst_packet),data); - if(dyn_slave_get_return_level()!=0 && id!=DYN_BROADCAST_ID) - { - if(error==RAM_SUCCESS) - dyn_slave_send_status_packet(DYN_NO_ERROR,dyn_get_read_length(inst_packet),data); - else - dyn_slave_send_status_packet(DYN_INST_ERROR,0,data); - } - break; - case DYN_WRITE: length=dyn_get_write_data(inst_packet,data); - error=write_operation(dyn_get_write_address(inst_packet),length,data); - if(dyn_slave_get_return_level()==2 && id!=DYN_BROADCAST_ID) - { - if(error==RAM_SUCCESS) - dyn_slave_send_status_packet(DYN_NO_ERROR,0,data); - else - dyn_slave_send_status_packet(DYN_INST_ERROR,0,data); - } - break; - case DYN_REG_WRITE: reg_length=dyn_get_reg_write_data(inst_packet,reg_data); - reg_address=dyn_get_reg_write_address(inst_packet); - reg_op=0x01;// signal there is a registered operation pending - if(dyn_slave_get_return_level()==2 && id!=DYN_BROADCAST_ID) - dyn_slave_send_status_packet(DYN_NO_ERROR,0,data); - break; - case DYN_ACTION: if(reg_op) - { - error=write_operation(reg_address,reg_length,reg_data); - reg_op=0x00; - } - break; - case DYN_RESET: - break; - case DYN_SYNC_READ: dyn_slave_send_status_packet(DYN_INST_ERROR,0,data); - break; - case DYN_SYNC_WRITE: if(dyn_sync_write_id_present(inst_packet,dyn_slave_get_address()))// the device is addressed - { - length=dyn_get_sync_write_data(inst_packet,dyn_slave_get_address(),data); - error=write_operation(dyn_get_sync_write_address(inst_packet),length,data); - } - break; - case DYN_BULK_READ: if(dyn_bulk_read_id_present(inst_packet,dyn_slave_get_address(),&prev_id)) - { - length=dyn_get_bulk_read_length(inst_packet,dyn_slave_get_address()); - error=read_operation(dyn_get_bulk_read_address(inst_packet,dyn_slave_get_address()),length,data); - if(prev_id==0xFF)// first device in the bulk read sequence - { - if(error==RAM_SUCCESS) - dyn_slave_send_status_packet(DYN_NO_ERROR,length,data); - else - dyn_slave_send_status_packet(DYN_INST_ERROR,0,data); - } - else// wait for the previous device in the sequence to send its data - { - do{ - while(!dyn_slave_is_packet_ready());// wait until a new packet is received - dyn_slave_get_inst_packet(inst_packet); - }while(dyn_get_id(inst_packet)!=prev_id); - if(error==RAM_SUCCESS) - dyn_slave_send_status_packet(DYN_NO_ERROR,length,data); - else - dyn_slave_send_status_packet(DYN_INST_ERROR,0,data); - } - } - break; - case DYN_BULK_WRITE: dyn_slave_send_status_packet(DYN_INST_ERROR,0,data); - break; - default: - break; - } - } - else - { - // send a checksum error answer - if(dyn_get_id(inst_packet)!=DYN_BROADCAST_ID) - dyn_slave_send_status_packet(DYN_CHECKSUM_ERROR,0,0x00); - } - } - else// the packet is addressed to another device - { - // send the incomming packet to the dynamixel bus -/* if(dyn_master_resend_inst_packet(inst_packet,status_packet)!=DYN_TIMEOUT) - { - // send the answer back to the computer - dyn_slave_resend_status_packet(status_packet); - }*/ - } - } - } - } -} - -// public functions -void comm_init(void) -{ - COMM_TIMHandle.Instance=COMM_TIMER; - COMM_TIMHandle.Init.Period = 1000; - COMM_TIMHandle.Init.Prescaler = 72; - COMM_TIMHandle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; - COMM_TIMHandle.Init.CounterMode = TIM_COUNTERMODE_UP; - HAL_TIM_Base_Init(&COMM_TIMHandle); - COMM_TIMER_ENABLE_CLK; - // initialize the timer interrupts - HAL_NVIC_SetPriority(COMM_TIMER_IRQn, 2, 0); - HAL_NVIC_EnableIRQ(COMM_TIMER_IRQn); - - /* initialize variables */ - reg_op=0x00; -} - -void comm_start(void) -{ - HAL_TIM_Base_Start_IT(&COMM_TIMHandle); -} - diff --git a/src/darwin_dyn_slave.c b/src/darwin_dyn_slave.c index c68b5ec63d55e84493123458bbe0e1dd49945686..e85810e8b2c8adb37e72d428b0bb939d803315d7 100755 --- a/src/darwin_dyn_slave.c +++ b/src/darwin_dyn_slave.c @@ -1,28 +1,90 @@ #include "darwin_dyn_slave.h" #include "darwin_time.h" #include "usart3.h" +#include "ram.h" +#include "gpio.h" + +/* timer for the execution of the dynamixel slave loop */ +#define DYN_SLAVE_TIMER TIM7 +#define DYN_SLAVE_TIMER_IRQn TIM7_IRQn +#define DYN_SLAVE_TIMER_IRQHandler TIM7_IRQHandler +#define DYN_SLAVE_TIMER_ENABLE_CLK __HAL_RCC_TIM7_CLK_ENABLE() /* private variables */ TDynamixelSlave darwin_dyn_slave; TTime darwin_dyn_slave_timer; TComm darwin_dyn_slave_comm; +UART_InitTypeDef darwin_comm_init; +TIM_HandleTypeDef darwin_dyn_slave_tim_handle; // private functions unsigned char darwin_on_read(unsigned short int address,unsigned short int length,unsigned char *data) { - return 0x00; + unsigned char error; + + if(ram_in_window(GPIO_BASE_ADDRESS,GPIO_MEM_LENGTH,address,length)) + gpio_process_read_cmd(address,length,data); + /* dynamixel slave internal operation registers */ + error=ram_read_table(address,length,data); + + return error; } unsigned char darwin_on_write(unsigned short int address,unsigned short int length,unsigned char *data) { + /* dynamixel slave internal operation registers */ + if(ram_in_range(BIOLOID_ID,address,length)) + { + dyn_slave_set_address(&darwin_dyn_slave,data[BIOLOID_ID-address]); + ram_data[BIOLOID_ID]=data[BIOLOID_ID-address]; + } + if(ram_in_range(BIOLOID_BAUD_RATE,address,length)) + { + darwin_comm_init.BaudRate=2000000/(data[BIOLOID_BAUD_RATE-address]+1); + usart3_config(&darwin_dyn_slave_comm,&darwin_comm_init); + ram_data[BIOLOID_BAUD_RATE]=data[BIOLOID_BAUD_RATE-address]; + } + if(ram_in_range(BIOLOID_RETURN_DELAY_TIME,address,length)) + { + dyn_slave_set_return_delay(&darwin_dyn_slave,data[BIOLOID_RETURN_DELAY_TIME-address]); + ram_data[BIOLOID_RETURN_DELAY_TIME]=data[BIOLOID_RETURN_DELAY_TIME-address]; + } + if(ram_in_range(BIOLOID_RETURN_LEVEL,address,length)) + { + dyn_slave_set_return_level(&darwin_dyn_slave,data[BIOLOID_RETURN_LEVEL-address]); + ram_data[BIOLOID_RETURN_LEVEL]=data[BIOLOID_RETURN_LEVEL-address]; + } + // GPIO commands + if(gpio_in_range(address,length)) + gpio_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]); + return 0x00; } +void darwin_on_ping(void) +{ +} + +/* interrupt service routines */ +void DYN_SLAVE_TIMER_IRQHandler(void) +{ + if(__HAL_TIM_GET_FLAG(&darwin_dyn_slave_tim_handle, TIM_FLAG_UPDATE) != RESET) + { + if(__HAL_TIM_GET_IT_SOURCE(&darwin_dyn_slave_tim_handle, TIM_IT_UPDATE) !=RESET) + { + __HAL_TIM_CLEAR_IT(&darwin_dyn_slave_tim_handle, TIM_IT_UPDATE); + dyn_slave_loop(&darwin_dyn_slave); + } + } +} + // public functions void darwin_dyn_slave_init(void) { TUSART_IRQ_Priorities priorities; - UART_InitTypeDef Init; // initialize timer structure time_init(&darwin_dyn_slave_timer,darwin_time_get_counts_per_us(),darwin_time_get_counts); @@ -45,9 +107,34 @@ void darwin_dyn_slave_init(void) priorities.dma_tx_subpriority=0; usart3_init(&darwin_dyn_slave_comm,&Init,&priorities); - dyn_slave_init(&darwin_dyn_slave,&darwin_dyn_slave_comm,0x01,DYN_VER1); + dyn_slave_init(&darwin_dyn_slave,&darwin_dyn_slave_comm,0x01,DYN_VER2); darwin_dyn_slave.on_read=darwin_on_read; darwin_dyn_slave.on_write=darwin_on_write; - //dyn_slave_set_return_delay(&battery_dyn_slave,ram_data[BATTERY_RETURN_DELAY_TIME]); - //dyn_slave_set_return_level(&battery_dyn_slave,ram_data[BATTERY_STATUS_RETURN_LEVEL]); + darwin_dyn_slave.on_ping=darwin_on_ping; + dyn_slave_set_return_delay(&darwin_dyn_slave,ram_data[DARWIN_RETURN_DELAY_TIME]); + dyn_slave_set_return_level(&darwin_dyn_slave,ram_data[DARWIN_STATUS_RETURN_LEVEL]); + + /* initialize timer for the execution of the dynamixel slave loop */ + DYN_SLAVE_TIMER_ENABLE_CLK; + bioloid_dyn_slave_tim_handle.Instance=DYN_SLAVE_TIMER; + bioloid_dyn_slave_tim_handle.Init.Period = 1000; + bioloid_dyn_slave_tim_handle.Init.Prescaler = 72; + bioloid_dyn_slave_tim_handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + bioloid_dyn_slave_tim_handle.Init.CounterMode = TIM_COUNTERMODE_UP; + HAL_TIM_Base_Init(&darwin_dyn_slave_tim_handle); + // initialize the timer interrupts + HAL_NVIC_SetPriority(DYN_SLAVE_TIMER_IRQn, 2, 0); + HAL_NVIC_EnableIRQ(DYN_SLAVE_TIMER_IRQn); + +} + +void darwin_dyn_slave_start(void) +{ + HAL_TIM_Base_Start_IT(&darwin_dyn_slave_tim_handle); } + +void darwin_dyn_slave_stop(void) +{ + HAL_TIM_Base_Stop_IT(&darwin_dyn_slave_tim_handle); +} + diff --git a/src/darwin_time.c b/src/darwin_time.c index d96933101d1c7580eeb9f31fb85d6fe1ac9599e9..1935291b86d03424083e39b4fa9d71e4d2f2197d 100755 --- a/src/darwin_time.c +++ b/src/darwin_time.c @@ -2,16 +2,23 @@ #include "gpio.h" TIM_HandleTypeDef us_timer_Handle; -unsigned long long int counts; +unsigned long long int timer_counts; +unsigned short int timer_counts_per_us; -void TIM1_UP_IRQHandler(void) +void SysTick_Handler(void) +{ + HAL_IncTick(); + HAL_SYSTICK_IRQHandler(); +} + +void TIM8_UP_IRQHandler(void) { if(__HAL_TIM_GET_FLAG(&us_timer_Handle, TIM_FLAG_UPDATE) != RESET) { if(__HAL_TIM_GET_IT_SOURCE(&us_timer_Handle, TIM_IT_UPDATE) !=RESET) { __HAL_TIM_CLEAR_IT(&us_timer_Handle, TIM_IT_UPDATE); - counts+=1; + timer_counts+=1; } } if(__HAL_TIM_GET_FLAG(&us_timer_Handle, TIM_FLAG_BREAK) != RESET) @@ -44,13 +51,17 @@ void darwin_time_init(void) TIM_ClockConfigTypeDef sClockSourceConfig; TIM_MasterConfigTypeDef sMasterConfig; - us_timer_Handle.Instance=TIM1; + /* initialize internal variables */ + timer_counts=0; + timer_counts_per_us=(SystemCoreClock/1000000)+1; + + us_timer_Handle.Instance=TIM8; us_timer_Handle.Init.Period=0xFFFF; us_timer_Handle.Init.Prescaler=0; us_timer_Handle.Init.ClockDivision=TIM_CLOCKDIVISION_DIV1; us_timer_Handle.Init.CounterMode=TIM_COUNTERMODE_UP; HAL_TIM_Base_Init(&us_timer_Handle); - __TIM1_CLK_ENABLE(); + __HAL_RCC_TIM8_CLK_ENABLE() __HAL_TIM_SetCounter(&us_timer_Handle,0); sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL; @@ -61,19 +72,19 @@ void darwin_time_init(void) HAL_TIMEx_MasterConfigSynchronization(&us_timer_Handle, &sMasterConfig); // enable interrupts - HAL_NVIC_SetPriority(TIM1_UP_IRQn, 3, 3); - HAL_NVIC_EnableIRQ(TIM1_UP_IRQn); + HAL_NVIC_SetPriority(TIM8_UP_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(TIM8_UP_IRQn); HAL_TIM_Base_Start_IT(&us_timer_Handle); - counts=0; + timer_counts=0; } unsigned long long int darwin_time_get_counts(void) { - return (counts<<16)+__HAL_TIM_GetCounter(&us_timer_Handle); + return (timer_counts<<16)+__HAL_TIM_GetCounter(&us_timer_Handle); } unsigned int darwin_time_get_counts_per_us(void) { - return (SystemCoreClock/1000000)+1; + return timer_counts_per_us; } diff --git a/src/eeprom.c b/src/eeprom.c index 5af13c96a942abdbc9e13c58cc38b72b017a2da5..20b740e3926f02b19f8478d5f67e4372c761d885 100755 --- a/src/eeprom.c +++ b/src/eeprom.c @@ -41,17 +41,11 @@ /* Includes ------------------------------------------------------------------*/ #include "eeprom.h" +#include "eeprom_init.h" +#include "darwin_registers.h" /* Private typedef -----------------------------------------------------------*/ /* Private define ------------------------------------------------------------*/ -#define EEPROM_SIZE 0x09 -#define DEFAULT_DEVICE_MODEL 0x7300 -#define DEFAULT_FIRMWARE_VERSION 0x0013 -#define DEFAULT_DEVICE_ID 0x00C0 -#define DEFAULT_BAUDRATE 0x0001 -#define DEFAULT_RETURN_DELAY 0x0000 -#define DEFAULT_MM_PERIOD 0x01FF -#define DEFAULT_RETURN_LEVEL 0x0002 /* Private macro -------------------------------------------------------------*/ /* Private variables ---------------------------------------------------------*/ @@ -59,26 +53,75 @@ uint16_t DataVar = 0; /* Virtual address defined by the user: 0xFFFF value is prohibited */ -uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE, - 0xFFFF, - DEFAULT_DEVICE_MODEL&0xFF,// model number LSB - DEVICE_MODEL_OFFSET, - DEFAULT_DEVICE_MODEL>>8,// model number MSB - DEVICE_MODEL_OFFSET+1, - DEFAULT_FIRMWARE_VERSION,// firmware version - FIRMWARE_VERSION_OFFSET, - DEFAULT_DEVICE_ID,// default device id - DEVICE_ID_OFFSET, - DEFAULT_BAUDRATE,// default baudrate - BAUDRATE_OFFSET, - DEFAULT_RETURN_DELAY,// return delay time - RETURN_DELAY_OFFSET, - DEFAULT_MM_PERIOD&0xFF, - MM_PERIOD_OFFSET, - DEFAULT_MM_PERIOD>>8, - MM_PERIOD_OFFSET+1, - DEFAULT_RETURN_LEVEL, - RETURN_LEVEL_OFFSET};// return level +uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE, 0xFFFF, + DEFAULT_DEVICE_MODEL&0xFF, DEVICE_MODEL_OFFSET,// model number LSB + DEFAULT_DEVICE_MODEL>>8, DEVICE_MODEL_OFFSET+1,// model number MSB + DEFAULT_FIRMWARE_VERSION, FIRMWARE_VERSION_OFFSET,// firmware version + DEFAULT_DEVICE_ID, DEVICE_ID_OFFSET,// default device id + DEFAULT_BAUDRATE, BAUDRATE_OFFSET,// default baudrate + DEFAULT_RETURN_DELAY, RETURN_DELAY_OFFSET,// return delay time + DEFAULT_MM_PERIOD&0xFF, MM_PERIOD_OFFSET, + DEFAULT_MM_PERIOD>>8, MM_PERIOD_OFFSET+1, + DEFAULT_BAL_KNEE_GAIN&0xFF, MM_BAL_KNEE_GAIN_OFFSET, + DEFAULT_BAL_KNEE_GAIN>>8, MM_BAL_KNEE_GAIN_OFFSET+1, + DEFAULT_BAL_ANKLE_ROLL_GAIN&0xFF, MM_BAL_ANKLE_ROLL_GAIN_OFFSET, + DEFAULT_BAL_ANKLE_ROLL_GAIN>>8, MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1, + DEFAULT_BAL_ANKLE_PITCH_GAIN&0xFF, MM_BAL_ANKLE_PITCH_GAIN_OFFSET, + DEFAULT_BAL_ANKLE_PITCH_GAIN>>8, MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1, + DEFAULT_BAL_HIP_ROLL_GAIN&0xFF, MM_BAL_HIP_ROLL_GAIN_OFFSET, + DEFAULT_BAL_HIP_ROLL_GAIN>>8, MM_BAL_HIP_ROLL_GAIN_OFFSET+1, + DEFAULT_RETURN_LEVEL, RETURN_LEVEL_OFFSET,// return level + DEFAULT_SERVO0_OFFSET, MM_SERVO0_OFFSET, + DEFAULT_SERVO1_OFFSET, MM_SERVO1_OFFSET, + DEFAULT_SERVO2_OFFSET, MM_SERVO2_OFFSET, + DEFAULT_SERVO3_OFFSET, MM_SERVO3_OFFSET, + DEFAULT_SERVO4_OFFSET, MM_SERVO4_OFFSET, + DEFAULT_SERVO5_OFFSET, MM_SERVO5_OFFSET, + DEFAULT_SERVO6_OFFSET, MM_SERVO6_OFFSET, + DEFAULT_SERVO7_OFFSET, MM_SERVO7_OFFSET, + DEFAULT_SERVO8_OFFSET, MM_SERVO8_OFFSET, + DEFAULT_SERVO9_OFFSET, MM_SERVO9_OFFSET, + DEFAULT_SERVO10_OFFSET, MM_SERVO10_OFFSET, + DEFAULT_SERVO11_OFFSET, MM_SERVO11_OFFSET, + DEFAULT_SERVO12_OFFSET, MM_SERVO12_OFFSET, + DEFAULT_SERVO13_OFFSET, MM_SERVO13_OFFSET, + DEFAULT_SERVO14_OFFSET, MM_SERVO14_OFFSET, + DEFAULT_SERVO15_OFFSET, MM_SERVO15_OFFSET, + DEFAULT_SERVO16_OFFSET, MM_SERVO16_OFFSET, + DEFAULT_SERVO17_OFFSET, MM_SERVO17_OFFSET, + DEFAULT_SERVO18_OFFSET, MM_SERVO18_OFFSET, + DEFAULT_SERVO19_OFFSET, MM_SERVO19_OFFSET, + DEFAULT_SERVO20_OFFSET, MM_SERVO20_OFFSET, + DEFAULT_SERVO21_OFFSET, MM_SERVO21_OFFSET, + DEFAULT_SERVO22_OFFSET, MM_SERVO22_OFFSET, + DEFAULT_SERVO23_OFFSET, MM_SERVO23_OFFSET, + DEFAULT_SERVO24_OFFSET, MM_SERVO24_OFFSET, + DEFAULT_SERVO25_OFFSET, MM_SERVO25_OFFSET, + DEFAULT_SERVO26_OFFSET, MM_SERVO26_OFFSET, + DEFAULT_SERVO27_OFFSET, MM_SERVO27_OFFSET, + DEFAULT_SERVO28_OFFSET, MM_SERVO28_OFFSET, + DEFAULT_SERVO29_OFFSET, MM_SERVO29_OFFSET, + DEFAULT_SERVO30_OFFSET, MM_SERVO30_OFFSET, + DEFAULT_SERVO31_OFFSET, MM_SERVO31_OFFSET, + DEFAULT_WALK_X_OFFSET, WALK_X_OFFSET, + DEFAULT_WALK_Y_OFFSET, WALK_Y_OFFSET, + DEFAULT_WALK_Z_OFFSET, WALK_Z_OFFSET, + DEFAULT_WALK_ROLL_OFFSET, WALK_ROLL_OFFSET, + DEFAULT_WALK_PITCH_OFFSET, WALK_PITCH_OFFSET, + DEFAULT_WALK_YAW_OFFSET, WALK_YAW_OFFSET, + DEFAULT_WALK_HIP_PITCH_OFF&0xFF, WALK_HIP_PITCH_OFF, + DEFAULT_WALK_HIP_PITCH_OFF>>8, WALK_HIP_PITCH_OFF+1, + DEFAULT_WALK_PERIOD_TIME&0xFF, WALK_PERIOD_TIME, + DEFAULT_WALK_PERIOD_TIME>>8, WALK_PERIOD_TIME+1, + DEFAULT_WALK_DSP_RATIO, WALK_DSP_RATIO, + DEFAULT_WALK_STEP_FW_BW_RATIO, WALK_STEP_FW_BW_RATIO, + DEFAULT_WALK_FOOT_HEIGHT, WALK_FOOT_HEIGHT, + DEFAULT_WALK_SWING_RIGHT_LEFT, WALK_SWING_RIGHT_LEFT, + DEFAULT_WALK_SWING_TOP_DOWN, WALK_SWING_TOP_DOWN, + 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}; /* Private function prototypes -----------------------------------------------*/ /* Private functions ---------------------------------------------------------*/ diff --git a/src/gpio.c b/src/gpio.c index d115d0557843e5860bf01eb1d73b8a773d77e8f6..d42448f6802b89dd12c052d5aba3e40a4d86b96b 100755 --- a/src/gpio.c +++ b/src/gpio.c @@ -80,13 +80,12 @@ __IO uint16_t led_tx_period; __IO uint16_t led_rx_period; __IO uint16_t led_2_period; __IO uint16_t led_3_period; -__IO uint16_t led_4_period; -__IO uint16_t led_5_R_period; -__IO uint16_t led_5_G_period; -__IO uint16_t led_5_B_period; -__IO uint16_t led_6_R_period; -__IO uint16_t led_6_G_period; -__IO uint16_t led_6_B_period; +__IO uint16_t led_5_R_color; +__IO uint16_t led_5_G_color; +__IO uint16_t led_5_B_color; +__IO uint16_t led_6_R_color; +__IO uint16_t led_6_G_color; +__IO uint16_t led_6_B_color; // Pushbuttons callbacks void (*start_pb_callback)(void); void (*mode_pb_callback)(void); @@ -97,14 +96,20 @@ void GPI_EXTI1_IRQHandler(void) if(__HAL_GPIO_EXTI_GET_IT(START_PB_PIN) != RESET) { __HAL_GPIO_EXTI_CLEAR_IT(START_PB_PIN); - if(start_pb_callback!=0) - start_pb_callback(); + if(ram_data[DARWIN_START_PB_CNTRL]&GPIO_INT_USED) + { + if(start_pb_callback!=0) + start_pb_callback(); + } } if(__HAL_GPIO_EXTI_GET_IT(MODE_PB_PIN) != RESET) { __HAL_GPIO_EXTI_CLEAR_IT(MODE_PB_PIN); - if(mode_pb_callback!=0) - mode_pb_callback(); + if(ram_data[DARWIN_MODE_PB_CNTRL]&GPIO_INT_USED) + { + if(mode_pb_callback!=0) + mode_pb_callback(); + } } } @@ -172,6 +177,9 @@ void GPO_TIMER1_IRQHandler(void) void GPO_TIMER2_IRQHandler(void) { + static uint8_t red_phase=0x00; + static uint8_t green_phase=0x00; + static uint8_t blue_phase=0x00; uint32_t capture; if(__HAL_TIM_GET_FLAG(&GPO_TIM2Handle, TIM_FLAG_CC1) != RESET) @@ -180,7 +188,16 @@ void GPO_TIMER2_IRQHandler(void) { __HAL_TIM_CLEAR_IT(&GPO_TIM2Handle, TIM_IT_CC1); capture = HAL_TIM_ReadCapturedValue(&GPO_TIM2Handle, TIM_CHANNEL_1); - __HAL_TIM_SET_COMPARE(&GPO_TIM2Handle, TIM_CHANNEL_1, (capture + led_5_R_period)); + if(red_phase==0x00) + { + __HAL_TIM_SET_COMPARE(&GPO_TIM2Handle, TIM_CHANNEL_1, (capture + led_5_R_color)); + red_phase=0x01; + } + else + { + __HAL_TIM_SET_COMPARE(&GPO_TIM2Handle, TIM_CHANNEL_1, (capture + (31-led_5_R_color))); + red_phase=0x00; + } HAL_GPIO_TogglePin(LED_5_R_GPIO_PORT,LED_5_R_PIN); } } @@ -190,7 +207,16 @@ void GPO_TIMER2_IRQHandler(void) { __HAL_TIM_CLEAR_IT(&GPO_TIM2Handle, TIM_IT_CC2); capture = HAL_TIM_ReadCapturedValue(&GPO_TIM2Handle, TIM_CHANNEL_2); - __HAL_TIM_SET_COMPARE(&GPO_TIM2Handle, TIM_CHANNEL_2, (capture + led_5_G_period)); + if(green_phase==0x00) + { + __HAL_TIM_SET_COMPARE(&GPO_TIM2Handle, TIM_CHANNEL_2, (capture + led_5_G_color)); + green_phase=0x01; + } + else + { + __HAL_TIM_SET_COMPARE(&GPO_TIM2Handle, TIM_CHANNEL_2, (capture + (31-led_5_G_color))); + green_phase=0x00; + } HAL_GPIO_TogglePin(LED_5_G_GPIO_PORT,LED_5_G_PIN); } } @@ -200,20 +226,19 @@ void GPO_TIMER2_IRQHandler(void) { __HAL_TIM_CLEAR_IT(&GPO_TIM2Handle, TIM_IT_CC3); capture = HAL_TIM_ReadCapturedValue(&GPO_TIM2Handle, TIM_CHANNEL_3); - __HAL_TIM_SET_COMPARE(&GPO_TIM2Handle, TIM_CHANNEL_3, (capture + led_5_B_period)); + if(blue_phase==0x00) + { + __HAL_TIM_SET_COMPARE(&GPO_TIM2Handle, TIM_CHANNEL_3, (capture + led_5_B_color)); + blue_phase=0x01; + } + else + { + __HAL_TIM_SET_COMPARE(&GPO_TIM2Handle, TIM_CHANNEL_3, (capture + (31-led_5_B_color))); + blue_phase=0x00; + } HAL_GPIO_TogglePin(LED_5_B_GPIO_PORT,LED_5_B_PIN); } } - if(__HAL_TIM_GET_FLAG(&GPO_TIM2Handle, TIM_FLAG_CC4) != RESET) - { - if(__HAL_TIM_GET_IT_SOURCE(&GPO_TIM2Handle, TIM_IT_CC4) !=RESET) - { - __HAL_TIM_CLEAR_IT(&GPO_TIM2Handle, TIM_IT_CC4); - capture = HAL_TIM_ReadCapturedValue(&GPO_TIM2Handle, TIM_CHANNEL_4); - __HAL_TIM_SET_COMPARE(&GPO_TIM2Handle, TIM_CHANNEL_4, (capture + led_4_period)); - HAL_GPIO_TogglePin(LED_4_GPIO_PORT,LED_4_PIN); - } - } /* TIM Update event */ if(__HAL_TIM_GET_FLAG(&GPO_TIM2Handle, TIM_FLAG_UPDATE) != RESET) if(__HAL_TIM_GET_IT_SOURCE(&GPO_TIM2Handle, TIM_IT_UPDATE) !=RESET) @@ -234,6 +259,9 @@ void GPO_TIMER2_IRQHandler(void) void GPO_TIMER3_IRQHandler(void) { + static uint8_t red_phase=0x00; + static uint8_t green_phase=0x00; + static uint8_t blue_phase=0x00; uint32_t capture; if(__HAL_TIM_GET_FLAG(&GPO_TIM3Handle, TIM_FLAG_CC1) != RESET) @@ -242,7 +270,16 @@ void GPO_TIMER3_IRQHandler(void) { __HAL_TIM_CLEAR_IT(&GPO_TIM3Handle, TIM_IT_CC1); capture = HAL_TIM_ReadCapturedValue(&GPO_TIM3Handle, TIM_CHANNEL_1); - __HAL_TIM_SET_COMPARE(&GPO_TIM3Handle, TIM_CHANNEL_1, (capture + led_6_R_period)); + if(red_phase==0x00) + { + __HAL_TIM_SET_COMPARE(&GPO_TIM3Handle, TIM_CHANNEL_1, (capture + led_6_R_color)); + red_phase=0x01; + } + else + { + __HAL_TIM_SET_COMPARE(&GPO_TIM3Handle, TIM_CHANNEL_1, (capture + (31-led_6_R_color))); + red_phase=0x00; + } HAL_GPIO_TogglePin(LED_6_R_GPIO_PORT,LED_6_R_PIN); } } @@ -252,7 +289,16 @@ void GPO_TIMER3_IRQHandler(void) { __HAL_TIM_CLEAR_IT(&GPO_TIM3Handle, TIM_IT_CC2); capture = HAL_TIM_ReadCapturedValue(&GPO_TIM3Handle, TIM_CHANNEL_2); - __HAL_TIM_SET_COMPARE(&GPO_TIM3Handle, TIM_CHANNEL_2, (capture + led_6_G_period)); + if(green_phase==0x00) + { + __HAL_TIM_SET_COMPARE(&GPO_TIM3Handle, TIM_CHANNEL_2, (capture + led_6_G_color)); + green_phase=0x01; + } + else + { + __HAL_TIM_SET_COMPARE(&GPO_TIM3Handle, TIM_CHANNEL_2, (capture + (31-led_6_G_color))); + green_phase=0x00; + } HAL_GPIO_TogglePin(LED_6_G_GPIO_PORT,LED_6_G_PIN); } } @@ -262,7 +308,16 @@ void GPO_TIMER3_IRQHandler(void) { __HAL_TIM_CLEAR_IT(&GPO_TIM3Handle, TIM_IT_CC3); capture = HAL_TIM_ReadCapturedValue(&GPO_TIM3Handle, TIM_CHANNEL_3); - __HAL_TIM_SET_COMPARE(&GPO_TIM3Handle, TIM_CHANNEL_3, (capture + led_6_B_period)); + if(blue_phase==0x00) + { + __HAL_TIM_SET_COMPARE(&GPO_TIM3Handle, TIM_CHANNEL_3, (capture + led_6_B_color)); + blue_phase=0x01; + } + else + { + __HAL_TIM_SET_COMPARE(&GPO_TIM3Handle, TIM_CHANNEL_3, (capture + (31-led_6_B_color))); + blue_phase=0x00; + } HAL_GPIO_TogglePin(LED_6_B_GPIO_PORT,LED_6_B_PIN); } } @@ -358,13 +413,12 @@ void gpio_init(void) led_rx_period=0; led_2_period=0; led_3_period=0; - led_4_period=0; - led_5_R_period=0; - led_5_G_period=0; - led_5_B_period=0; - led_6_R_period=0; - led_6_G_period=0; - led_6_B_period=0; + led_5_R_color=0; + led_5_G_color=0; + led_5_B_color=0; + led_6_R_color=0; + led_6_G_color=0; + led_6_B_color=0; start_pb_callback=0; mode_pb_callback=0; @@ -451,23 +505,18 @@ void gpio_set_led(led_t led_id) { case LED_TX: HAL_GPIO_WritePin(LED_TX_GPIO_PORT,LED_TX_PIN,GPIO_PIN_RESET); - ram_set_bit(DARWIN_LED_PANNEL,3); break; case LED_RX: HAL_GPIO_WritePin(LED_RX_GPIO_PORT,LED_RX_PIN,GPIO_PIN_RESET); - ram_set_bit(DARWIN_LED_PANNEL,4); break; case LED_2: HAL_GPIO_WritePin(LED_2_GPIO_PORT,LED_2_PIN,GPIO_PIN_RESET); - ram_set_bit(DARWIN_LED_PANNEL,0); break; case LED_3: HAL_GPIO_WritePin(LED_3_GPIO_PORT,LED_3_PIN,GPIO_PIN_RESET); - ram_set_bit(DARWIN_LED_PANNEL,1); break; case LED_4: HAL_GPIO_WritePin(LED_4_GPIO_PORT,LED_4_PIN,GPIO_PIN_RESET); - ram_set_bit(DARWIN_LED_PANNEL,2); break; case LED_5_R: HAL_GPIO_WritePin(LED_5_R_GPIO_PORT,LED_5_R_PIN,GPIO_PIN_RESET); @@ -496,23 +545,18 @@ void gpio_clear_led(led_t led_id) { case LED_TX: HAL_GPIO_WritePin(LED_TX_GPIO_PORT,LED_TX_PIN,GPIO_PIN_SET); - ram_clear_bit(DARWIN_LED_PANNEL,3); break; case LED_RX: HAL_GPIO_WritePin(LED_RX_GPIO_PORT,LED_RX_PIN,GPIO_PIN_SET); - ram_clear_bit(DARWIN_LED_PANNEL,4); break; case LED_2: HAL_GPIO_WritePin(LED_2_GPIO_PORT,LED_2_PIN,GPIO_PIN_SET); - ram_clear_bit(DARWIN_LED_PANNEL,0); break; case LED_3: HAL_GPIO_WritePin(LED_3_GPIO_PORT,LED_3_PIN,GPIO_PIN_SET); - ram_clear_bit(DARWIN_LED_PANNEL,1); break; case LED_4: HAL_GPIO_WritePin(LED_4_GPIO_PORT,LED_4_PIN,GPIO_PIN_SET); - ram_clear_bit(DARWIN_LED_PANNEL,2); break; case LED_5_R: HAL_GPIO_WritePin(LED_5_R_GPIO_PORT,LED_5_R_PIN,GPIO_PIN_SET); @@ -628,82 +672,132 @@ void gpio_blink_led(led_t led_id, int16_t period_ms) else HAL_TIM_OC_Stop_IT(&GPO_TIM1Handle, TIM_CHANNEL_4); break; - case LED_4: - if(period_ms>1) + default: break; + } +} + +void gpio_set_color(led_t led_id, uint8_t value) +{ + TIM_OC_InitTypeDef TIM_OCInitStructure; + + TIM_OCInitStructure.OCMode = TIM_OCMODE_TIMING; + TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH; + TIM_OCInitStructure.OCFastMode = TIM_OCFAST_DISABLE; + switch(led_id) + { + case LED_5_R: + if(value==0) { - led_4_period=period_ms; - TIM_OCInitStructure.Pulse = led_4_period; - HAL_TIM_OC_ConfigChannel(&GPO_TIM2Handle, &TIM_OCInitStructure,TIM_CHANNEL_4); - HAL_TIM_OC_Start_IT(&GPO_TIM2Handle, TIM_CHANNEL_4); + HAL_TIM_OC_Stop_IT(&GPO_TIM2Handle, TIM_CHANNEL_1); + gpio_clear_led(led_id); } - else - HAL_TIM_OC_Stop_IT(&GPO_TIM2Handle, TIM_CHANNEL_4); - break; - case LED_5_R: - if(period_ms>1) + else if(value<31) { - led_5_R_period=period_ms; - TIM_OCInitStructure.Pulse = led_5_R_period; + led_5_R_color=value; + TIM_OCInitStructure.Pulse = led_5_R_color; HAL_TIM_OC_ConfigChannel(&GPO_TIM2Handle, &TIM_OCInitStructure,TIM_CHANNEL_1); HAL_TIM_OC_Start_IT(&GPO_TIM2Handle, TIM_CHANNEL_1); } else + { HAL_TIM_OC_Stop_IT(&GPO_TIM2Handle, TIM_CHANNEL_1); + gpio_set_led(led_id); + } break; case LED_5_G: - if(period_ms>1) + if(value==0) + { + HAL_TIM_OC_Stop_IT(&GPO_TIM2Handle, TIM_CHANNEL_2); + gpio_clear_led(led_id); + } + else if(value<31) { - led_5_G_period=period_ms; - TIM_OCInitStructure.Pulse = led_5_G_period; + led_5_G_color=value; + TIM_OCInitStructure.Pulse = led_5_G_color; HAL_TIM_OC_ConfigChannel(&GPO_TIM2Handle, &TIM_OCInitStructure,TIM_CHANNEL_2); HAL_TIM_OC_Start_IT(&GPO_TIM2Handle, TIM_CHANNEL_2); } else + { HAL_TIM_OC_Stop_IT(&GPO_TIM2Handle, TIM_CHANNEL_2); + gpio_set_led(led_id); + } break; case LED_5_B: - if(period_ms>1) + if(value==0) + { + HAL_TIM_OC_Stop_IT(&GPO_TIM2Handle, TIM_CHANNEL_3); + gpio_clear_led(led_id); + } + else if(value<31) { - led_5_B_period=period_ms; - TIM_OCInitStructure.Pulse = led_5_B_period; + led_5_B_color=value; + TIM_OCInitStructure.Pulse = led_5_B_color; HAL_TIM_OC_ConfigChannel(&GPO_TIM2Handle, &TIM_OCInitStructure,TIM_CHANNEL_3); HAL_TIM_OC_Start_IT(&GPO_TIM2Handle, TIM_CHANNEL_3); } else + { HAL_TIM_OC_Stop_IT(&GPO_TIM2Handle, TIM_CHANNEL_3); + gpio_set_led(led_id); + } break; case LED_6_R: - if(period_ms>1) + if(value==0) { - led_6_R_period=period_ms; - TIM_OCInitStructure.Pulse = led_6_R_period; + HAL_TIM_OC_Stop_IT(&GPO_TIM3Handle, TIM_CHANNEL_1); + gpio_clear_led(led_id); + } + else if(value<31) + { + led_6_R_color=value; + TIM_OCInitStructure.Pulse = led_6_R_color; HAL_TIM_OC_ConfigChannel(&GPO_TIM3Handle, &TIM_OCInitStructure,TIM_CHANNEL_1); HAL_TIM_OC_Start_IT(&GPO_TIM3Handle, TIM_CHANNEL_1); } else + { HAL_TIM_OC_Stop_IT(&GPO_TIM3Handle, TIM_CHANNEL_1); + gpio_set_led(led_id); + } break; case LED_6_G: - if(period_ms>1) + if(value==0) + { + HAL_TIM_OC_Stop_IT(&GPO_TIM3Handle, TIM_CHANNEL_2); + gpio_clear_led(led_id); + } + else if(value<31) { - led_6_G_period=period_ms; - TIM_OCInitStructure.Pulse = led_6_G_period; + led_6_G_color=value; + TIM_OCInitStructure.Pulse = led_6_G_color; HAL_TIM_OC_ConfigChannel(&GPO_TIM3Handle, &TIM_OCInitStructure,TIM_CHANNEL_2); HAL_TIM_OC_Start_IT(&GPO_TIM3Handle, TIM_CHANNEL_2); } else + { HAL_TIM_OC_Stop_IT(&GPO_TIM3Handle, TIM_CHANNEL_2); + gpio_set_led(led_id); + } break; case LED_6_B: - if(period_ms>1) + if(value==0) { - led_6_B_period=period_ms; - TIM_OCInitStructure.Pulse = led_6_B_period; + HAL_TIM_OC_Stop_IT(&GPO_TIM3Handle, TIM_CHANNEL_3); + gpio_clear_led(led_id); + } + else if(value<31) + { + led_6_B_color=value; + TIM_OCInitStructure.Pulse = led_6_B_color; HAL_TIM_OC_ConfigChannel(&GPO_TIM3Handle, &TIM_OCInitStructure,TIM_CHANNEL_3); HAL_TIM_OC_Start_IT(&GPO_TIM3Handle, TIM_CHANNEL_3); } else + { HAL_TIM_OC_Stop_IT(&GPO_TIM3Handle, TIM_CHANNEL_3); + gpio_set_led(led_id); + } break; default: break; } @@ -742,3 +836,74 @@ void gpio_set_pushbutton_callback(pushbutton_t pb_id,void (*callback)(void)) break; } } + +uint8_t gpio_in_range(unsigned short int address,unsigned short int length) +{ + return ram_in_window(GPIO_BASE_ADDRESS,GPIO_MEM_LENGTH,address,length); +} + +void gpio_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data) +{ + if(ram_in_range(DARWIN_START_PB_CNTRL,address,length)) + if(!(ram_data[DARWIN_START_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */ + { + if(gpio_is_pushbutton_pressed(START_PB)) + ram_data[DARWIN_START_PB_CNTRL]|=GPIO_VALUE; + else + ram_data[DARWIN_START_PB_CNTRL]&=(~GPIO_VALUE); + } + if(ram_in_range(DARWIN_MODE_PB_CNTRL,address,length)) + if(!(ram_data[DARWIN_MODE_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */ + { + if(gpio_is_pushbutton_pressed(MODE_PB)) + ram_data[DARWIN_MODE_PB_CNTRL]|=GPIO_VALUE; + else + ram_data[DARWIN_MODE_PB_CNTRL]&=(~GPIO_VALUE); + } +} + +void gpio_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data) +{ + uint16_t period; + + if(ram_in_range(DARWIN_TX_LED_CNTRL,address,length)) + { + if(!(ram_data[DARWIN_TX_LED_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */ + { + period=led_tx_period; + if(ram_in_range(DARWIN_TX_LED_PERIOD_L,address,length)) + period=(period&0xFF00)+data[DARWIN_TX_LED_PERIOD_L-address]; + if(ram_in_range(DARWIN_TX_LED_PERIOD_H,address,length)) + period=(period&0x00FF)+(data[DARWIN_TX_LED_PERIOD_H-address]<<8); + if(data[DARWIN_TX_LED_CNTRL-address]&GPIO_BLINK) + gpio_blink_led(LED_TX,period); + else + { + gpio_blink_led(LED_TX,0x0000); + if(data[DARWIN_TX_LED_CNTRL-address]&GPIO_TOGGLE) + { + gpio_toggle_led(LED_TX); + if(ram_data[DARWIN_TX_LED_CNTRL]&GPIO_VALUE) + ram_data[DARWIN_TX_LED_CNTRL]&=(~GPIO_VALUE); + else + ram_data[DARWIN_TX_LED_CNTRL]|=GPIO_VALUE; + } + else + { + if(data[DARWIN_TX_LED_CNTRL-address]&GPIO_VALUE) + { + gpio_set_led(LED_TX); + ram_data[DARWIN_TX_LED_CNTRL]|=GPIO_VALUE; + } + else + { + gpio_clear_led(LED_TX); + ram_data[DARWIN_TX_LED_CNTRL]&=(~GPIO_VALUE); + } + } + } + } + } + +} + diff --git a/src/ram.c b/src/ram.c index 65cf3bc6a3ffc215f9179f2d3f102e573a79d72e..fdec190780ca59ae34ba6e2c5123cef1ca127a86 100755 --- a/src/ram.c +++ b/src/ram.c @@ -1,4 +1,5 @@ #include "ram.h" +#include "eeprom.h" uint8_t ram_data[RAM_SIZE]; @@ -14,35 +15,94 @@ void ram_init(void) if(EE_ReadVariable(DEVICE_MODEL_OFFSET+1,&eeprom_data)==0) ram_data[DEVICE_MODEL_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF); if(EE_ReadVariable(FIRMWARE_VERSION_OFFSET,&eeprom_data)==0) - ram_data[FIRMWARE_VERSION_OFFSET]=(uint8_t)eeprom_data; + ram_data[FIRMWARE_VERSION_OFFSET]=(uint8_t)(eeprom_data&0x00FF); if(EE_ReadVariable(DEVICE_ID_OFFSET,&eeprom_data)==0) - ram_data[DEVICE_ID_OFFSET]=(uint8_t)eeprom_data; + ram_data[DEVICE_ID_OFFSET]=(uint8_t)(eeprom_data&0x00FF); if(EE_ReadVariable(BAUDRATE_OFFSET,&eeprom_data)==0) - ram_data[BAUDRATE_OFFSET]=(uint8_t)eeprom_data; + ram_data[BAUDRATE_OFFSET]=(uint8_t)(eeprom_data&0x00FF); if(EE_ReadVariable(RETURN_DELAY_OFFSET,&eeprom_data)==0) - ram_data[RETURN_DELAY_OFFSET]=(uint8_t)eeprom_data; + ram_data[RETURN_DELAY_OFFSET]=(uint8_t)(eeprom_data&0x00FF); if(EE_ReadVariable(MM_PERIOD_OFFSET,&eeprom_data)==0) - ram_data[MM_PERIOD_OFFSET]=(uint8_t)eeprom_data; + ram_data[MM_PERIOD_OFFSET]=(uint8_t)(eeprom_data&0x00FF); if(EE_ReadVariable(MM_PERIOD_OFFSET+1,&eeprom_data)==0) - ram_data[MM_PERIOD_OFFSET+1]=(uint8_t)eeprom_data; + ram_data[MM_PERIOD_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(MM_BAL_KNEE_GAIN_OFFSET,&eeprom_data)==0) + ram_data[MM_BAL_KNEE_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(MM_BAL_KNEE_GAIN_OFFSET+1,&eeprom_data)==0) + ram_data[MM_BAL_KNEE_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(MM_BAL_ANKLE_ROLL_GAIN_OFFSET,&eeprom_data)==0) + ram_data[MM_BAL_ANKLE_ROLL_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1,&eeprom_data)==0) + ram_data[MM_BAL_ANKLE_ROLL_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(MM_BAL_ANKLE_PITCH_GAIN_OFFSET,&eeprom_data)==0) + ram_data[MM_BAL_ANKLE_PITCH_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1,&eeprom_data)==0) + ram_data[MM_BAL_ANKLE_PITCH_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(MM_BAL_HIP_ROLL_GAIN_OFFSET,&eeprom_data)==0) + ram_data[MM_BAL_HIP_ROLL_GAIN_OFFSET]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(MM_BAL_HIP_ROLL_GAIN_OFFSET+1,&eeprom_data)==0) + ram_data[MM_BAL_HIP_ROLL_GAIN_OFFSET+1]=(uint8_t)(eeprom_data&0x00FF); if(EE_ReadVariable(RETURN_LEVEL_OFFSET,&eeprom_data)==0) - ram_data[RETURN_LEVEL_OFFSET]=(uint8_t)eeprom_data; + ram_data[RETURN_LEVEL_OFFSET]=(uint8_t)(eeprom_data&0x00FF); + for(i=0;i<32;i++) + { + if(EE_ReadVariable(MM_SERVO0_OFFSET+i,&eeprom_data)==0) + ram_data[DARWIN_MM_SERVO0_OFFSET+i]=(uint8_t)(eeprom_data&0x00FF); + } + if(EE_ReadVariable(WALK_X_OFFSET,&eeprom_data)==0) + ram_data[WALK_X_OFFSET]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(WALK_Y_OFFSET,&eeprom_data)==0) + ram_data[WALK_Y_OFFSET]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(WALK_Z_OFFSET,&eeprom_data)==0) + ram_data[WALK_Z_OFFSET]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(WALK_ROLL_OFFSET,&eeprom_data)==0) + ram_data[WALK_ROLL_OFFSET]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(WALK_PITCH_OFFSET,&eeprom_data)==0) + ram_data[WALK_PITCH_OFFSET]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(WALK_YAW_OFFSET,&eeprom_data)==0) + ram_data[WALK_YAW_OFFSET]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(WALK_HIP_PITCH_OFF,&eeprom_data)==0) + ram_data[WALK_HIP_PITCH_OFF]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(WALK_HIP_PITCH_OFF+1,&eeprom_data)==0) + ram_data[WALK_HIP_PITCH_OFF+1]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(WALK_PERIOD_TIME,&eeprom_data)==0) + ram_data[WALK_PERIOD_TIME]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(WALK_PERIOD_TIME+1,&eeprom_data)==0) + ram_data[WALK_PERIOD_TIME+1]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(WALK_DSP_RATIO,&eeprom_data)==0) + ram_data[WALK_DSP_RATIO]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(WALK_STEP_FW_BW_RATIO,&eeprom_data)==0) + ram_data[WALK_STEP_FW_BW_RATIO]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(WALK_FOOT_HEIGHT,&eeprom_data)==0) + ram_data[WALK_FOOT_HEIGHT]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(WALK_SWING_RIGHT_LEFT,&eeprom_data)==0) + ram_data[WALK_SWING_RIGHT_LEFT]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(WALK_SWING_TOP_DOWN,&eeprom_data)==0) + ram_data[WALK_SWING_TOP_DOWN]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(WALK_PELVIS_OFFSET,&eeprom_data)==0) + ram_data[WALK_PELVIS_OFFSET]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(WALK_ARM_SWING_GAIN,&eeprom_data)==0) + ram_data[WALK_ARM_SWING_GAIN]=(uint8_t)(eeprom_data&0x00FF); + if(EE_ReadVariable(WALK_MAX_VEL,&eeprom_data)==0) + 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); } -inline void ram_read_byte(uint8_t address,uint8_t *data) +inline void ram_read_byte(uint16_t address,uint8_t *data) { (*data)=ram_data[address]; } -inline void ram_read_word(uint8_t address, uint16_t *data) +inline void ram_read_word(uint16_t address, uint16_t *data) { (*data)=ram_data[address]; (*data)+=ram_data[address+1]*256; } -uint8_t ram_read_table(uint8_t address, uint8_t length,uint8_t *data) +uint8_t ram_read_table(uint16_t address, uint16_t length,uint8_t *data) { - uint8_t i; + uint16_t i; if((address+length)<=(RAM_SIZE-1)) { @@ -54,7 +114,7 @@ uint8_t ram_read_table(uint8_t address, uint8_t length,uint8_t *data) return RAM_BAD_ADDRESS; } -uint8_t ram_set_bit(uint8_t address, uint8_t bit) +uint8_t ram_set_bit(uint16_t address, uint8_t bit) { if(bit>=0 && bit<8) { @@ -65,7 +125,7 @@ uint8_t ram_set_bit(uint8_t address, uint8_t bit) return RAM_BAD_BIT; } -uint8_t ram_clear_bit(uint8_t address, uint8_t bit) +uint8_t ram_clear_bit(uint16_t address, uint8_t bit) { if(bit>=0 && bit<8) { @@ -76,14 +136,14 @@ uint8_t ram_clear_bit(uint8_t address, uint8_t bit) return RAM_BAD_BIT; } -uint8_t ram_write_byte(uint8_t address, uint8_t data) +uint8_t ram_write_byte(uint16_t address, uint8_t data) { ram_data[address]=data; return RAM_SUCCESS; } -uint8_t ram_write_word(uint8_t address, uint16_t data) +uint8_t ram_write_word(uint16_t address, uint16_t data) { if(address < (RAM_SIZE-1)) { @@ -96,9 +156,9 @@ uint8_t ram_write_word(uint8_t address, uint16_t data) return RAM_BAD_ADDRESS; } -uint8_t ram_write_table(uint8_t address, uint8_t length,uint8_t *data) +uint8_t ram_write_table(uint16_t address, uint16_t length,uint8_t *data) { - uint8_t i; + uint16_t i; if((address+length)<RAM_SIZE) { @@ -110,11 +170,22 @@ uint8_t ram_write_table(uint8_t address, uint8_t length,uint8_t *data) return RAM_BAD_ADDRESS; } -inline uint8_t ram_in_range(uint8_t start_reg,uint8_t end_reg,uint8_t address,uint8_t length) +inline uint8_t ram_in_range(uint16_t reg,uint16_t address,uint16_t length) { - if(start_reg>=address && end_reg<(address+length)) + if(reg>=address && reg<(address+length)) return 0x01; else return 0x00; } +uint8_t ram_in_window(uint16_t start_reg,uint16_t reg_length,uint16_t start_address,uint16_t address_length) +{ + uint16_t end_reg=start_reg+reg_length; + uint16_t end_address=start_address+address_length; + + if((start_reg>=start_address && start_reg<end_address) || (end_reg>=start_address && end_reg<end_address) || + (start_address>=start_reg && start_address<end_reg) || (end_address>=start_reg && end_address<end_reg)) + return 0x01; + else + return 0x00; +} diff --git a/src/scheduler.c b/src/scheduler.c new file mode 100644 index 0000000000000000000000000000000000000000..8be673dbc9de21f8f7697ab27a1f6542efa92ae0 --- /dev/null +++ b/src/scheduler.c @@ -0,0 +1,11 @@ +#include "scheduler.h" + +#define SCHEDULER_TIMER TIM1 +#define SCHEDULER_TIMER_IRQn TIM1_IRQn +#define SCHEDULER_TIMER_IRQHandler TIM1_IRQHandler +#define SCHEDULER_TIMER_CLK __HAL_RCC_TIM1_CLK_ENABLE() + +void scheduler_init(void) +{ + +}