diff --git a/Makefile b/Makefile
index 14e6134ad6d9f7504a8b0d6a768503f61c633991..9e9a1e4fcc23f2d163d3e07f7eee3c60fa9326bf 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=$(HOME)/Desktop/IRI/humanoids/stm32_hal
+STM32_LIBRARIES_PATH=$(HOME)/Desktop/IRI/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
@@ -129,7 +133,6 @@ doc:
clean:
-rm -rf $(BUILD_PATH)
- #-rm -rf doc/html
-
-#.PHONY: all clean doc
+ -rm -rf doc/html
+.PHONY: all clean doc
diff --git a/include/darwin_registers.h b/include/darwin_registers.h
index e8808233769725336f052d9a785ceb155faddf7e..596fdb262010913b41fc1df95d0be9ca320edf10 100644
--- a/include/darwin_registers.h
+++ b/include/darwin_registers.h
@@ -1,3 +1,5 @@
+/** @file */
+
#ifndef _DARWIN_REGISTERS_H
#define _DARWIN_REGISTERS_H
@@ -618,7 +620,7 @@ typedef enum {
DARWIN_SMART_CHARGER_ID = 0x0273,
//Cambio registro
- DARWIN_GRIPPER_CNTRL = 0x0276, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
+ DARWIN_GRIPPER_CNTRL = 0x0274, // 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;
@@ -701,7 +703,7 @@ typedef enum {
#define HEAD_STOP 0x02
#define HEAD_STATUS 0x10
-#define GRIPPER_BASE_ADDRESS 0x0276 //Modificado
+#define GRIPPER_BASE_ADDRESS 0x0274
#define GRIPPER_MEM_LENGTH 1
#define GRIPPER_EEPROM_ADDRESS 0x0054
#define GRIPPER_EEPROM_LENGTH 14
@@ -710,7 +712,7 @@ typedef enum {
#define GRIPPER_OPEN_LEFT 0x04
#define GRIPPER_CLOSE_LEFT 0x08
-#define SMART_CHARGER_BASE_ADDRESS 0x0247 //DARWIN_BATT_CHARGER_STATUS
+#define SMART_CHARGER_BASE_ADDRESS 0x0247
#define SMART_CHARGER_MEM_LENGTH 45
#define SMART_CHARGER_EEPROM_BASE 0x0060
#define SMART_CHARGER_EEPROM_LENGTH 2
diff --git a/include/dyn_battery.h b/include/dyn_battery.h
index 3ab7747e59727ef1824308b304e9eccc1172dcbf..e41d3e263c8db4609899087620a7be5aee3caf60 100644
--- a/include/dyn_battery.h
+++ b/include/dyn_battery.h
@@ -1,3 +1,5 @@
+/** @file */
+
#ifndef _DYN_BATTERY_H
#define _DYN_BATTERY_H
diff --git a/include/motion_manager.h b/include/motion_manager.h
index d4ef7cc6d08496509871222b4acdead47134c42e..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
@@ -74,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
index cdc61a2c5dd3171a2ab05e8ff9ea930bb71e984d..d6f8659ee1635a6f3a4446b1ddb1cc3aedbf0e76 100644
--- a/include/smart_charger.h
+++ b/include/smart_charger.h
@@ -11,29 +11,21 @@ extern "C" {
#include "motion_manager.h"
#include "dynamixel_master.h"
-/**
- * \brief Battery dynamixel bus version
- *
- * Pointer to TDynamixelMaster structure.
- */
-typedef struct{
- TDynamixelMaster *dyn_version;
-}TBatteryVersion;
-
// public functions
/**
- * \brief Function to initialize the smart charger module
+ * \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 internal variables:
- * smart charger's dynamixel id, module enabled signal, smart charger detected signal, smart charger's period and counter (for
- * read operations), write signal and fifo write.
+ * 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 smart charger's period: 1500ms
* Default motion manager's period: 7800us
*/
void smart_charger_init(uint16_t period_us);
@@ -41,23 +33,23 @@ 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 depending on the Dynamixel bus where the module is detected.
+ * 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.
+ * \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" so it performs the read operation with the desired 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.
+ * \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?
/**
@@ -78,7 +70,7 @@ void smart_charger_disable(void);
// operation functions
/**
- * \brief Function to check if the register entered belongs to the smart charger module
+ * \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.
@@ -90,11 +82,11 @@ void smart_charger_disable(void);
*/
uint8_t smart_charger_in_range(unsigned short int address,unsigned short int length);
/**
- * \note Possible implementation in the future: Select which group of registers to read periodically
+ * \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 write commands received from the master (PC)
+ * \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
@@ -106,18 +98,18 @@ 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);
-// motion manager interface functions
/**
- * \brief Function to handle read and write operations of the smart charger's module
+ * \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, which manages all the Dynamixel modules
- * connected to any of the Dynamixel buses of the Darwin robot.
+ * 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.
+ * 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);
diff --git a/src/motion_manager.c b/src/motion_manager.c
index 1633cdac592d80024502f46ae489c5905a484e3d..fc30ca6bc96de64da23f5b59de8155f44867cfd0 100755
--- a/src/motion_manager.c
+++ b/src/motion_manager.c
@@ -24,7 +24,7 @@ TIM_HandleTypeDef MANAGER_TIM_Handle;
uint16_t manager_motion_period;
uint16_t manager_motion_period_us;
uint8_t manager_num_servos;
-TServoInfo manager_servos[MANAGER_MAX_NUM_SERVOS]; //motion_manager.h
+TServoInfo manager_servos[MANAGER_MAX_NUM_SERVOS];
// current angles used for all motion modules
int64_t manager_current_angles[MANAGER_MAX_NUM_SERVOS];
int8_t manager_current_slopes[MANAGER_MAX_NUM_SERVOS];
@@ -204,13 +204,13 @@ void manager_init(uint16_t period_us)
darwin_dyn_master_init();
darwin_dyn_master_v2_init();
- ram_data[DARWIN_MM_CNTRL]|=MANAGER_SCANNING; //0X80 -> bit scanning
+ ram_data[DARWIN_MM_CNTRL]|=MANAGER_SCANNING;
// enable power to the servos
darwin_dyn_master_enable_power();
HAL_Delay(1000);
// detect the servos connected on the v1 bus
dyn_master_scan(&darwin_dyn_master,&num,servo_ids);
- ram_data[DARWIN_MM_NUM_SERVOS]=num; //tmb sumara los otros dispositivos (cargador)
+ ram_data[DARWIN_MM_NUM_SERVOS]=num;
manager_num_servos=0;
for(i=0;i<MANAGER_MAX_NUM_SERVOS && current<num;i++)
{
@@ -281,37 +281,27 @@ void manager_init(uint16_t period_us)
manager_servos[i].center_angle=180<<7;
manager_servos[i].max_speed=270;
break;
-/* case DYN_BATTERY_MODEL: ram_data[DARWIN_SMART_CHARGER_CNTRL]|=SMART_CHARGER_DET; //smart charger detected
- ram_data[DARWIN_SMART_CHARGER_ID] = servo_ids[current];
- //battery_version.dyn_version = &darwin_dyn_master;
- smart_charger_set_version(darwin_dyn_master);
- //hacer un read table?
- dyn_master_read_table(&darwin_dyn_master,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_CHARGER_STATUS,SMART_CHARGER_MEM_LENGTH,(uint8_t *)&ram_data[DARWIN_BATT_CHARGER_STATUS]);
- gpio_set_led(LED_3);
- break;*/
default: break;
}
- //Config for all servos except battery
- // if(model!=DYN_BATTERY_MODEL){
- manager_servos[i].id=servo_ids[current];
- manager_servos[i].model=model;
- manager_servos[i].module=MM_NONE;
- manager_servos[i].enabled=0x00;
- manager_servos[i].dyn_version=DYN_VER1;
- // get the servo's current position
- dyn_master_read_word(&darwin_dyn_master,manager_servos[i].id,P_PRESENT_POSITION_L,&manager_servos[i].current_value);
- manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value);
- ram_data[DARWIN_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
- ram_data[DARWIN_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8);
- // read the servo limits
- dyn_master_read_word(&darwin_dyn_master,manager_servos[i].id,P_CW_ANGLE_LIMIT_L,&value);
- manager_servos[i].cw_angle_limit=manager_value_to_angle(i,value);
- dyn_master_read_word(&darwin_dyn_master,manager_servos[i].id,P_CCW_ANGLE_LIMIT_L,&value);
- manager_servos[i].ccw_angle_limit=manager_value_to_angle(i,value);
- // set the action current angles
- manager_current_angles[i]=manager_servos[i].current_angle<<9;
- manager_num_servos++;
- // }
+ //Configuration for all servos
+ manager_servos[i].id=servo_ids[current];
+ manager_servos[i].model=model;
+ manager_servos[i].module=MM_NONE;
+ manager_servos[i].enabled=0x00;
+ manager_servos[i].dyn_version=DYN_VER1;
+ // get the servo's current position
+ dyn_master_read_word(&darwin_dyn_master,manager_servos[i].id,P_PRESENT_POSITION_L,&manager_servos[i].current_value);
+ manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value);
+ ram_data[DARWIN_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
+ ram_data[DARWIN_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8);
+ // read the servo limits
+ dyn_master_read_word(&darwin_dyn_master,manager_servos[i].id,P_CW_ANGLE_LIMIT_L,&value);
+ manager_servos[i].cw_angle_limit=manager_value_to_angle(i,value);
+ dyn_master_read_word(&darwin_dyn_master,manager_servos[i].id,P_CCW_ANGLE_LIMIT_L,&value);
+ manager_servos[i].ccw_angle_limit=manager_value_to_angle(i,value);
+ // set the action current angles
+ manager_current_angles[i]=manager_servos[i].current_angle<<9;
+ manager_num_servos++;
current++;
}
else
@@ -332,17 +322,15 @@ void manager_init(uint16_t period_us)
}
}
- //Other devices detected
+ //Other devices detected - smart charger
for(i=current; i<num; i++){
- dyn_master_read_word(&darwin_dyn_master,servo_ids[i],P_MODEL_NUMBER_L,&model);
+ 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
- //gpio_set_led(LED_TX);
- // if(ram_data[DARWIN_SMART_CHARGER_CNTRL] ==1)
- // gpio_set_led(LED_3);
- ram_data[DARWIN_SMART_CHARGER_ID] = servo_ids[i];
- smart_charger_set_version(&darwin_dyn_master);
+ 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]);
break;
default: break;
@@ -350,8 +338,7 @@ void manager_init(uint16_t period_us)
}
darwin_dyn_master_disable_power();
-
-/* ORIGINAL
+
// 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;
@@ -371,7 +358,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;
@@ -394,67 +381,16 @@ void manager_init(uint16_t period_us)
current++;
}
}
-*/
-
- // 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; //aumentar manager_max_num_servos (31)? la bateria se incluira
- current=0;
- for(i=0;i<MANAGER_MAX_NUM_SERVOS && current<num;i++)
- {
- if(i==servo_ids[current])
- {
- present_servos|=(0x00000001<<i);
- // read the model of the i-th device
- dyn_master_read_word(&darwin_dyn_master_v2,servo_ids[current],XL_MODEL_NUMBER_L,&model); //o BATTERY_MODEL_NUMBER_L
- switch(model)
- {
- case SERVO_XL320: manager_servos[i].encoder_resolution=1023;
- manager_servos[i].gear_ratio=238;
- manager_servos[i].max_angle=300<<7;
- manager_servos[i].center_angle=150<<7;
- manager_servos[i].max_speed=354;
-
- manager_servos[i].id=servo_ids[current];
- manager_servos[i].model=model;
- manager_servos[i].module=MM_NONE;
- manager_servos[i].enabled=0x00;
- manager_servos[i].dyn_version=DYN_VER2;
- // get the servo's current position
- //CAMBIAR i POR current? -NO
- dyn_master_read_word(&darwin_dyn_master_v2,manager_servos[i].id,XL_PRESENT_POSITION_L,&manager_servos[i].current_value);
- manager_servos[i].current_angle=manager_value_to_angle(i,manager_servos[i].current_value);
- ram_data[DARWIN_MM_SERVO0_CUR_POS_L+i*2]=(manager_servos[i].current_angle&0xFF);
- ram_data[DARWIN_MM_SERVO0_CUR_POS_H+i*2]=(manager_servos[i].current_angle>>8);
- // read the servo limits
- dyn_master_read_word(&darwin_dyn_master_v2,manager_servos[i].id,XL_CW_ANGLE_LIMIT_L,&value);
- manager_servos[i].cw_angle_limit=manager_value_to_angle(i,value);
- dyn_master_read_word(&darwin_dyn_master_v2,manager_servos[i].id,XL_CCW_ANGLE_LIMIT_L,&value);
- manager_servos[i].ccw_angle_limit=manager_value_to_angle(i,value);
- // set the action current angles
- manager_current_angles[i]=manager_servos[i].current_angle<<9;
- break;
- /* case DYN_BATTERY_MODEL: ram_data[DARWIN_SMART_CHARGER_CNTRL]|=SMART_CHARGER_DET; //smart charger detected
- ram_data[DARWIN_SMART_CHARGER_ID] = servo_ids[current];
- smart_charger_set_version(darwin_dyn_master_v2);
- dyn_master_read_table(&darwin_dyn_master_v2,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_CHARGER_STATUS,SMART_CHARGER_MEM_LENGTH,(uint8_t *)&ram_data[DARWIN_BATT_CHARGER_STATUS]);
- break;*/
- default: break;
- }
- //antes habia la inicializacion general
- manager_num_servos++;
- current++;
- }
- }
- //Other devices detected
+ //Other devices detected - smart charger
for(i=current; i<num; i++){
- dyn_master_read_word(&darwin_dyn_master,servo_ids[i],P_MODEL_NUMBER_L,&model);
+ 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_set_version(&darwin_dyn_master_v2);
+ 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]);
break;
default: break;
diff --git a/src/smart_charger.c b/src/smart_charger.c
index b6ce84ebb909638222ac8bc1e47b0f65d26670df..6570023a5f6fc141eb59ceb1ba0ed12d216e408a 100644
--- a/src/smart_charger.c
+++ b/src/smart_charger.c
@@ -2,37 +2,31 @@
#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 enable signal //----bool?
-uint8_t smart_charger_detected; //smart charger detect signal
-
-uint16_t smart_charger_period; //smart charger period value
-uint16_t counter;
-uint16_t smart_charger_count; //counter = sc period / mm period --> Default: (1500ms*1000)/7800us
-uint8_t smart_charger_write; //write smart charger signal
-
-//unsigned short int write_address;
-//unsigned short int write_length;
-//unsigned char write_data[2];
+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
-uint8_t *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 fifo[fifosize]; //Vector with data to write (data_write)
+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)
//---------------------------------------
-TBatteryVersion battery_version;
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 disabled
+ 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];
@@ -40,26 +34,23 @@ void smart_charger_init(uint16_t period_us)
smart_charger_count = (smart_charger_period*1000)/period_us;
smart_charger_write = 0x00;
- infifo=outfifo=&fifo[0];
+ infifo=outfifo=&write_fifo[0];
numdata=0; //fifo empty
}
inline void smart_charger_set_version(TDynamixelMaster *master)
{
- battery_version.dyn_version=master;
dyn_battery_master=master;
}
inline TDynamixelMaster* smart_charger_get_version(void)
{
- return battery_version.dyn_version;
return dyn_battery_master;
}
-
-void smart_charger_set_period(uint16_t period) //en ms
+void smart_charger_set_period(uint16_t period) //en ms
{
- uint16_t mm_period=manager_get_period_us(); //default: 7800us
+ 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;
@@ -94,16 +85,6 @@ uint8_t smart_charger_in_range(unsigned short int address,unsigned short int len
return 0x00;
}
-/*INFO smart_charger_process_read_cmd
- * Mientras solo se quiera la aplicacion de carga automatica no es necesario utilizarlo
-
- * Si quiero leer solo el time to empty, time to full o battery status puedo hacer un read_ram directamente (habra una diferencia de counter)
- * Si quiero leer otro dato tendre que hacer un read table y despues leer el registro que queria con read_ram
-
- //--escoger que se quiere leer?
- //if(ram_in_range(DARWIN_BATT_CHARGER_STATUS,address,length))
- //if(ram_in_range(DARWIN_BATT_INPUT_CURRENT_L,address,length))
-*/
void smart_charger_process_read_cmd(unsigned short int address,unsigned short int length,unsigned char *data)
{
}
@@ -128,80 +109,49 @@ void smart_charger_process_write_cmd(unsigned short int address,unsigned short i
smart_charger_set_period(period);
}
- //Write Battery limit current
- if(ram_in_range(BATTERY_LIMIT_CURRENT_L,address,length)&& ram_in_range(BATTERY_LIMIT_CURRENT_H,address,length))
+ //Write Battery limit current (EEPROM)
+ if(ram_in_range(BATTERY_INPUT_MAX_CURRENT_L,address,length)&& ram_in_range(BATTERY_INPUT_MAX_CURRENT_H,address,length))
{
smart_charger_write = 0x01;
- if(infifo==&fifo[fifosize]);
- infifo=&fifo[0];
- if(numdata<fifosize){//hay espacio en fifo (putp==getp)
- for(i=BATTERY_LIMIT_CURRENT_L;i<=BATTERY_LIMIT_CURRENT_H;i++)
+ if(infifo==&write_fifo[fifosize]);
+ infifo=&write_fifo[0];
+ if(numdata<fifosize) //free space in fifo
+ {
+ for(i=BATTERY_INPUT_MAX_CURRENT_L;i<=BATTERY_INPUT_MAX_CURRENT_H;i++)
{
*infifo=data[i-address];
infifo++;
- numdata++;
- //write_data[i]=data[i];
+ numdata++;
}
}
- }
-
-/* //Write ------ TENER EN CUENTA MOTION MANAGER!
- if(ram_in_range(BATTERY_LIMIT_CURRENT_L,address,length)&& ram_in_range(BATTERY_LIMIT_CURRENT_H,address,length)) //no hace falta si pudiese escribir donde sea
- {
- smart_charger_write = 0x01;
- //write_address = BATTERY_LIMIT_CURRENT_L;
- //write_length = 2;
- // write_data = data;
- //Cambiarlo por una FIFO
- //Tener en cuenta que se tendra que modificar periodo motion manager ---> manager_set_period?
-
- for(i=0;i<2;i++)
- {
- write_data[i]=data[i];
- }
- }
-*/
- //else
- //return BAD_ADDRESS; //Definir BAD_ADDRESS + quitar void
+ }
}
// motion manager interface function
-//Read time to empty, time to full and battery_status
void smart_charger_process(void)
{
- unsigned char error;
counter++;
- //Write smart_charger
+ //Write smart_charger - Battery limit current (EEPROM)
if(smart_charger_detected && smart_charger_enabled && smart_charger_write && counter!=smart_charger_count)
{
- //smart_charger_write = 0x00;
- //dyn_master_write_table(battery_version.dyn_version,smart_charger_id,write_address,write_length,write_data);
- dyn_master_write_table(battery_version.dyn_version,smart_charger_id,BATTERY_LIMIT_CURRENT_L,2,outfifo);
- if(outfifo==&fifo[fifosize-1])
- outfifo=&fifo[0];
+ 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];
else
outfifo=outfifo+2;
numdata=numdata-2;
- if(numdata==0) //no hay datos nuevos en la fifo
+ if(numdata==0) //empty fifo
smart_charger_write = 0x00;
}
- //Read smart charger
+ //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(battery_version.dyn_version,smart_charger_id,BATTERY_AVG_TIME_EMPTY_L,6,&ram_data[DARWIN_BATT_AVG_TIME_EMPTY_L]);
- error=dyn_master_read_table(dyn_battery_master,smart_charger_id,BATTERY_AVG_TIME_EMPTY_L,6,&ram_data[DARWIN_BATT_AVG_TIME_EMPTY_L]);
- if(error==DYN_SUCCESS){
- gpio_set_led(LED_3);
- }else if(error==DYN_INST_ERROR){
- gpio_set_led(LED_TX);
- }
- gpio_toggle_led(LED_4);
- }
- //gpio_clear_led(LED_3);
+ dyn_master_read_table(dyn_battery_master,smart_charger_id,BATTERY_AVG_TIME_EMPTY_L,6,&ram_data[DARWIN_BATT_AVG_TIME_EMPTY_L]);
+ }
}