diff --git a/Makefile b/Makefile
index c31048f2ab1f85686d2f3b5e12a5990b0b5a87cc..d9f8ca6f938025b485289b2e98cff5b00d252123 100755
--- a/Makefile
+++ b/Makefile
@@ -7,28 +7,28 @@ STM32_LIBRARIES_PATH=$(HOME)/humanoids/stm32_libraries
PROJECT_NAME=darwin_firmware
#TARGET_FILES=$(wildcard src/*.c)
TARGET_FILES=src/cm730_fw.c
-#TARGET_FILES+=src/test_charger.c
+TARGET_FILES+=src/eeprom.c
TARGET_FILES+=src/system_stm32f1xx.c
-TARGET_FILES+=src/gpio.c
-TARGET_FILES+=src/ram.c
+#TARGET_FILES+=src/gpio.c
+TARGET_FILES+=src/darwin_sch.c
TARGET_FILES+=src/darwin_time.c
-TARGET_FILES+=src/eeprom.c
-TARGET_FILES+=src/adc_dma.c
-TARGET_FILES+=src/imu.c
+#TARGET_FILES+=src/adc_dma.c
+#TARGET_FILES+=src/imu.c
TARGET_FILES+=src/darwin_dyn_slave.c
-TARGET_FILES+=src/darwin_dyn_master.c
-TARGET_FILES+=src/darwin_dyn_master_v2.c
+#TARGET_FILES+=src/darwin_dyn_master.c
+#TARGET_FILES+=src/darwin_dyn_master_v2.c
TARGET_FILES+=src/stm32f1xx_hal_msp.c
-TARGET_FILES+=src/motion_manager.c
-TARGET_FILES+=src/action.c
-TARGET_FILES+=src/motion_pages.c
-TARGET_FILES+=src/walking.c
-TARGET_FILES+=src/darwin_math.c
-TARGET_FILES+=src/darwin_kinematics.c
-TARGET_FILES+=src/joint_motion.c
-TARGET_FILES+=src/head_tracking.c
-TARGET_FILES+=src/grippers.c
-TARGET_FILES+=src/smart_charger.c
+#TARGET_FILES+=src/motion_manager.c
+#TARGET_FILES+=src/action.c
+#TARGET_FILES+=src/motion_pages.c
+#TARGET_FILES+=src/walking.c
+#TARGET_FILES+=src/darwin_math.c
+#TARGET_FILES+=src/darwin_kinematics.c
+#TARGET_FILES+=src/joint_motion.c
+#TARGET_FILES+=src/head_tracking.c
+#TARGET_FILES+=src/grippers.c
+#TARGET_FILES+=src/smart_charger.c
+#TARGET_FILES+=src/stairs.c
TARGET_PROCESSOR=STM32F103RE
@@ -40,15 +40,17 @@ STM32_STARTUP_FILES_PATH = $(HAL_PATH)/startup_code/
STM32_LINKER_SCRIPTS_PATH = ./linker_script
UTILS_PATH=$(STM32_LIBRARIES_PATH)/utils
COMM_PATH=$(STM32_LIBRARIES_PATH)/comm
+MEMORY_PATH=$(STM32_LIBRARIES_PATH)/memory
USART_PATH=$(STM32_LIBRARIES_PATH)/f1/usart
DYNAMIXEL_PATH=$(STM32_LIBRARIES_PATH)/dynamixel_base
+SCHEDULER_PATH=$(STM32_LIBRARIES_PATH)/scheduler
BUILD_PATH=build
COMPILE_OPTS = -mlittle-endian -mcpu=cortex-m3 -mthumb -mthumb-interwork
COMPILE_OPTS += -Wall -O2 -fno-common -msoft-float -DUSE_HAL_DRIVER
-COMPILE_OPTS += -ffreestanding -nostdlib -D$(PROCESSOR_MACRO)
+COMPILE_OPTS += -ffreestanding -nostdlib -D$(PROCESSOR_MACRO) -DHSE_VALUE=8000000 -imacros ./include/darwin_conf.h
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
+INCLUDE_DIRS += -I$(UTILS_PATH)/include -I$(COMM_PATH)/include -I$(MEMORY_PATH)/include -I$(DYNAMIXEL_PATH)/include -I$(USART_PATH)/include -I$(SCHEDULER_PATH)/include -I./include
DOC_DIR = ./doc
@@ -62,7 +64,6 @@ ASFLAGS = $(COMPILE_OPTS) -c
LD = $(TCHAIN_PREFIX)gcc
LDFLAGS = -mthumb -mcpu=cortex-m3 -Wl,-Map=$@.map,-cref $(INCLUDE_DIRS) -T $(STM32_LINKER_SCRIPTS_PATH)/darwin.ld --specs=nosys.specs
-EXT_LIB = $(COMM_PATH)/lib/comm_m3.a $(UTILS_PATH)/lib/utils_m3.a $(DYNAMIXEL_PATH)/lib/dynamixel_m3.a
OBJCP = $(TCHAIN_PREFIX)objcopy
OBJCPFLAGS_HEX = -O ihex
@@ -94,7 +95,16 @@ TARGET_FILES+=$(HAL_PATH)/src/stm32f1xx_hal.c
TARGET_FILES+=$(USART_PATH)/src/usart3.c
TARGET_FILES+=$(USART_PATH)/src/usart2.c
-TARGET_FILES+=$(USART_PATH)/src/usart1.c
+TARGET_FILES+=$(USART_PATH)/src/usart1_remap.c
+
+TARGET_FILES+=$(wildcard $(UTILS_PATH)/src/*.c)
+TARGET_FILES+=$(wildcard $(COMM_PATH)/src/*.c)
+TARGET_FILES+=$(wildcard $(MEMORY_PATH)/src/*.c)
+TARGET_FILES+=$(DYNAMIXEL_PATH)/src/dynamixel2.c
+TARGET_FILES+=$(DYNAMIXEL_PATH)/src/dynamixel.c
+TARGET_FILES+=$(DYNAMIXEL_PATH)/src/dynamixel_slave.c
+TARGET_FILES+=$(DYNAMIXEL_PATH)/src/dynamixel_slave_device.c
+TARGET_FILES+=$(wildcard $(SCHEDULER_PATH)/src/*.c)
DARWIN_OBJS_TMP = $(notdir $(TARGET_FILES:.c=.o))
DARWIN_OBJS = $(patsubst %,$(BUILD_PATH)/%,$(DARWIN_OBJS_TMP))
@@ -112,6 +122,16 @@ $(BUILD_PATH)/%.o: $(HAL_PATH)/src/%.c
$(CC) -c $(CFLAGS) -o $@ $<
$(BUILD_PATH)/%.o: $(USART_PATH)/src/%.c
$(CC) -c $(CFLAGS) -o $@ $<
+$(BUILD_PATH)/%.o: $(UTILS_PATH)/src/%.c
+ $(CC) -c $(CFLAGS) -o $@ $<
+$(BUILD_PATH)/%.o: $(COMM_PATH)/src/%.c
+ $(CC) -c $(CFLAGS) -o $@ $<
+$(BUILD_PATH)/%.o: $(MEMORY_PATH)/src/%.c
+ $(CC) -c $(CFLAGS) -o $@ $<
+$(BUILD_PATH)/%.o: $(DYNAMIXEL_PATH)/src/%.c
+ $(CC) -c $(CFLAGS) -o $@ $<
+$(BUILD_PATH)/%.o: $(SCHEDULER_PATH)/src/%.c
+ $(CC) -c $(CFLAGS) -o $@ $<
$(MAIN_OUT_ELF): mkdir_build $(DARWIN_OBJS) $(BUID_PATH)/$(STARTUP_FILE:.s=.o)
$(LD) $(LDFLAGS) $(DARWIN_OBJS) $(BUILD_PATH)/$(STARTUP_FILE:.s=.o) $(EXT_LIB) -lm --output $@
diff --git a/include/action.h b/include/action.h
index 98626f1bbc5bd44544809bb9a1e6b935ecdd1e55..6a3817af3a1509331ce20e4953bbf37f25c20e96 100755
--- a/include/action.h
+++ b/include/action.h
@@ -10,8 +10,8 @@ extern "C" {
// public functions
void action_init(uint16_t period_us);
-inline void action_set_period(uint16_t period_us);
-inline uint16_t action_get_period(void);
+void action_set_period(uint16_t period_us);
+uint16_t action_get_period(void);
uint8_t action_load_page(uint8_t page_id);
void action_start_page(void);
void action_stop_page(void);
diff --git a/include/adc_dma.h b/include/adc_dma.h
index 0fd1dc41c7b437f8177404d968c381193d708393..c593122317b670ccb1c49b6f0233d0665732f972 100755
--- a/include/adc_dma.h
+++ b/include/adc_dma.h
@@ -6,23 +6,22 @@ extern "C" {
#endif
#include "stm32f1xx.h"
+#include "adc_dma_registers.h"
+#include "memory.h"
#define ADC_NUM_CHANNELS 12
typedef enum {ADC_CH1=0,ADC_CH2=1,ADC_CH3=2,ADC_CH4=3,ADC_CH5=4,ADC_CH6=5,ADC_CH7=6,ADC_CH8=7,
- ADC_CH9=8,ADC_CH10=9,ADC_CH12=10,ADC_CH14=11} adc_ch_t;
+ ADC_CH9=8,ADC_CH10=9,ADC_CH12=10} adc_ch_t;
-void adc_init(void);
+uint8_t adc_init(TMemory *memory);
void adc_start(void);
void adc_set_period(uint8_t period_ms);
-inline uint8_t adc_get_period(void);
+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
}
diff --git a/include/adc_dma_registers.h b/include/adc_dma_registers.h
new file mode 100644
index 0000000000000000000000000000000000000000..a0a63669cf3fdf56043a96788aa992cdcca6876d
--- /dev/null
+++ b/include/adc_dma_registers.h
@@ -0,0 +1,30 @@
+#ifndef _ADC_DMA_REGISTERS_H
+#define _ADC_DMA_REGISTERS_H
+
+#ifndef RAM_ADC_DMA_BASE_ADDRESS
+ #define RAM_ADC_DMA_BASE_ADDRESS ((unsigned short int)0x0000)
+#endif
+
+#define RAM_ADC_DMA_LENGTH 26
+
+#define ADC_CNTRL RAM_ADC_DMA_BASE_ADDRESS// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | | | | Enable
+#define ADC_PERIOD (RAM_ADC_DMA_BASE_ADDRESS+1)
+#define ADC_CH1_VOLTAGE (RAM_ADC_DMA_BASE_ADDRESS+2)
+#define ADC_CH2_VOLTAGE (RAM_ADC_DMA_BASE_ADDRESS+4)
+#define ADC_CH3_VOLTAGE (RAM_ADC_DMA_BASE_ADDRESS+6)
+#define ADC_CH4_VOLTAGE (RAM_ADC_DMA_BASE_ADDRESS+8)
+#define ADC_CH5_VOLTAGE (RAM_ADC_DMA_BASE_ADDRESS+10)
+#define ADC_CH6_VOLTAGE (RAM_ADC_DMA_BASE_ADDRESS+12)
+#define ADC_CH7_VOLTAGE (RAM_ADC_DMA_BASE_ADDRESS+14)
+#define ADC_CH8_VOLTAGE (RAM_ADC_DMA_BASE_ADDRESS+16)
+#define ADC_CH9_VOLTAGE (RAM_ADC_DMA_BASE_ADDRESS+18)
+#define ADC_CH10_VOLTAGE (RAM_ADC_DMA_BASE_ADDRESS+20)
+#define ADC_TEMP (RAM_ADC_DMA_BASE_ADDRESS+22)
+#define ADC_CH12_VOLTAGE (RAM_ADC_DMA_BASE_ADDRESS+24)
+
+#define ADC_START 0x01
+#define ADC_STOP 0x02
+#define ADC_RUNNING 0x10
+
+#endif
diff --git a/include/darwin_conf.h b/include/darwin_conf.h
new file mode 100644
index 0000000000000000000000000000000000000000..44ebbf8622d2d392c3cfbae49e8ccedf0e091c15
--- /dev/null
+++ b/include/darwin_conf.h
@@ -0,0 +1,32 @@
+#ifndef _DARWIN_CONF_H
+#define _DARWIN_CONF_H
+
+#define RAM_SIZE 1024
+#define EEPROM_SIZE 256
+
+/* Dynamixel slave configuration */
+#define EEPROM_DYN_SLAVE_BASE_ADDRESS1 ((unsigned short int)0x0000)
+#define EEPROM_DYN_SLAVE_BASE_ADDRESS2 ((unsigned short int)0x0010)
+
+#define DEFAULT_DEVICE_MODEL 0x001D
+#define DEFAULT_FIRMWARE_VERSION 0x0001
+#define DEFAULT_DEVICE_ID 0x0003
+#define DEFAULT_BAUDRATE 0x0022
+#define DEFAULT_RETURN_DELAY 0x0000
+#define DEFAULT_RETURN_LEVEL 0x0002
+
+#define MAX_DYN_SLAVE_TX_BUFFER_LEN 1024
+#define MAX_DYN_SLAVE_RX_BUFFER_LEN 1024
+#define MAX_DYN_SLAVE_REG_BUFFER_LEN 1024
+#define MAX_NUM_SLAVE_DEVICES 4
+
+/* GPIO configuration */
+#define RAM_GPIO_BASE_ADDRESS ((unsigned short int)0x0100)
+
+/* ADC configuration */
+#define RAM_ADC_DMA_BASE_ADDRESS ((unsigned short int)0x0115)
+
+/* ADC configuration */
+#define RAM_IMU_BASE_ADDRESS ((unsigned short int)0x012F)
+
+#endif
diff --git a/include/darwin_dyn_master.h b/include/darwin_dyn_master.h
index 8031f12a3c6a3b3ec81f4cea5d40c54a19bdcfab..cca8fc192ff897c7d4d33b188fc57d7a432238ad 100644
--- a/include/darwin_dyn_master.h
+++ b/include/darwin_dyn_master.h
@@ -12,8 +12,8 @@ extern "C" {
extern TDynamixelMaster darwin_dyn_master;
void darwin_dyn_master_init(void);
-inline void darwin_dyn_master_enable_power(void);
-inline void darwin_dyn_master_disable_power(void);
+void darwin_dyn_master_enable_power(void);
+void darwin_dyn_master_disable_power(void);
#ifdef __cplusplus
}
diff --git a/include/darwin_dyn_master_v2.h b/include/darwin_dyn_master_v2.h
index b4a1b34d01c20f6e411ad3902e9d86682697c99c..2f27508d9df49a542348b01eb6b5139663dbcb3d 100644
--- a/include/darwin_dyn_master_v2.h
+++ b/include/darwin_dyn_master_v2.h
@@ -12,6 +12,8 @@ extern "C" {
extern TDynamixelMaster darwin_dyn_master_v2;
void darwin_dyn_master_v2_init(void);
+void darwin_dyn_master_v2_enable_power(void);
+void darwin_dyn_master_v2_disable_power(void);
#ifdef __cplusplus
}
diff --git a/include/darwin_dyn_slave.h b/include/darwin_dyn_slave.h
index b3a38c9467c69f2be448e3c19c64fa92437468e4..493b39530e1e93df9a0169bae902f329accc9fd5 100644
--- a/include/darwin_dyn_slave.h
+++ b/include/darwin_dyn_slave.h
@@ -7,8 +7,12 @@ extern "C" {
#include "stm32f1xx.h"
#include "dynamixel_slave.h"
+#include "dynamixel_slave_device.h"
+#include "dynamixel_slave_registers.h"
+#include "scheduler.h"
+#include "memory.h"
-void darwin_dyn_slave_init(void);
+uint8_t darwin_dyn_slave_init(TMemory **memory,TScheduler *scheduler);
void darwin_dyn_slave_start(void);
void darwin_dyn_slave_stop(void);
diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index 86ee43f0353cf4e4b92c807470bc6b3cee4becbc..c431cb92c468e9ad855da073dda74233f2d061ec 100644
--- a/include/darwin_registers.h
+++ b/include/darwin_registers.h
@@ -7,17 +7,11 @@
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)
@@ -75,205 +69,181 @@ extern "C" {
#define HEAD_TILT_I ((unsigned short int)0x004E)
#define HEAD_TILT_D ((unsigned short int)0x0050)
#define HEAD_TILT_I_CLAMP ((unsigned short int)0x0052)
-#define GRIPPER_LEFT_ID ((unsigned short int)0x0054)
-#define GRIPPER_LEFT_MAX_ANGLE ((unsigned short int)0x0055)
-#define GRIPPER_LEFT_MIN_ANGLE ((unsigned short int)0x0057)
-#define GRIPPER_LEFT_MAX_FORCE ((unsigned short int)0x0059)
-#define GRIPPER_RIGHT_ID ((unsigned short int)0x005B)
-#define GRIPPER_RIGHT_MAX_ANGLE ((unsigned short int)0x005C)
-#define GRIPPER_RIGHT_MIN_ANGLE ((unsigned short int)0x005E)
-#define GRIPPER_RIGHT_MAX_FORCE ((unsigned short int)0x0060)
-#define SMART_CHARGER_PERIOD ((unsigned short int)0x0062)
+#define GRIPPER_LEFT_TOP_ID ((unsigned short int)0x0054)
+#define GRIPPER_LEFT_BOT_ID ((unsigned short int)0x0055)
+#define GRIPPER_LEFT_MAX_ANGLE ((unsigned short int)0x0056)
+#define GRIPPER_LEFT_MIN_ANGLE ((unsigned short int)0x0058)
+#define GRIPPER_LEFT_MAX_FORCE ((unsigned short int)0x005A)
+#define GRIPPER_RIGHT_TOP_ID ((unsigned short int)0x005C)
+#define GRIPPER_RIGHT_BOT_ID ((unsigned short int)0x005D)
+#define GRIPPER_RIGHT_MAX_ANGLE ((unsigned short int)0x005E)
+#define GRIPPER_RIGHT_MIN_ANGLE ((unsigned short int)0x0060)
+#define GRIPPER_RIGHT_MAX_FORCE ((unsigned short int)0x0062)
+#define SMART_CHARGER_PERIOD ((unsigned short int)0x0064)
+#define STAIRS_PHASE1_TIME ((unsigned short int)0x0066)
+#define STAIRS_PHASE2_TIME ((unsigned short int)0x0068)
+#define STAIRS_PHASE3_TIME ((unsigned short int)0x006A)
+#define STAIRS_PHASE4_TIME ((unsigned short int)0x006C)
+#define STAIRS_PHASE5_TIME ((unsigned short int)0x006E)
+#define STAIRS_PHASE6_TIME ((unsigned short int)0x0070)
+#define STAIRS_PHASE7_TIME ((unsigned short int)0x0072)
+#define STAIRS_PHASE8_TIME ((unsigned short int)0x0074)
+#define STAIRS_PHASE9_TIME ((unsigned short int)0x0076)
+#define STAIRS_X_OFFSET ((unsigned short int)0x0078)
+#define STAIRS_Y_OFFSET ((unsigned short int)0x0079)
+#define STAIRS_Z_OFFSET ((unsigned short int)0x007A)
+#define STAIRS_R_OFFSET ((unsigned short int)0x007B)
+#define STAIRS_P_OFFSET ((unsigned short int)0x007C)
+#define STAIRS_A_OFFSET ((unsigned short int)0x007D)
+#define STAIRS_Y_SHIFT ((unsigned short int)0x007E)
+#define STAIRS_X_SHIFT ((unsigned short int)0x007F)
+#define STAIRS_Z_OVERSHOOT ((unsigned short int)0x0080)
+#define STAIRS_Z_HEIGHT ((unsigned short int)0x0081)
+#define STAIRS_HIP_PITCH_OFF ((unsigned short int)0x0082)
+#define STAIRS_R_SHIFT ((unsigned short int)0x0084)
+#define STAIRS_P_SHIFT ((unsigned short int)0x0085)
+#define STAIRS_A_SHIFT ((unsigned short int)0x0086)
+#define STAIRS_Y_SPREAD ((unsigned short int)0x0087)
+#define STAIRS_X_SHIFT_BODY ((unsigned short int)0x0088)
#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_HEAD_PAN_P_L = HEAD_PAN_P,// constant in fixed point format 0|16
- DARWIN_HEAD_PAN_P_H = HEAD_PAN_P+1,
- DARWIN_HEAD_PAN_I_L = HEAD_PAN_I,// constant in fixed point format 0|16
- DARWIN_HEAD_PAN_I_H = HEAD_PAN_I+1,
- DARWIN_HEAD_PAN_D_L = HEAD_PAN_D,// constant in fixed point format 0|16
- DARWIN_HEAD_PAN_D_H = HEAD_PAN_D+1,
- DARWIN_HEAD_PAN_I_CLAMP_L = HEAD_PAN_I_CLAMP,// max error in fixed point format 9|7
- DARWIN_HEAD_PAN_I_CLAMP_H = HEAD_PAN_I_CLAMP+1,
- DARWIN_HEAD_TILT_P_L = HEAD_TILT_P,// constant in fixed point format 0|16
- DARWIN_HEAD_TILT_P_H = HEAD_TILT_P+1,
- DARWIN_HEAD_TILT_I_L = HEAD_TILT_I,// constant in fixed point format 0|16
- DARWIN_HEAD_TILT_I_H = HEAD_TILT_I+1,
- DARWIN_HEAD_TILT_D_L = HEAD_TILT_D,// constant in fixed point format 0|16
- DARWIN_HEAD_TILT_D_H = HEAD_TILT_D+1,
- DARWIN_HEAD_TILT_I_CLAMP_L = HEAD_TILT_I_CLAMP,// max error in fixed point format 9|7
- DARWIN_HEAD_TILT_I_CLAMP_H = HEAD_TILT_I_CLAMP+1,
- DARWIN_GRIPPER_LEFT_ID = GRIPPER_LEFT_ID,
- DARWIN_GRIPPER_LEFT_MAX_ANGLE_L = GRIPPER_LEFT_MAX_ANGLE,
- DARWIN_GRIPPER_LEFT_MAX_ANGLE_H = GRIPPER_LEFT_MAX_ANGLE+1,
- DARWIN_GRIPPER_LEFT_MIN_ANGLE_L = GRIPPER_LEFT_MIN_ANGLE,
- DARWIN_GRIPPER_LEFT_MIN_ANGLE_H = GRIPPER_LEFT_MIN_ANGLE+1,
- DARWIN_GRIPPER_LEFT_MAX_FORCE_L = GRIPPER_LEFT_MAX_FORCE,
- DARWIN_GRIPPER_LEFT_MAX_FORCE_H = GRIPPER_LEFT_MAX_FORCE+1,
- DARWIN_GRIPPER_RIGHT_ID = GRIPPER_RIGHT_ID,
- DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L = GRIPPER_RIGHT_MAX_ANGLE,
- DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H = GRIPPER_RIGHT_MAX_ANGLE+1,
- DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L = GRIPPER_RIGHT_MIN_ANGLE,
- DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H = GRIPPER_RIGHT_MIN_ANGLE+1,
- DARWIN_GRIPPER_RIGHT_MAX_FORCE_L = GRIPPER_RIGHT_MAX_FORCE,
- DARWIN_GRIPPER_RIGHT_MAX_FORCE_H = GRIPPER_RIGHT_MAX_FORCE+1,
- DARWIN_SMART_CHARGER_PERIOD_L = SMART_CHARGER_PERIOD, //en ms
- DARWIN_SMART_CHARGER_PERIOD_H = SMART_CHARGER_PERIOD+1,
-//RAM
- 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_B | value_B | toggle_G | value_G | toggle_R | value_R | 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_B | value_B | toggle_G | value_G | toggle_R | value_R | 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
- // | | | running | | | start | stop
- DARWIN_ADC_PERIOD = 0x0116,
- DARWIN_ADC_CH1_L = 0x0117,
- DARWIN_ADC_CH1_H = 0x0118,
- DARWIN_ADC_CH2_L = 0x0119,
- DARWIN_ADC_CH2_H = 0x011A,
- DARWIN_ADC_CH3_L = 0x011B,
- DARWIN_ADC_CH3_H = 0x011C,
- DARWIN_ADC_CH4_L = 0x011D,
- DARWIN_ADC_CH4_H = 0x011E,
- DARWIN_ADC_CH5_L = 0x011F,
- DARWIN_ADC_CH5_H = 0x0120,
- DARWIN_ADC_CH6_L = 0x0121,
- DARWIN_ADC_CH6_H = 0x0122,
- DARWIN_ADC_CH7_L = 0x0123,
- DARWIN_ADC_CH7_H = 0x0124,
- DARWIN_ADC_CH8_L = 0x0125,
- DARWIN_ADC_CH8_H = 0x0126,
- DARWIN_ADC_CH9_L = 0x0127,
- DARWIN_ADC_CH9_H = 0x0128,
- DARWIN_ADC_CH10_L = 0x0129,
- DARWIN_ADC_CH10_H = 0x012A,
- DARWIN_ADC_TEMP_L = 0x012B,
- DARWIN_ADC_TEMP_H = 0x012C,
- DARWIN_ADC_CH12_L = 0x012D,
- DARWIN_ADC_CH12_H = 0x012E,
- DARWIN_ADC_VREF_L = 0x012F,
- DARWIN_ADC_VREF_H = 0x0130,
- DARWIN_ADC_CH14_L = 0x0131,
- DARWIN_ADC_CH14_H = 0x0132,
- DARWIN_IMU_CNTRL = 0x013B, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // accel_det | gyro_det | calibrating | running | | start_cal | stop | start
- DARWIN_IMU_CAL_SAMPLES_L = 0x013C,
- DARWIN_IMU_CAL_SAMPLES_H = 0x013D,
- DARWIN_IMU_GYRO_X_L = 0x013E,
- DARWIN_IMU_GYRO_X_H = 0x013F,
- DARWIN_IMU_GYRO_Y_L = 0x0140,
- DARWIN_IMU_GYRO_Y_H = 0x0141,
- DARWIN_IMU_GYRO_Z_L = 0x0142,
- DARWIN_IMU_GYRO_Z_H = 0x0143,
- DARWIN_IMU_ACCEL_X_L = 0x0144,
- DARWIN_IMU_ACCEL_X_H = 0x0145,
- DARWIN_IMU_ACCEL_Y_L = 0x0146,
- DARWIN_IMU_ACCEL_Y_H = 0x0147,
- DARWIN_IMU_ACCEL_Z_L = 0x0148,
- DARWIN_IMU_ACCEL_Z_H = 0x0149,
+ 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_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_HEAD_PAN_P_L = HEAD_PAN_P,// constant in fixed point format 0|16
+ DARWIN_HEAD_PAN_P_H = HEAD_PAN_P+1,
+ DARWIN_HEAD_PAN_I_L = HEAD_PAN_I,// constant in fixed point format 0|16
+ DARWIN_HEAD_PAN_I_H = HEAD_PAN_I+1,
+ DARWIN_HEAD_PAN_D_L = HEAD_PAN_D,// constant in fixed point format 0|16
+ DARWIN_HEAD_PAN_D_H = HEAD_PAN_D+1,
+ DARWIN_HEAD_PAN_I_CLAMP_L = HEAD_PAN_I_CLAMP,// max error in fixed point format 9|7
+ DARWIN_HEAD_PAN_I_CLAMP_H = HEAD_PAN_I_CLAMP+1,
+ DARWIN_HEAD_TILT_P_L = HEAD_TILT_P,// constant in fixed point format 0|16
+ DARWIN_HEAD_TILT_P_H = HEAD_TILT_P+1,
+ DARWIN_HEAD_TILT_I_L = HEAD_TILT_I,// constant in fixed point format 0|16
+ DARWIN_HEAD_TILT_I_H = HEAD_TILT_I+1,
+ DARWIN_HEAD_TILT_D_L = HEAD_TILT_D,// constant in fixed point format 0|16
+ DARWIN_HEAD_TILT_D_H = HEAD_TILT_D+1,
+ DARWIN_HEAD_TILT_I_CLAMP_L = HEAD_TILT_I_CLAMP,// max error in fixed point format 9|7
+ DARWIN_HEAD_TILT_I_CLAMP_H = HEAD_TILT_I_CLAMP+1,
+ DARWIN_GRIPPER_LEFT_TOP_ID = GRIPPER_LEFT_TOP_ID,
+ DARWIN_GRIPPER_LEFT_BOT_ID = GRIPPER_LEFT_BOT_ID,
+ DARWIN_GRIPPER_LEFT_MAX_ANGLE_L = GRIPPER_LEFT_MAX_ANGLE,
+ DARWIN_GRIPPER_LEFT_MAX_ANGLE_H = GRIPPER_LEFT_MAX_ANGLE+1,
+ DARWIN_GRIPPER_LEFT_MIN_ANGLE_L = GRIPPER_LEFT_MIN_ANGLE,
+ DARWIN_GRIPPER_LEFT_MIN_ANGLE_H = GRIPPER_LEFT_MIN_ANGLE+1,
+ DARWIN_GRIPPER_LEFT_MAX_FORCE_L = GRIPPER_LEFT_MAX_FORCE,
+ DARWIN_GRIPPER_LEFT_MAX_FORCE_H = GRIPPER_LEFT_MAX_FORCE+1,
+ DARWIN_GRIPPER_RIGHT_TOP_ID = GRIPPER_RIGHT_TOP_ID,
+ DARWIN_GRIPPER_RIGHT_BOT_ID = GRIPPER_RIGHT_BOT_ID,
+ DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L = GRIPPER_RIGHT_MAX_ANGLE,
+ DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H = GRIPPER_RIGHT_MAX_ANGLE+1,
+ DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L = GRIPPER_RIGHT_MIN_ANGLE,
+ DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H = GRIPPER_RIGHT_MIN_ANGLE+1,
+ DARWIN_GRIPPER_RIGHT_MAX_FORCE_L = GRIPPER_RIGHT_MAX_FORCE,
+ DARWIN_GRIPPER_RIGHT_MAX_FORCE_H = GRIPPER_RIGHT_MAX_FORCE+1,
+ DARWIN_SMART_CHARGER_PERIOD_L = SMART_CHARGER_PERIOD, //en ms
+ DARWIN_SMART_CHARGER_PERIOD_H = SMART_CHARGER_PERIOD+1,
+ DARWIN_STAIRS_PHASE1_TIME_L = STAIRS_PHASE1_TIME,
+ DARWIN_STAIRS_PHASE1_TIME_H = STAIRS_PHASE1_TIME+1,
+ DARWIN_STAIRS_PHASE2_TIME_L = STAIRS_PHASE2_TIME,
+ DARWIN_STAIRS_PHASE2_TIME_H = STAIRS_PHASE2_TIME+1,
+ DARWIN_STAIRS_PHASE3_TIME_L = STAIRS_PHASE3_TIME,
+ DARWIN_STAIRS_PHASE3_TIME_H = STAIRS_PHASE3_TIME+1,
+ DARWIN_STAIRS_PHASE4_TIME_L = STAIRS_PHASE4_TIME,
+ DARWIN_STAIRS_PHASE4_TIME_H = STAIRS_PHASE4_TIME+1,
+ DARWIN_STAIRS_PHASE5_TIME_L = STAIRS_PHASE5_TIME,
+ DARWIN_STAIRS_PHASE5_TIME_H = STAIRS_PHASE5_TIME+1,
+ DARWIN_STAIRS_PHASE6_TIME_L = STAIRS_PHASE6_TIME,
+ DARWIN_STAIRS_PHASE6_TIME_H = STAIRS_PHASE6_TIME+1,
+ DARWIN_STAIRS_PHASE7_TIME_L = STAIRS_PHASE7_TIME,
+ DARWIN_STAIRS_PHASE7_TIME_H = STAIRS_PHASE7_TIME+1,
+ DARWIN_STAIRS_PHASE8_TIME_L = STAIRS_PHASE8_TIME,
+ DARWIN_STAIRS_PHASE8_TIME_H = STAIRS_PHASE8_TIME+1,
+ DARWIN_STAIRS_PHASE9_TIME_L = STAIRS_PHASE9_TIME,
+ DARWIN_STAIRS_PHASE9_TIME_H = STAIRS_PHASE9_TIME+1,
+ DARWIN_STAIRS_X_OFFSET = STAIRS_X_OFFSET,
+ DARWIN_STAIRS_Y_OFFSET = STAIRS_Y_OFFSET,
+ DARWIN_STAIRS_Z_OFFSET = STAIRS_Z_OFFSET,
+ DARWIN_STAIRS_Y_SHIFT = STAIRS_Y_SHIFT,
+ DARWIN_STAIRS_R_OFFSET = STAIRS_R_OFFSET,
+ DARWIN_STAIRS_P_OFFSET = STAIRS_P_OFFSET,
+ DARWIN_STAIRS_A_OFFSET = STAIRS_A_OFFSET,
+ DARWIN_STAIRS_X_SHIFT = STAIRS_X_SHIFT,
+ DARWIN_STAIRS_Z_OVERSHOOT = STAIRS_Z_OVERSHOOT,
+ DARWIN_STAIRS_Z_HEIGHT = STAIRS_Z_HEIGHT,
+ DARWIN_STAIRS_HIP_PITCH_OFF_L = STAIRS_HIP_PITCH_OFF,
+ DARWIN_STAIRS_HIP_PITCH_OFF_H = STAIRS_HIP_PITCH_OFF+1,
+ DARWIN_STAIRS_R_SHIFT = STAIRS_R_SHIFT,
+ DARWIN_STAIRS_P_SHIFT = STAIRS_P_SHIFT,
+ DARWIN_STAIRS_A_SHIFT = STAIRS_A_SHIFT,
+ DARWIN_STAIRS_Y_SPREAD = STAIRS_Y_SPREAD,
+ DARWIN_STAIRS_X_SHIFT_BODY = STAIRS_X_SHIFT_BODY,
+//RAM
DARWIN_MM_NUM_SERVOS = 0x014A,
- DARWIN_MM_CNTRL = 0x014B, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // scanning | | | | | Enable power | Enable balance | Enable manager
+ DARWIN_MM_CNTRL = 0x014B, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // scanning | fwd fall | bwd fall | | Enable power v2 | Enable power | Enable balance | Enable manager
DARWIN_MM_MODULE_EN0 = 0x014C, // 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 |
@@ -568,102 +538,26 @@ typedef enum {
DARWIN_HEAD_MIN_TILT_H = 0x0244,
DARWIN_HEAD_TILT_TARGET_L = 0x0245, // angle in fixed point format 9|7
DARWIN_HEAD_TILT_TARGET_H = 0x0246,
-
-// to do: include more registers os smart charger's memory map to read
-/* DARWIN_BATT_CHARGER_STATUS = 0x0247,
- DARWIN_BATT_INPUT_CURRENT_L = 0x0248,
- DARWIN_BATT_INPUT_CURRENT_H = 0x0249,
- DARWIN_BATT_CHARGE_CURRENT_L = 0x024A,
- DARWIN_BATT_CHARGE_CURRENT_H = 0x024B,
- DARWIN_BATT_CHARGE_VOLTAGE_L = 0x024C,
- DARWIN_BATT_CHARGE_VOLTAGE_H = 0x024D,
- DARWIN_BATT_LIMIT_CURRENT_L = 0x024E,
- DARWIN_BATT_LIMIT_CURRENT_H = 0x024F, //
- DARWIN_BATT_TEMPERATURE_L = 0x0250,
- DARWIN_BATT_TEMPERATURE_H = 0x0251,
- DARWIN_BATT_VOLTAGE_L = 0x0252,
- DARWIN_BATT_VOLTAGE_H = 0x0253,
- DARWIN_BATT_CURRENT_L = 0x0254,
- DARWIN_BATT_CURRENT_H = 0x0255,
- DARWIN_BATT_AVG_CURRENT_L = 0x0256,
- DARWIN_BATT_AVG_CURRENT_H = 0x0257,
- DARWIN_BATT_RELATIVE_SOC = 0x0258,//
- DARWIN_BATT_ABSOLUTE_SOC = 0x0259,
- DARWIN_BATT_REMAINING_CAP_L = 0x025A,
- DARWIN_BATT_REMAINING_CAP_H = 0x025B,
- DARWIN_BATT_FULL_CHARGE_CAP_L = 0x025C,
- DARWIN_BATT_FULL_CHARGE_CAP_H = 0x025D,
- DARWIN_BATT_RUN_TIME_EMPTY_L = 0x025E, //
- DARWIN_BATT_RUN_TIME_EMPTY_H = 0x025F,
- DARWIN_BATT_AVG_TIME_EMPTY_L = 0x0260,
- DARWIN_BATT_AVG_TIME_EMPTY_H = 0x0261,
- DARWIN_BATT_AVG_TIME_FULL_L = 0x0262,
- DARWIN_BATT_AVG_TIME_FULL_H = 0x0263,
- DARWIN_BATT_STATUS_L = 0x0264,
- DARWIN_BATT_STATUS_H = 0x0265,
- DARWIN_BATT_DESIGN_CAP_L = 0x0266,
- DARWIN_BATT_DESIGN_CAP_H = 0x0267,
- DARWIN_BATT_DESIGN_VOLTAGE_L = 0x0268,
- DARWIN_BATT_DESIGN_VOLTAGE_H = 0x0269,
- DARWIN_BATT_CELL1_VOLTAGE_L = 0x026A,
- DARWIN_BATT_CELL1_VOLTAGE_H = 0x026B,
- DARWIN_BATT_CELL2_VOLTAGE_L = 0x026C,
- DARWIN_BATT_CELL2_VOLTAGE_H = 0x026D,
- DARWIN_BATT_CELL3_VOLTAGE_L = 0x026E,
- DARWIN_BATT_CELL3_VOLTAGE_H = 0x026F,
- DARWIN_BATT_CELL4_VOLTAGE_L = 0x0270,
- DARWIN_BATT_CELL4_VOLTAGE_H = 0x0271,
-*/
-//añadidos
+
DARWIN_SMART_CHARGER_ID = 0x0247,
- DARWIN_SMART_CHARGER_STATUS = 0x0248,
- DARWIN_SMART_CHARGER_LIMIT_CURRENT_L = 0x0249,
- DARWIN_SMART_CHARGER_LIMIT_CURRENT_H = 0x024A,
- DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L = 0x024B,
- DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_H = 0x024C,
- DARWIN_SMART_CHARGER_AVG_TIME_FULL_L = 0x024D,
- DARWIN_SMART_CHARGER_AVG_TIME_FULL_H = 0x024E,
- DARWIN_SMART_CHARGER_BATT_STATUS_L = 0x024F,
- DARWIN_SMART_CHARGER_BATT_STATUS_H = 0x0250,
- DARWIN_SMART_CHARGER_CNTRL = 0x0251, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // | detected | enable
-//Cambio registro
- DARWIN_GRIPPER_CNTRL = 0x0252 // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
- // left opened | right opened | | | close left | open left | close right | open right
+ DARWIN_SMART_CHARGER_LIMIT_CURRENT_L = 0x0248, // Input max current in mA
+ DARWIN_SMART_CHARGER_LIMIT_CURRENT_H = 0x0249,
+ DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L = 0x024A, // Average time to empty batteries in min
+ DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_H = 0x024B,
+ DARWIN_SMART_CHARGER_AVG_TIME_FULL_L = 0x024C, // Average time to full batteries in min
+ DARWIN_SMART_CHARGER_AVG_TIME_FULL_H = 0x024D,
+ DARWIN_SMART_CHARGER_BATT_STATUS_L = 0x024E, // Battery status
+ DARWIN_SMART_CHARGER_BATT_STATUS_H = 0x024F,
+ DARWIN_SMART_CHARGER_CNTRL = 0x0250, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | detected | enable
+ DARWIN_GRIPPER_CNTRL = 0x0251, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // left opened | right opened | left moving | right moving | close left | open left | close right | open right
+ DARWIN_STAIRS_CNTRL = 0x0252 // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // current phase | climbing | stop stairs | start stairs down | start stairs up
}darwin_registers;
-#define GPIO_BASE_ADDRESS 0x0100
-#define GPIO_MEM_LENGTH 21
-#define GPIO_INT_USED 0x01
-#define GPIO_VALUE 0x02
-#define GPIO_TOGGLE 0x04
-#define GPIO_VALUE_R 0x02
-#define GPIO_TOGGLE_R 0x04
-#define GPIO_VALUE_G 0x08
-#define GPIO_TOGGLE_G 0x10
-#define GPIO_VALUE_B 0x20
-#define GPIO_TOGGLE_B 0x40
-#define GPIO_BLINK 0x08
-#define GPIO_COLOR 0x80
-#define GPIO_RED_COLOR 0x001F
-#define GPIO_GREEN_COLOR 0x03E0
-#define GPIO_BLUE_COLOR 0x7C00
-
-#define ADC_BASE_ADDRESS 0x0115
-#define ADC_MEM_LENGTH 30
-#define ADC_START 0x01
-#define ADC_STOP 0x02
-#define ADC_RUNNING 0x10
-
#define IMU_BASE_ADDRESS 0x013B
#define IMU_MEM_LENGTH 15
-#define IMU_START 0x01
-#define IMU_STOP 0x02
-#define IMU_START_CAL 0x04
-#define IMU_RUNNING 0x10
-#define IMU_CALIBRATING 0x20
-#define IMU_ACCEL_DET 0x40
-#define IMU_GYRO_DET 0x80
#define MANAGER_BASE_ADDRESS 0x014A
#define MANAGER_MEM_LENGTH 86
@@ -674,7 +568,10 @@ typedef enum {
#define MANAGER_ENABLE 0x01
#define MANAGER_EN_BAL 0x02
#define MANAGER_EN_PWR 0x04
+#define MANAGER_EN_PWR_V2 0x08
#define MANAGER_SCANNING 0x80
+#define MANAGER_FWD_FALL 0x40
+#define MANAGER_BWD_FALL 0x20
#define MANAGER_EVEN_SER_EN 0x80
#define MANAGER_EVEN_SER_MOD 0x70
#define MANAGER_ODD_SER_EN 0x08
@@ -711,23 +608,35 @@ typedef enum {
#define HEAD_STOP 0x02
#define HEAD_STATUS 0x10
-#define GRIPPER_BASE_ADDRESS 0x0252//0x0274
+#define GRIPPER_BASE_ADDRESS 0x0251
#define GRIPPER_MEM_LENGTH 1
#define GRIPPER_EEPROM_ADDRESS 0x0054
-#define GRIPPER_EEPROM_LENGTH 14
+#define GRIPPER_EEPROM_LENGTH 16
#define GRIPPER_OPEN_RIGHT 0x01
#define GRIPPER_CLOSE_RIGHT 0x02
#define GRIPPER_OPEN_LEFT 0x04
#define GRIPPER_CLOSE_LEFT 0x08
+#define GRIPPER_MOVING_LEFT 0x20
+#define GRIPPER_MOVING_RIGHT 0x10
+#define GRIPPER_OPENED_LEFT 0x80
+#define GRIPPER_OPENED_RIGHT 0x40
#define SMART_CHARGER_BASE_ADDRESS 0x0247
-#define SMART_CHARGER_MEM_LENGTH 10 //45
-#define SMART_CHARGER_EEPROM_BASE 0x0060
+#define SMART_CHARGER_MEM_LENGTH 11
+#define SMART_CHARGER_EEPROM_BASE 0x0062
#define SMART_CHARGER_EEPROM_LENGTH 2
#define SMART_CHARGER_DET 0x01
#define SMART_CHARGER_EN 0x02
-
+#define STAIRS_BASE_ADDRESS 0x0252
+#define STAIRS_MEM_LENGTH 1
+#define STAIRS_EEPROM_ADDRESS 0x0066
+#define STAIRS_EEPROM_LENGTH 35
+#define STAIRS_START_UP 0x01
+#define STAIRS_START_DOWN 0x02
+#define STAIRS_STOP 0x04
+#define STAIRS_STATUS 0x08
+#define STAIRS_PHASE 0xF0
#ifdef __cplusplus
}
diff --git a/include/darwin_sch.h b/include/darwin_sch.h
new file mode 100644
index 0000000000000000000000000000000000000000..21ce2fe1eb96001d08885159bb470b35522dfbeb
--- /dev/null
+++ b/include/darwin_sch.h
@@ -0,0 +1,10 @@
+#ifndef _DARWIN_SCH_H
+#define _DARWIN_SCH_H
+
+#include "stm32f1xx.h"
+#include "scheduler.h"
+
+TScheduler *darwin_sch_init(void);
+
+#endif
+
diff --git a/include/dyn_battery.h b/include/dyn_battery.h
index ac02ce82b99b8996815da0b769d379eaa18ce3e6..70e47b622cbb9e935c4cacd2d708d15971979527 100644
--- a/include/dyn_battery.h
+++ b/include/dyn_battery.h
@@ -45,7 +45,7 @@ typedef enum {
BATTERY_OUTPUT_VOLTAGE_H = 0x1E,
/* RAM */
BATTERY_CHARGER_STATUS = 0x1F,
- BATTERY_INPUT_CURRENT_L = 0x20,//corriente consumida (mA)
+ BATTERY_INPUT_CURRENT_L = 0x20,
BATTERY_INPUT_CURRENT_H = 0x21,
BATTERY_CHARGE_CURRENT_L = 0x22,
BATTERY_CHARGE_CURRENT_H = 0x23,
@@ -61,25 +61,25 @@ typedef enum {
BATTERY_CURRENT_H = 0x2D,
BATTERY_AVG_CURRENT_L = 0x2E,
BATTERY_AVG_CURRENT_H = 0x2F,
- BATTERY_RELATIVE_SOC = 0x30,//State Of Charge (%)
+ BATTERY_RELATIVE_SOC = 0x30,
BATTERY_ABSOLUTE_SOC = 0x31,
- BATTERY_REMAINING_CAP_L = 0x32,//mAh (tiempo de vida)
+ BATTERY_REMAINING_CAP_L = 0x32,
BATTERY_REMAINING_CAP_H = 0x33,
- BATTERY_FULL_CHARGE_CAP_L = 0x34,//
+ BATTERY_FULL_CHARGE_CAP_L = 0x34,
BATTERY_FULL_CHARGE_CAP_H = 0x35,
- BATTERY_RUN_TIME_EMPTY_L = 0x36,//(min)
+ BATTERY_RUN_TIME_EMPTY_L = 0x36,
BATTERY_RUN_TIME_EMPTY_H = 0x37,
BATTERY_AVG_TIME_EMPTY_L = 0x38,
- BATTERY_AVG_TIME_EMPTY_H = 0x39,//tiempo hasta que este descargada (min)
- BATTERY_AVG_TIME_FULL_L = 0x3A,//tiempo hasta que este cargada (min)
+ BATTERY_AVG_TIME_EMPTY_H = 0x39,
+ BATTERY_AVG_TIME_FULL_L = 0x3A,
BATTERY_AVG_TIME_FULL_H = 0x3B,
- BATTERY_STATUS_L = 0x3C,//cargando o descargando, cargada, descargada,...
+ BATTERY_STATUS_L = 0x3C,
BATTERY_STATUS_H = 0x3D,
- BATTERY_DESIGN_CAP_L = 0x3E,//Capacidad de la bateria - numero constante
+ BATTERY_DESIGN_CAP_L = 0x3E,
BATTERY_DESIGN_CAP_H = 0x3F,
BATTERY_DESIGN_VOLTAGE_L = 0x40,
BATTERY_DESIGN_VOLTAGE_H = 0x41,
- BATTERY_CELL1_VOLTAGE_L = 0x42,//Voltage de cada celda
+ BATTERY_CELL1_VOLTAGE_L = 0x42,
BATTERY_CELL1_VOLTAGE_H = 0x43,
BATTERY_CELL2_VOLTAGE_L = 0x44,
BATTERY_CELL2_VOLTAGE_H = 0x45,
diff --git a/include/eeprom.h b/include/eeprom.h
index c20035882aa091380fd791694b2318e5029ac777..5d227885a7c436bf53c08f10f51c986163b46e31 100755
--- a/include/eeprom.h
+++ b/include/eeprom.h
@@ -83,7 +83,7 @@ extern "C" {
#define PAGE_FULL ((uint8_t)0x80)
/* Variables' number */
-#define NB_OF_VAR ((uint8_t)0x60)
+#define NB_OF_VAR ((uint8_t)0x89)
/* Exported types ------------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
diff --git a/include/eeprom_init.h b/include/eeprom_init.h
index 7ba89ed6261ee183c407170d0a0fff7e1a2587ab..0f1baf36750d925815fdc5a6c47f95a76bdfab9f 100644
--- a/include/eeprom_init.h
+++ b/include/eeprom_init.h
@@ -5,86 +5,104 @@
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 //7800us
-#define DEFAULT_BAL_KNEE_GAIN 0x4CCD // 0.3 in fixed point format 0|16
-#define DEFAULT_BAL_ANKLE_ROLL_GAIN 0xFFFF // 0.99999
-#define DEFAULT_BAL_ANKLE_PITCH_GAIN 0xE666 // 0.9
-#define DEFAULT_BAL_HIP_ROLL_GAIN 0x8000 // 0.5
-#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 0x34B6 // 13.18 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 0x0040 // 8 degrees/s in fixed point format 5 | 3
-#define DEFAULT_HEAD_PAN_P 0x028F // 0.01 in fixed point format 0|16
-#define DEFAULT_HEAD_PAN_I 0x0000 // 0.0 in fixed point format 0|16
-#define DEFAULT_HEAD_PAN_D 0x0000 // 0.0 in fixed point format 0|16
-#define DEFAULT_HEAD_PAN_I_CLAMP 0x0000 // 0.0 in fixed point format 9|7
-#define DEFAULT_HEAD_TILT_P 0x028F // 0.01 in fixed point format 0|16
-#define DEFAULT_HEAD_TILT_I 0x0000 // 0.0 in fixed point format 0|16
-#define DEFAULT_HEAD_TILT_D 0x0000 // 0.0 in fixed point format 0|16
-#define DEFAULT_HEAD_TILT_I_CLAMP 0x0000 // 0.0 in fixed point format 9|7
-#define DEFAULT_GRIPPER_LEFT_ID 0x0017 // ID 23 for the left gripper
-#define DEFAULT_GRIPPER_LEFT_MAX_ANGLE 0x0F00 // 30 in fixed point format 9|7
-#define DEFAULT_GRIPPER_LEFT_MIN_ANGLE 0xF100 // -30 in fixed point format 9|7
-#define DEFAULT_GRIPPER_LEFT_MAX_FORCE 0x03FF // 1023 max force in binary format
-#define DEFAULT_GRIPPER_RIGHT_ID 0x0018 // ID 24 for the left gripper
-#define DEFAULT_GRIPPER_RIGHT_MAX_ANGLE 0x0F00 // 30 in fixed point format 9|7
-#define DEFAULT_GRIPPER_RIGHT_MIN_ANGLE 0xF100 // -30 in fixed point format 9|7
-#define DEFAULT_GRIPPER_RIGHT_MAX_FORCE 0x03FF // 1023 max force in binary format
-
-#define DEFAULT_SMART_CHARGER_PERIOD 0x05DC //1500 ms (7,8ms*200)
-
-
+#define DEFAULT_MM_PERIOD 0x1E78 //7800us
+#define DEFAULT_BAL_KNEE_GAIN 0x4CCD // 0.3 in fixed point format 0|16
+#define DEFAULT_BAL_ANKLE_ROLL_GAIN 0xFFFF // 0.99999
+#define DEFAULT_BAL_ANKLE_PITCH_GAIN 0xE666 // 0.9
+#define DEFAULT_BAL_HIP_ROLL_GAIN 0x8000 // 0.5
+#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 0x34B6 // 13.18 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 0x0040 // 8 degrees/s in fixed point format 5 | 3
+#define DEFAULT_HEAD_PAN_P 0x028F // 0.01 in fixed point format 0|16
+#define DEFAULT_HEAD_PAN_I 0x0000 // 0.0 in fixed point format 0|16
+#define DEFAULT_HEAD_PAN_D 0x0000 // 0.0 in fixed point format 0|16
+#define DEFAULT_HEAD_PAN_I_CLAMP 0x0000 // 0.0 in fixed point format 9|7
+#define DEFAULT_HEAD_TILT_P 0x028F // 0.01 in fixed point format 0|16
+#define DEFAULT_HEAD_TILT_I 0x0000 // 0.0 in fixed point format 0|16
+#define DEFAULT_HEAD_TILT_D 0x0000 // 0.0 in fixed point format 0|16
+#define DEFAULT_HEAD_TILT_I_CLAMP 0x0000 // 0.0 in fixed point format 9|7
+#define DEFAULT_GRIPPER_LEFT_TOP_ID 0x0015 // ID 21 for the left gripper
+#define DEFAULT_GRIPPER_LEFT_BOT_ID 0x0016 // ID 22 for the left gripper
+#define DEFAULT_GRIPPER_LEFT_MAX_ANGLE 0x0F00 // 30 in fixed point format 9|7
+#define DEFAULT_GRIPPER_LEFT_MIN_ANGLE 0xF100 // -30 in fixed point format 9|7
+#define DEFAULT_GRIPPER_LEFT_MAX_FORCE 0x0080 // 1023 max force in binary format
+#define DEFAULT_GRIPPER_RIGHT_TOP_ID 0x0017 // ID 23 for the left gripper
+#define DEFAULT_GRIPPER_RIGHT_BOT_ID 0x0018 // ID 24 for the left gripper
+#define DEFAULT_GRIPPER_RIGHT_MAX_ANGLE 0x0F00 // 30 in fixed point format 9|7
+#define DEFAULT_GRIPPER_RIGHT_MIN_ANGLE 0xF100 // -30 in fixed point format 9|7
+#define DEFAULT_GRIPPER_RIGHT_MAX_FORCE 0x0080 // 1023 max force in binary format
+#define DEFAULT_SMART_CHARGER_PERIOD 0x05DC // 1500 ms
+#define DEFAULT_STAIRS_PHASE1_TIME 0x0640 // 1600 ms
+#define DEFAULT_STAIRS_PHASE2_TIME 0x0C80 // 3200 ms
+#define DEFAULT_STAIRS_PHASE3_TIME 0x12C0 // 4800 ms
+#define DEFAULT_STAIRS_PHASE4_TIME 0x1900 // 6400 ms
+#define DEFAULT_STAIRS_PHASE5_TIME 0x1F40 // 8000 ms
+#define DEFAULT_STAIRS_PHASE6_TIME 0x2580 // 9600 ms
+#define DEFAULT_STAIRS_PHASE7_TIME 0x2BC0 // 11200 ms
+#define DEFAULT_STAIRS_PHASE8_TIME 0x3200 // 12800 ms
+#define DEFAULT_STAIRS_PHASE9_TIME 0x3840 // 14400 ms
+#define DEFAULT_STAIRS_X_OFFSET 0xFFF6 // -10 mm
+#define DEFAULT_STAIRS_Y_OFFSET 0x0005 // 5mm
+#define DEFAULT_STAIRS_Z_OFFSET 0x0014 // 20 mm
+#define DEFAULT_STAIRS_R_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
+#define DEFAULT_STAIRS_P_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
+#define DEFAULT_STAIRS_A_OFFSET 0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
+#define DEFAULT_STAIRS_Y_SHIFT 0x0028 // 40 mm
+#define DEFAULT_STAIRS_X_SHIFT 0x0050 // 80 mm
+#define DEFAULT_STAIRS_Z_OVERSHOOT 0x000F // 15 mm
+#define DEFAULT_STAIRS_Z_HEIGHT 0x001E // 30 mm
+#define DEFAULT_STAIRS_HIP_PITCH_OFF 0x34B6 // 13.18 degrees in fixed point format 6 (1+5) | 10
+#define DEFAULT_STAIRS_R_SHIFT 0x000B // 2.875 degrees in fixed point format 5 (1+5) | 2
+#define DEFAULT_STAIRS_P_SHIFT 0x0017 // 5.73 degrees in fixed point format 5 (1+5) | 2
+#define DEFAULT_STAIRS_A_SHIFT 0x0045 // 17.19 degrees in fixed point format 6 (1+5) | 2
+#define DEFAULT_STAIRS_Y_SPREAD 0x0014 // 20 mm
+#define DEFAULT_STAIRS_X_SHIFT_BODY 0x0023 // 35 mm
#ifdef __cplusplus
}
diff --git a/include/gpio.h b/include/gpio.h
index 902deed83c2b080d7ddbf6b36d6c79f7a48717fa..cd2d43fd7f8b5e24c045fa80842911b9995e911c 100755
--- a/include/gpio.h
+++ b/include/gpio.h
@@ -6,6 +6,8 @@ extern "C" {
#endif
#include "stm32f1xx.h"
+#include "gpio_registers.h"
+#include "memory.h"
#define GPIO_RGB(R,G,B) ((R&0x1F)+((G&0x1F)<<5)+((B&0x1F)<<10))
@@ -30,7 +32,7 @@ typedef struct
typedef enum {START_PB=0,MODE_PB=1} pushbutton_t;
-void gpio_init(void);
+uint8_t gpio_init(TMemory *memory);
// LED functions
void gpio_set_led(led_t led_id);
void gpio_clear_led(led_t led_id);
@@ -40,10 +42,6 @@ void gpio_set_color(led_t led_id, uint16_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
}
diff --git a/include/gpio_registers.h b/include/gpio_registers.h
new file mode 100644
index 0000000000000000000000000000000000000000..c5376e3079099d0be14bc2c63545115892b2fbba
--- /dev/null
+++ b/include/gpio_registers.h
@@ -0,0 +1,56 @@
+#ifndef _GPIO_REGISTERS_H
+#define _GPIO_REGISTERS_H
+
+#ifndef RAM_GPIO_BASE_ADDRESS
+ #define RAM_GPIO_BASE_ADDRESS ((unsigned short int)0x0000)
+#endif
+
+#define RAM_GPIO_LENGTH 21
+
+#define TX_LED_CNTRL RAM_GPIO_BASE_ADDRESS// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | blink | toggle | value | internally used
+#define TX_LED_PERIOD (RAM_GPIO_BASE_ADDRESS+1)
+#define RX_LED_CNTRL (RAM_GPIO_BASE_ADDRESS+3)// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | blink | toggle | value | internally used
+#define RX_LED_PERIOD (RAM_GPIO_BASE_ADDRESS+4)
+#define PLAY_LED_CNTRL (RAM_GPIO_BASE_ADDRESS+6)// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | blink | toggle | value | internally used
+#define PLAY_LED_PERIOD (RAM_GPIO_BASE_ADDRESS+7)
+#define EDIT_LED_CNTRL (RAM_GPIO_BASE_ADDRESS+9)// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | blink | toggle | value | internally used
+#define EDIT_LED_PERIOD (RAM_GPIO_BASE_ADDRESS+10)
+#define MNG_LED_CNTRL (RAM_GPIO_BASE_ADDRESS+12)// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | | toggle | value | internally used
+#define AUX1_LED_CNTRL (RAM_GPIO_BASE_ADDRESS+13)// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // color | toggle_B | value_B | toggle_G | value_G | toggle_R | value_R | internally used
+#define AUX1_LED_COLOR (RAM_GPIO_BASE_ADDRESS+14)// bit 15 | bit 14 | bit 13 | bit 12 | bit 11 | bit 10 | bit 9 | bit 8
+ // green value LSB | red value
+ // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | blue value | green value MSB
+#define AUX2_LED_CNTRL (RAM_GPIO_BASE_ADDRESS+16)// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // color | toggle_B | value_B | toggle_G | value_G | toggle_R | value_R | internally used
+#define AUX2_LED_COLOR (RAM_GPIO_BASE_ADDRESS+17)// bit 15 | bit 14 | bit 13 | bit 12 | bit 11 | bit 10 | bit 9 | bit 8
+ // green value LSB | red value
+ // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | blue value | green value MSB
+#define START_PB_CNTRL (RAM_GPIO_BASE_ADDRESS+19)// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // | | | | | | value | internally used
+#define MODE_PB_CNTRL (RAM_GPIO_BASE_ADDRESS+20)// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+
+#define GPIO_INT_USED 0x01
+#define GPIO_VALUE 0x02
+#define GPIO_TOGGLE 0x04
+#define GPIO_VALUE_R 0x02
+#define GPIO_TOGGLE_R 0x04
+#define GPIO_VALUE_G 0x08
+#define GPIO_TOGGLE_G 0x10
+#define GPIO_VALUE_B 0x20
+#define GPIO_TOGGLE_B 0x40
+#define GPIO_BLINK 0x08
+#define GPIO_COLOR 0x80
+#define GPIO_RED_COLOR 0x001F
+#define GPIO_GREEN_COLOR 0x03E0
+#define GPIO_BLUE_COLOR 0x7C00
+
+#endif
+
diff --git a/include/grippers.h b/include/grippers.h
index 9df31bcba3b0a176d7c83c76f7a89e40b98b23b2..b8818bd90bffa713d8cdb8b80bdaaa3769a4eedd 100644
--- a/include/grippers.h
+++ b/include/grippers.h
@@ -12,17 +12,19 @@ typedef enum {GRIPPER_LEFT,GRIPPER_RIGHT} grippers_t;
// public functions
void grippers_init(uint16_t period_us);
-inline void grippers_set_period(uint16_t period_us);
-inline uint16_t grippers_get_period(void);
+void grippers_set_period(uint16_t period_us);
+uint16_t grippers_get_period(void);
void grippers_open(grippers_t gripper);
void grippers_close(grippers_t gripper);
+void grippers_stop(grippers_t gripper);
uint8_t gripper_is_open(grippers_t gripper);
+uint8_t gripper_is_close(grippers_t gripper);
+uint8_t gripper_is_moving(grippers_t gripper);
// operation functions
uint8_t grippers_in_range(unsigned short int address, unsigned short int length);
void grippers_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
void grippers_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
-// motion manager interface functions
void grippers_process(void);
#ifdef __cplusplus
diff --git a/include/head_tracking.h b/include/head_tracking.h
index 31d3a53c3c1a6bd0db61046305b257beef4ccee5..f5562649aa1bc791cf19d7e402df00d8e5f18c62 100644
--- a/include/head_tracking.h
+++ b/include/head_tracking.h
@@ -10,8 +10,8 @@ extern "C" {
// public functions
void head_tracking_init(uint16_t period_us);
-inline void head_tracking_set_period(uint16_t period_us);
-inline uint16_t head_tracking_get_period(void);
+void head_tracking_set_period(uint16_t period_us);
+uint16_t head_tracking_get_period(void);
void head_tracking_start(void);
void head_tracking_stop(void);
uint8_t head_is_tracking(void);
diff --git a/include/imu.h b/include/imu.h
index 079304bf1b235660acb081ab1186a926366633c0..b74bf3e94cd5d75508f8985eb1e912338007e5fa 100755
--- a/include/imu.h
+++ b/include/imu.h
@@ -6,18 +6,18 @@ extern "C" {
#endif
#include "stm32f1xx.h"
+#include "imu_registers.h"
+#include "memory.h"
//public functions
-void imu_init(void);
+uint8_t imu_init(TMemory *memory);
void imu_start(void);
void imu_stop(void);
void imu_set_calibration_samples(uint16_t num_samples);
void imu_start_gyro_cal(void);
void imu_get_gyro_data(int32_t *x, int32_t *y, int32_t *z);
+void imu_get_accel_data(int32_t *x, int32_t *y, int32_t *z);
uint8_t imu_is_gyro_calibrated(void);
-// operation functions
-uint8_t imu_in_range(unsigned short int address,unsigned short int length);
-void imu_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
#ifdef __cplusplus
}
diff --git a/include/imu_registers.h b/include/imu_registers.h
new file mode 100644
index 0000000000000000000000000000000000000000..92c38a8ef391a2938faf1ff01a515ea64b459685
--- /dev/null
+++ b/include/imu_registers.h
@@ -0,0 +1,28 @@
+#ifndef _IMU_REGISTERS_H
+#define _IMU_REGISTERS_H
+
+#ifndef RAM_IMU_BASE_ADDRESS
+ #define RAM_IMU_BASE_ADDRESS ((unsigned short int)0x0000)
+#endif
+
+#define RAM_IMU_LENGTH 15
+
+#define IMU_CNTRL RAM_IMU_BASE_ADDRESS// bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ // accel_det | gyro_det | calibrating | running | | start_cal | stop | start
+#define IMU_CAL_SAMPLES (RAM_IMU_BASE_ADDRESS+1)
+#define IMU_GYRO_X (RAM_IMU_BASE_ADDRESS+3)
+#define IMU_GYRO_Y (RAM_IMU_BASE_ADDRESS+5)
+#define IMU_GYRO_Z (RAM_IMU_BASE_ADDRESS+7)
+#define IMU_ACCEL_X (RAM_IMU_BASE_ADDRESS+9)
+#define IMU_ACCEL_Y (RAM_IMU_BASE_ADDRESS+11)
+#define IMU_ACCEL_Z (RAM_IMU_BASE_ADDRESS+13)
+
+#define IMU_START 0x01
+#define IMU_STOP 0x02
+#define IMU_START_CAL 0x04
+#define IMU_RUNNING 0x10
+#define IMU_CALIBRATING 0x20
+#define IMU_ACCEL_DET 0x40
+#define IMU_GYRO_DET 0x80
+
+#endif
diff --git a/include/joint_motion.h b/include/joint_motion.h
index 70587b38aa8d0caad2dc7aaf26318d7360ee1525..f2f0e1621409487cc494c876ab7d90b55a343028 100644
--- a/include/joint_motion.h
+++ b/include/joint_motion.h
@@ -12,8 +12,8 @@ extern "C" {
// public functions
void joint_motion_init(uint16_t period_us);
-inline void joint_motion_set_period(uint16_t period_us);
-inline uint16_t joint_motion_get_period(void);
+void joint_motion_set_period(uint16_t period_us);
+uint16_t joint_motion_get_period(void);
void joint_motion_set_group_servos(uint8_t group_id,uint32_t servos);
uint32_t joint_motion_get_group_servos(uint8_t group_id);
void joint_motion_start(uint8_t group_id);
diff --git a/include/motion_manager.h b/include/motion_manager.h
index 10f053f73afb1aeef86c1b705e4e2f68b0feb327..7f10bd79c389068a30bd2655ab96df022e751d0d 100755
--- a/include/motion_manager.h
+++ b/include/motion_manager.h
@@ -7,7 +7,7 @@ extern "C" {
#endif
#include "stm32f1xx.h"
-#include "dynamixel_common.h"
+#include "dyn_common.h"
typedef enum {
R_SHOULDER_PITCH = 1,
@@ -29,7 +29,11 @@ typedef enum {
R_ANKLE_ROLL = 17,
L_ANKLE_ROLL = 18,
L_PAN = 19,
- L_TILT = 20} servo_id_t;
+ L_TILT = 20,
+ L_GRIPPER_TOP = 21,
+ L_GRIPPER_BOT = 22,
+ R_GRIPPER_TOP = 23,
+ R_GRIPPER_BOT = 24} servo_id_t;
typedef enum {MM_NONE = 0,
@@ -38,6 +42,10 @@ typedef enum {MM_NONE = 0,
MM_JOINTS = 3,
MM_HEAD = 4} TModules;
+typedef enum {MM_FWD_FALL = 0,
+ MM_BWD_FALL = 1,
+ MM_STANDING = 2} TFall;
+
typedef struct
{
void (*process_fnct)(void);
@@ -78,23 +86,25 @@ extern int8_t manager_current_slopes[MANAGER_MAX_NUM_SERVOS];
/** \brief Initialize motion manager module
*/
void manager_init(uint16_t period_us);
-inline uint16_t manager_get_period(void);
-inline uint16_t manager_get_period_us(void);
-inline void manager_set_period(uint16_t period_us);
-inline void manager_enable(void);
-inline void manager_disable(void);
-inline uint8_t manager_is_enabled(void);
+uint16_t manager_get_period(void);
+uint16_t manager_get_period_us(void);
+void manager_set_period(uint16_t period_us);
+void manager_enable(void);
+void manager_disable(void);
+uint8_t manager_is_enabled(void);
void manager_enable_balance(void);
void manager_disable_balance(void);
-inline uint8_t manager_get_num_servos(void);
+uint8_t manager_has_fallen(void);
+TFall manager_get_fallen_position(void);
+uint8_t manager_get_num_servos(void);
void manager_set_module(uint8_t servo_id,TModules module);
-inline TModules manager_get_module(uint8_t servo_id);
+TModules manager_get_module(uint8_t servo_id);
void manager_enable_servo(uint8_t servo_id);
-inline void manager_disable_servo(uint8_t servo_id);
-inline uint8_t manager_is_servo_enabled(uint8_t servo_id);
+void manager_disable_servo(uint8_t servo_id);
+uint8_t manager_is_servo_enabled(uint8_t servo_id);
void manager_set_offset(uint8_t servo_id,int8_t offset);
-inline int16_t manager_get_cw_angle_limit(uint8_t servo_id);
-inline int16_t manager_get_ccw_angle_limit(uint8_t servo_id);
+int16_t manager_get_cw_angle_limit(uint8_t servo_id);
+int16_t manager_get_ccw_angle_limit(uint8_t servo_id);
// motion modules handling
// operation functions
uint8_t manager_in_range(unsigned short int address, unsigned short int length);
diff --git a/include/ram.h b/include/ram.h
deleted file mode 100755
index d920367d293c06bb7aa4e8a4ce2fe2cd1a47bd0b..0000000000000000000000000000000000000000
--- a/include/ram.h
+++ /dev/null
@@ -1,37 +0,0 @@
-#ifndef _RAM_H
-#define _RAM_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "stm32f1xx.h"
-#include "darwin_registers.h"
-
-#define RAM_SUCCESS 0
-#define RAM_BAD_ADDRESS -1
-#define RAM_WRITE_ERROR -2
-#define RAM_BAD_BIT -3
-#define RAM_BAD_ACCESS -4
-
-#define RAM_SIZE 1024
-
-extern uint8_t ram_data[RAM_SIZE];
-
-void ram_init(void);
-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
deleted file mode 100644
index 2970a430412faae59d7b903aa320ac604d960600..0000000000000000000000000000000000000000
--- a/include/scheduler.h
+++ /dev/null
@@ -1,23 +0,0 @@
-#ifndef _SCHEDULER_H
-#define _SCHEDULER_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include "stm32f1xx.h"
-
-typedef enum {SCH_CH1=0,SCH_CH2=1,SCH_CH3=2,SCH_CH4=3} sch_ch_t;
-
-void scheduler_init(void);
-void scheduler_set_period(sch_ch_t channel_id, uint16_t period_ms);
-void scheduler_set_one_shot(sch_ch_t channel_id, uint16_t time_ms);
-void scheduler_set_function(sch_ch_t channel_id, void (*function)(void));
-void scheduler_start(sch_ch_t channel_id);
-void scheduler_stop(sch_ch_t channel_id);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif
diff --git a/include/smart_charger.h b/include/smart_charger.h
index d6f8659ee1635a6f3a4446b1ddb1cc3aedbf0e76..0ad973b46e1dcfd5ca21c29baad09f33df037e9f 100644
--- a/include/smart_charger.h
+++ b/include/smart_charger.h
@@ -37,13 +37,13 @@ void smart_charger_init(uint16_t period_us);
*
* \param master pointer to a TDynamixelMaster structure ((darwin_dyn_master or darwin_dyn_master_v2).
*/
-inline void smart_charger_set_version(TDynamixelMaster *master);
+void smart_charger_set_master(TDynamixelMaster *master);
/**
* \brief Function to get the master version of the Dynamixel protocol set to the module
* \return the Dynamixel master structure associated to the smart charger module (darwin_dyn_master or darwin_dyn_master_v2).
*/
-inline TDynamixelMaster* smart_charger_get_version(void);
+TDynamixelMaster* smart_charger_get_master(void);
/**
* \brief Function to set smart charger's period
*
@@ -118,4 +118,4 @@ void smart_charger_process(void);
}
#endif
-#endif
\ No newline at end of file
+#endif
diff --git a/include/stairs.h b/include/stairs.h
new file mode 100755
index 0000000000000000000000000000000000000000..a34b4dc37a37d8474c7183b785637bfe81d82d04
--- /dev/null
+++ b/include/stairs.h
@@ -0,0 +1,31 @@
+#ifndef _STAIRS_H
+#define _STAIRS_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f1xx.h"
+#include "motion_manager.h"
+
+// public functions
+void stairs_init(uint16_t period_us);
+void stairs_set_period(uint16_t period_us);
+uint16_t stairs_get_period(void);
+void stairs_start(uint8_t up);
+void stairs_stop(void);
+uint8_t is_climbing_stairs(void);
+uint8_t stairs_get_phase(void);
+
+// operation functions
+uint8_t stairs_in_range(unsigned short int address, unsigned short int length);
+void stairs_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
+void stairs_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
+// motion manager interface functions
+void stairs_process(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/include/walking.h b/include/walking.h
index 4e0975562df333d8a93448f32ea4aadc18299535..30bc04e06e2d59fcf8dc4be369adcad289250b66 100755
--- a/include/walking.h
+++ b/include/walking.h
@@ -10,8 +10,8 @@ extern "C" {
// public functions
void walking_init(uint16_t period_us);
-inline void walking_set_period(uint16_t period_us);
-inline uint16_t walking_get_period(void);
+void walking_set_period(uint16_t period_us);
+uint16_t walking_get_period(void);
void walking_start(void);
void walking_stop(void);
uint8_t is_walking(void);
diff --git a/src/action.c b/src/action.c
index 47c264feb283772deff24992d488a6d5e398dad8..bfe36ed583094b592b765389de7c77598a6e456a 100755
--- a/src/action.c
+++ b/src/action.c
@@ -273,13 +273,13 @@ void action_init(uint16_t period_us)
action_period_us=period_us;
}
-inline void action_set_period(uint16_t period_us)
+void action_set_period(uint16_t period_us)
{
action_period=(period_us<<16)/1000000;
action_period_us=period_us;
}
-inline uint16_t action_get_period(void)
+uint16_t action_get_period(void)
{
return action_period_us;
}
diff --git a/src/adc_dma.c b/src/adc_dma.c
index d24f466d8bdf495add858283b077871a3f177eb5..cebb7a93f7e6d133e49aeec4b93edf75c18e2828 100755
--- a/src/adc_dma.c
+++ b/src/adc_dma.c
@@ -7,15 +7,13 @@
#define ADC1_CH4 ADC_CHANNEL_7
#define ADC1_CH5 ADC_CHANNEL_8
#define ADC1_CH6 ADC_CHANNEL_TEMPSENSOR
-#define ADC1_CH7 ADC_CHANNEL_VREFINT
#define ADC2_CH1 ADC_CHANNEL_9
#define ADC2_CH2 ADC_CHANNEL_10
#define ADC2_CH3 ADC_CHANNEL_11
#define ADC2_CH4 ADC_CHANNEL_12
-#define ADC2_CH5 ADC_CHANNEL_13
-#define ADC2_CH6 ADC_CHANNEL_14
-#define ADC2_CH7 ADC_CHANNEL_15
+#define ADC2_CH5 ADC_CHANNEL_14
+#define ADC2_CH6 ADC_CHANNEL_15
#define ADC1_CH1_PIN GPIO_PIN_0
#define ADC1_CH1_PORT GPIOA
@@ -47,15 +45,12 @@
#define ADC2_CH4_PIN GPIO_PIN_2
#define ADC2_CH4_PORT GPIOC
#define ADC2_CH4_ENABLE_PORT_CLK __HAL_RCC_GPIOC_CLK_ENABLE()
-#define ADC2_CH5_PIN GPIO_PIN_3
+#define ADC2_CH5_PIN GPIO_PIN_4
#define ADC2_CH5_PORT GPIOC
#define ADC2_CH5_ENABLE_PORT_CLK __HAL_RCC_GPIOC_CLK_ENABLE()
-#define ADC2_CH6_PIN GPIO_PIN_4
+#define ADC2_CH6_PIN GPIO_PIN_5
#define ADC2_CH6_PORT GPIOC
#define ADC2_CH6_ENABLE_PORT_CLK __HAL_RCC_GPIOC_CLK_ENABLE()
-#define ADC2_CH7_PIN GPIO_PIN_5
-#define ADC2_CH7_PORT GPIOC
-#define ADC2_CH7_ENABLE_PORT_CLK __HAL_RCC_GPIOC_CLK_ENABLE()
#define ADC2_ENABLE_CLK __ADC2_CLK_ENABLE()
@@ -82,7 +77,10 @@ ADC_HandleTypeDef hadc2;
DMA_HandleTypeDef hdma_adc1;
TIM_HandleTypeDef ADC_TIM_Handle;
-uint32_t adc_data[9];// temporal buffer to store ADC data before conversion
+/* memory module */
+TMemModule adc_mem_module;
+
+uint32_t adc_data[6];// temporal buffer to store ADC data before conversion
uint16_t adc_period_ms;
// private functions
@@ -104,6 +102,24 @@ uint16_t adc_convert_voltage(uint16_t value)
return conv_value*(1<<12);
}
+void adc_write_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ if(ram_in_range(ADC_CNTRL,address,length))
+ {
+ if(data[ADC_CNTRL-address]&ADC_START)
+ adc_start();
+ else if(data[ADC_CNTRL-address]&ADC_STOP)
+ adc_stop();
+ }
+ if(ram_in_range(ADC_PERIOD,address,length))
+ adc_set_period(data[ADC_PERIOD-address]);
+}
+
+void adc_read_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ ram_read_table(address,length,data);
+}
+
// interrupt handlers
void ADC_TIMER_IRQHandler(void)
{
@@ -153,29 +169,29 @@ void ADC_DMA_IRQHandler(void)
if(__HAL_DMA_GET_IT_SOURCE(&hdma_adc1, DMA_IT_TC) != RESET)
{
__HAL_DMA_CLEAR_FLAG(&hdma_adc1, __HAL_DMA_GET_TC_FLAG_INDEX(&hdma_adc1));
- for(i=0;i<7;i++)
+ for(i=0;i<6;i++)
{
if(i==5)
{
value=adc_convert_temperature(adc_data[i]&0x0000FFFF);
- ram_data[DARWIN_ADC_TEMP_L]=value%256;
- ram_data[DARWIN_ADC_TEMP_H]=value/256;
+ ram_data[ADC_TEMP]=value%256;
+ ram_data[ADC_TEMP+1]=value/256;
}
else
{
value=adc_convert_voltage(adc_data[i]&0x0000FFFF);
- ram_data[DARWIN_ADC_CH1_L+i*4]=value%256;
- ram_data[DARWIN_ADC_CH1_H+i*4]=value/256;
+ ram_data[ADC_CH1_VOLTAGE+i*4]=value%256;
+ ram_data[ADC_CH1_VOLTAGE+1+i*4]=value/256;
}
value=adc_convert_voltage((adc_data[i]&0xFFFF0000)>>16);
- ram_data[DARWIN_ADC_CH2_L+i*4]=value%256;
- ram_data[DARWIN_ADC_CH2_H+i*4]=value/256;
+ ram_data[ADC_CH2_VOLTAGE+i*4]=value%256;
+ ram_data[ADC_CH2_VOLTAGE+1+i*4]=value/256;
}
}
}
// public functions
-void adc_init(void)
+uint8_t adc_init(TMemory *memory)
{
GPIO_InitTypeDef GPIO_InitStruct;
ADC_MultiModeTypeDef multimode;
@@ -188,7 +204,7 @@ void adc_init(void)
/* initialize the internal variables */
adc_period_ms=360;// equivalent to 10 ms
- ram_data[DARWIN_ADC_PERIOD]=10;
+ ram_data[ADC_PERIOD]=10;
/* enable clocks */
ADC1_CH1_ENABLE_PORT_CLK;
@@ -202,7 +218,6 @@ void adc_init(void)
ADC2_CH4_ENABLE_PORT_CLK;
ADC2_CH5_ENABLE_PORT_CLK;
ADC2_CH6_ENABLE_PORT_CLK;
- ADC2_CH7_ENABLE_PORT_CLK;
ADC_ENABLE_DMA_CLK;
@@ -214,7 +229,7 @@ void adc_init(void)
hadc1.Init.DiscontinuousConvMode = DISABLE;
hadc1.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T1_CC1;
hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
- hadc1.Init.NbrOfConversion = 7;
+ hadc1.Init.NbrOfConversion = 6;
HAL_ADC_Init(&hadc1);
multimode.Mode = ADC_DUALMODE_REGSIMULT;
@@ -246,10 +261,6 @@ void adc_init(void)
sConfig.Rank = 6;
HAL_ADC_ConfigChannel(&hadc1, &sConfig);
- sConfig.Channel = ADC1_CH7;
- sConfig.Rank = 7;
- HAL_ADC_ConfigChannel(&hadc1, &sConfig);
-
// configure GPIO
GPIO_InitStruct.Pin = ADC1_CH1_PIN|ADC1_CH2_PIN|ADC1_CH3_PIN|ADC1_CH4_PIN;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
@@ -266,7 +277,7 @@ void adc_init(void)
hadc2.Init.DiscontinuousConvMode = DISABLE;
hadc2.Init.ExternalTrigConv = ADC_SOFTWARE_START;//ADC_EXTERNALTRIGCONV_T1_CC1;
hadc2.Init.DataAlign = ADC_DATAALIGN_RIGHT;
- hadc2.Init.NbrOfConversion = 7;
+ hadc2.Init.NbrOfConversion = 6;
HAL_ADC_Init(&hadc2);
// configure ADC2 channels
@@ -295,13 +306,9 @@ void adc_init(void)
sConfig.Rank = 6;
HAL_ADC_ConfigChannel(&hadc2, &sConfig);
- sConfig.Channel = ADC2_CH7;
- sConfig.Rank = 7;
- HAL_ADC_ConfigChannel(&hadc2, &sConfig);
-
// configure GPIO
GPIO_InitStruct.Pin = ADC2_CH2_PIN|ADC2_CH3_PIN|ADC2_CH4_PIN|ADC2_CH5_PIN|
- ADC2_CH6_PIN|ADC2_CH7_PIN;
+ ADC2_CH6_PIN;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
@@ -366,23 +373,34 @@ void adc_init(void)
HAL_ADCEx_Calibration_Start(&hadc2);
HAL_ADC_Start(&hadc2);
- HAL_ADCEx_MultiModeStart_DMA(&hadc1,adc_data,7);
+ HAL_ADCEx_MultiModeStart_DMA(&hadc1,adc_data,6);
+
+ /* initialize memory module */
+ mem_module_init(&adc_mem_module);
+ adc_mem_module.write_cmd=adc_write_cmd;
+ adc_mem_module.read_cmd=adc_read_cmd;
+ if(!mem_module_add_ram_segment(&adc_mem_module,RAM_ADC_DMA_BASE_ADDRESS,RAM_ADC_DMA_LENGTH))
+ return 0x00;
+ if(!mem_add_module(memory,&adc_mem_module))
+ return 0x00;
+
+ return 0x01;
}
void adc_start(void)
{
- if((ram_data[DARWIN_ADC_CNTRL]&ADC_RUNNING)==0x00)
+ if((ram_data[ADC_CNTRL]&ADC_RUNNING)==0x00)
{
- ram_data[DARWIN_ADC_CNTRL]|=ADC_RUNNING;
+ ram_data[ADC_CNTRL]|=ADC_RUNNING;
HAL_TIM_OC_Start_IT(&ADC_TIM_Handle, TIM_CHANNEL_1);
}
}
void adc_stop(void)
{
- if(ram_data[DARWIN_ADC_CNTRL]&ADC_RUNNING)
+ if(ram_data[ADC_CNTRL]&ADC_RUNNING)
{
- ram_data[DARWIN_ADC_CNTRL]&=(~ADC_RUNNING);
+ ram_data[ADC_CNTRL]&=(~ADC_RUNNING);
HAL_TIM_OC_Stop_IT(&ADC_TIM_Handle, TIM_CHANNEL_1);
}
}
@@ -390,12 +408,12 @@ void adc_stop(void)
void adc_set_period(uint8_t period_ms)
{
adc_period_ms=period_ms*36;
- ram_data[DARWIN_ADC_PERIOD]=period_ms;
+ ram_data[ADC_PERIOD]=period_ms;
}
-inline uint8_t adc_get_period(void)
+uint8_t adc_get_period(void)
{
- return ram_data[DARWIN_ADC_PERIOD];
+ return ram_data[ADC_PERIOD];
}
uint16_t adc_get_channel(adc_ch_t channel)
@@ -404,29 +422,27 @@ uint16_t adc_get_channel(adc_ch_t channel)
switch(channel)
{
- case ADC_CH1: value=ram_data[DARWIN_ADC_CH1_L]+ram_data[DARWIN_ADC_CH1_H]*256;
+ case ADC_CH1: value=ram_data[ADC_CH1_VOLTAGE]+ram_data[ADC_CH1_VOLTAGE+1]*256;
break;
- case ADC_CH2: value=ram_data[DARWIN_ADC_CH2_L]+ram_data[DARWIN_ADC_CH2_H]*256;
+ case ADC_CH2: value=ram_data[ADC_CH2_VOLTAGE]+ram_data[ADC_CH2_VOLTAGE+1]*256;
break;
- case ADC_CH3: value=ram_data[DARWIN_ADC_CH3_L]+ram_data[DARWIN_ADC_CH3_H]*256;
+ case ADC_CH3: value=ram_data[ADC_CH3_VOLTAGE]+ram_data[ADC_CH3_VOLTAGE+1]*256;
break;
- case ADC_CH4: value=ram_data[DARWIN_ADC_CH4_L]+ram_data[DARWIN_ADC_CH4_H]*256;
+ case ADC_CH4: value=ram_data[ADC_CH4_VOLTAGE]+ram_data[ADC_CH4_VOLTAGE+1]*256;
break;
- case ADC_CH5: value=ram_data[DARWIN_ADC_CH5_L]+ram_data[DARWIN_ADC_CH5_H]*256;
+ case ADC_CH5: value=ram_data[ADC_CH5_VOLTAGE]+ram_data[ADC_CH5_VOLTAGE+1]*256;
break;
- case ADC_CH6: value=ram_data[DARWIN_ADC_CH6_L]+ram_data[DARWIN_ADC_CH6_H]*256;
+ case ADC_CH6: value=ram_data[ADC_CH6_VOLTAGE]+ram_data[ADC_CH6_VOLTAGE+1]*256;
break;
- case ADC_CH7: value=ram_data[DARWIN_ADC_CH7_L]+ram_data[DARWIN_ADC_CH7_H]*256;
+ case ADC_CH7: value=ram_data[ADC_CH7_VOLTAGE]+ram_data[ADC_CH7_VOLTAGE+1]*256;
break;
- case ADC_CH8: value=ram_data[DARWIN_ADC_CH8_L]+ram_data[DARWIN_ADC_CH8_H]*256;
+ case ADC_CH8: value=ram_data[ADC_CH8_VOLTAGE]+ram_data[ADC_CH8_VOLTAGE+1]*256;
break;
- case ADC_CH9: value=ram_data[DARWIN_ADC_CH9_L]+ram_data[DARWIN_ADC_CH9_H]*256;
+ case ADC_CH9: value=ram_data[ADC_CH9_VOLTAGE]+ram_data[ADC_CH9_VOLTAGE+1]*256;
break;
- case ADC_CH10: value=ram_data[DARWIN_ADC_CH10_L]+ram_data[DARWIN_ADC_CH10_H]*256;
- break;
- case ADC_CH12: value=ram_data[DARWIN_ADC_CH12_L]+ram_data[DARWIN_ADC_CH12_H]*256;
+ case ADC_CH10: value=ram_data[ADC_CH10_VOLTAGE]+ram_data[ADC_CH10_VOLTAGE+1]*256;
break;
- case ADC_CH14: value=ram_data[DARWIN_ADC_CH14_L]+ram_data[DARWIN_ADC_CH14_H]*256;
+ case ADC_CH12: value=ram_data[ADC_CH12_VOLTAGE]+ram_data[ADC_CH12_VOLTAGE+1]*256;
break;
default: value=0x0000;
}
@@ -462,8 +478,6 @@ uint16_t adc_get_channel_raw(adc_ch_t channel)
break;
case ADC_CH12: value=(adc_data[5]&0xFFFF0000)>>16;
break;
- case ADC_CH14: value=(adc_data[6]&0xFFFF0000)>>16;
- break;
default: value=0x0000;
}
@@ -472,25 +486,6 @@ uint16_t adc_get_channel_raw(adc_ch_t channel)
uint16_t adc_get_temperature(void)
{
- return ram_data[DARWIN_ADC_TEMP_L]+ram_data[DARWIN_ADC_TEMP_H]*256;
-}
-
-// operation functions
-uint8_t adc_in_range(unsigned short int address,unsigned short int length)
-{
- return ram_in_window(ADC_BASE_ADDRESS,ADC_MEM_LENGTH,address,length);
-}
-
-void adc_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
-{
- if(ram_in_range(DARWIN_ADC_CNTRL,address,length))
- {
- if(data[DARWIN_ADC_CNTRL-address]&ADC_START)
- adc_start();
- else if(data[DARWIN_ADC_CNTRL-address]&ADC_STOP)
- adc_stop();
- }
- if(ram_in_range(DARWIN_ADC_PERIOD,address,length))
- adc_set_period(data[DARWIN_ADC_PERIOD-address]);
+ return ram_data[ADC_TEMP]+ram_data[ADC_TEMP+1]*256;
}
diff --git a/src/cm730_fw.c b/src/cm730_fw.c
index d074c765ddd1f91e06636c2809f33fa476b6acb2..326cbe94588553ddb046f74b1512287e9b9d9947 100755
--- a/src/cm730_fw.c
+++ b/src/cm730_fw.c
@@ -1,45 +1,55 @@
#include "stm32f1xx_hal.h"
-#include "gpio.h"
-#include "eeprom.h"
-#include "ram.h"
-#include "adc_dma.h"
-#include "imu.h"
-#include "darwin_time.h"
+#include "darwin_conf.h"
+#include "memory.h"
+#include "darwin_sch.h"
#include "darwin_dyn_slave.h"
-#include "darwin_dyn_master.h"
-#include "motion_manager.h"
-#include "action.h"
-#include "action_id.h"
+#include "darwin_time.h"
+#include "eeprom.h"
+//#include "gpio.h"
+//#include "adc_dma.h"
+//#include "imu.h"
+//#include "darwin_dyn_master.h"
+//#include "darwin_dyn_master_v2.h"
+//#include "motion_manager.h"
+//#include "action.h"
+//#include "action_id.h"
+
+TMemory *darwin_memory;
+TScheduler *scheduler;
int main(void)
{
- uint16_t eeprom_data,period;
- uint8_t i;
+// uint16_t eeprom_data,period;
/* initialize the HAL module */
HAL_Init();
/* initialize EEPROM */
EE_Init();
- // initialize the Dynamixel RAM memory space
- ram_init();
+ /* initialize the scheduler */
+ scheduler=darwin_sch_init();
/* initialize the GPIO module */
- gpio_init();
+// gpio_init(&darwin_memory);
// initialize adc
- adc_init();
+// adc_init(&darwin_memory);
// initialize imu
- imu_init();
+// imu_init(&darwin_memory);
// initialize time module
darwin_time_init();
/* initialize the dynamixel slave interface */
- darwin_dyn_slave_init();
- darwin_dyn_slave_start();
+ darwin_dyn_slave_init(&darwin_memory,scheduler);
+ darwin_memory->eeprom_write_data=EE_WriteVariable;
+ darwin_memory->eeprom_read_data=EE_ReadVariable;
/* initialize motion manager module */
- EE_ReadVariable(MM_PERIOD_OFFSET,&eeprom_data);
- period=eeprom_data&0x00FF;
- EE_ReadVariable(MM_PERIOD_OFFSET+1,&eeprom_data);
- period+=((eeprom_data&0x00FF)<<8);
- manager_init(period);
- gpio_set_led(LED_4);
+// 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);
+// gpio_set_led(LED_4);
+ /* initialize the ram module */
+ ram_init(darwin_memory);
+
+ darwin_dyn_slave_start();
while(1);/* main function does not return */
}
diff --git a/src/darwin_dyn_master.c b/src/darwin_dyn_master.c
index 7b3c0e9aa368f5741416b101b7544f9fbce3c8ba..9762cf6fee1474dc5059537fdd592db6530a0320 100755
--- a/src/darwin_dyn_master.c
+++ b/src/darwin_dyn_master.c
@@ -89,16 +89,16 @@ void darwin_dyn_master_init(void)
/* configure dynamixel master module */
dyn_master_set_rx_timeout(&darwin_dyn_master,50);
- dyn_master_set_return_level(&darwin_dyn_master,return_all);
+ dyn_master_set_return_level(&darwin_dyn_master,RETURN_ALL);
}
-inline void darwin_dyn_master_enable_power(void)
+void darwin_dyn_master_enable_power(void)
{
HAL_GPIO_WritePin(POWER_GPIO_PORT,POWER_PIN,GPIO_PIN_SET);
ram_data[DARWIN_MM_CNTRL]|=MANAGER_EN_PWR;
}
-inline void darwin_dyn_master_disable_power(void)
+void darwin_dyn_master_disable_power(void)
{
HAL_GPIO_WritePin(POWER_GPIO_PORT,POWER_PIN,GPIO_PIN_RESET);
ram_data[DARWIN_MM_CNTRL]&=(~MANAGER_EN_PWR);
diff --git a/src/darwin_dyn_master_v2.c b/src/darwin_dyn_master_v2.c
index 5dc5e5daf5ded2ca691a234b8c2dc9e1d6f9aae4..732bd5636a1954dd9cea8d1eaae1fe09161afd5c 100755
--- a/src/darwin_dyn_master_v2.c
+++ b/src/darwin_dyn_master_v2.c
@@ -11,6 +11,10 @@
#define TX_EN_PIN GPIO_PIN_1
#define TX_EN_GPIO_PORT GPIOA
+#define POWER_GPIO_CLK __GPIOC_CLK_ENABLE()
+#define POWER_PIN GPIO_PIN_3
+#define POWER_GPIO_PORT GPIOC
+
/* private variables */
TDynamixelMaster darwin_dyn_master_v2;
TTime darwin_dyn_master_v2_timer;
@@ -78,6 +82,19 @@ void darwin_dyn_master_v2_init(void)
/* configure dynamixel master module */
dyn_master_set_rx_timeout(&darwin_dyn_master_v2,50);
- dyn_master_set_return_level(&darwin_dyn_master_v2,return_all);
+ dyn_master_set_return_level(&darwin_dyn_master_v2,RETURN_ALL);
+ darwin_dyn_master_v2_disable_power();
+}
+
+void darwin_dyn_master_v2_enable_power(void)
+{
+ HAL_GPIO_WritePin(POWER_GPIO_PORT,POWER_PIN,GPIO_PIN_SET);
+ ram_data[DARWIN_MM_CNTRL]|=MANAGER_EN_PWR_V2;
+}
+
+void darwin_dyn_master_v2_disable_power(void)
+{
+ HAL_GPIO_WritePin(POWER_GPIO_PORT,POWER_PIN,GPIO_PIN_RESET);
+ ram_data[DARWIN_MM_CNTRL]&=(~MANAGER_EN_PWR_V2);
}
diff --git a/src/darwin_dyn_slave.c b/src/darwin_dyn_slave.c
index dd7f12fac2b7730652638b6bf9e393ab0b2f1155..09c22bc7de4c1b161e080f2d0b1a01071e5f7ab3 100755
--- a/src/darwin_dyn_slave.c
+++ b/src/darwin_dyn_slave.c
@@ -2,103 +2,32 @@
#include "darwin_time.h"
#include "usart3.h"
#include "ram.h"
-#include "eeprom.h"
-#include "gpio.h"
-#include "adc_dma.h"
-#include "imu.h"
-#include "motion_manager.h"
-#include "action.h"
-#include "walking.h"
-#include "joint_motion.h"
-#include "head_tracking.h"
-#include "grippers.h"
-#include "smart_charger.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;
+TDynamixelSlaveDevice darwin_dyn_slave_device;
TTime darwin_dyn_slave_timer;
TComm darwin_dyn_slave_comm;
UART_InitTypeDef darwin_comm_init;
-TIM_HandleTypeDef darwin_dyn_slave_tim_handle;
+TScheduler *darwin_dyn_slave_sch;
+
+/* memory module */
+TMemory *darwin_dyn_slave_memory;
+
+/* eeprom data */
+dyn_slave_control_eeprom_data(darwin_dyn_slave,".eeprom",EEPROM_DYN_SLAVE_BASE_ADDRESS1,EEPROM_DYN_SLAVE_BASE_ADDRESS2);
// private functions
unsigned char darwin_on_read(unsigned short int address,unsigned short int length,unsigned char *data)
{
- 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;
+ mem_do_read(darwin_dyn_slave_memory,address,length,data);
+
+ return 0x00;
}
unsigned char darwin_on_write(unsigned short int address,unsigned short int length,unsigned char *data)
{
- unsigned short int i,j;
-
- /* dynamixel slave internal operation registers */
- if(ram_in_range(DARWIN_ID,address,length))
- {
- dyn_slave_set_address(&darwin_dyn_slave,data[DARWIN_ID-address]);
- ram_data[DARWIN_ID]=data[DARWIN_ID-address];
- }
- if(ram_in_range(DARWIN_BAUD_RATE,address,length))
- {
- darwin_comm_init.BaudRate=2000000/(data[DARWIN_BAUD_RATE-address]+1);
- usart3_config(&darwin_dyn_slave_comm,&darwin_comm_init);
- ram_data[DARWIN_BAUD_RATE]=data[DARWIN_BAUD_RATE-address];
- }
- if(ram_in_range(DARWIN_RETURN_DELAY_TIME,address,length))
- {
- dyn_slave_set_return_delay(&darwin_dyn_slave,data[DARWIN_RETURN_DELAY_TIME-address]);
- ram_data[DARWIN_RETURN_DELAY_TIME]=data[DARWIN_RETURN_DELAY_TIME-address];
- }
- if(ram_in_range(DARWIN_RETURN_LEVEL,address,length))
- {
- dyn_slave_set_return_level(&darwin_dyn_slave,data[DARWIN_RETURN_LEVEL-address]);
- ram_data[DARWIN_RETURN_LEVEL]=data[DARWIN_RETURN_LEVEL-address];
- }
- // GPIO commands
- if(gpio_in_range(address,length))
- gpio_process_write_cmd(address,length,data);
- // ADC commands
- if(adc_in_range(address,length))
- adc_process_write_cmd(address,length,data);
- // IMU commands
- if(imu_in_range(address,length))
- imu_process_write_cmd(address,length,data);
- // Manager commands
- if(manager_in_range(address,length))
- manager_process_write_cmd(address,length,data);
- // action commands
- if(action_in_range(address,length))
- action_process_write_cmd(address,length,data);
- // walk commands
- if(walking_in_range(address,length))
- walking_process_write_cmd(address,length,data);
- // joint motion commands
- if(joint_motion_in_range(address,length))
- joint_motion_process_write_cmd(address,length,data);
- // head_tracking commands
- if(head_tracking_in_range(address,length))
- head_tracking_process_write_cmd(address,length,data);
- // gripper commands
- if(grippers_in_range(address,length))
- grippers_process_write_cmd(address,length,data);
- // smart charger commands
- if(smart_charger_in_range(address,length))
- smart_charger_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]);
+ mem_do_write(darwin_dyn_slave_memory,address,length,data);
return 0x00;
}
@@ -107,30 +36,18 @@ 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)
+uint8_t darwin_dyn_slave_init(TMemory **memory,TScheduler *scheduler)
{
TUSART_IRQ_Priorities priorities;
+ uint8_t status;
// initialize timer structure
time_init(&darwin_dyn_slave_timer,darwin_time_get_counts_per_us(),darwin_time_get_counts);
/* initialize the comm object */
comm_init(&darwin_dyn_slave_comm,0x01,&darwin_dyn_slave_timer);
- darwin_comm_init.BaudRate = 2000000/(ram_data[BAUDRATE_OFFSET]+1);
+ darwin_comm_init.BaudRate = 2000000/(ram_data[DEFAULT_BAUDRATE]+1);
darwin_comm_init.WordLength = UART_WORDLENGTH_8B;
darwin_comm_init.StopBits = UART_STOPBITS_1;
darwin_comm_init.Parity = UART_PARITY_NONE;
@@ -146,34 +63,30 @@ void darwin_dyn_slave_init(void)
priorities.dma_tx_subpriority=0;
usart3_init(&darwin_dyn_slave_comm,&darwin_comm_init,&priorities);
- dyn_slave_init(&darwin_dyn_slave,&darwin_dyn_slave_comm,ram_data[DEVICE_ID_OFFSET],DYN_VER2);
- darwin_dyn_slave.on_read=darwin_on_read;
- darwin_dyn_slave.on_write=darwin_on_write;
- 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_RETURN_LEVEL]);
-
- /* initialize timer for the execution of the dynamixel slave loop */
- DYN_SLAVE_TIMER_ENABLE_CLK;
- darwin_dyn_slave_tim_handle.Instance=DYN_SLAVE_TIMER;
- darwin_dyn_slave_tim_handle.Init.Period = 1000;
- darwin_dyn_slave_tim_handle.Init.Prescaler = 72;
- darwin_dyn_slave_tim_handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
- darwin_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, 2);
- HAL_NVIC_EnableIRQ(DYN_SLAVE_TIMER_IRQn);
-
+ // initialize slave device
+ dyn_slave_device_init(&darwin_dyn_slave_device,DEFAULT_DEVICE_ID,EEPROM_DYN_SLAVE_BASE_ADDRESS1,EEPROM_DYN_SLAVE_BASE_ADDRESS2);
+ darwin_dyn_slave_device.on_read=darwin_on_read;
+ darwin_dyn_slave_device.on_write=darwin_on_write;
+ darwin_dyn_slave_device.on_ping=darwin_on_ping;
+ dyn_slave_device_set_return_delay(&darwin_dyn_slave_device,DEFAULT_RETURN_DELAY);
+ dyn_slave_device_set_return_level(&darwin_dyn_slave_device,DEFAULT_RETURN_LEVEL);
+ (*memory)=dyn_slave_device_get_memory(&darwin_dyn_slave_device);
+ darwin_dyn_slave_memory=dyn_slave_device_get_memory(&darwin_dyn_slave_device);
+
+ status=dyn_slave_init(&darwin_dyn_slave,&darwin_dyn_slave_comm,scheduler,SCHED_CH1,DYN_VER2);
+ darwin_dyn_slave.set_baudrate=usart3_set_baudrate;
+ dyn_slave_add_device(&darwin_dyn_slave,&darwin_dyn_slave_device);
+
+ return status;
}
void darwin_dyn_slave_start(void)
{
- HAL_TIM_Base_Start_IT(&darwin_dyn_slave_tim_handle);
+ dyn_slave_start(&darwin_dyn_slave);
}
void darwin_dyn_slave_stop(void)
{
- HAL_TIM_Base_Stop_IT(&darwin_dyn_slave_tim_handle);
+ dyn_slave_stop(&darwin_dyn_slave);
}
diff --git a/src/darwin_sch.c b/src/darwin_sch.c
new file mode 100644
index 0000000000000000000000000000000000000000..f27bee62d274c22231afdaa561f2795f422ed73b
--- /dev/null
+++ b/src/darwin_sch.c
@@ -0,0 +1,128 @@
+#include "darwin_sch.h"
+
+/* private variables */
+TIM_HandleTypeDef darwin_sch_timer_handle;
+TScheduler darwin_scheduler;
+
+/* interrupt handlers */
+void TIM1_CC_IRQHandler(void)
+{
+ /* Capture compare 1 event */
+ if(__HAL_TIM_GET_FLAG(&darwin_sch_timer_handle, TIM_FLAG_CC1) != RESET)
+ {
+ if(__HAL_TIM_GET_IT_SOURCE(&darwin_sch_timer_handle, TIM_IT_CC1) !=RESET)
+ {
+ __HAL_TIM_CLEAR_IT(&darwin_sch_timer_handle, TIM_IT_CC1);
+ scheduler_interrupt(&darwin_scheduler,SCHED_CH1);
+ }
+ }
+ /* Capture compare 2 event */
+ if(__HAL_TIM_GET_FLAG(&darwin_sch_timer_handle, TIM_FLAG_CC2) != RESET)
+ {
+ if(__HAL_TIM_GET_IT_SOURCE(&darwin_sch_timer_handle, TIM_IT_CC2) !=RESET)
+ {
+ __HAL_TIM_CLEAR_IT(&darwin_sch_timer_handle, TIM_IT_CC2);
+ scheduler_interrupt(&darwin_scheduler,SCHED_CH2);
+ }
+ }
+ /* Capture compare 3 event */
+ if(__HAL_TIM_GET_FLAG(&darwin_sch_timer_handle, TIM_FLAG_CC3) != RESET)
+ {
+ if(__HAL_TIM_GET_IT_SOURCE(&darwin_sch_timer_handle, TIM_IT_CC3) !=RESET)
+ {
+ __HAL_TIM_CLEAR_IT(&darwin_sch_timer_handle, TIM_IT_CC3);
+ scheduler_interrupt(&darwin_scheduler,SCHED_CH3);
+ }
+ }
+ /* Capture compare 4 event */
+ if(__HAL_TIM_GET_FLAG(&darwin_sch_timer_handle, TIM_FLAG_CC4) != RESET)
+ {
+ if(__HAL_TIM_GET_IT_SOURCE(&darwin_sch_timer_handle, TIM_IT_CC4) !=RESET)
+ {
+ __HAL_TIM_CLEAR_IT(&darwin_sch_timer_handle, TIM_IT_CC4);
+ scheduler_interrupt(&darwin_scheduler,SCHED_CH4);
+ }
+ }
+}
+
+/* private functions */
+void darwin_sch_start(unsigned short int channel_id)
+{
+ HAL_TIM_OC_Start_IT(&darwin_sch_timer_handle,channel_id);
+}
+
+void darwin_sch_stop(unsigned short int channel_id)
+{
+ HAL_TIM_OC_Stop_IT(&darwin_sch_timer_handle,channel_id);
+}
+
+void darwin_sch_set_pulse(unsigned short int channel_id,unsigned short int pulse, unsigned char running)
+{
+ TIM_OC_InitTypeDef out_comp_config;
+ unsigned short int capture;
+
+ if(running==0x00)
+ {
+ if(darwin_scheduler.channels[channel_id].enabled==0x00)
+ {
+ out_comp_config.OCMode = TIM_OCMODE_TIMING;
+ capture = HAL_TIM_ReadCapturedValue(&darwin_sch_timer_handle,channel_id);
+ out_comp_config.Pulse = capture+pulse;
+ out_comp_config.OCPolarity = TIM_OCPOLARITY_HIGH;
+ out_comp_config.OCFastMode = TIM_OCFAST_DISABLE;
+ HAL_TIM_OC_ConfigChannel(&darwin_sch_timer_handle, &out_comp_config, channel_id);
+ }
+ }
+ else
+ {
+ capture = HAL_TIM_ReadCapturedValue(&darwin_sch_timer_handle, channel_id);
+ __HAL_TIM_SetCompare(&darwin_sch_timer_handle,channel_id, capture+pulse);
+ }
+}
+
+/* public functions */
+TScheduler *darwin_sch_init(void)
+{
+ TIM_ClockConfigTypeDef sClockSourceConfig;
+ TIM_MasterConfigTypeDef sMasterConfig;
+ TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig;
+
+ __HAL_RCC_TIM1_CLK_ENABLE();
+
+ darwin_sch_timer_handle.Instance = TIM1;
+ darwin_sch_timer_handle.Init.Prescaler = 168;
+ darwin_sch_timer_handle.Init.CounterMode = TIM_COUNTERMODE_UP;
+ darwin_sch_timer_handle.Init.Period = 0xFFFF;
+ darwin_sch_timer_handle.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+ darwin_sch_timer_handle.Init.RepetitionCounter = 0;
+ HAL_TIM_Base_Init(&darwin_sch_timer_handle);
+
+ sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+ HAL_TIM_ConfigClockSource(&darwin_sch_timer_handle, &sClockSourceConfig);
+
+ HAL_TIM_OC_Init(&darwin_sch_timer_handle);
+
+ sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+ sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+ HAL_TIMEx_MasterConfigSynchronization(&darwin_sch_timer_handle, &sMasterConfig);
+
+ sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE;
+ sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE;
+ sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF;
+ sBreakDeadTimeConfig.DeadTime = 0;
+ sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE;
+ sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH;
+ sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE;
+ HAL_TIMEx_ConfigBreakDeadTime(&darwin_sch_timer_handle, &sBreakDeadTimeConfig);
+
+ /* Peripheral interrupt init */
+ HAL_NVIC_SetPriority(TIM1_CC_IRQn, 2, 0);
+ HAL_NVIC_EnableIRQ(TIM1_CC_IRQn);
+
+ scheduler_init(&darwin_scheduler,4,168);
+ darwin_scheduler.start=darwin_sch_start;
+ darwin_scheduler.stop=darwin_sch_stop;
+ darwin_scheduler.set_pulse=darwin_sch_set_pulse;
+
+ return &darwin_scheduler;
+}
diff --git a/src/eeprom.c b/src/eeprom.c
index d511af23b9cf8ebb04bf8cd314b10cfea15fc435..ca45c913d0c898359c8f5dca5611b6f01aacf936 100755
--- a/src/eeprom.c
+++ b/src/eeprom.c
@@ -53,108 +53,7 @@
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, 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,
- DEFAULT_HEAD_PAN_P&0xFF, HEAD_PAN_P,
- DEFAULT_HEAD_PAN_P>>8, HEAD_PAN_P+1,
- DEFAULT_HEAD_PAN_I&0xFF, HEAD_PAN_I,
- DEFAULT_HEAD_PAN_I>>8, HEAD_PAN_I+1,
- DEFAULT_HEAD_PAN_D&0xFF, HEAD_PAN_D,
- DEFAULT_HEAD_PAN_D>>8, HEAD_PAN_D+1,
- DEFAULT_HEAD_PAN_I_CLAMP&0xFF, HEAD_PAN_I_CLAMP,
- DEFAULT_HEAD_PAN_I_CLAMP>>8, HEAD_PAN_I_CLAMP+1,
- DEFAULT_HEAD_PAN_D>>8, HEAD_PAN_D+1,
- DEFAULT_HEAD_TILT_P&0xFF, HEAD_TILT_P,
- DEFAULT_HEAD_TILT_P>>8, HEAD_TILT_P+1,
- DEFAULT_HEAD_TILT_I&0xFF, HEAD_TILT_I,
- DEFAULT_HEAD_TILT_I>>8, HEAD_TILT_I+1,
- DEFAULT_HEAD_TILT_D&0xFF, HEAD_TILT_D,
- DEFAULT_HEAD_TILT_D>>8, HEAD_TILT_D+1,
- DEFAULT_HEAD_TILT_I_CLAMP&0xFF, HEAD_TILT_I_CLAMP,
- DEFAULT_HEAD_TILT_I_CLAMP>>8, HEAD_TILT_I_CLAMP+1,
- DEFAULT_GRIPPER_LEFT_ID, GRIPPER_LEFT_ID,
- DEFAULT_GRIPPER_LEFT_MAX_ANGLE&0xFF, GRIPPER_LEFT_MAX_ANGLE,
- DEFAULT_GRIPPER_LEFT_MAX_ANGLE>>8, GRIPPER_LEFT_MAX_ANGLE+1,
- DEFAULT_GRIPPER_LEFT_MIN_ANGLE&0xFF, GRIPPER_LEFT_MIN_ANGLE,
- DEFAULT_GRIPPER_LEFT_MIN_ANGLE>>8, GRIPPER_LEFT_MIN_ANGLE+1,
- DEFAULT_GRIPPER_LEFT_MAX_FORCE&0xFF, GRIPPER_LEFT_MAX_FORCE,
- DEFAULT_GRIPPER_LEFT_MAX_FORCE>>8, GRIPPER_LEFT_MAX_FORCE+1,
- DEFAULT_GRIPPER_RIGHT_ID, GRIPPER_RIGHT_ID,
- DEFAULT_GRIPPER_RIGHT_MAX_ANGLE&0xFF, GRIPPER_RIGHT_MAX_ANGLE,
- DEFAULT_GRIPPER_RIGHT_MAX_ANGLE>>8, GRIPPER_RIGHT_MAX_ANGLE+1,
- DEFAULT_GRIPPER_RIGHT_MIN_ANGLE&0xFF, GRIPPER_RIGHT_MIN_ANGLE,
- DEFAULT_GRIPPER_RIGHT_MIN_ANGLE>>8, GRIPPER_RIGHT_MIN_ANGLE+1,
- DEFAULT_GRIPPER_RIGHT_MAX_FORCE&0xFF, GRIPPER_RIGHT_MAX_FORCE,
- DEFAULT_GRIPPER_RIGHT_MAX_FORCE>>8, GRIPPER_RIGHT_MAX_FORCE+1,
- DEFAULT_SMART_CHARGER_PERIOD&0xFF, SMART_CHARGER_PERIOD,
- DEFAULT_SMART_CHARGER_PERIOD>>8, SMART_CHARGER_PERIOD+1};
+uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE, 0xFFFF};
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
@@ -736,7 +635,7 @@ static uint16_t EE_PageTransfer(uint16_t VirtAddress, uint16_t Data)
HAL_StatusTypeDef flashstatus = HAL_OK;
uint32_t newpageaddress = EEPROM_START_ADDRESS;
uint32_t oldpageid = 0;
- uint16_t validpage = PAGE0, varidx = 0;
+ uint16_t validpage = PAGE0, varidx = 0, address=0;
uint16_t eepromstatus = 0, readstatus = 0;
uint32_t page_error = 0;
FLASH_EraseInitTypeDef s_eraseinit;
@@ -784,15 +683,16 @@ static uint16_t EE_PageTransfer(uint16_t VirtAddress, uint16_t Data)
/* Transfer process: transfer variables from old to the new active page */
for (varidx = 0; varidx < NB_OF_VAR; varidx++)
{
- if (eeprom_data[varidx] != VirtAddress) /* Check each variable except the one passed as parameter */
+ address=((__IO uint16_t*)oldpageid)[1+varidx*2];
+ if (address != VirtAddress) /* Check each variable except the one passed as parameter */
{
/* Read the other last variable updates */
- readstatus = EE_ReadVariable(eeprom_data[varidx], &DataVar);
+ readstatus = EE_ReadVariable(address, &DataVar);
/* In case variable corresponding to the virtual address was found */
if (readstatus != 0x1)
{
/* Transfer the variable to the new active page */
- eepromstatus = EE_VerifyPageFullWriteVariable(eeprom_data[varidx], DataVar);
+ eepromstatus = EE_VerifyPageFullWriteVariable(address, DataVar);
/* If program operation was failed, a Flash error code is returned */
if (eepromstatus != HAL_OK)
{
diff --git a/src/gpio.c b/src/gpio.c
index b86a6eec6344e4e9de9b84a1adb7b9f0a115b154..35315b2f0162a00e695a438062def0ea47d90f0b 100755
--- a/src/gpio.c
+++ b/src/gpio.c
@@ -75,21 +75,24 @@
TIM_HandleTypeDef GPO_TIM1Handle;
TIM_HandleTypeDef GPO_TIM2Handle;
TIM_HandleTypeDef GPO_TIM3Handle;
+TMemModule gpio_mem_module;
+
// Pushbuttons callbacks
void (*start_pb_callback)(void);
void (*mode_pb_callback)(void);
+
// LED's info structures
-const led_info_t leds[GPIO_NUM_LEDS]={{LED_TX_GPIO_PORT,LED_TX_PIN,&GPO_TIM1Handle,TIM_CHANNEL_1,DARWIN_TX_LED_CNTRL,DARWIN_TX_LED_PERIOD_L,GPIO_VALUE,GPIO_TOGGLE,GPIO_BLINK},
- {LED_RX_GPIO_PORT,LED_RX_PIN,&GPO_TIM1Handle,TIM_CHANNEL_2,DARWIN_RX_LED_CNTRL,DARWIN_RX_LED_PERIOD_L,GPIO_VALUE,GPIO_TOGGLE,GPIO_BLINK},
- {LED_2_GPIO_PORT,LED_2_PIN,&GPO_TIM1Handle,TIM_CHANNEL_3,DARWIN_PLAY_LED_CNTRL,DARWIN_PLAY_LED_PERIOD_L,GPIO_VALUE,GPIO_TOGGLE,GPIO_BLINK},
- {LED_3_GPIO_PORT,LED_3_PIN,&GPO_TIM1Handle,TIM_CHANNEL_4,DARWIN_EDIT_LED_CNTRL,DARWIN_EDIT_LED_PERIOD_L,GPIO_VALUE,GPIO_TOGGLE,GPIO_BLINK},
- {LED_4_GPIO_PORT,LED_4_PIN,0,0,DARWIN_MNG_LED_CNTRL,0x0000,GPIO_VALUE,GPIO_TOGGLE,0x00},
- {LED_5_R_GPIO_PORT,LED_5_R_PIN,&GPO_TIM2Handle,TIM_CHANNEL_1,DARWIN_AUX1_LED_CNTRL,DARWIN_AUX1_LED_COLOR_L,GPIO_VALUE_R,GPIO_TOGGLE_R,0x00},
- {LED_5_G_GPIO_PORT,LED_5_G_PIN,&GPO_TIM2Handle,TIM_CHANNEL_2,DARWIN_AUX1_LED_CNTRL,DARWIN_AUX1_LED_COLOR_L,GPIO_VALUE_G,GPIO_TOGGLE_G,0x00},
- {LED_5_B_GPIO_PORT,LED_5_B_PIN,&GPO_TIM2Handle,TIM_CHANNEL_3,DARWIN_AUX1_LED_CNTRL,DARWIN_AUX1_LED_COLOR_L,GPIO_VALUE_B,GPIO_TOGGLE_B,0x00},
- {LED_6_R_GPIO_PORT,LED_6_R_PIN,&GPO_TIM3Handle,TIM_CHANNEL_1,DARWIN_AUX2_LED_CNTRL,DARWIN_AUX2_LED_COLOR_L,GPIO_VALUE_R,GPIO_TOGGLE_R,0x00},
- {LED_6_G_GPIO_PORT,LED_6_G_PIN,&GPO_TIM3Handle,TIM_CHANNEL_2,DARWIN_AUX2_LED_CNTRL,DARWIN_AUX2_LED_COLOR_L,GPIO_VALUE_G,GPIO_TOGGLE_G,0x00},
- {LED_6_B_GPIO_PORT,LED_6_B_PIN,&GPO_TIM3Handle,TIM_CHANNEL_3,DARWIN_AUX2_LED_CNTRL,DARWIN_AUX2_LED_COLOR_L,GPIO_VALUE_B,GPIO_TOGGLE_B,0x00}};
+const led_info_t leds[GPIO_NUM_LEDS]={{LED_TX_GPIO_PORT,LED_TX_PIN,&GPO_TIM1Handle,TIM_CHANNEL_1,TX_LED_CNTRL,TX_LED_PERIOD,GPIO_VALUE,GPIO_TOGGLE,GPIO_BLINK},
+ {LED_RX_GPIO_PORT,LED_RX_PIN,&GPO_TIM1Handle,TIM_CHANNEL_2,RX_LED_CNTRL,RX_LED_PERIOD,GPIO_VALUE,GPIO_TOGGLE,GPIO_BLINK},
+ {LED_2_GPIO_PORT,LED_2_PIN,&GPO_TIM1Handle,TIM_CHANNEL_3,PLAY_LED_CNTRL,PLAY_LED_PERIOD,GPIO_VALUE,GPIO_TOGGLE,GPIO_BLINK},
+ {LED_3_GPIO_PORT,LED_3_PIN,&GPO_TIM1Handle,TIM_CHANNEL_4,EDIT_LED_CNTRL,EDIT_LED_PERIOD,GPIO_VALUE,GPIO_TOGGLE,GPIO_BLINK},
+ {LED_4_GPIO_PORT,LED_4_PIN,0,0,MNG_LED_CNTRL,0x0000,GPIO_VALUE,GPIO_TOGGLE,0x00},
+ {LED_5_R_GPIO_PORT,LED_5_R_PIN,&GPO_TIM2Handle,TIM_CHANNEL_1,AUX1_LED_CNTRL,AUX1_LED_COLOR,GPIO_VALUE_R,GPIO_TOGGLE_R,0x00},
+ {LED_5_G_GPIO_PORT,LED_5_G_PIN,&GPO_TIM2Handle,TIM_CHANNEL_2,AUX1_LED_CNTRL,AUX1_LED_COLOR,GPIO_VALUE_G,GPIO_TOGGLE_G,0x00},
+ {LED_5_B_GPIO_PORT,LED_5_B_PIN,&GPO_TIM2Handle,TIM_CHANNEL_3,AUX1_LED_CNTRL,AUX1_LED_COLOR,GPIO_VALUE_B,GPIO_TOGGLE_B,0x00},
+ {LED_6_R_GPIO_PORT,LED_6_R_PIN,&GPO_TIM3Handle,TIM_CHANNEL_1,AUX2_LED_CNTRL,AUX2_LED_COLOR,GPIO_VALUE_R,GPIO_TOGGLE_R,0x00},
+ {LED_6_G_GPIO_PORT,LED_6_G_PIN,&GPO_TIM3Handle,TIM_CHANNEL_2,AUX2_LED_CNTRL,AUX2_LED_COLOR,GPIO_VALUE_G,GPIO_TOGGLE_G,0x00},
+ {LED_6_B_GPIO_PORT,LED_6_B_PIN,&GPO_TIM3Handle,TIM_CHANNEL_3,AUX2_LED_CNTRL,AUX2_LED_COLOR,GPIO_VALUE_B,GPIO_TOGGLE_B,0x00}};
uint16_t leds_data[GPIO_NUM_LEDS];
// IRQ handler functions
@@ -98,7 +101,7 @@ void GPI_EXTI1_IRQHandler(void)
if(__HAL_GPIO_EXTI_GET_IT(START_PB_PIN) != RESET)
{
__HAL_GPIO_EXTI_CLEAR_IT(START_PB_PIN);
- if(ram_data[DARWIN_START_PB_CNTRL]&GPIO_INT_USED)
+ if(ram_data[START_PB_CNTRL]&GPIO_INT_USED)
{
if(start_pb_callback!=0)
start_pb_callback();
@@ -107,7 +110,7 @@ void GPI_EXTI1_IRQHandler(void)
if(__HAL_GPIO_EXTI_GET_IT(MODE_PB_PIN) != RESET)
{
__HAL_GPIO_EXTI_CLEAR_IT(MODE_PB_PIN);
- if(ram_data[DARWIN_MODE_PB_CNTRL]&GPIO_INT_USED)
+ if(ram_data[MODE_PB_CNTRL]&GPIO_INT_USED)
{
if(mode_pb_callback!=0)
mode_pb_callback();
@@ -397,8 +400,44 @@ void gpio_process_led(led_t led_id,uint16_t address,uint16_t length,uint8_t *dat
}
}
+void gpio_read_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ if(ram_in_range(START_PB_CNTRL,address,length))
+ if(!(ram_data[START_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
+ {
+ if(gpio_is_pushbutton_pressed(START_PB))
+ ram_data[START_PB_CNTRL]|=GPIO_VALUE;
+ else
+ ram_data[START_PB_CNTRL]&=(~GPIO_VALUE);
+ }
+ if(ram_in_range(MODE_PB_CNTRL,address,length))
+ if(!(ram_data[MODE_PB_CNTRL]&GPIO_INT_USED))/* GPIO is not internally used */
+ {
+ if(gpio_is_pushbutton_pressed(MODE_PB))
+ ram_data[MODE_PB_CNTRL]|=GPIO_VALUE;
+ else
+ ram_data[MODE_PB_CNTRL]&=(~GPIO_VALUE);
+ }
+}
+
+void gpio_write_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ /* process standard leds */
+ gpio_process_led(LED_TX,address,length,data);
+ gpio_process_led(LED_RX,address,length,data);
+ gpio_process_led(LED_2,address,length,data);
+ gpio_process_led(LED_3,address,length,data);
+ gpio_process_led(LED_4,address,length,data);
+ gpio_process_led(LED_5_R,address,length,data);
+ gpio_process_led(LED_5_G,address,length,data);
+ gpio_process_led(LED_5_B,address,length,data);
+ gpio_process_led(LED_6_R,address,length,data);
+ gpio_process_led(LED_6_G,address,length,data);
+ gpio_process_led(LED_6_B,address,length,data);
+}
+
// public functions
-void gpio_init(void)
+uint8_t gpio_init(TMemory *memory)
{
GPIO_InitTypeDef GPIO_InitStructure;
TIM_ClockConfigTypeDef sClockSourceConfig;
@@ -547,6 +586,17 @@ void gpio_init(void)
gpio_clear_led(LED_6_R);
gpio_clear_led(LED_6_G);
gpio_clear_led(LED_6_B);
+
+ /* initialize memory module */
+ mem_module_init(&gpio_mem_module);
+ gpio_mem_module.write_cmd=gpio_write_cmd;
+ gpio_mem_module.read_cmd=gpio_read_cmd;
+ if(!mem_module_add_ram_segment(&gpio_mem_module,RAM_GPIO_BASE_ADDRESS,RAM_GPIO_LENGTH))
+ return 0x00;
+ if(!mem_add_module(memory,&gpio_mem_module))
+ return 0x00;
+
+ return 0x01;
}
void gpio_set_led(led_t led_id)
@@ -658,44 +708,4 @@ void gpio_set_pushbutton_callback(pushbutton_t pb_id,void (*callback)(void))
}
}
-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)
-{
- /* process standard leds */
- gpio_process_led(LED_TX,address,length,data);
- gpio_process_led(LED_RX,address,length,data);
- gpio_process_led(LED_2,address,length,data);
- gpio_process_led(LED_3,address,length,data);
- gpio_process_led(LED_4,address,length,data);
- gpio_process_led(LED_5_R,address,length,data);
- gpio_process_led(LED_5_G,address,length,data);
- gpio_process_led(LED_5_B,address,length,data);
- gpio_process_led(LED_6_R,address,length,data);
- gpio_process_led(LED_6_G,address,length,data);
- gpio_process_led(LED_6_B,address,length,data);
-}
diff --git a/src/grippers.c b/src/grippers.c
index c5fae087946fe969081ef44b37459cbbd41ee25c..14ab402f9e7c8597bb42e27e0d09349dda066e7f 100644
--- a/src/grippers.c
+++ b/src/grippers.c
@@ -2,51 +2,57 @@
#include "dyn_servos.h"
#include "grippers.h"
#include "ram.h"
+#include "joint_motion.h"
// private variables
int64_t grippers_period;
int16_t grippers_period_us;
-uint8_t grippers_left_opened;
-uint8_t grippers_left_open;
-uint8_t grippers_left_close;
-uint8_t grippers_right_opened;
-uint8_t grippers_right_open;
-uint8_t grippers_right_close;
+uint8_t gripper_left_target;
+uint8_t gripper_right_target;
// public functions
void grippers_init(uint16_t period_us)
{
uint16_t force;
+ uint32_t servo_mask=0x00000000;
/* initialize private variables */
- grippers_left_opened=0x00;
- grippers_left_open=0x00;
- grippers_left_close=0x00;
- grippers_right_opened=0x00;
- grippers_right_open=0x00;
- grippers_right_close=0x00;
+ gripper_left_target=0x00;//close;
+ gripper_right_target=0x00;//close;
grippers_period=(period_us<<16)/1000000;
grippers_period_us=period_us;
/* configure the maximum force of the servos */
- if(ram_data[DARWIN_GRIPPER_LEFT_ID]!=0xFF)
+ servo_mask=0x00000000;
+ if(ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]!=0xFF && ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]!=0xFF)
{
force=ram_data[DARWIN_GRIPPER_LEFT_MAX_FORCE_L]+(ram_data[DARWIN_GRIPPER_LEFT_MAX_FORCE_H]<<8);
- dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_LEFT_ID],XL_TORQUE_LIMIT_L,force);
+ dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_LEFT_TOP_ID],XL_TORQUE_LIMIT_L,force);
+ dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_LEFT_BOT_ID],XL_TORQUE_LIMIT_L,force);
+ servo_mask|=1<<ram_data[DARWIN_GRIPPER_LEFT_TOP_ID];
+ servo_mask|=1<<ram_data[DARWIN_GRIPPER_LEFT_BOT_ID];
+ joint_motion_set_group_servos(1,servo_mask);
}
- if(ram_data[DARWIN_GRIPPER_RIGHT_ID]!=0xFF)
+ servo_mask=0x00000000;
+ if(ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]!=0xFF && ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]!=0xFF)
{
force=ram_data[DARWIN_GRIPPER_RIGHT_MAX_FORCE_L]+(ram_data[DARWIN_GRIPPER_RIGHT_MAX_FORCE_H]<<8);
- dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_RIGHT_ID],XL_TORQUE_LIMIT_L,force);
+ dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID],XL_TORQUE_LIMIT_L,force);
+ dyn_master_write_word(&darwin_dyn_master_v2,ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID],XL_TORQUE_LIMIT_L,force);
+ servo_mask|=1<<ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID];
+ servo_mask|=1<<ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID];
+ joint_motion_set_group_servos(2,servo_mask);
}
+ grippers_close(GRIPPER_LEFT);
+ grippers_close(GRIPPER_RIGHT);
}
-inline void grippers_set_period(uint16_t period_us)
+void grippers_set_period(uint16_t period_us)
{
grippers_period=(period_us<<16)/1000000;
grippers_period_us=period_us;
}
-inline uint16_t grippers_get_period(void)
+uint16_t grippers_get_period(void)
{
return grippers_period_us;
}
@@ -54,27 +60,101 @@ inline uint16_t grippers_get_period(void)
void grippers_open(grippers_t gripper)
{
if(gripper==GRIPPER_LEFT)
- grippers_left_open=0x01;
+ {
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L];
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H];
+ ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]]=60;
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L];
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H];
+ ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]]=60;
+ joint_motion_start(1);
+ }
else if(gripper==GRIPPER_RIGHT)
- grippers_right_open=0x01;
+ {
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L];
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H];
+ ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]]=60;
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L];
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H];
+ ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]]=60;
+ joint_motion_start(2);
+ }
}
void grippers_close(grippers_t gripper)
{
if(gripper==GRIPPER_LEFT)
- grippers_left_close=0x01;
+ {
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L];
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H];
+ ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_TOP_ID]]=60;
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L];
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H];
+ ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_LEFT_BOT_ID]]=60;
+ joint_motion_start(1);
+ }
else if(gripper==GRIPPER_RIGHT)
- grippers_right_close=0x01;
+ {
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L];
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H];
+ ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_TOP_ID]]=60;
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L];
+ ram_data[DARWIN_JOINT_SERVO0_ANGLE_L+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]*2+1]=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H];
+ ram_data[DARWIN_JOINT_SERVO0_SPEED+ram_data[DARWIN_GRIPPER_RIGHT_BOT_ID]]=60;
+ joint_motion_start(2);
+ }
+}
+
+void grippers_stop(grippers_t gripper)
+{
+ if(gripper==GRIPPER_LEFT)
+ joint_motion_stop(1);
+ else
+ joint_motion_stop(2);
}
uint8_t gripper_is_open(grippers_t gripper)
{
if(gripper==GRIPPER_LEFT)
- return grippers_left_opened;
- else if(gripper==GRIPPER_RIGHT)
- return grippers_right_opened;
+ {
+ if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_LEFT)
+ return 0x01;
+ else
+ return 0x00;
+ }
else
- return 0x00;
+ {
+ if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_RIGHT)
+ return 0x01;
+ else
+ return 0x00;
+ }
+}
+
+uint8_t gripper_is_close(grippers_t gripper)
+{
+ if(gripper==GRIPPER_LEFT)
+ {
+ if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_LEFT)
+ return 0x00;
+ else
+ return 0x01;
+ }
+ else
+ {
+ if(ram_data[DARWIN_GRIPPER_CNTRL]&GRIPPER_OPENED_RIGHT)
+ return 0x00;
+ else
+ return 0x01;
+ }
+}
+
+uint8_t gripper_is_moving(grippers_t gripper)
+{
+ if(gripper==GRIPPER_LEFT)
+ return are_joints_moving(1);
+ else
+ return are_joints_moving(2);
}
// operation functions
@@ -107,45 +187,32 @@ void grippers_process_write_cmd(unsigned short int address,unsigned short int le
}
}
-// motion manager interface functions
void grippers_process(void)
{
- uint16_t angle;
-
- if(ram_data[DARWIN_GRIPPER_LEFT_ID]!=0xFF)
+ if(!are_joints_moving(1))
+ {
+ ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_MOVING_LEFT;
+ if(gripper_left_target==0x01)
+ ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_OPENED_LEFT;
+ else
+ ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_LEFT;
+ }
+ else
{
- if(grippers_left_opened==0x01)
- {
- if(grippers_left_close==0x01)
- {
- grippers_left_close=0x00;
- angle=ram_data[DARWIN_GRIPPER_LEFT_MIN_ANGLE_L]+(ram_data[DARWIN_GRIPPER_LEFT_MIN_ANGLE_H]<<8);
- manager_current_angles[ram_data[DARWIN_GRIPPER_LEFT_ID]]=angle<<9;
- }
- else if(grippers_left_open==0x01)
- {
- grippers_left_open=0x00;
- angle=ram_data[DARWIN_GRIPPER_LEFT_MAX_ANGLE_L]+(ram_data[DARWIN_GRIPPER_LEFT_MAX_ANGLE_H]<<8);
- manager_current_angles[ram_data[DARWIN_GRIPPER_LEFT_ID]]=angle<<9;
- }
- }
+ ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_LEFT;
+ ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_MOVING_LEFT;
}
- if(ram_data[DARWIN_GRIPPER_RIGHT_ID]!=0xFF)
+ if(!are_joints_moving(2))
+ {
+ ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_MOVING_RIGHT;
+ if(gripper_right_target==0x01)
+ ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_OPENED_RIGHT;
+ else
+ ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_RIGHT;
+ }
+ else
{
- if(grippers_right_opened==0x01)
- {
- if(grippers_right_close==0x01)
- {
- grippers_right_close=0x00;
- angle=ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_L]+(ram_data[DARWIN_GRIPPER_RIGHT_MIN_ANGLE_H]<<8);
- manager_current_angles[ram_data[DARWIN_GRIPPER_RIGHT_ID]]=angle<<9;
- }
- else if(grippers_right_open==0x01)
- {
- grippers_right_open=0x00;
- angle=ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_L]+(ram_data[DARWIN_GRIPPER_RIGHT_MAX_ANGLE_H]<<8);
- manager_current_angles[ram_data[DARWIN_GRIPPER_RIGHT_ID]]=angle<<9;
- }
- }
+ ram_data[DARWIN_GRIPPER_CNTRL]&=~GRIPPER_OPENED_RIGHT;
+ ram_data[DARWIN_GRIPPER_CNTRL]|=GRIPPER_MOVING_RIGHT;
}
}
diff --git a/src/head_tracking.c b/src/head_tracking.c
index 6226a9ab912b05fe8fb866c7dbf9b322bee88d27..12ee27c4d4485534af863f89d724864df4104f16 100644
--- a/src/head_tracking.c
+++ b/src/head_tracking.c
@@ -55,13 +55,13 @@ void head_tracking_init(uint16_t period_us)
head_tracking_enabled=0x00;
}
-inline void head_tracking_set_period(uint16_t period_us)
+void head_tracking_set_period(uint16_t period_us)
{
head_tracking_period=(period_us<<16)/1000000;
head_tracking_period_us=period_us;
}
-inline uint16_t head_tracking_get_period(void)
+uint16_t head_tracking_get_period(void)
{
return head_tracking_period_us;
}
@@ -69,11 +69,13 @@ inline uint16_t head_tracking_get_period(void)
void head_tracking_start(void)
{
head_tracking_enabled=0x01;
+ ram_data[DARWIN_HEAD_CNTRL]|=HEAD_STATUS;
}
void head_tracking_stop(void)
{
head_tracking_enabled=0x00;
+ ram_data[DARWIN_HEAD_CNTRL]&=(~HEAD_STATUS);
}
uint8_t head_is_tracking(void)
diff --git a/src/imu.c b/src/imu.c
index 3d3f0478946591baac10d9cfa0017ce762f82be9..37d017caafb78c70fb17921a3d62ac645a3b1790 100755
--- a/src/imu.c
+++ b/src/imu.c
@@ -31,11 +31,11 @@
// accelerometer registers
#define ACCEL_ID 0x32
#define ACCEL_WHO_AM_I 0x0F
-#define ACCEL_CNTRL_REG1 0x20
-#define ACCEL_CNTRL_REG2 0x21
-#define ACCEL_CNTRL_REG3 0x22
-#define ACCEL_CNTRL_REG4 0x23
-#define ACCEL_CNTRL_REG5 0x24
+#define ACCEL_CNTRL_REG1 0x20
+#define ACCEL_CNTRL_REG2 0x21
+#define ACCEL_CNTRL_REG3 0x22
+#define ACCEL_CNTRL_REG4 0x23
+#define ACCEL_CNTRL_REG5 0x24
#define ACCEL_HP_FILTER_RESET 0x25
#define ACCEL_REFERENCE 0x26
#define ACCEL_STATUS_REG 0x27
@@ -58,11 +58,11 @@
// gyroscope registers
#define GYRO_ID 0xD3
#define GYRO_WHO_AM_I 0x0F
-#define GYRO_CNTRL_REG1 0x20
-#define GYRO_CNTRL_REG2 0x21
-#define GYRO_CNTRL_REG3 0x22
-#define GYRO_CNTRL_REG4 0x23
-#define GYRO_CNTRL_REG5 0x24
+#define GYRO_CNTRL_REG1 0x20
+#define GYRO_CNTRL_REG2 0x21
+#define GYRO_CNTRL_REG3 0x22
+#define GYRO_CNTRL_REG4 0x23
+#define GYRO_CNTRL_REG5 0x24
#define GYRO_REFERENCE 0x25
#define GYRO_OUT_TEMP 0x26
#define GYRO_STATUS_REG 0x27
@@ -72,7 +72,7 @@
#define GYRO_OUT_Y_H 0x2B
#define GYRO_OUT_Z_L 0x2C
#define GYRO_OUT_Z_H 0x2D
-#define GYRO_FIFO_CNTRL_REG 0x2E
+#define GYRO_FIFO_CNTRL_REG 0x2E
#define GYRO_FIFO_SRC_REG 0x2F
#define GYRO_INT1_CFG 0x30
#define GYRO_INT1_SRC 0x31
@@ -110,14 +110,17 @@ uint16_t gyro_calibration_num_samples;
const uint8_t accel_conf_reg=ACCEL_CNTRL_REG1;
const uint8_t accel_conf_len=5;
uint8_t accel_conf_data[5];
+int32_t accel_data[3];
const uint8_t accel_data_reg=ACCEL_OUT_X_L;
const uint8_t accel_data_len=6;
SPI_HandleTypeDef hspi2;
TIM_HandleTypeDef htim;
+TMemModule imu_mem_module;
+
// private functions
-inline void imu_wait_op_done(void)
+void imu_wait_op_done(void)
{
while(imu_op_state!=IMU_DONE);
}
@@ -207,12 +210,12 @@ uint8_t imu_accel_detect(void)
imu_spi_get_data(&data);
if(data==ACCEL_ID)
{
- ram_data[DARWIN_IMU_CNTRL]|=IMU_ACCEL_DET;
+ ram_data[IMU_CNTRL]|=IMU_ACCEL_DET;
return 0x01;
}
else
{
- ram_data[DARWIN_IMU_CNTRL]&=(~IMU_ACCEL_DET);
+ ram_data[IMU_CNTRL]&=(~IMU_ACCEL_DET);
return 0x00;
}
}
@@ -254,13 +257,12 @@ void imu_accel_get_data(void)
void imu_accel_convert_data(uint8_t *data_in,uint8_t *data_out)
{
uint8_t i;
- int16_t value;
for(i=0;i<3;i++)
{
- value=(data_in[i*2]+(data_in[i*2+1]<<8));
- data_out[i*2]=value%256;
- data_out[i*2+1]=value/256;
+ accel_data[i]=(int16_t)(data_in[i*2]+(data_in[i*2+1]<<8));
+ data_out[i*2]=accel_data[i]%256;
+ data_out[i*2+1]=accel_data[i]/256;
}
}
@@ -275,12 +277,12 @@ uint8_t imu_gyro_detect(void)
imu_spi_get_data(&data);
if(data==GYRO_ID)
{
- ram_data[DARWIN_IMU_CNTRL]|=IMU_GYRO_DET;
+ ram_data[IMU_CNTRL]|=IMU_GYRO_DET;
return 0x01;
}
else
{
- ram_data[DARWIN_IMU_CNTRL]&=(~IMU_GYRO_DET);
+ ram_data[IMU_CNTRL]&=(~IMU_GYRO_DET);
return 0x00;
}
}
@@ -331,6 +333,32 @@ void imu_gyro_convert_data(uint8_t *data_in,uint8_t *data_out)
}
}
+void imu_write_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ uint16_t num_samples;
+
+ if(ram_in_range(IMU_CNTRL,address,length))
+ {
+ if(data[IMU_CNTRL-address]&IMU_START)
+ imu_start();
+ else if(data[IMU_CNTRL-address]&IMU_STOP)
+ imu_stop();
+ else if(data[IMU_CNTRL-address]&IMU_START_CAL)
+ imu_start_gyro_cal();
+ }
+ ram_read_word(IMU_CAL_SAMPLES,&num_samples);
+ if(ram_in_range(IMU_CAL_SAMPLES,address,length))
+ num_samples=(num_samples&0xFF00)+data[IMU_CAL_SAMPLES-address];
+ if(ram_in_range(IMU_CAL_SAMPLES+1,address,length))
+ num_samples=(num_samples&0x00FF)+(data[IMU_CAL_SAMPLES+1-address]<<8);
+ imu_set_calibration_samples(num_samples);
+}
+
+void imu_read_cmd(void *module,unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ ram_read_table(address,length,data);
+}
+
// interrupt handlers
void IMU_SPI_IRQHANDLER(void)
{
@@ -390,10 +418,10 @@ void IMU_TIMER_IRQHandler(void)
{
switch(state)
{
- case IMU_GET_ACCEL: if(ram_data[DARWIN_IMU_CNTRL]&IMU_GYRO_DET)
+ case IMU_GET_ACCEL: if(ram_data[IMU_CNTRL]&IMU_GYRO_DET)
{
imu_spi_get_data(data);
- imu_gyro_convert_data(data,&ram_data[DARWIN_IMU_GYRO_X_L]);
+ imu_gyro_convert_data(data,&ram_data[IMU_GYRO_X]);
if(gyro_calibration)
{
gyro_accum[0]+=gyro_data[0];
@@ -409,23 +437,23 @@ void IMU_TIMER_IRQHandler(void)
gyro_accum[0]=0;
gyro_accum[1]=0;
gyro_accum[2]=0;
- ram_data[DARWIN_IMU_CNTRL]&=(~IMU_CALIBRATING);
- if((ram_data[DARWIN_IMU_CNTRL]&IMU_RUNNING)==0x00)
+ ram_data[IMU_CNTRL]&=(~IMU_CALIBRATING);
+ if((ram_data[IMU_CNTRL]&IMU_RUNNING)==0x00)
imu_stop_flag=0x01;
gyro_calibration=0x00;
}
}
}
- if(ram_data[DARWIN_IMU_CNTRL]&IMU_ACCEL_DET)
+ if(ram_data[IMU_CNTRL]&IMU_ACCEL_DET)
imu_accel_get_data();
state=IMU_GET_GYRO;
break;
- case IMU_GET_GYRO: if(ram_data[DARWIN_IMU_CNTRL]&IMU_ACCEL_DET)
+ case IMU_GET_GYRO: if(ram_data[IMU_CNTRL]&IMU_ACCEL_DET)
{
imu_spi_get_data(data);
- imu_accel_convert_data(data,&ram_data[DARWIN_IMU_ACCEL_X_L]);
+ imu_accel_convert_data(data,&ram_data[IMU_ACCEL_X]);
}
- if(ram_data[DARWIN_IMU_CNTRL]&IMU_GYRO_DET)
+ if(ram_data[IMU_CNTRL]&IMU_GYRO_DET)
imu_gyro_get_data();
state=IMU_GET_ACCEL;
break;
@@ -438,7 +466,7 @@ void IMU_TIMER_IRQHandler(void)
// public functions
-void imu_init(void)
+uint8_t imu_init(TMemory *memory)
{
GPIO_InitTypeDef GPIO_InitStructure;
uint8_t i;
@@ -534,11 +562,22 @@ void imu_init(void)
imu_accel_get_config();
imu_accel_set_config();
}
+
+ /* initialize memory module */
+ mem_module_init(&imu_mem_module);
+ imu_mem_module.write_cmd=imu_write_cmd;
+ imu_mem_module.read_cmd=imu_read_cmd;
+ if(!mem_module_add_ram_segment(&imu_mem_module,RAM_IMU_BASE_ADDRESS,RAM_IMU_LENGTH))
+ return 0x00;
+ if(!mem_add_module(memory,&imu_mem_module))
+ return 0x00;
+
+ return 0x01;
}
void imu_start(void)
{
- if((ram_data[DARWIN_IMU_CNTRL]&IMU_RUNNING)==0x00)// imu is not running
+ if((ram_data[IMU_CNTRL]&IMU_RUNNING)==0x00)// imu is not running
{
// start the accelerometer
imu_accel_start();
@@ -546,33 +585,33 @@ void imu_start(void)
// start the timer to get the imu data
HAL_TIM_Base_Start_IT(&htim);
- ram_data[DARWIN_IMU_CNTRL]|=IMU_RUNNING;
+ ram_data[IMU_CNTRL]|=IMU_RUNNING;
}
}
void imu_stop(void)
{
- if(ram_data[DARWIN_IMU_CNTRL]&IMU_RUNNING)
+ if(ram_data[IMU_CNTRL]&IMU_RUNNING)
{
imu_stop_flag=0x01;
while(!imu_stopped);
imu_stopped=0x00;
imu_accel_stop();
imu_gyro_stop();
- ram_data[DARWIN_IMU_CNTRL]&=(~IMU_RUNNING);
+ ram_data[IMU_CNTRL]&=(~IMU_RUNNING);
}
}
void imu_set_calibration_samples(uint16_t num_samples)
{
gyro_calibration_num_samples=num_samples;
- ram_data[DARWIN_IMU_CAL_SAMPLES_L]=gyro_calibration_num_samples%256;
- ram_data[DARWIN_IMU_CAL_SAMPLES_H]=gyro_calibration_num_samples/256;
+ ram_data[IMU_CAL_SAMPLES]=gyro_calibration_num_samples%256;
+ ram_data[IMU_CAL_SAMPLES+1]=gyro_calibration_num_samples/256;
}
void imu_start_gyro_cal(void)
{
- if((ram_data[DARWIN_IMU_CNTRL]&IMU_CALIBRATING)==0x00)// imu is not running
+ if((ram_data[IMU_CNTRL]&IMU_CALIBRATING)==0x00)// imu is not running
{
// start the accelerometer
imu_accel_start();
@@ -581,7 +620,7 @@ void imu_start_gyro_cal(void)
// start the timer to get the imu data
HAL_TIM_Base_Start_IT(&htim);
}
- ram_data[DARWIN_IMU_CNTRL]|=IMU_CALIBRATING;
+ ram_data[IMU_CNTRL]|=IMU_CALIBRATING;
gyro_center[0]=0;
gyro_center[1]=0;
gyro_center[2]=0;
@@ -590,7 +629,7 @@ void imu_start_gyro_cal(void)
uint8_t imu_is_gyro_calibrated(void)
{
- if(ram_data[DARWIN_IMU_CNTRL])
+ if(ram_data[IMU_CNTRL])
return 0x00;
else
return 0x01;
@@ -603,30 +642,10 @@ void imu_get_gyro_data(int32_t *x, int32_t *y, int32_t *z)
*z=gyro_data[2];
}
-// operation functions
-uint8_t imu_in_range(unsigned short int address,unsigned short int length)
+void imu_get_accel_data(int32_t *x, int32_t *y, int32_t *z)
{
- return ram_in_window(IMU_BASE_ADDRESS,IMU_MEM_LENGTH,address,length);
-}
-
-void imu_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
-{
- uint16_t num_samples;
-
- if(ram_in_range(DARWIN_IMU_CNTRL,address,length))
- {
- if(data[DARWIN_IMU_CNTRL-address]&IMU_START)
- imu_start();
- else if(data[DARWIN_IMU_CNTRL-address]&IMU_STOP)
- imu_stop();
- else if(data[DARWIN_IMU_CNTRL-address]&IMU_START_CAL)
- imu_start_gyro_cal();
- }
- ram_read_word(DARWIN_IMU_CAL_SAMPLES_L,&num_samples);
- if(ram_in_range(DARWIN_IMU_CAL_SAMPLES_L,address,length))
- num_samples=(num_samples&0xFF00)+data[DARWIN_IMU_CAL_SAMPLES_L-address];
- if(ram_in_range(DARWIN_IMU_CAL_SAMPLES_H,address,length))
- num_samples=(num_samples&0x00FF)+(data[DARWIN_IMU_CAL_SAMPLES_H-address]<<8);
- imu_set_calibration_samples(num_samples);
+ *x=accel_data[0];
+ *y=accel_data[1];
+ *z=accel_data[2];
}
diff --git a/src/joint_motion.c b/src/joint_motion.c
index 8e8147aa5f2568e9fbb920ffdccddc71433295b6..bd537d08b82d5557a02bbcbcc8535aae354557ba 100644
--- a/src/joint_motion.c
+++ b/src/joint_motion.c
@@ -6,6 +6,7 @@
int64_t joint_target_angles[MANAGER_MAX_NUM_SERVOS];//48|16
int64_t joint_target_speeds[MANAGER_MAX_NUM_SERVOS];//48|16
int64_t joint_target_accels[MANAGER_MAX_NUM_SERVOS];//48|16
+int64_t joint_target_stop_angles[MANAGER_MAX_NUM_SERVOS];//48|16
int64_t joint_current_angles[MANAGER_MAX_NUM_SERVOS];//48|16
int64_t joint_current_speeds[MANAGER_MAX_NUM_SERVOS];//48|16
@@ -47,13 +48,13 @@ void joint_motion_init(uint16_t period_us)
joint_motion_period_us=period_us;
}
-inline void joint_motion_set_period(uint16_t period_us)
+void joint_motion_set_period(uint16_t period_us)
{
joint_motion_period=(period_us<<16)/1000000;
joint_motion_period_us=period_us;
}
-inline uint16_t joint_motion_get_period(void)
+uint16_t joint_motion_get_period(void)
{
return joint_motion_period_us;
}
@@ -120,6 +121,8 @@ void joint_motion_start(uint8_t group_id)
ram_data[DARWIN_JOINT_SERVO0_ACCEL+i]=joint_target_accels[i]>>16;
}
}
+ // stop angles
+ joint_target_stop_angles[i]=joint_target_speeds[i]*joint_target_speeds[i]/(2*joint_target_accels[i]);
// the current angles, speeds and accelerations are in RAM
if(joint_target_angles[i]>=joint_current_angles[i])
joint_dir[i]=1;
@@ -129,6 +132,7 @@ void joint_motion_start(uint8_t group_id)
}
}
joint_moving[group_id]=0x01;
+ joint_stop[group_id]=0x00;
ram_data[DARWIN_JOINT_GRP0_CNTRL+group_id*5]|=JOINT_STATUS;
}
@@ -216,6 +220,7 @@ void joint_motion_process(void)
moving=0x01;
if(joint_current_speeds[i]!=joint_dir[i]*joint_target_speeds[i])// it is necessary to change the current speed
{
+ joint_current_angles[i]+=((joint_current_speeds[i]*joint_motion_period)>>17);
if(joint_current_speeds[i]>joint_dir[i]*joint_target_speeds[i])
{
joint_current_speeds[i]-=((joint_target_accels[i]*joint_motion_period)>>16);// reduce speed
@@ -228,7 +233,7 @@ void joint_motion_process(void)
if(joint_current_speeds[i]>joint_dir[i]*joint_target_speeds[i])
joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i];
}
- joint_current_angles[i]+=((joint_current_speeds[i]*joint_motion_period)>>16);
+ joint_current_angles[i]+=((joint_current_speeds[i]*joint_motion_period)>>17);
if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i])
joint_current_angles[i]=joint_target_angles[i];
if(joint_dir[i]==-1 && joint_current_angles[i]<joint_target_angles[i])
@@ -236,7 +241,7 @@ void joint_motion_process(void)
}
else
{
- if(abs(joint_target_angles[i]-joint_current_angles[i])>(joint_target_speeds[i]*joint_target_speeds[i]/(2*joint_target_accels[i])))// constant speed phase
+ if(abs(joint_target_angles[i]-joint_current_angles[i])>joint_target_stop_angles[i])// constant speed phase
{
joint_current_angles[i]+=((joint_current_speeds[i]*joint_motion_period)>>16);
if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i])
@@ -246,9 +251,12 @@ void joint_motion_process(void)
}
else// deceleration phase
{
+ joint_current_angles[i]=joint_current_angles[i]+((joint_current_speeds[i]*joint_motion_period)>>17);
joint_target_speeds[i]=joint_target_speeds[i]-((joint_target_accels[i]*joint_motion_period)>>16);
+ if(joint_target_speeds[i]<0)
+ joint_target_speeds[i]=0;
joint_current_speeds[i]=joint_dir[i]*joint_target_speeds[i];
- joint_current_angles[i]=joint_current_angles[i]+((joint_current_speeds[i]*joint_motion_period)>>16);
+ joint_current_angles[i]=joint_current_angles[i]+((joint_current_speeds[i]*joint_motion_period)>>17);
if(joint_dir[i]==1 && joint_current_angles[i]>joint_target_angles[i])
joint_current_angles[i]=joint_target_angles[i];
if(joint_dir[i]==-1 && joint_current_angles[i]<joint_target_angles[i])
@@ -256,6 +264,10 @@ void joint_motion_process(void)
}
}
}
+ else
+ {
+ joint_current_speeds[i]=0;
+ }
manager_current_angles[i]=joint_current_angles[i];
manager_current_slopes[i]=5;
}
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 4672002a904cf6b4af702a78645883c7176ac278..a5476c585680376ea5ced025d30328613062cc63 100755
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -11,6 +11,8 @@
#include "imu.h"
#include "smart_charger.h"
#include "dyn_battery.h"
+#include "stairs.h"
+#include <stdlib.h>
#define MANAGER_TIMER TIM5
#define ENABLE_MANAGER_TIMER_CLK __HAL_RCC_TIM5_CLK_ENABLE()
@@ -69,24 +71,24 @@ void manager_send_motion_command(void)
dyn_master_sync_write(&darwin_dyn_master_v2,num,servo_ids,XL_P_GAIN,3,write_data);
}
-inline uint16_t manager_angle_to_value(uint8_t servo_id,int16_t angle)
+uint16_t manager_angle_to_value(uint8_t servo_id,int16_t angle)
{
return ((angle+manager_servos[servo_id].center_angle)*manager_servos[servo_id].encoder_resolution)/manager_servos[servo_id].max_angle;
}
-inline int16_t manager_value_to_angle(uint8_t servo_id,uint16_t value)
+int16_t manager_value_to_angle(uint8_t servo_id,uint16_t value)
{
return (((int16_t)((value*manager_servos[servo_id].max_angle)/manager_servos[servo_id].encoder_resolution))-manager_servos[servo_id].center_angle);
}
-inline uint16_t manager_speed_to_value(uint8_t servo_id,uint16_t speed)
+uint16_t manager_speed_to_value(uint8_t servo_id,uint16_t speed)
{
if(speed>manager_servos[servo_id].max_speed)
speed=manager_servos[servo_id].max_speed;
return (speed*3)>>1;
}
-inline uint16_t manager_value_to_speed(uint8_t servo_id,uint16_t value)
+uint16_t manager_value_to_speed(uint8_t servo_id,uint16_t value)
{
return (value*2)/3;
}
@@ -131,6 +133,7 @@ void manager_balance(void)
{
uint32_t knee_gain,ankle_roll_gain,ankle_pitch_gain,hip_roll_gain;
int32_t gyro_x,gyro_y,gyro_z;
+ int32_t accel_x,accel_y,accel_z;
if(manager_balance_enabled==0x01)// balance is enabled
{
@@ -146,10 +149,30 @@ void manager_balance(void)
manager_balance_offset[R_ANKLE_PITCH]=((((int64_t)gyro_y*(int64_t)ankle_pitch_gain)/12000)>>9);
manager_balance_offset[L_KNEE]=((((int64_t)gyro_y*(int64_t)knee_gain)/12000)>>9);
manager_balance_offset[L_ANKLE_PITCH]=-((((int64_t)gyro_y*(int64_t)ankle_pitch_gain)/12000)>>9);
- manager_balance_offset[R_HIP_ROLL]=-((((int64_t)gyro_x*(int64_t)hip_roll_gain)/12000)>>9);
- manager_balance_offset[L_HIP_ROLL]=-((((int64_t)gyro_x*(int64_t)hip_roll_gain)/12000)>>9);
- manager_balance_offset[R_ANKLE_ROLL]=((((int64_t)gyro_x*(int64_t)ankle_roll_gain)/12000)>>9);
- manager_balance_offset[L_ANKLE_ROLL]=((((int64_t)gyro_x*(int64_t)ankle_roll_gain)/12000)>>9);
+ manager_balance_offset[R_HIP_ROLL]=((((int64_t)gyro_x*(int64_t)hip_roll_gain)/12000)>>9);
+ manager_balance_offset[L_HIP_ROLL]=((((int64_t)gyro_x*(int64_t)hip_roll_gain)/12000)>>9);
+ manager_balance_offset[R_ANKLE_ROLL]=-((((int64_t)gyro_x*(int64_t)ankle_roll_gain)/12000)>>9);
+ manager_balance_offset[L_ANKLE_ROLL]=-((((int64_t)gyro_x*(int64_t)ankle_roll_gain)/12000)>>9);
+ }
+ // fall detection (using accelerometer)
+ imu_get_accel_data(&accel_x,&accel_y,&accel_z);
+ if(abs(accel_y)>abs(accel_z))
+ {
+ if(accel_y>0)
+ {
+ ram_data[DARWIN_MM_CNTRL]&=~(MANAGER_FWD_FALL);
+ ram_data[DARWIN_MM_CNTRL]|=MANAGER_BWD_FALL;
+ }
+ else
+ {
+ ram_data[DARWIN_MM_CNTRL]&=~(MANAGER_BWD_FALL);
+ ram_data[DARWIN_MM_CNTRL]|=MANAGER_FWD_FALL;
+ }
+ }
+ else
+ {
+ ram_data[DARWIN_MM_CNTRL]&=~(MANAGER_FWD_FALL);
+ ram_data[DARWIN_MM_CNTRL]&=~(MANAGER_BWD_FALL);
}
}
@@ -173,8 +196,10 @@ void MANAGER_TIMER_IRQHandler(void)
head_tracking_process();
// call the walking process
walking_process();
- // call the grippers process
+ // call the gripper process
grippers_process();
+ // call the stairs process
+ stairs_process();
// balance the robot
manager_balance();
// access to smart charger
@@ -322,24 +347,25 @@ void manager_init(uint16_t period_us)
}
//Other devices detected - smart charger
- for(i=current; i<num; i++){
+ for(i=current; i<num; i++)
+ {
dyn_master_read_word(&darwin_dyn_master,servo_ids[i],BATTERY_MODEL_NUMBER_L,&model);
switch(model)
{
case DYN_BATTERY_MODEL: ram_data[DARWIN_SMART_CHARGER_CNTRL]=SMART_CHARGER_DET; //smart charger detected
- ram_data[DARWIN_SMART_CHARGER_ID] = servo_ids[i]; //smart charger ID
- smart_charger_set_version(&darwin_dyn_master); //set bus Dynamixel master version
- // Read smart charger's memory map
- //dyn_master_read_table(&darwin_dyn_master,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_CHARGER_STATUS,43,&ram_data[DARWIN_BATT_CHARGER_STATUS]);
- //Read charger status
- dyn_master_read_byte(&darwin_dyn_master,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_CHARGER_STATUS,&ram_data[DARWIN_SMART_CHARGER_STATUS]);
- break;
+ ram_data[DARWIN_SMART_CHARGER_ID] = servo_ids[i]; //smart charger ID
+ smart_charger_set_master(&darwin_dyn_master); //set bus Dynamixel master version
+ // Set smart charger's memory map
+ dyn_master_read_table(&darwin_dyn_master,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_AVG_TIME_EMPTY_L,6,&ram_data[DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L]);
+ dyn_master_write_table(&darwin_dyn_master,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_INPUT_MAX_CURRENT_L,2,&ram_data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L]);
+ break;
default: break;
- }
+ }
}
darwin_dyn_master_disable_power();
+ darwin_dyn_master_v2_enable_power();
// detect the servos connected on the v2 bus
dyn_master_scan(&darwin_dyn_master_v2,&num,servo_ids);
ram_data[DARWIN_MM_NUM_SERVOS]+=num;
@@ -384,22 +410,24 @@ void manager_init(uint16_t period_us)
}
//Other devices detected - smart charger
- for(i=current; i<num; i++){
+ for(i=current; i<num; i++)
+ {
dyn_master_read_word(&darwin_dyn_master_v2,servo_ids[i],BATTERY_MODEL_NUMBER_L,&model);
switch(model)
{
+ // gpio_set_led(LED_RX);
case DYN_BATTERY_MODEL: ram_data[DARWIN_SMART_CHARGER_CNTRL]=SMART_CHARGER_DET; //smart charger detected
- ram_data[DARWIN_SMART_CHARGER_ID] = servo_ids[i]; //smart charger ID
- smart_charger_set_version(&darwin_dyn_master_v2); //Set bus Dynamixel master version
- // Read smart charger's memory map
- //dyn_master_read_table(&darwin_dyn_master_v2,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_CHARGER_STATUS,43,&ram_data[DARWIN_BATT_CHARGER_STATUS]);
- //Read charger status
- dyn_master_read_byte(&darwin_dyn_master_v2,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_CHARGER_STATUS,&ram_data[DARWIN_SMART_CHARGER_STATUS]);
+ ram_data[DARWIN_SMART_CHARGER_ID] = servo_ids[i]; //smart charger ID
+ smart_charger_set_master(&darwin_dyn_master_v2); //Set bus Dynamixel master version
+ // Set smart charger's memory map
+ dyn_master_read_table(&darwin_dyn_master_v2,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_AVG_TIME_EMPTY_L,6,&ram_data[DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L]);
+ //dyn_master_write_table(&darwin_dyn_master_v2,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_INPUT_MAX_CURRENT_L,2,&ram_data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L]);
break;
default: break;
- }
+ }
}
-
+
+ darwin_dyn_master_v2_disable_power();
ram_data[DARWIN_MM_PRESENT_SERVOS1]=(present_servos&0x000000FF);
ram_data[DARWIN_MM_PRESENT_SERVOS2]=((present_servos&0x0000FF00)>>8);
@@ -445,6 +473,7 @@ void manager_init(uint16_t period_us)
head_tracking_init(period_us);
grippers_init(period_us);
smart_charger_init(period_us);
+ stairs_init(period_us);
}
uint16_t manager_get_period(void)
@@ -469,7 +498,7 @@ void manager_set_period(uint16_t period_us)
head_tracking_init(period_us);
}
-inline void manager_enable(void)
+void manager_enable(void)
{
TIM_OC_InitTypeDef TIM_OCInitStructure;
uint16_t capture;
@@ -484,13 +513,13 @@ inline void manager_enable(void)
ram_data[DARWIN_MM_CNTRL]|=MANAGER_ENABLE;
}
-inline void manager_disable(void)
+void manager_disable(void)
{
HAL_TIM_OC_Stop_IT(&MANAGER_TIM_Handle, TIM_CHANNEL_1);
ram_data[DARWIN_MM_CNTRL]&=(~MANAGER_ENABLE);
}
-inline uint8_t manager_is_enabled(void)
+uint8_t manager_is_enabled(void)
{
return ram_data[DARWIN_MM_CNTRL]&MANAGER_ENABLE;
}
@@ -507,7 +536,25 @@ void manager_disable_balance(void)
ram_data[DARWIN_MM_CNTRL]&=(~MANAGER_EN_BAL);
}
-inline uint8_t manager_get_num_servos(void)
+uint8_t manager_has_fallen(void)
+{
+ if(ram_data[DARWIN_MM_CNTRL]&(MANAGER_FWD_FALL|MANAGER_BWD_FALL))
+ return 0x01;
+ else
+ return 0x00;
+}
+
+TFall manager_get_fallen_position(void)
+{
+ if(ram_data[DARWIN_MM_CNTRL]&MANAGER_FWD_FALL)
+ return MM_FWD_FALL;
+ else if(ram_data[DARWIN_MM_CNTRL]&MANAGER_BWD_FALL)
+ return MM_BWD_FALL;
+ else
+ return MM_STANDING;
+}
+
+uint8_t manager_get_num_servos(void)
{
return manager_num_servos;
}
@@ -528,7 +575,7 @@ void manager_set_module(uint8_t servo_id,TModules module)
}
}
-inline TModules manager_get_module(uint8_t servo_id)
+TModules manager_get_module(uint8_t servo_id)
{
if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
return manager_servos[servo_id].module;
@@ -552,7 +599,7 @@ void manager_enable_servo(uint8_t servo_id)
}
}
-inline void manager_disable_servo(uint8_t servo_id)
+void manager_disable_servo(uint8_t servo_id)
{
uint8_t byte;
@@ -568,7 +615,7 @@ inline void manager_disable_servo(uint8_t servo_id)
}
}
-inline uint8_t manager_is_servo_enabled(uint8_t servo_id)
+uint8_t manager_is_servo_enabled(uint8_t servo_id)
{
if(servo_id>=0 && servo_id<MANAGER_MAX_NUM_SERVOS)
return manager_servos[servo_id].enabled;
@@ -585,12 +632,12 @@ void manager_set_offset(uint8_t servo_id, int8_t offset)
}
}
-inline int16_t manager_get_cw_angle_limit(uint8_t servo_id)
+int16_t manager_get_cw_angle_limit(uint8_t servo_id)
{
return manager_servos[servo_id].cw_angle_limit;
}
-inline int16_t manager_get_ccw_angle_limit(uint8_t servo_id)
+int16_t manager_get_ccw_angle_limit(uint8_t servo_id)
{
return manager_servos[servo_id].ccw_angle_limit;
}
@@ -650,6 +697,10 @@ void manager_process_write_cmd(unsigned short int address,unsigned short int len
darwin_dyn_master_enable_power();
else
darwin_dyn_master_disable_power();
+ if(data[DARWIN_MM_CNTRL-address]&MANAGER_EN_PWR_V2)
+ darwin_dyn_master_v2_enable_power();
+ else
+ darwin_dyn_master_v2_disable_power();
}
// balance gains
for(i=MM_BAL_KNEE_GAIN_OFFSET;i<=MM_BAL_HIP_ROLL_GAIN_OFFSET+1;i++)
diff --git a/src/ram.c b/src/ram.c
deleted file mode 100755
index f2c505dfb8b5e82c15e1311d9a27dad51bd60416..0000000000000000000000000000000000000000
--- a/src/ram.c
+++ /dev/null
@@ -1,255 +0,0 @@
-#include "ram.h"
-#include "eeprom.h"
-
-uint8_t ram_data[RAM_SIZE];
-
-void ram_init(void)
-{
- uint16_t eeprom_data,i;
-
- for(i=0;i<RAM_SIZE;i++)
- ram_data[i]=0x00;
- // read contents from EEPROM to RAM
- if(EE_ReadVariable(DEVICE_MODEL_OFFSET,&eeprom_data)==0)
- ram_data[DEVICE_MODEL_OFFSET]=(uint8_t)(eeprom_data&0x00FF);
- 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&0x00FF);
- if(EE_ReadVariable(DEVICE_ID_OFFSET,&eeprom_data)==0)
- 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&0x00FF);
- if(EE_ReadVariable(RETURN_DELAY_OFFSET,&eeprom_data)==0)
- 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&0x00FF);
- if(EE_ReadVariable(MM_PERIOD_OFFSET+1,&eeprom_data)==0)
- 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&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);
- if(EE_ReadVariable(HEAD_PAN_P,&eeprom_data)==0)
- ram_data[HEAD_PAN_P]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_PAN_P+1,&eeprom_data)==0)
- ram_data[HEAD_PAN_P+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_PAN_I,&eeprom_data)==0)
- ram_data[HEAD_PAN_I]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_PAN_I+1,&eeprom_data)==0)
- ram_data[HEAD_PAN_I+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_PAN_D,&eeprom_data)==0)
- ram_data[HEAD_PAN_D]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_PAN_D+1,&eeprom_data)==0)
- ram_data[HEAD_PAN_D+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_PAN_I_CLAMP,&eeprom_data)==0)
- ram_data[HEAD_PAN_I_CLAMP]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_PAN_I_CLAMP+1,&eeprom_data)==0)
- ram_data[HEAD_PAN_I_CLAMP+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_TILT_P,&eeprom_data)==0)
- ram_data[HEAD_TILT_P]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_TILT_P+1,&eeprom_data)==0)
- ram_data[HEAD_TILT_P+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_TILT_I,&eeprom_data)==0)
- ram_data[HEAD_TILT_I]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_TILT_I+1,&eeprom_data)==0)
- ram_data[HEAD_TILT_I+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_TILT_D,&eeprom_data)==0)
- ram_data[HEAD_TILT_D]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_TILT_D+1,&eeprom_data)==0)
- ram_data[HEAD_TILT_D+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_TILT_I_CLAMP,&eeprom_data)==0)
- ram_data[HEAD_TILT_I_CLAMP]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(HEAD_TILT_I_CLAMP+1,&eeprom_data)==0)
- ram_data[HEAD_TILT_I_CLAMP+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_LEFT_ID,&eeprom_data)==0)
- ram_data[GRIPPER_LEFT_ID]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_LEFT_MAX_ANGLE,&eeprom_data)==0)
- ram_data[GRIPPER_LEFT_MAX_ANGLE]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_LEFT_MAX_ANGLE+1,&eeprom_data)==0)
- ram_data[GRIPPER_LEFT_MAX_ANGLE+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_LEFT_MIN_ANGLE,&eeprom_data)==0)
- ram_data[GRIPPER_LEFT_MIN_ANGLE]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_LEFT_MIN_ANGLE+1,&eeprom_data)==0)
- ram_data[GRIPPER_LEFT_MIN_ANGLE+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_LEFT_MAX_FORCE,&eeprom_data)==0)
- ram_data[GRIPPER_LEFT_MAX_FORCE]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_LEFT_MAX_FORCE+1,&eeprom_data)==0)
- ram_data[GRIPPER_LEFT_MAX_FORCE+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_RIGHT_ID,&eeprom_data)==0)
- ram_data[GRIPPER_RIGHT_ID]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_RIGHT_MAX_ANGLE,&eeprom_data)==0)
- ram_data[GRIPPER_RIGHT_MAX_ANGLE]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_RIGHT_MAX_ANGLE+1,&eeprom_data)==0)
- ram_data[GRIPPER_RIGHT_MAX_ANGLE+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_RIGHT_MIN_ANGLE,&eeprom_data)==0)
- ram_data[GRIPPER_RIGHT_MIN_ANGLE]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_RIGHT_MIN_ANGLE+1,&eeprom_data)==0)
- ram_data[GRIPPER_RIGHT_MIN_ANGLE+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_RIGHT_MAX_FORCE,&eeprom_data)==0)
- ram_data[GRIPPER_RIGHT_MAX_FORCE]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(GRIPPER_RIGHT_MAX_FORCE+1,&eeprom_data)==0)
- ram_data[GRIPPER_RIGHT_MAX_FORCE+1]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(SMART_CHARGER_PERIOD,&eeprom_data)==0)
- ram_data[SMART_CHARGER_PERIOD]=(uint8_t)(eeprom_data&0x00FF);
- if(EE_ReadVariable(SMART_CHARGER_PERIOD+1,&eeprom_data)==0)
- ram_data[SMART_CHARGER_PERIOD+1]=(uint8_t)(eeprom_data&0x00FF);
-}
-
-inline void ram_read_byte(uint16_t address,uint8_t *data)
-{
- (*data)=ram_data[address];
-}
-
-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(uint16_t address, uint16_t length,uint8_t *data)
-{
- uint16_t i;
-
- if((address+length)<=(RAM_SIZE-1))
- {
- for(i=0;i<length;i++)
- data[i]=ram_data[address+i];
- return RAM_SUCCESS;
- }
- else
- return RAM_BAD_ADDRESS;
-}
-
-uint8_t ram_set_bit(uint16_t address, uint8_t bit)
-{
- if(bit>=0 && bit<8)
- {
- ram_data[address]|=(0x01<<bit);
- return RAM_SUCCESS;
- }
- else
- return RAM_BAD_BIT;
-}
-
-uint8_t ram_clear_bit(uint16_t address, uint8_t bit)
-{
- if(bit>=0 && bit<8)
- {
- ram_data[address]&=(~(0x01<<bit));
- return RAM_SUCCESS;
- }
- else
- return RAM_BAD_BIT;
-}
-
-uint8_t ram_write_byte(uint16_t address, uint8_t data)
-{
- ram_data[address]=data;
-
- return RAM_SUCCESS;
-}
-
-uint8_t ram_write_word(uint16_t address, uint16_t data)
-{
- if(address < (RAM_SIZE-1))
- {
- ram_data[address]=data%256;
- ram_data[address+1]=data/256;
-
- return RAM_SUCCESS;
- }
- else
- return RAM_BAD_ADDRESS;
-}
-
-uint8_t ram_write_table(uint16_t address, uint16_t length,uint8_t *data)
-{
- uint16_t i;
-
- if((address+length)<RAM_SIZE)
- {
- for(i=0;i<length;i++)
- ram_data[address+i]=data[i];
- return RAM_SUCCESS;
- }
- else
- return RAM_BAD_ADDRESS;
-}
-
-inline uint8_t ram_in_range(uint16_t reg,uint16_t address,uint16_t 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
deleted file mode 100644
index b8e0d999b713a1120c8a22453d93bec19e06bc0d..0000000000000000000000000000000000000000
--- a/src/scheduler.c
+++ /dev/null
@@ -1,58 +0,0 @@
-#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()
-
-typedef void (*scheduler_function)(void);
-
-scheduler_function sch_ch1_fnct;
-scheduler_function sch_ch2_fnct;
-scheduler_function sch_ch3_fnct;
-scheduler_function sch_ch4_fnct;
-uint16_t sch_ch1_period;
-uint16_t sch_ch2_period;
-uint16_t sch_ch3_period;
-uint16_t sch_ch4_period;
-
-void scheduler_init(void)
-{
- /* initialize internal variables */
- sch_ch1_fnct=0;
- sch_ch2_fnct=0;
- sch_ch3_fnct=0;
- sch_ch4_fnct=0;
- sch_ch1_period=0;
- sch_ch2_period=0;
- sch_ch3_period=0;
- sch_ch4_period=0;
- /* initialize timer 1 */
-
-}
-
-void scheduler_set_period(sch_ch_t channel_id, uint16_t period_ms)
-{
-
-}
-
-void scheduler_set_one_shot(sch_ch_t channel_id, uint16_t time_ms)
-{
-
-}
-
-void scheduler_set_function(sch_ch_t channel_id, void (*function)(void))
-{
-
-}
-
-void scheduler_start(sch_ch_t channel_id)
-{
-
-}
-
-void scheduler_stop(sch_ch_t channel_id)
-{
-
-}
-
diff --git a/src/smart_charger.c b/src/smart_charger.c
index 70de2e7d9e8861e467e41ffc048098053149e7f2..c31f13e41403bcd7c5abcd12018e235fb0916723 100644
--- a/src/smart_charger.c
+++ b/src/smart_charger.c
@@ -8,19 +8,11 @@
uint8_t smart_charger_id; //smart charger dynamixel ID
uint8_t smart_charger_enabled; //smart charger module enable signal
uint8_t smart_charger_detected; //smart charger detect signal
-uint16_t smart_charger_period; //smart charger period value
-uint16_t counter; //for read access operations
+uint16_t smart_charger_period; //smart charger period value in ms
uint16_t smart_charger_count; //counter = sc period / mm period --> Default: (1500ms*1000)/7800us
uint8_t smart_charger_write; //write smart charger signal
uint8_t init_regs[6]={0xFF,0xFF,0xFF,0xFF,0xFF,0xFF};
-#define fifosize 32
-unsigned char *infifo; //Pointer to fifo position where to store next input data
-unsigned char *outfifo; //Pointer to fifo position where is stored the data to be written
-unsigned char numdata; //Number of elements stored in the fifo
-unsigned char write_fifo[fifosize]; //Vector with data to write (data_write)
-unsigned char current[2];
-
//---------------------------------------
TDynamixelMaster *dyn_battery_master;
@@ -29,32 +21,28 @@ TDynamixelMaster *dyn_battery_master;
void smart_charger_init(uint16_t period_us)
{
smart_charger_id = ram_data[DARWIN_SMART_CHARGER_ID];
- smart_charger_enabled = 0x00;
+ smart_charger_enabled = ram_data[DARWIN_SMART_CHARGER_CNTRL]&SMART_CHARGER_EN;
smart_charger_detected = ram_data[DARWIN_SMART_CHARGER_CNTRL]&SMART_CHARGER_DET;
- counter = 0;
- smart_charger_period = ram_data[DARWIN_SMART_CHARGER_PERIOD_L];
- smart_charger_period = (ram_data[DARWIN_SMART_CHARGER_PERIOD_H])<<8;
+ smart_charger_period = ram_data[DARWIN_SMART_CHARGER_PERIOD_L] + (ram_data[DARWIN_SMART_CHARGER_PERIOD_H]<<8);
smart_charger_count = (smart_charger_period*1000)/period_us;
smart_charger_write = 0x00;
- infifo=outfifo=&write_fifo[0];
- numdata=0; //fifo empty
ram_write_table(DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L, 6, init_regs);
}
-inline void smart_charger_set_version(TDynamixelMaster *master)
+void smart_charger_set_master(TDynamixelMaster *master)
{
dyn_battery_master=master;
}
-inline TDynamixelMaster* smart_charger_get_version(void)
+TDynamixelMaster* smart_charger_get_master(void)
{
return dyn_battery_master;
}
-void smart_charger_set_period(uint16_t period) //en ms
+void smart_charger_set_period(uint16_t period)
{
- uint16_t mm_period=manager_get_period_us(); //default: 7800us
- smart_charger_count = (period*1000)/mm_period; //(ms*1000)/us
+ uint16_t mm_period=manager_get_period_us();
+ smart_charger_count = (period*1000)/mm_period;
smart_charger_period = period;
ram_data[DARWIN_SMART_CHARGER_PERIOD_L]=period&0x00FF;
ram_data[DARWIN_SMART_CHARGER_PERIOD_H]=(period&0xFF00)>>8;
@@ -94,8 +82,8 @@ void smart_charger_process_read_cmd(unsigned short int address,unsigned short in
void smart_charger_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
{
- //uint8_t i;
- uint16_t period,i;
+ uint16_t period;
+
//Enable/Disable smart charger
if(ram_in_range(DARWIN_SMART_CHARGER_CNTRL,address,length))
@@ -115,28 +103,8 @@ void smart_charger_process_write_cmd(unsigned short int address,unsigned short i
//Write Battery limit current
if(ram_in_range(DARWIN_SMART_CHARGER_LIMIT_CURRENT_L,address,length) && ram_in_range(DARWIN_SMART_CHARGER_LIMIT_CURRENT_H,address,length))
{
- current[0]=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L-address];
- current[1]=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_H-address];
-
-
- /* if(infifo==&write_fifo[fifosize]);
- infifo=&write_fifo[0]; //go to first position of fifo
- if(numdata<fifosize) //free space in fifo
- {
- for(i=DARWIN_SMART_CHARGER_LIMIT_CURRENT_L;i<=DARWIN_SMART_CHARGER_LIMIT_CURRENT_H;i++)
- {
- *infifo=data[i-address];
- infifo++;
- numdata++;
- }
- */
- /* *infifo=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L-address]; //copy first byte to fifo
- numdata++;
- infifo++;
- *infifo=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_H-address]; //copy second byte to fifo
- numdata++;
- infifo++; */
- // }
+ ram_data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L]=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L-address];
+ ram_data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_H]=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_H-address];
smart_charger_write = 0x01;
}
}
@@ -144,37 +112,28 @@ void smart_charger_process_write_cmd(unsigned short int address,unsigned short i
// motion manager interface function
void smart_charger_process(void)
-{
- gpio_set_led(LED_TX);//amarillo
- counter++;
+{
+ //gpio_set_led(LED_3);
uint8_t error;
+ static uint16_t counter=0;
+
+ counter++;
+
//Write smart_charger - Battery limit current (EEPROM)
if(smart_charger_detected && smart_charger_enabled && smart_charger_write && counter!=smart_charger_count)
{
- error=dyn_master_write_table(dyn_battery_master,smart_charger_id,BATTERY_INPUT_MAX_CURRENT_L,2, current);
- if(error==DYN_SUCCESS){
-/* dyn_master_write_table(dyn_battery_master,smart_charger_id,BATTERY_INPUT_MAX_CURRENT_L,2,outfifo);
- if(outfifo==&write_fifo[fifosize-1])
- outfifo=&write_fifo[0]; //go to first position of fifo
- else
- outfifo=outfifo+2;
- numdata=numdata-2;
- if(numdata==0) //empty fifo
- */
+ error=dyn_master_write_table(dyn_battery_master,smart_charger_id,BATTERY_INPUT_MAX_CURRENT_L,2,&ram_data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L]);
+ if(error==DYN_SUCCESS)
smart_charger_write = 0x00;
- }
}
//Read smart charger - time to empty, time to full and battery_status
if(smart_charger_detected && smart_charger_enabled && counter==smart_charger_count)
{
-
error=dyn_master_read_table(dyn_battery_master,smart_charger_id,BATTERY_AVG_TIME_EMPTY_L,6,&ram_data[DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L]);
if(error!=DYN_SUCCESS){
+ // gpio_set_led(LED_TX);
ram_write_table(DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L, 6, init_regs);
- gpio_set_led(LED_RX); //naranja
- }else{
- gpio_set_led(LED_3);//verde
}
counter = 0;
}
diff --git a/src/stairs.c b/src/stairs.c
new file mode 100755
index 0000000000000000000000000000000000000000..42555871998c5a3e25655b333e53ae049db7e11f
--- /dev/null
+++ b/src/stairs.c
@@ -0,0 +1,580 @@
+#include "stairs.h"
+#include "darwin_kinematics.h"
+#include "darwin_math.h"
+#include "ram.h"
+#include <math.h>
+
+typedef enum {SHIFT_WEIGHT_LEFT=0x10,RISE_RIGHT_FOOT=0x20,ADVANCE_RIGHT_FOOT=0x30,CONTACT_RIGHT_FOOT=0x40,SHIFT_WEIGHT_RIGHT=0x50,RISE_LEFT_FOOT=0x60,ADVANCE_LEFT_FOOT=0x70,CONTACT_LEFT_FOOT=0x80,CENTER_WEIGHT=0x90} stairs_phase_t;
+
+// internal motion variables
+float stairs_Z_stair_height;
+float stairs_Z_overshoot;
+float stairs_Y_shift_amplitude;
+float stairs_X_shift_amplitude;
+float stairs_R_shift_amplitude;
+float stairs_P_shift_amplitude;
+float stairs_A_shift_amplitude;
+float stairs_Y_spread_amplitude;
+float stairs_X_shift_body;
+
+// internal offset variables
+float stairs_Hip_Pitch_Offset;
+float stairs_X_Offset;
+float stairs_Y_Offset;
+float stairs_Z_Offset;
+float stairs_R_Offset;
+float stairs_P_Offset;
+float stairs_A_Offset;
+
+// internal time variables
+float stairs_Time;
+float stairs_period;
+float stairs_shift_weight_left_time;
+float stairs_rise_right_foot_time;
+float stairs_advance_right_foot_time;
+float stairs_contact_right_foot_time;
+float stairs_shift_weight_right_time;
+float stairs_rise_left_foot_time;
+float stairs_advance_left_foot_time;
+float stairs_contact_left_foot_time;
+float stairs_center_weight_time;
+
+// control variables
+uint8_t stairs_Ctrl_Running;
+uint8_t stairs_up;
+
+// private functions
+
+// public functions
+void stairs_init(uint16_t period_us)
+{
+ // initialize the internal motion variables
+
+
+ // initialize internal control variables
+ stairs_Time=0;
+ stairs_period=((float)period_us)/1000.0;
+ stairs_Ctrl_Running=0x00;
+ stairs_up=0x00;
+}
+
+void stairs_set_period(uint16_t period_us)
+{
+ stairs_period=((float)period_us)/1000.0;
+}
+
+uint16_t stairs_get_period(void)
+{
+ return (uint16_t)(stairs_period*1000);
+}
+
+void stairs_start(uint8_t up)
+{
+ // load all parameters
+ stairs_shift_weight_left_time=((float)ram_data[DARWIN_STAIRS_PHASE1_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE1_TIME_H]<<8));
+ stairs_rise_right_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE2_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE2_TIME_H]<<8));
+ stairs_advance_right_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE3_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE3_TIME_H]<<8));
+ stairs_contact_right_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE4_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE4_TIME_H]<<8));
+ stairs_shift_weight_right_time=((float)ram_data[DARWIN_STAIRS_PHASE5_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE5_TIME_H]<<8));
+ stairs_rise_left_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE6_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE6_TIME_H]<<8));
+ stairs_advance_left_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE7_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE7_TIME_H]<<8));
+ stairs_contact_left_foot_time=((float)ram_data[DARWIN_STAIRS_PHASE8_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE8_TIME_H]<<8));
+ stairs_center_weight_time=((float)ram_data[DARWIN_STAIRS_PHASE9_TIME_L]+(ram_data[DARWIN_STAIRS_PHASE9_TIME_H]<<8));
+ stairs_Z_stair_height=((float)((int8_t)ram_data[DARWIN_STAIRS_Z_HEIGHT]));
+ stairs_Z_overshoot=((float)((int8_t)ram_data[DARWIN_STAIRS_Z_OVERSHOOT]));
+ stairs_Y_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_SHIFT]));
+ stairs_X_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_X_SHIFT]));
+ stairs_Hip_Pitch_Offset=((float)((int16_t)(ram_data[DARWIN_WALK_HIP_PITCH_OFF_L]+(ram_data[DARWIN_WALK_HIP_PITCH_OFF_H]<<8))))/1024.0;
+ stairs_R_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_R_SHIFT]))*PI/720.0;// (r_shift/4)*(pi/180)
+ stairs_P_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_P_SHIFT]))*PI/720.0;// (p_shift/4)*(pi/180)
+ stairs_A_shift_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_A_SHIFT]))*PI/720.0;// (a_shift/4)*(pi/180)
+ stairs_Y_spread_amplitude=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_SPREAD]));
+ stairs_X_shift_body=((float)((int8_t)ram_data[DARWIN_STAIRS_X_SHIFT_BODY]));
+ stairs_X_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_X_OFFSET]));
+ stairs_Y_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_OFFSET]));
+ stairs_Z_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_Z_OFFSET]));
+ stairs_R_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_X_OFFSET]))*PI/1440.0;
+ stairs_P_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_Y_OFFSET]))*PI/1440.0;
+ stairs_A_Offset=((float)((int8_t)ram_data[DARWIN_STAIRS_Z_OFFSET]))*PI/1440.0;
+ // start operation
+ ram_data[DARWIN_STAIRS_CNTRL]|=STAIRS_STATUS;
+ stairs_Ctrl_Running=0x01;
+ stairs_up=up;
+}
+
+void stairs_stop(void)
+{
+ stairs_Ctrl_Running=0x00;
+}
+
+uint8_t is_climbing_stairs(void)
+{
+ if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS)
+ return 0x01;
+ else
+ return 0x00;
+}
+
+uint8_t stairs_get_phase(void)
+{
+ return (int8_t)ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_PHASE;
+}
+
+// motion manager interface functions
+void stairs_process(void)
+{
+ float angle[14]={0},ep[12]={0};
+ float delta;
+
+ if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS)
+ {
+ ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_PHASE);
+ if(stairs_up)
+ {
+ if(stairs_Time>=0 && stairs_Time<=stairs_shift_weight_left_time)
+ {
+ //1
+ delta=stairs_Time/stairs_shift_weight_left_time;
+ ep[0]=stairs_X_Offset;
+ ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta;
+ ep[2]=stairs_Z_Offset;
+ ep[3]=stairs_R_Offset;
+ ep[4]=stairs_P_Offset;
+ ep[5]=stairs_A_Offset;
+ ep[6]=stairs_X_Offset;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta;
+ ep[8]=stairs_Z_Offset;
+ ep[9]=stairs_R_Offset;
+ ep[10]=stairs_P_Offset;
+ ep[11]=stairs_A_Offset;
+ ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_LEFT;
+ }
+ else if(stairs_Time>stairs_shift_weight_left_time && stairs_Time<=stairs_rise_right_foot_time)
+ {
+ //2
+ delta=(stairs_Time-stairs_shift_weight_left_time)/(stairs_rise_right_foot_time-stairs_shift_weight_left_time);
+ ep[0]=stairs_X_Offset;
+ ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude;
+ ep[2]=stairs_Z_Offset;
+ ep[3]=stairs_R_Offset-stairs_R_shift_amplitude*delta;
+ ep[4]=stairs_P_Offset;
+ ep[5]=stairs_A_Offset;
+ ep[6]=stairs_X_Offset;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude;
+ ep[8]=stairs_Z_Offset+(stairs_Z_overshoot+stairs_Z_stair_height)*delta;
+ ep[9]=stairs_R_Offset-stairs_R_shift_amplitude*delta;
+ ep[10]=stairs_P_Offset;
+ ep[11]=stairs_A_Offset;
+ ram_data[DARWIN_STAIRS_CNTRL]|=RISE_RIGHT_FOOT;
+ }
+ else if(stairs_Time>stairs_rise_right_foot_time && stairs_Time<=stairs_advance_right_foot_time)
+ {
+ //3
+ delta=(stairs_Time-stairs_rise_right_foot_time)/(stairs_advance_right_foot_time-stairs_rise_right_foot_time);
+ ep[0]=stairs_X_Offset;
+ ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)*delta;
+ ep[2]=stairs_Z_Offset;
+ ep[3]=stairs_R_Offset-stairs_R_shift_amplitude;
+ ep[4]=stairs_P_Offset+stairs_P_shift_amplitude*delta;
+ ep[5]=stairs_A_Offset+stairs_A_shift_amplitude*delta;
+ ep[6]=stairs_X_Offset+stairs_X_shift_amplitude*delta;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)*delta;
+ ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height;
+ ep[9]=stairs_R_Offset-stairs_R_shift_amplitude;
+ ep[10]=stairs_P_Offset+stairs_P_shift_amplitude*delta;
+ ep[11]=stairs_A_Offset+stairs_A_shift_amplitude*delta;
+ ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_RIGHT_FOOT;
+ }
+ else if(stairs_Time>stairs_advance_right_foot_time && stairs_Time<=stairs_contact_right_foot_time)
+ {
+ //4
+ delta=(stairs_Time-stairs_advance_right_foot_time)/(stairs_contact_right_foot_time-stairs_advance_right_foot_time);
+ ep[0]=stairs_X_Offset;
+ ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0);
+ ep[2]=stairs_Z_Offset;
+ ep[3]=stairs_R_Offset-stairs_R_shift_amplitude;
+ ep[4]=stairs_P_Offset+stairs_P_shift_amplitude;
+ ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ep[6]=stairs_X_Offset+stairs_X_shift_amplitude;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0);
+ ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_overshoot*delta;
+ ep[9]=stairs_R_Offset-stairs_R_shift_amplitude;
+ ep[10]=stairs_P_Offset+stairs_P_shift_amplitude;
+ ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_RIGHT_FOOT;
+ }
+ else if(stairs_Time>stairs_contact_right_foot_time && stairs_Time<=stairs_shift_weight_right_time)
+ {
+ //5
+ delta=(stairs_Time-stairs_contact_right_foot_time)/(stairs_shift_weight_right_time-stairs_contact_right_foot_time);
+ ep[0]=stairs_X_Offset-stairs_X_shift_body*delta;
+ ep[1]=-stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta;
+ ep[2]=stairs_Z_Offset;
+ ep[3]=stairs_R_Offset-stairs_R_shift_amplitude+2.0*stairs_R_shift_amplitude*delta;
+ ep[4]=stairs_P_Offset+stairs_P_shift_amplitude-3.0*stairs_P_shift_amplitude*delta;
+ ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body*delta;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta;
+ ep[8]=stairs_Z_Offset+stairs_Z_stair_height;
+ ep[9]=stairs_R_Offset-stairs_R_shift_amplitude+2.0*stairs_R_shift_amplitude*delta;
+ ep[10]=stairs_P_Offset+stairs_P_shift_amplitude-3.0*stairs_P_shift_amplitude*delta;
+ ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_RIGHT;
+ }
+ else if(stairs_Time>stairs_shift_weight_right_time && stairs_Time<=stairs_rise_left_foot_time)
+ {
+ //6
+ delta=(stairs_Time-stairs_shift_weight_right_time)/(stairs_rise_left_foot_time-stairs_shift_weight_right_time);
+ ep[0]=stairs_X_Offset-stairs_X_shift_body;
+ ep[1]=-stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0);
+ ep[2]=stairs_Z_Offset+(stairs_Z_overshoot+stairs_Z_stair_height)*delta;
+ ep[3]=stairs_R_Offset+stairs_R_shift_amplitude;
+ ep[4]=stairs_P_Offset-2.0*stairs_P_shift_amplitude;
+ ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body;
+ ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0);
+ ep[8]=stairs_Z_Offset+stairs_Z_stair_height;
+ ep[9]=stairs_R_Offset+stairs_R_shift_amplitude;
+ ep[10]=stairs_P_Offset-2.0*stairs_P_shift_amplitude;
+ ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ram_data[DARWIN_STAIRS_CNTRL]|=RISE_LEFT_FOOT;
+ }
+ else if(stairs_Time>stairs_rise_left_foot_time && stairs_Time<=stairs_advance_left_foot_time)
+ {
+ //7
+ delta=(stairs_Time-stairs_rise_left_foot_time)/(stairs_advance_left_foot_time-stairs_rise_left_foot_time);
+ ep[0]=stairs_X_Offset-stairs_X_shift_body+stairs_X_shift_body*delta;
+ ep[1]=-stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)+(stairs_Y_spread_amplitude/2.0)*delta;
+ ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height;
+ ep[3]=stairs_R_Offset+stairs_R_shift_amplitude;
+ ep[4]=stairs_P_Offset-2.0*stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta;
+ ep[5]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta;
+ ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body-(stairs_X_shift_amplitude-stairs_X_shift_body)*delta;
+ ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(stairs_Y_spread_amplitude/2.0)*delta;
+ ep[8]=stairs_Z_Offset+stairs_Z_stair_height;
+ ep[9]=stairs_R_Offset+stairs_R_shift_amplitude;
+ ep[10]=stairs_P_Offset-2.0*stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta;
+ ep[11]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta;
+ ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_LEFT_FOOT;
+ }
+ else if(stairs_Time>stairs_advance_left_foot_time && stairs_Time<=stairs_contact_left_foot_time)
+ {
+ //8
+ delta=(stairs_Time-stairs_advance_left_foot_time)/(stairs_contact_left_foot_time-stairs_advance_left_foot_time);
+ ep[0]=stairs_X_Offset;
+ ep[1]=-stairs_Y_Offset/2.0-stairs_Y_shift_amplitude;
+ ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_overshoot*delta;
+ ep[3]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
+ ep[4]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta;
+ ep[5]=stairs_A_Offset;
+ ep[6]=stairs_X_Offset;
+ ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude;
+ ep[8]=stairs_Z_Offset+stairs_Z_stair_height;
+ ep[9]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
+ ep[10]=stairs_P_Offset-stairs_P_shift_amplitude+stairs_P_shift_amplitude*delta;
+ ep[11]=stairs_A_Offset;
+ ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_LEFT_FOOT;
+ }
+ else if(stairs_Time>stairs_contact_left_foot_time && stairs_Time<=stairs_center_weight_time)
+ {
+ //9
+ delta=(stairs_Time-stairs_contact_left_foot_time)/(stairs_center_weight_time-stairs_contact_left_foot_time);
+ ep[0]=stairs_X_Offset;
+ ep[1]=-stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta;
+ ep[2]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height*delta;
+ ep[3]=stairs_R_Offset;
+ ep[4]=stairs_P_Offset;
+ ep[5]=stairs_A_Offset;
+ ep[6]=stairs_X_Offset;
+ ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta;
+ ep[8]=stairs_Z_Offset+stairs_Z_stair_height-stairs_Z_stair_height*delta;
+ ep[9]=stairs_R_Offset;
+ ep[10]=stairs_P_Offset;
+ ep[11]=stairs_A_Offset;
+ ram_data[DARWIN_STAIRS_CNTRL]|=CENTER_WEIGHT;
+ }
+ else
+ {
+ ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_STATUS);
+ stairs_Ctrl_Running=0x00;
+ }
+ }
+ else
+ {
+ if(stairs_Time>=0 && stairs_Time<=stairs_shift_weight_left_time)
+ {
+ //1
+ delta=stairs_Time/stairs_shift_weight_left_time;
+ ep[0]=stairs_X_Offset;
+ ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta;
+ ep[2]=stairs_Z_Offset+stairs_Z_stair_height*delta;
+ ep[3]=stairs_R_Offset;
+ ep[4]=stairs_P_Offset;
+ ep[5]=stairs_A_Offset;
+ ep[6]=stairs_X_Offset;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude*delta;
+ ep[8]=stairs_Z_Offset+stairs_Z_stair_height*delta;
+ ep[9]=stairs_R_Offset;
+ ep[10]=stairs_P_Offset;
+ ep[11]=stairs_A_Offset;
+ ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_LEFT;
+ }
+ else if(stairs_Time>stairs_shift_weight_left_time && stairs_Time<=stairs_rise_right_foot_time)
+ {
+ //2
+ delta=(stairs_Time-stairs_shift_weight_left_time)/(stairs_rise_right_foot_time-stairs_shift_weight_left_time);
+ ep[0]=stairs_X_Offset;
+ ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude;
+ ep[2]=stairs_Z_Offset+stairs_Z_stair_height;
+ ep[3]=stairs_R_Offset-stairs_R_shift_amplitude;
+ ep[4]=stairs_P_Offset;
+ ep[5]=stairs_A_Offset;
+ ep[6]=stairs_X_Offset;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude;
+ ep[8]=stairs_Z_Offset+stairs_Z_stair_height+stairs_Z_overshoot*delta;
+ ep[9]=stairs_R_Offset-stairs_R_shift_amplitude*delta;
+ ep[10]=stairs_P_Offset;
+ ep[11]=stairs_A_Offset;
+ ram_data[DARWIN_STAIRS_CNTRL]|=RISE_RIGHT_FOOT;
+ }
+ else if(stairs_Time>stairs_rise_right_foot_time && stairs_Time<=stairs_advance_right_foot_time)
+ {
+ //3
+ delta=(stairs_Time-stairs_rise_right_foot_time)/(stairs_advance_right_foot_time-stairs_rise_right_foot_time);
+ ep[0]=stairs_X_Offset;
+ ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)*delta;
+ ep[2]=stairs_Z_Offset+stairs_Z_stair_height;
+ ep[3]=stairs_R_Offset-stairs_R_shift_amplitude;
+ ep[4]=stairs_P_Offset+stairs_P_shift_amplitude*delta;
+ ep[5]=stairs_A_Offset+stairs_A_shift_amplitude*delta;
+ ep[6]=stairs_X_Offset+stairs_X_shift_amplitude*delta;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)*delta;
+ ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height;
+ ep[9]=stairs_R_Offset-stairs_R_shift_amplitude;
+ ep[10]=stairs_P_Offset+stairs_P_shift_amplitude*delta;
+ ep[11]=stairs_A_Offset+stairs_A_shift_amplitude*delta;
+ ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_RIGHT_FOOT;
+ }
+ else if(stairs_Time>stairs_advance_right_foot_time && stairs_Time<=stairs_contact_right_foot_time)
+ {
+ //4
+ delta=(stairs_Time-stairs_advance_right_foot_time)/(stairs_contact_right_foot_time-stairs_advance_right_foot_time);
+ ep[0]=stairs_X_Offset;
+ ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0);
+ ep[2]=stairs_Z_Offset+stairs_Z_stair_height+stairs_Z_overshoot*delta;
+ ep[3]=stairs_R_Offset-stairs_R_shift_amplitude;
+ ep[4]=stairs_P_Offset+stairs_P_shift_amplitude;
+ ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ep[6]=stairs_X_Offset+stairs_X_shift_amplitude;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0);
+ ep[8]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-stairs_Z_stair_height*delta;
+ ep[9]=stairs_R_Offset-stairs_R_shift_amplitude;
+ ep[10]=stairs_P_Offset+stairs_P_shift_amplitude;
+ ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_RIGHT_FOOT;
+ }
+ else if(stairs_Time>stairs_contact_right_foot_time && stairs_Time<=stairs_shift_weight_right_time)
+ {
+ //5
+ delta=(stairs_Time-stairs_contact_right_foot_time)/(stairs_shift_weight_right_time-stairs_contact_right_foot_time);
+ ep[0]=stairs_X_Offset-stairs_X_shift_body*delta;
+ ep[1]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta;
+ ep[2]=stairs_Z_Offset+stairs_Z_stair_height+stairs_Z_overshoot;
+ ep[3]=stairs_R_Offset-stairs_R_shift_amplitude+2.0*stairs_R_shift_amplitude*delta;
+ ep[4]=stairs_P_Offset+stairs_P_shift_amplitude-(2.0*stairs_P_shift_amplitude)*delta;
+ ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body*delta;
+ ep[7]=stairs_Y_Offset/2.0+stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(2*stairs_Y_shift_amplitude)*delta;
+ ep[8]=stairs_Z_Offset+stairs_Z_overshoot;
+ ep[9]=stairs_R_Offset-stairs_R_shift_amplitude+2.0*stairs_R_shift_amplitude*delta;
+ ep[10]=stairs_P_Offset+stairs_P_shift_amplitude-(2.0*stairs_P_shift_amplitude)*delta;
+ ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ram_data[DARWIN_STAIRS_CNTRL]|=SHIFT_WEIGHT_RIGHT;
+ }
+ else if(stairs_Time>stairs_shift_weight_right_time && stairs_Time<=stairs_rise_left_foot_time)
+ {
+ //6
+ delta=(stairs_Time-stairs_shift_weight_right_time)/(stairs_rise_left_foot_time-stairs_shift_weight_right_time);
+ ep[0]=stairs_X_Offset-stairs_X_shift_body;
+ ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0);
+ ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height;
+ ep[3]=stairs_R_Offset+stairs_R_shift_amplitude;
+ ep[4]=stairs_P_Offset-1.0*stairs_P_shift_amplitude;
+ ep[5]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body;
+ ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0);
+ ep[8]=stairs_Z_Offset+stairs_Z_overshoot-stairs_Z_overshoot*delta;
+ ep[9]=stairs_R_Offset+stairs_R_shift_amplitude;
+ ep[10]=stairs_P_Offset-1.0*stairs_P_shift_amplitude;
+ ep[11]=stairs_A_Offset+stairs_A_shift_amplitude;
+ ram_data[DARWIN_STAIRS_CNTRL]|=RISE_LEFT_FOOT;
+ }
+ else if(stairs_Time>stairs_rise_left_foot_time && stairs_Time<=stairs_advance_left_foot_time)
+ {
+ //7
+ delta=(stairs_Time-stairs_rise_left_foot_time)/(stairs_advance_left_foot_time-stairs_rise_left_foot_time);
+ ep[0]=stairs_X_Offset-stairs_X_shift_body+stairs_X_shift_body*delta;
+ ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude-(stairs_Y_spread_amplitude/2.0)+(stairs_Y_spread_amplitude/2.0)*delta;
+ ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height;
+ ep[3]=stairs_R_Offset+stairs_R_shift_amplitude;//+stairs_R_shift_amplitude*delta;
+ ep[4]=stairs_P_Offset-1.0*stairs_P_shift_amplitude+1.0*stairs_P_shift_amplitude*delta;
+ ep[5]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta;
+ ep[6]=stairs_X_Offset+stairs_X_shift_amplitude-stairs_X_shift_body-(stairs_X_shift_amplitude-stairs_X_shift_body)*delta;
+ ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+(stairs_Y_spread_amplitude/2.0)-(stairs_Y_spread_amplitude/2.0)*delta;
+ ep[8]=stairs_Z_Offset;
+ ep[9]=stairs_R_Offset+stairs_R_shift_amplitude;//+stairs_R_shift_amplitude*delta;
+ ep[10]=stairs_P_Offset-1.0*stairs_P_shift_amplitude+1.0*stairs_P_shift_amplitude*delta;
+ ep[11]=stairs_A_Offset+stairs_A_shift_amplitude-stairs_A_shift_amplitude*delta;
+ ram_data[DARWIN_STAIRS_CNTRL]|=ADVANCE_LEFT_FOOT;
+ }
+ else if(stairs_Time>stairs_advance_left_foot_time && stairs_Time<=stairs_contact_left_foot_time)
+ {
+ //8
+ delta=(stairs_Time-stairs_advance_left_foot_time)/(stairs_contact_left_foot_time-stairs_advance_left_foot_time);
+ ep[0]=stairs_X_Offset;
+ ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude;
+ ep[2]=stairs_Z_Offset+stairs_Z_overshoot+stairs_Z_stair_height-(stairs_Z_stair_height+stairs_Z_overshoot)*delta;
+ ep[3]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
+ ep[4]=stairs_P_Offset;
+ ep[5]=stairs_A_Offset;
+ ep[6]=stairs_X_Offset;
+ ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude;
+ ep[8]=stairs_Z_Offset;
+ ep[9]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
+ ep[10]=stairs_P_Offset;
+ ep[11]=stairs_A_Offset;
+ ram_data[DARWIN_STAIRS_CNTRL]|=CONTACT_LEFT_FOOT;
+ }
+ else if(stairs_Time>stairs_contact_left_foot_time && stairs_Time<=stairs_center_weight_time)
+ {
+ //9
+ delta=(stairs_Time-stairs_contact_left_foot_time)/(stairs_center_weight_time-stairs_contact_left_foot_time);
+ ep[0]=stairs_X_Offset;
+ ep[1]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta;
+ ep[2]=stairs_Z_Offset;
+ ep[3]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
+ ep[4]=stairs_P_Offset;
+ ep[5]=stairs_A_Offset;
+ ep[6]=stairs_X_Offset;
+ ep[7]=stairs_Y_Offset/2.0-stairs_Y_shift_amplitude+stairs_Y_shift_amplitude*delta;
+ ep[8]=stairs_Z_Offset;
+ ep[9]=stairs_R_Offset+stairs_R_shift_amplitude-stairs_R_shift_amplitude*delta;
+ ep[10]=stairs_P_Offset;
+ ep[11]=stairs_A_Offset;
+ ram_data[DARWIN_STAIRS_CNTRL]|=CENTER_WEIGHT;
+ }
+ else
+ {
+ ram_data[DARWIN_STAIRS_CNTRL]&=(~STAIRS_STATUS);
+ stairs_Ctrl_Running=0x00;
+ }
+ }
+
+ if(ram_data[DARWIN_STAIRS_CNTRL]&STAIRS_STATUS)
+ stairs_Time += stairs_period;
+ else
+ stairs_Time=0;
+
+ // Compute angles with the inverse kinematics
+ if((darwin_leg_ik(&angle[0],ep[0],ep[1],ep[2],ep[3],ep[4],ep[5])!=1) ||
+ (darwin_leg_ik(&angle[6],ep[6],ep[7],ep[8],ep[9],ep[10],ep[11])!=1))
+ return;// Do not use angles
+
+ // Compute motor value
+ if(manager_get_module(R_HIP_YAW)==MM_WALKING)
+ {
+ manager_current_angles[R_HIP_YAW]=((-180.0*angle[0])/PI)*65536.0;
+ manager_current_slopes[R_HIP_YAW]=5;
+ }
+ if(manager_get_module(R_HIP_ROLL)==MM_WALKING)
+ {
+ manager_current_angles[R_HIP_ROLL]=((-180.0*angle[1])/PI)*65536.0;
+ manager_current_slopes[R_HIP_ROLL]=5;
+ }
+ if(manager_get_module(R_HIP_PITCH)==MM_WALKING)
+ {
+ manager_current_angles[R_HIP_PITCH]=((180.0*angle[2])/PI-stairs_Hip_Pitch_Offset)*65536.0;
+ manager_current_slopes[R_HIP_PITCH]=5;
+ }
+ if(manager_get_module(R_KNEE)==MM_WALKING)
+ {
+ manager_current_angles[R_KNEE]=((180.0*angle[3])/PI)*65536.0;
+ manager_current_slopes[R_KNEE]=5;
+ }
+ if(manager_get_module(R_ANKLE_PITCH)==MM_WALKING)
+ {
+ manager_current_angles[R_ANKLE_PITCH]=((-180.0*angle[4])/PI)*65536.0;
+ manager_current_slopes[R_ANKLE_PITCH]=5;
+ }
+ if(manager_get_module(R_ANKLE_ROLL)==MM_WALKING)
+ {
+ manager_current_angles[R_ANKLE_ROLL]=((180.0*angle[5])/PI)*65536.0;
+ manager_current_slopes[R_ANKLE_ROLL]=5;
+ }
+ if(manager_get_module(L_HIP_YAW)==MM_WALKING)
+ {
+ manager_current_angles[L_HIP_YAW]=((-180.0*angle[6])/PI)*65536.0;
+ manager_current_slopes[L_HIP_YAW]=5;
+ }
+ if(manager_get_module(L_HIP_ROLL)==MM_WALKING)
+ {
+ manager_current_angles[L_HIP_ROLL]=((-180.0*angle[7])/PI)*65536.0;
+ manager_current_slopes[L_HIP_ROLL]=5;
+ }
+ if(manager_get_module(L_HIP_PITCH)==MM_WALKING)
+ {
+ manager_current_angles[L_HIP_PITCH]=((-180.0*angle[8])/PI+stairs_Hip_Pitch_Offset)*65536.0;
+ manager_current_slopes[L_HIP_PITCH]=5;
+ }
+ if(manager_get_module(L_KNEE)==MM_WALKING)
+ {
+ manager_current_angles[L_KNEE]=((-180.0*angle[9])/PI)*65536.0;
+ manager_current_slopes[L_KNEE]=5;
+ }
+ if(manager_get_module(L_ANKLE_PITCH)==MM_WALKING)
+ {
+ manager_current_angles[L_ANKLE_PITCH]=((180.0*angle[10])/PI)*65536.0;
+ manager_current_slopes[L_ANKLE_PITCH]=5;
+ }
+ if(manager_get_module(L_ANKLE_ROLL)==MM_WALKING)
+ {
+ manager_current_angles[L_ANKLE_ROLL]=((180.0*angle[11])/PI)*65536.0;
+ manager_current_slopes[L_ANKLE_ROLL]=5;
+ }
+ }
+}
+
+// operation functions
+uint8_t stairs_in_range(unsigned short int address, unsigned short int length)
+{
+ if(ram_in_window(STAIRS_BASE_ADDRESS,STAIRS_MEM_LENGTH,address,length) ||
+ ram_in_window(STAIRS_EEPROM_ADDRESS,STAIRS_EEPROM_LENGTH,address,length))
+ return 0x01;
+ else
+ return 0x00;
+}
+
+void stairs_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
+{
+
+}
+
+void stairs_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ uint16_t i;
+
+ // walk control
+ if(ram_in_range(DARWIN_STAIRS_CNTRL,address,length))
+ {
+ if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_START_UP)
+ stairs_start(0x01);
+ if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_START_DOWN)
+ stairs_start(0x00);
+ if(data[DARWIN_STAIRS_CNTRL-address]&STAIRS_STOP)
+ stairs_stop();
+ }
+ for(i=STAIRS_EEPROM_ADDRESS;i<=STAIRS_EEPROM_ADDRESS+STAIRS_EEPROM_LENGTH;i++)
+ if(ram_in_range(i,address,length))
+ ram_data[i]=data[i-address];
+}
+
diff --git a/src/test_charger.c b/src/test_charger.c
index 5a547fceff13e6ed90be00b86f97142b1f391d35..23cb9b869118c00872faf71c98f56b5019823381 100644
--- a/src/test_charger.c
+++ b/src/test_charger.c
@@ -74,7 +74,7 @@ int main(void)
gpio_set_led(LED_3);//Si existen dispositivos conectados -> luz verde
}
-
+/*
////READ CHARGER STATUS
unsigned short int address=DARWIN_SMART_CHARGER_STATUS;
unsigned short int length=1;
@@ -84,9 +84,9 @@ int main(void)
error=darwin_on_read(address,length,&data_read);
if(data_read&0x01)
gpio_set_led(LED_2); //verde
-
+*/
-/* ////READ PERIOD SMART CHARGER
+ ////READ PERIOD SMART CHARGER
unsigned short int address=DARWIN_SMART_CHARGER_PERIOD_L;
unsigned short int length=2;
unsigned char data_read[2];
@@ -98,7 +98,7 @@ int main(void)
gpio_set_led(LED_3); //verde
else if(dread==0)
gpio_set_led(LED_4); //azul
-*/
+
/* ////READ ID SMART CHARGER
unsigned char data_read2;
@@ -110,7 +110,7 @@ int main(void)
//else if(data_read==0)
*/
-/* ////WRITE PERIOD SMART CHARGER
+ ////WRITE PERIOD SMART CHARGER
//HAL_Delay(500);
unsigned short int address=DARWIN_SMART_CHARGER_PERIOD_L;
unsigned short int length=2;
@@ -126,9 +126,9 @@ int main(void)
gpio_set_led(LED_3); //verde
else if(dread==1500)
gpio_set_led(LED_4); //azul
-*/
-/* ////ENABLE SMART CHARGER MODULE
+
+ ////ENABLE SMART CHARGER MODULE
address=DARWIN_SMART_CHARGER_CNTRL;
length=1;
uint8_t data_read;
@@ -145,14 +145,21 @@ int main(void)
darwin_on_read(address,length,&data_read);
if(data_read==3) //Smart charger control = detected + enabled
gpio_set_led(LED_3); //verde
-*/
+
-/* ////ENABLE MOTION MANAGER TIMER
+ ////ENABLE MOTION MANAGER TIMER
address=DARWIN_MM_CNTRL;
data_write=MANAGER_ENABLE;
darwin_on_write(address,length,&data_write);
HAL_Delay(500);
-*/
+
+ ////WRITE LIMIT CURRENT
+/* address=DARWIN_SMART_CHARGER_LIMIT_CURRENT_L;
+ length=2;
+ data_write=0.512;
+ darwin_on_write(address,length,&data_write);
+ HAL_Delay(500);
+*/
/* //Read battery status
address=DARWIN_BATT_AVG_TIME_EMPTY_L;
diff --git a/src/walking.c b/src/walking.c
index e261ad9c31cb093312882e26c674b8f1dd6060f9..57277dec02844b3cb7f8eb400228833f35e35f30 100755
--- a/src/walking.c
+++ b/src/walking.c
@@ -67,7 +67,7 @@ uint8_t m_Ctrl_Running;
float walking_period;
// private functions
-inline float wsin(float time, float period, float period_shift, float mag, float mag_shift)
+float wsin(float time, float period, float period_shift, float mag, float mag_shift)
{
return mag*sin(((2.0*PI*time)/period)-period_shift)+mag_shift;
}
@@ -228,12 +228,12 @@ void walking_init(uint16_t period_us)
walking_process();
}
-inline void walking_set_period(uint16_t period_us)
+void walking_set_period(uint16_t period_us)
{
walking_period=((float)period_us)/1000.0;
}
-inline uint16_t walking_get_period(void)
+uint16_t walking_get_period(void)
{
return (uint16_t)(walking_period*1000);
}