From 425d0f368282dc865e5e0880065ccb49315b053d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu>
Date: Thu, 31 Mar 2016 21:46:31 +0000
Subject: [PATCH] Major changes to the GPIO module to allow chnage of the eye
LED's. Chnaged the dynamixel slave module to the version 2 of the protocol.
Added header files of rthe register definitions and eeprom initialization
values. Added a scheduler to time several tasks with a single timer. Removed
some old files.
---
Makefile | 2 +-
include/adc_dma.h | 20 ++
include/darwin_comm.h | 10 -
include/darwin_dyn_slave.h | 16 +-
include/darwin_registers.h | 379 +++++++++++++++++++++++++++++++++++++
include/darwin_time.h | 10 +-
include/eeprom.h | 23 +--
include/eeprom_init.h | 75 ++++++++
include/gpio.h | 15 +-
include/ram.h | 277 ++-------------------------
include/scheduler.h | 16 ++
src/adc_dma.c | 6 +-
src/cm730_fw.c | 52 +----
src/comm.c | 363 -----------------------------------
src/darwin_dyn_slave.c | 97 +++++++++-
src/darwin_time.c | 31 ++-
src/eeprom.c | 99 +++++++---
src/gpio.c | 309 +++++++++++++++++++++++-------
src/ram.c | 109 +++++++++--
src/scheduler.c | 11 ++
20 files changed, 1083 insertions(+), 837 deletions(-)
delete mode 100644 include/darwin_comm.h
create mode 100644 include/darwin_registers.h
create mode 100644 include/eeprom_init.h
create mode 100644 include/scheduler.h
delete mode 100644 src/comm.c
create mode 100644 src/scheduler.c
diff --git a/Makefile b/Makefile
index b649d63..c448792 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 b11e677..931594a 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 cb2d4e6..0000000
--- 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 6790437..b3a38c9 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 0000000..9ed6332
--- /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 6408b19..b961448 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 b58e7c9..e0b5189 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 0000000..1917b78
--- /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 ae2b05b..38aa2e9 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 2f300a3..b096a64 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 0000000..9f0098e
--- /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 e8fc3e7..48e2495 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 db059f1..eb2a542 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 0d0431a..0000000
--- 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 c68b5ec..e85810e 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 d969331..1935291 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 5af13c9..20b740e 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 d115d05..d42448f 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 65cf3bc..fdec190 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 0000000..8be673d
--- /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)
+{
+
+}
--
GitLab