Skip to content
Snippets Groups Projects
Commit 9a06538b authored by Irene Garcia Camacho's avatar Irene Garcia Camacho
Browse files

Minor changes in some files related to the smart charger module

parent 2950b8dd
No related branches found
No related tags found
2 merge requests!5Dynamixel manager,!2Smart charger fw
Hide whitespace changes
Inline Side-by-side
include/darwin_registers.h
+ 15
− 63
View file @ 9a06538b
......@@ -568,67 +568,19 @@ 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
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 | | | close left | open left | close right | open right
}darwin_registers;
......@@ -711,7 +663,7 @@ 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
......@@ -721,8 +673,8 @@ typedef enum {
#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_MEM_LENGTH 10
#define SMART_CHARGER_EEPROM_BASE 0x0062
#define SMART_CHARGER_EEPROM_LENGTH 2
#define SMART_CHARGER_DET 0x01
#define SMART_CHARGER_EN 0x02
......
include/dyn_battery.h
+ 10
− 10
View file @ 9a06538b
......@@ -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,
......
include/smart_charger.h
+ 2
− 2
View file @ 9a06538b
......@@ -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);
inline 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);
inline TDynamixelMaster* smart_charger_get_master(void);
/**
* \brief Function to set smart charger's period
*
......
src/cm730_fw.c
+ 0
− 1
View file @ 9a06538b
......@@ -14,7 +14,6 @@
int main(void)
{
uint16_t eeprom_data,period;
uint8_t i;
/* initialize the HAL module */
HAL_Init();
......
src/motion_manager.c
+ 17
− 17
View file @ 9a06538b
......@@ -322,20 +322,20 @@ 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_read_byte(&darwin_dyn_master,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_INPUT_MAX_CURRENT_L,&ram_data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L]);
break;
default: break;
}
}
}
darwin_dyn_master_disable_power();
......@@ -384,20 +384,20 @@ 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)
{
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_read_byte(&darwin_dyn_master_v2,ram_data[DARWIN_SMART_CHARGER_ID],BATTERY_INPUT_MAX_CURRENT_L,&ram_data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L]);
break;
default: break;
}
}
}
......
src/smart_charger.c
+ 22
− 21
View file @ 9a06538b
......@@ -8,17 +8,17 @@
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 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
uint8_t init_regs[6]={0xFF,0xFF,0xFF,0xFF,0xFF,0xFF};
#define fifosize 32
#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 write_fifo[FIFOSIZE]; //Vector with data to write (data_write)
unsigned char current[2];
//---------------------------------------
......@@ -29,11 +29,10 @@ 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;
// counter = 0;
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];
......@@ -41,20 +40,20 @@ void smart_charger_init(uint16_t period_us)
ram_write_table(DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L, 6, init_regs);
}
inline void smart_charger_set_version(TDynamixelMaster *master)
inline void smart_charger_set_master(TDynamixelMaster *master)
{
dyn_battery_master=master;
}
inline TDynamixelMaster* smart_charger_get_version(void)
inline 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 +93,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;//,i;
//Enable/Disable smart charger
if(ram_in_range(DARWIN_SMART_CHARGER_CNTRL,address,length))
......@@ -119,9 +118,9 @@ void smart_charger_process_write_cmd(unsigned short int address,unsigned short i
current[1]=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_H-address];
/* if(infifo==&write_fifo[fifosize]);
/* if(infifo==&write_fifo[FIFOSIZE]);
infifo=&write_fifo[0]; //go to first position of fifo
if(numdata<fifosize) //free space in fifo
if(numdata<FIFOSIZE) //free space in fifo
{
for(i=DARWIN_SMART_CHARGER_LIMIT_CURRENT_L;i<=DARWIN_SMART_CHARGER_LIMIT_CURRENT_H;i++)
{
......@@ -144,17 +143,19 @@ 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++;
{
uint8_t error;
static uint16_t counter;
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])
if(outfifo==&write_fifo[FIFOSIZE-1])
outfifo=&write_fifo[0]; //go to first position of fifo
else
outfifo=outfifo+2;
......
src/test_charger.c
+ 17
− 10
View file @ 9a06538b
......@@ -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;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment