diff --git a/Makefile b/Makefile
index 67bc17e7240d4097e90dd11ae2f2ecc1b4878537..3a546a5a14f5e51887b6daa6a8bb228f8f23cd47 100755
--- a/Makefile
+++ b/Makefile
@@ -1,12 +1,13 @@
# setup
# modified by zerom for WinARM 8/2010
-STM32_HAL_PATH=$(HOME)/humanoids/stm32_hal
-STM32_LIBRARIES_PATH=$(HOME)/humanoids/stm32_libraries
+STM32_HAL_PATH=~/humanoids/stm32_hal
+STM32_LIBRARIES_PATH=~/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/system_stm32f1xx.c
TARGET_FILES+=src/gpio.c
TARGET_FILES+=src/ram.c
@@ -27,6 +28,7 @@ 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_PROCESSOR=STM32F103RE
@@ -48,6 +50,8 @@ COMPILE_OPTS += -ffreestanding -nostdlib -D$(PROCESSOR_MACRO)
INCLUDE_DIRS = -I$(HAL_PATH)/include -I$(HAL_PATH)/include/core -I$(HAL_PATH)/include/devices
INCLUDE_DIRS += -I$(UTILS_PATH)/include -I$(COMM_PATH)/include -I$(DYNAMIXEL_PATH)/include -I$(USART_PATH)/include -I./include
+DOC_DIR = ./doc
+
TCHAIN_PREFIX=arm-none-eabi-
CC = $(TCHAIN_PREFIX)gcc
@@ -123,6 +127,12 @@ $(MAIN_OUT_LSS): $(MAIN_OUT_ELF)
$(BUID_PATH)/$(STARTUP_FILE:.s=.o): $(STM32_STARTUP_FILES_PATH)/$(STARTUP_FILE)
$(AS) $(ASFLAGS) -o $(BUILD_PATH)/$(STARTUP_FILE:.s=.o) $(STM32_STARTUP_FILES_PATH)/$(STARTUP_FILE) > $(BUILD_PATH)/$(STARTUP_FILE:.s=.lst)
+
+doc:
+ doxygen $(DOC_DIR)/doxygen.conf
+
clean:
-rm -rf $(BUILD_PATH)
+ -rm -rf doc/html
+.PHONY: all clean doc
diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index c25574127598baf83a49bed84ce57f36fcc95f1a..929e720b32f970f457791ce25bd62a8d50e297cb 100644
--- a/include/darwin_registers.h
+++ b/include/darwin_registers.h
@@ -1,3 +1,5 @@
+/** @file */
+
#ifndef _DARWIN_REGISTERS_H
#define _DARWIN_REGISTERS_H
@@ -81,6 +83,8 @@ extern "C" {
#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 GRIPPER_RIGHT_MAX_FORCE ((unsigned short int)0x0062)
+#define SMART_CHARGER_PERIOD ((unsigned short int)0x0064)
#define LAST_EEPROM_OFFSET ((unsigned short int)0x00FF)
@@ -183,6 +187,9 @@ typedef enum {
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,
@@ -562,7 +569,9 @@ 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,
- DARWIN_BATT_CHARGER_STATUS = 0x0247,
+
+// 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,
@@ -570,7 +579,7 @@ typedef enum {
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_LIMIT_CURRENT_H = 0x024F, //
DARWIN_BATT_TEMPERATURE_L = 0x0250,
DARWIN_BATT_TEMPERATURE_H = 0x0251,
DARWIN_BATT_VOLTAGE_L = 0x0252,
@@ -579,13 +588,13 @@ typedef enum {
DARWIN_BATT_CURRENT_H = 0x0255,
DARWIN_BATT_AVG_CURRENT_L = 0x0256,
DARWIN_BATT_AVG_CURRENT_H = 0x0257,
- DARWIN_BATT_RELATIVE_SOC = 0x0258,
+ 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_L = 0x025E, //
DARWIN_BATT_RUN_TIME_EMPTY_H = 0x025F,
DARWIN_BATT_AVG_TIME_EMPTY_L = 0x0260,
DARWIN_BATT_AVG_TIME_EMPTY_H = 0x0261,
@@ -605,9 +614,24 @@ typedef enum {
DARWIN_BATT_CELL3_VOLTAGE_H = 0x026F,
DARWIN_BATT_CELL4_VOLTAGE_L = 0x0270,
DARWIN_BATT_CELL4_VOLTAGE_H = 0x0271,
- DARWIN_GRIPPER_CNTRL = 0x0272, // 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_registers;
+*/
+//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_registers;
#define GPIO_BASE_ADDRESS 0x0100
#define GPIO_MEM_LENGTH 21
@@ -688,7 +712,7 @@ typedef enum {
#define HEAD_STOP 0x02
#define HEAD_STATUS 0x10
-#define GRIPPER_BASE_ADDRESS 0x0272
+#define GRIPPER_BASE_ADDRESS 0x0252//0x0274
#define GRIPPER_MEM_LENGTH 1
#define GRIPPER_EEPROM_ADDRESS 0x0054
#define GRIPPER_EEPROM_LENGTH 14
@@ -697,6 +721,15 @@ typedef enum {
#define GRIPPER_OPEN_LEFT 0x04
#define GRIPPER_CLOSE_LEFT 0x08
+#define SMART_CHARGER_BASE_ADDRESS 0x0247
+#define SMART_CHARGER_MEM_LENGTH 10 //45
+#define SMART_CHARGER_EEPROM_BASE 0x0060
+#define SMART_CHARGER_EEPROM_LENGTH 2
+#define SMART_CHARGER_DET 0x01
+#define SMART_CHARGER_EN 0x02
+
+
+
#ifdef __cplusplus
}
#endif
diff --git a/include/dyn_battery.h b/include/dyn_battery.h
new file mode 100644
index 0000000000000000000000000000000000000000..ac02ce82b99b8996815da0b769d379eaa18ce3e6
--- /dev/null
+++ b/include/dyn_battery.h
@@ -0,0 +1,100 @@
+/** @file */
+
+#ifndef _DYN_BATTERY_H
+#define _DYN_BATTERY_H
+
+//#define RAM_SIZE 82
+//#define EEPROM_SIZE 31
+
+#define DYN_BATTERY_MODEL 0x8000
+#define DYN_BATTERY_VERSION 0x02
+
+/* battery charger memory map */
+/* memory map */
+typedef enum {
+ BATTERY_MODEL_NUMBER_L = 0x00,
+ BATTERY_MODEL_NUMBER_H = 0x01,
+ BATTERY_VERSION = 0x02,
+ BATTERY_ID = 0x03,
+ BATTERY_BAUD_RATE = 0x04,
+ BATTERY_RETURN_DELAY_TIME = 0x05,
+ BATTERY_WARNING_TIME = 0x06,
+ BATTERY_WARNING_TIME_HIST = 0x07,
+ BATTERY_WARNING_FREQ_L = 0x08,
+ BATTERY_WARNING_FREQ_H = 0x09,
+ BATTERY_WARNING_TIME_ON_L = 0x0A,
+ BATTERY_WARNING_TIME_ON_H = 0x0B,
+ BATTERY_WARNING_TIME_OFF_L = 0x0C,
+ BATTERY_WARNING_TIME_OFF_H = 0x0D,
+ BATTERY_CHARGER_ADDR = 0x0E,
+ BATTERY_FUEL_GAUGE_ADDR = 0x0F,
+ BATTERY_STATUS_RETURN_LEVEL = 0x10,
+ BATTERY_ERROR_TIME = 0x11,
+ BATTERY_ERROR_TIME_HIST = 0x12,
+ BATTERY_ERROR_FREQ_L = 0x13,
+ BATTERY_ERROR_FREQ_H = 0x14,
+ BATTERY_ERROR_TIME_ON_L = 0x15,
+ BATTERY_ERROR_TIME_ON_H = 0x16,
+ BATTERY_ERROR_TIME_OFF_L = 0x17,
+ BATTERY_ERROR_TIME_OFF_H = 0x18,
+ BATTERY_INPUT_MAX_CURRENT_L = 0x19,
+ BATTERY_INPUT_MAX_CURRENT_H = 0x1A,
+ BATTERY_OUTPUT_CURRENT_L = 0x1B,
+ BATTERY_OUTPUT_CURRENT_H = 0x1C,
+ BATTERY_OUTPUT_VOLTAGE_L = 0x1D,
+ BATTERY_OUTPUT_VOLTAGE_H = 0x1E,
+ /* RAM */
+ BATTERY_CHARGER_STATUS = 0x1F,
+ BATTERY_INPUT_CURRENT_L = 0x20,//corriente consumida (mA)
+ BATTERY_INPUT_CURRENT_H = 0x21,
+ BATTERY_CHARGE_CURRENT_L = 0x22,
+ BATTERY_CHARGE_CURRENT_H = 0x23,
+ BATTERY_CHARGE_VOLTAGE_L = 0x24,
+ BATTERY_CHARGE_VOLTAGE_H = 0x25,
+ BATTERY_LIMIT_CURRENT_L = 0x26,
+ BATTERY_LIMIT_CURRENT_H = 0x27,
+ BATTERY_TEMPERATURE_L = 0x28,
+ BATTERY_TEMPERATURE_H = 0x29,
+ BATTERY_VOLTAGE_L = 0x2A,
+ BATTERY_VOLTAGE_H = 0x2B,
+ BATTERY_CURRENT_L = 0x2C,
+ BATTERY_CURRENT_H = 0x2D,
+ BATTERY_AVG_CURRENT_L = 0x2E,
+ BATTERY_AVG_CURRENT_H = 0x2F,
+ BATTERY_RELATIVE_SOC = 0x30,//State Of Charge (%)
+ BATTERY_ABSOLUTE_SOC = 0x31,
+ BATTERY_REMAINING_CAP_L = 0x32,//mAh (tiempo de vida)
+ BATTERY_REMAINING_CAP_H = 0x33,
+ BATTERY_FULL_CHARGE_CAP_L = 0x34,//
+ BATTERY_FULL_CHARGE_CAP_H = 0x35,
+ BATTERY_RUN_TIME_EMPTY_L = 0x36,//(min)
+ 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_FULL_H = 0x3B,
+ BATTERY_STATUS_L = 0x3C,//cargando o descargando, cargada, descargada,...
+ BATTERY_STATUS_H = 0x3D,
+ BATTERY_DESIGN_CAP_L = 0x3E,//Capacidad de la bateria - numero constante
+ 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_H = 0x43,
+ BATTERY_CELL2_VOLTAGE_L = 0x44,
+ BATTERY_CELL2_VOLTAGE_H = 0x45,
+ BATTERY_CELL3_VOLTAGE_L = 0x46,
+ BATTERY_CELL3_VOLTAGE_H = 0x47,
+ BATTERY_CELL4_VOLTAGE_L = 0x48,
+ BATTERY_CELL4_VOLTAGE_H = 0x49,
+ BATTERY_BUZZER_CONTROL = 0x4A,
+ BATTERY_BUZZER_STATUS = 0x4B,
+ BATTERY_BUZZER_FREQ_L = 0x4C,
+ BATTERY_BUZZER_FREQ_H = 0x4D,
+ BATTERY_BUZZER_TIME_ON_L = 0x4E,
+ BATTERY_BUZZER_TIME_ON_H = 0x4F,
+ BATTERY_BUZZER_TIME_OFF_L = 0x50,
+ BATTERY_BUZZER_TIME_OFF_H = 0x51
+} battery_registers_t;
+
+#endif
diff --git a/include/eeprom_init.h b/include/eeprom_init.h
index b636703311e13ee36d73b97ef44bbfa484f75d23..7ba89ed6261ee183c407170d0a0fff7e1a2587ab 100644
--- a/include/eeprom_init.h
+++ b/include/eeprom_init.h
@@ -10,7 +10,7 @@ extern "C" {
#define DEFAULT_DEVICE_ID 0x0002
#define DEFAULT_BAUDRATE 0x0001
#define DEFAULT_RETURN_DELAY 0x0000
-#define DEFAULT_MM_PERIOD 0x1E78
+#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
@@ -81,6 +81,10 @@ extern "C" {
#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)
+
+
#ifdef __cplusplus
}
diff --git a/include/motion_manager.h b/include/motion_manager.h
index a764496424536f49ea63ff0ddaa96d13295c591b..10f053f73afb1aeef86c1b705e4e2f68b0feb327 100755
--- a/include/motion_manager.h
+++ b/include/motion_manager.h
@@ -1,3 +1,4 @@
+/** @file */
#ifndef _MOTION_MANAGER_H
#define _MOTION_MANAGER_H
@@ -66,6 +67,7 @@ typedef struct{
TDynVersion dyn_version;
}TServoInfo;
+
#define MANAGER_MAX_NUM_SERVOS 31
// public variables
@@ -73,6 +75,8 @@ extern int64_t manager_current_angles[MANAGER_MAX_NUM_SERVOS];
extern int8_t manager_current_slopes[MANAGER_MAX_NUM_SERVOS];
// public functions
+/** \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);
diff --git a/include/smart_charger.h b/include/smart_charger.h
new file mode 100644
index 0000000000000000000000000000000000000000..d6f8659ee1635a6f3a4446b1ddb1cc3aedbf0e76
--- /dev/null
+++ b/include/smart_charger.h
@@ -0,0 +1,121 @@
+/** @file */
+
+#ifndef _SMART_CHARGER_H
+#define _SMART_CHARGER_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "stm32f1xx.h"
+#include "motion_manager.h"
+#include "dynamixel_master.h"
+
+
+// public functions
+/**
+ * \brief Function to initialize the smart charger module variables
+ *
+ * This function is called at the end of manager_init() function. Performs the initialization of the local variables:
+ * smart_charger_id, smart_charger_enable, smart_charger_detect, smart_charger_period, counter, smart_charger_write and fifo.
+ *
+ * \param period_us motion manager's period in microseconds. Used to calculate smart charger's counter.
+ *
+ * smart charger counter = period charger in ms / period manager in us
+ *
+ * "counter": number of times it gets into the motion manager's timer.
+ * "smart_charger_counter": It determines when to access the smart charger to perform a read operation
+ * Default smart charger's period: 1500ms
+ * Default motion manager's period: 7800us
+ */
+void smart_charger_init(uint16_t period_us);
+/**
+ * \brief Funtion to set the Dynamixel protocol version associated to the smart charger module
+ *
+ * Used to define the master version of the Dynamixel protocol of the smart charger module.
+ * This function is called in manager_init() function with argument depending on the Dynamixel bus where the module is detected.
+ *
+ * \param master pointer to a TDynamixelMaster structure ((darwin_dyn_master or darwin_dyn_master_v2).
+ */
+inline void smart_charger_set_version(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);
+/**
+ * \brief Function to set smart charger's period
+ *
+ * This function calculates the value of "smart_charger_counter" variable so it performs the read operation with the desired period.
+ *
+ * \param period Period in milliseconds to access the smart charger for a read operation of part of its memory map.
+ */
+void smart_charger_set_period(uint16_t period); //inline?
+/**
+ * \brief Function to get smart charger's period
+ *
+ * \return the period of the smart charger, this means the time to make a read operation.
+ * This number is between 0 and 65535 ms
+ */
+uint16_t smart_charger_get_period(void); //inline?
+/**
+ * \brief Function to enable smart charger module
+ */
+void smart_charger_enable(void);
+/**
+ * \brief Function to disable smart charger module
+ */
+void smart_charger_disable(void);
+
+// operation functions
+/**
+ * \brief Function to check if a register entered belongs to the smart charger module
+ *
+ * This function is called in darwin_on_write() function to check if any of the registers entered
+ * pertains to the group of eeprom or ram assigned to the smart charger module.
+ *
+ * \param address start address of eeprom/ram
+ * \param length number of registers included
+ *
+ * \return this funciton returns 1 if any register belongs to this module or otherwise a 0.
+ */
+uint8_t smart_charger_in_range(unsigned short int address,unsigned short int length);
+/**
+ * \note Function not used
+ */
+void smart_charger_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
+/**
+ * \brief Funtion to handle received write commands
+ *
+ * This function is called in darwin_on_write() function when a command is sent to the smart charger module.
+ * Allows to enable or disable smart charger module, change or get smart charger's period, write limit current
+ * of the batteries.
+ *
+ * \param address start address to write on.
+ * \param length number of register to write on.
+ * \param data pointer to a vector with the data to write.
+ */
+void smart_charger_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data);
+
+/**
+ * \brief Motion manager interface function. Performs read and write operations to the smart charger's module
+ *
+ * This function is called in MANAGER_TIMER_IRQHandler() function from motion_manager.c (motion manager interrupt handler),
+ * which manages all the data transmission to the Dynamixel modules connected to any of the Dynamixel buses of the Darwin robot.
+ *
+ * It executes the read and write operations of the smart charger module:
+ * Read operations are executed every time "counter" reaches "smart_charger_counter" value. The battery values readen are
+ * run time to empty, average time to empty, average time to full and battery status.
+ *
+ * Write operations are executed in any manager timer cicle unless it matches with a read operation. It's possible to set the limit
+ * current of the batteries.
+ */
+void smart_charger_process(void);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
\ No newline at end of file
diff --git a/src/darwin_dyn_master.c b/src/darwin_dyn_master.c
index 23c7525f167bf41e05fc372fc4136d29ed94b684..7b3c0e9aa368f5741416b101b7544f9fbce3c8ba 100755
--- a/src/darwin_dyn_master.c
+++ b/src/darwin_dyn_master.c
@@ -67,7 +67,7 @@ void darwin_dyn_master_init(void)
/* initialize the comm object */
comm_init(&darwin_dyn_master_comm,0x01,&darwin_dyn_master_timer);
- Init.BaudRate = 1000000;
+ Init.BaudRate = 1000000; //57600;
Init.WordLength = UART_WORDLENGTH_8B;
Init.StopBits = UART_STOPBITS_1;
Init.Parity = UART_PARITY_NONE;
diff --git a/src/darwin_dyn_master_v2.c b/src/darwin_dyn_master_v2.c
index 392c5adc5f9c4f9b6f294e0f744ce85a1a1b928b..5dc5e5daf5ded2ca691a234b8c2dc9e1d6f9aae4 100755
--- a/src/darwin_dyn_master_v2.c
+++ b/src/darwin_dyn_master_v2.c
@@ -56,7 +56,7 @@ void darwin_dyn_master_v2_init(void)
/* initialize the comm object */
comm_init(&darwin_dyn_master_v2_comm,0x01,&darwin_dyn_master_v2_timer);
- Init.BaudRate = 1000000;
+ Init.BaudRate = 1000000; //57600;
Init.WordLength = UART_WORDLENGTH_8B;
Init.StopBits = UART_STOPBITS_1;
Init.Parity = UART_PARITY_NONE;
diff --git a/src/darwin_dyn_slave.c b/src/darwin_dyn_slave.c
index 0750b0937bde2814e02ea3ab473d4a110e481fca..dd7f12fac2b7730652638b6bf9e393ab0b2f1155 100755
--- a/src/darwin_dyn_slave.c
+++ b/src/darwin_dyn_slave.c
@@ -12,6 +12,7 @@
#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
@@ -92,6 +93,9 @@ unsigned char darwin_on_write(unsigned short int address,unsigned short int leng
// 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]);
diff --git a/src/eeprom.c b/src/eeprom.c
index c5caef423ff350cc15ad1eeb015987de27471d87..d511af23b9cf8ebb04bf8cd314b10cfea15fc435 100755
--- a/src/eeprom.c
+++ b/src/eeprom.c
@@ -152,7 +152,9 @@ uint16_t eeprom_data[] __attribute__ ((section (".eeprom")))={VALID_PAGE,
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_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};
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 6a5e3f188a4ef2fc873571c318fe202b611b11c6..4672002a904cf6b4af702a78645883c7176ac278 100755
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -9,6 +9,8 @@
#include "head_tracking.h"
#include "grippers.h"
#include "imu.h"
+#include "smart_charger.h"
+#include "dyn_battery.h"
#define MANAGER_TIMER TIM5
#define ENABLE_MANAGER_TIMER_CLK __HAL_RCC_TIM5_CLK_ENABLE()
@@ -31,6 +33,7 @@ uint8_t manager_balance_enabled;
// manager offsets
int16_t manager_offset[MANAGER_MAX_NUM_SERVOS];
+
// private functions
void manager_send_motion_command(void)
{
@@ -173,13 +176,15 @@ void MANAGER_TIMER_IRQHandler(void)
// call the grippers process
grippers_process();
// balance the robot
- manager_balance();
+ manager_balance();
+ // access to smart charger
+ smart_charger_process();
// get the target angles from all modules
manager_get_target_position();
// send the motion commands to the servos
manager_send_motion_command();
// get the disabled servos position
- // manager_get_current_position();
+ // manager_get_current_position();
}
}
}
@@ -277,6 +282,7 @@ void manager_init(uint16_t period_us)
break;
default: break;
}
+ //Configuration for all servos
manager_servos[i].id=servo_ids[current];
manager_servos[i].model=model;
manager_servos[i].module=MM_NONE;
@@ -314,6 +320,24 @@ void manager_init(uint16_t period_us)
manager_servos[i].ccw_angle_limit=0;
}
}
+
+ //Other devices detected - smart charger
+ 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;
+ default: break;
+ }
+ }
+
darwin_dyn_master_disable_power();
// detect the servos connected on the v2 bus
@@ -335,7 +359,7 @@ void manager_init(uint16_t period_us)
manager_servos[i].center_angle=150<<7;
manager_servos[i].max_speed=354;
break;
- default: break;
+ default: break;
}
manager_servos[i].id=servo_ids[current];
manager_servos[i].model=model;
@@ -358,7 +382,25 @@ void manager_init(uint16_t period_us)
current++;
}
}
-
+
+ //Other devices detected - smart charger
+ for(i=current; i<num; i++){
+ dyn_master_read_word(&darwin_dyn_master_v2,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_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]);
+ break;
+ default: break;
+ }
+ }
+
+
ram_data[DARWIN_MM_PRESENT_SERVOS1]=(present_servos&0x000000FF);
ram_data[DARWIN_MM_PRESENT_SERVOS2]=((present_servos&0x0000FF00)>>8);
ram_data[DARWIN_MM_PRESENT_SERVOS3]=((present_servos&0x00FF0000)>>16);
@@ -402,6 +444,7 @@ void manager_init(uint16_t period_us)
joint_motion_init(period_us);
head_tracking_init(period_us);
grippers_init(period_us);
+ smart_charger_init(period_us);
}
uint16_t manager_get_period(void)
diff --git a/src/ram.c b/src/ram.c
index 3d77ee0e898d5c198d2a21c0bcf014127af8f21c..f2c505dfb8b5e82c15e1311d9a27dad51bd60416 100755
--- a/src/ram.c
+++ b/src/ram.c
@@ -147,6 +147,10 @@ void ram_init(void)
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)
diff --git a/src/smart_charger.c b/src/smart_charger.c
new file mode 100644
index 0000000000000000000000000000000000000000..daa8ef803b42c9ce9088255ffb6a6c3bf8645785
--- /dev/null
+++ b/src/smart_charger.c
@@ -0,0 +1,168 @@
+
+#include "ram.h"
+#include "dyn_battery.h"
+#include "smart_charger.h"
+#include "gpio.h"
+
+// private variables
+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_count; //counter = sc period / mm period --> Default: (1500ms*1000)/7800us
+uint8_t smart_charger_write; //write smart charger signal
+
+#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)
+
+//---------------------------------------
+
+TDynamixelMaster *dyn_battery_master;
+
+// public functions
+void smart_charger_init(uint16_t period_us)
+{
+ smart_charger_id = ram_data[DARWIN_SMART_CHARGER_ID];
+ smart_charger_enabled = 0x00;
+ 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_count = (smart_charger_period*1000)/period_us;
+
+ smart_charger_write = 0x00;
+ infifo=outfifo=&write_fifo[0];
+ numdata=0; //fifo empty
+}
+
+inline void smart_charger_set_version(TDynamixelMaster *master)
+{
+ dyn_battery_master=master;
+}
+
+inline TDynamixelMaster* smart_charger_get_version(void)
+{
+ return dyn_battery_master;
+}
+
+void smart_charger_set_period(uint16_t period) //en ms
+{
+ uint16_t mm_period=manager_get_period_us(); //default: 7800us
+ smart_charger_count = (period*1000)/mm_period; //(ms*1000)/us
+ smart_charger_period = period;
+ ram_data[DARWIN_SMART_CHARGER_PERIOD_L]=period&0x00FF;
+ ram_data[DARWIN_SMART_CHARGER_PERIOD_H]=(period&0xFF00)>>8;
+}
+
+uint16_t smart_charger_get_period(void)
+{
+ return smart_charger_period;
+}
+
+void smart_charger_enable(void)
+{
+ smart_charger_enabled=0x01;
+ ram_data[DARWIN_SMART_CHARGER_CNTRL]|=SMART_CHARGER_EN;
+}
+
+void smart_charger_disable(void)
+{
+ smart_charger_enabled=0x00;
+ ram_data[DARWIN_SMART_CHARGER_CNTRL]&=(~SMART_CHARGER_EN);
+}
+
+
+// operation functions
+uint8_t smart_charger_in_range(unsigned short int address,unsigned short int length)
+{
+ if(ram_in_window(SMART_CHARGER_BASE_ADDRESS,SMART_CHARGER_MEM_LENGTH,address,length) ||
+ ram_in_window(SMART_CHARGER_EEPROM_BASE,SMART_CHARGER_EEPROM_LENGTH,address,length))
+ return 0x01;
+ else
+ return 0x00;
+}
+
+void smart_charger_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
+{
+}
+
+void smart_charger_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
+{
+ //uint8_t i;
+ uint16_t period,i;
+
+ //Enable/Disable smart charger
+ if(ram_in_range(DARWIN_SMART_CHARGER_CNTRL,address,length))
+ {
+ if(data[DARWIN_SMART_CHARGER_CNTRL-address]&SMART_CHARGER_EN)
+ smart_charger_enable();
+ else
+ smart_charger_disable();
+ }
+ //Configure smart charger period
+ if(ram_in_range(DARWIN_SMART_CHARGER_PERIOD_L,address,length) && ram_in_range(DARWIN_SMART_CHARGER_PERIOD_H,address,length))
+ {
+ period=data[DARWIN_SMART_CHARGER_PERIOD_L-address]+(data[DARWIN_SMART_CHARGER_PERIOD_H-address]<<8);
+ smart_charger_set_period(period);
+ }
+
+ //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))
+ {
+ smart_charger_write = 0x01;
+
+ 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++; */
+ }
+ }
+}
+
+
+// motion manager interface function
+void smart_charger_process(void)
+{
+ counter++;
+uint8_t error;
+ //Write smart_charger - Battery limit current (EEPROM)
+ if(smart_charger_detected && smart_charger_enabled && smart_charger_write && counter!=smart_charger_count)
+ {
+ 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
+ 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)
+ {
+ counter = 0;
+ 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_3);//verde
+ else
+ gpio_set_led(LED_4);
+ }
+}
+
diff --git a/src/test_charger.c b/src/test_charger.c
new file mode 100644
index 0000000000000000000000000000000000000000..a024361a344a8b73eaa6911e0f28144af77db7ba
--- /dev/null
+++ b/src/test_charger.c
@@ -0,0 +1,167 @@
+
+/* Test smart charger with CM730
+ * main behaving as master (PC) to the CM730
+ * Initialization of eeprom, ram, gpio and motion manager (not manager_timer)
+ *
+ * UNCOMMENT DESIRED BLOCKS TO TEST (Debugation with leds)
+ * Scan of smart charger in bus dynamixel v1
+ * Read smart chargers period (1500)
+ * Read smart chargers ID (192)
+ * Write smart charger period
+ * Enable smart charger
+ * Enable motion manager timer
+ * Read battery status of smart charger
+ */
+
+#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_dyn_master.h"
+#include "darwin_dyn_slave.h"
+#include "motion_manager.h"
+#include "darwin_registers.h"
+
+
+int main(void)
+{
+ 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 GPIO module */
+ gpio_init();
+ // initialize adc
+// adc_init();
+ // initialize imu
+// imu_init();
+ // initialize time module
+ darwin_time_init();
+ /* initialize the dynamixel slave interface */
+ //darwin_dyn_slave_init();
+ //darwin_dyn_slave_start();
+ /* initialize motion manager module */
+ EE_ReadVariable(MM_PERIOD_OFFSET,&eeprom_data); //period: 7800us
+ 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); //azul
+
+ /* unsigned short int address;
+ unsigned short int length;
+ unsigned char data_write;
+ */
+
+ ////SCAN
+ unsigned char num;
+ unsigned char servo_ids[254];
+ dyn_master_scan(&darwin_dyn_master,&num, servo_ids); //dynamixel_master.c
+ if(num==0){
+ gpio_set_led(LED_4);//azul
+ }else{
+ 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;
+ unsigned char data_read;
+ uint8_t error;
+ //unsigned char *pdata = (uint8_t)&data_read;
+ error=darwin_on_read(address,length,&data_read);
+ if(data_read&0x01)
+ gpio_set_led(LED_2); //verde
+
+
+/* ////READ PERIOD SMART CHARGER
+ unsigned short int address=DARWIN_SMART_CHARGER_PERIOD_L;
+ unsigned short int length=2;
+ unsigned char data_read[2];
+ uint8_t error;
+ //unsigned char *pdata = (uint8_t)&data_read;
+ error=darwin_on_read(address,length,data_read);
+ uint16_t dread = data_read[0] + (data_read[1]<<8);
+ if(dread==1500)
+ gpio_set_led(LED_3); //verde
+ else if(dread==0)
+ gpio_set_led(LED_4); //azul
+*/
+
+/* ////READ ID SMART CHARGER
+ unsigned char data_read2;
+ address=DARWIN_SMART_CHARGER_ID;
+ length=1;
+ darwin_on_read(address,length,&data_read2);
+ if(data_read2==192)
+ gpio_set_led(LED_TX); //verde
+ //else if(data_read==0)
+*/
+
+/* ////WRITE PERIOD SMART CHARGER
+ //HAL_Delay(500);
+ unsigned short int address=DARWIN_SMART_CHARGER_PERIOD_L;
+ unsigned short int length=2;
+ unsigned char data_write[2]={0x40, 0x06}; //1600
+ //data_write=0x05DC;
+ darwin_on_write(address,length,data_write); //data_write = data_write[0] address
+ HAL_Delay(500);
+ unsigned char data_read[2];
+ length=2;
+ darwin_on_read(address, length,data_read); //data_read = data_read[0] address
+ uint16_t dread = data_read[0] + (data_read[1]<<8);
+ if(dread==1600)
+ gpio_set_led(LED_3); //verde
+ else if(dread==1500)
+ gpio_set_led(LED_4); //azul
+*/
+
+/* ////ENABLE SMART CHARGER MODULE
+ address=DARWIN_SMART_CHARGER_CNTRL;
+ length=1;
+ uint8_t data_read;
+ ram_read_byte(address,&data_read);
+ //darwin_on_read(address,length,&data_read);
+ if(data_read==1) //Smart charger control = detected
+ gpio_set_led(LED_3); //verde
+ else if(data_read==0) //smart charger NOT detected
+ gpio_set_led(LED_4); //azul
+
+ data_write=SMART_CHARGER_EN;//Smart charger enable
+ darwin_on_write(address,length,&data_write);
+ HAL_Delay(500);
+ 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
+ address=DARWIN_MM_CNTRL;
+ data_write=MANAGER_ENABLE;
+ darwin_on_write(address,length,&data_write);
+ HAL_Delay(500);
+*/
+
+/* //Read battery status
+ address=DARWIN_BATT_AVG_TIME_EMPTY_L;
+ length=6;
+ darwin_on_read(address,length,pdata);
+ if(data_read[]==128) //BATTERY_STATUS_L
+ gpio_set_led(LED_4);
+ else
+ gpio_set_led(LED_3);
+*/
+
+ gpio_set_led(LED_RX); //naranja
+
+ while(1);/* main function does not return */
+}
+