From daf3828c149bcda06ca89f45f02e972bae3ec2bf Mon Sep 17 00:00:00 2001 From: Irene Garcia <igarcia@iri.upc.edu> Date: Tue, 21 Feb 2017 17:35:08 +0100 Subject: [PATCH] Created the smart charger module files Implemented an API for the smart charger. This module it's in charge of the smart charger device connected to any of the Dynamixel bus of the sub-controller of the Darwin Robot. It provides a set of functions that allows the user to enable or disable the module and change or obtain the smart charger's period, this means the time to perform a read access of the smart charger's and batteries info. On the other hand, it implements a periodic reading of part of the memory map of the smart charger to obtain the batteries time to full or time to empty. This information is used for an automatic battery charger application. Other battery information (as current consumption, voltage, etc) is also available with this structure. --- include/smart_charger.h | 129 +++++++++++++++++++++++++ src/smart_charger.c | 207 ++++++++++++++++++++++++++++++++++++++++ 2 files changed, 336 insertions(+) create mode 100644 include/smart_charger.h create mode 100644 src/smart_charger.c diff --git a/include/smart_charger.h b/include/smart_charger.h new file mode 100644 index 0000000..cdc61a2 --- /dev/null +++ b/include/smart_charger.h @@ -0,0 +1,129 @@ +/** @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" + +/** + * \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 + * + * 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. + * + * \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 depending on the Dynamixel bus where the module is detected. + * + * \param master pointer to a TDynamixelMaster structure. + */ +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. + * + * \param period Period in milliseconds to access the smart charger for a read operation. + */ +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 the 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 Possible implementation in the future: Select which group of registers to read periodically + */ +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) + * + * 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); + +// motion manager interface functions +/** + * \brief Function to handle read and write operations of 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. + * + * 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. + */ +void smart_charger_process(void); + + +#ifdef __cplusplus +} +#endif + +#endif \ No newline at end of file diff --git a/src/smart_charger.c b/src/smart_charger.c new file mode 100644 index 0000000..b6ce84e --- /dev/null +++ b/src/smart_charger.c @@ -0,0 +1,207 @@ + +#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]; +#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) + +//--------------------------------------- + +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_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=&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 +{ + 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; +} + +/*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) +{ +} + +void smart_charger_process_write_cmd(unsigned short int address,unsigned short int length,unsigned char *data) +{ + uint8_t i; + uint16_t period; + + //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(BATTERY_LIMIT_CURRENT_L,address,length)&& ram_in_range(BATTERY_LIMIT_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++) + { + *infifo=data[i-address]; + infifo++; + numdata++; + //write_data[i]=data[i]; + } + } + } + +/* //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 + 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]; + else + outfifo=outfifo+2; + numdata=numdata-2; + if(numdata==0) //no hay datos nuevos en la fifo + smart_charger_write = 0x00; + } + + //Read smart charger + 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); +} + -- GitLab