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

Modified the smart charger's ram registers 

Selected the useful registers for the automatic charge algorithm.
Possible future work: Include other registers of the charger for read more data for other applications
(charge voltage and current, cells voltage, time of life of the batteries...)
parent c3784af3
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
include/darwin_registers.h
+ 21
− 12
View file @ f0fab19e
......@@ -570,7 +570,8 @@ typedef enum {
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,
......@@ -613,16 +614,24 @@ typedef enum {
DARWIN_BATT_CELL3_VOLTAGE_H = 0x026F,
DARWIN_BATT_CELL4_VOLTAGE_L = 0x0270,
DARWIN_BATT_CELL4_VOLTAGE_H = 0x0271,
//añadidos
DARWIN_SMART_CHARGER_CNTRL = 0x0272, // bit 7 | bit 6 | bit 5 | bit 4 | bit 3 | bit 2 | bit 1 | bit 0
// | detected | enable
DARWIN_SMART_CHARGER_ID = 0x0273,
*/
//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 = 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;
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
......@@ -703,7 +712,7 @@ typedef enum {
#define HEAD_STOP 0x02
#define HEAD_STATUS 0x10
#define GRIPPER_BASE_ADDRESS 0x0274
#define GRIPPER_BASE_ADDRESS 0x0252//0x0274
#define GRIPPER_MEM_LENGTH 1
#define GRIPPER_EEPROM_ADDRESS 0x0054
#define GRIPPER_EEPROM_LENGTH 14
......@@ -713,7 +722,7 @@ typedef enum {
#define GRIPPER_CLOSE_LEFT 0x08
#define SMART_CHARGER_BASE_ADDRESS 0x0247
#define SMART_CHARGER_MEM_LENGTH 45
#define SMART_CHARGER_MEM_LENGTH 10 //45
#define SMART_CHARGER_EEPROM_BASE 0x0060
#define SMART_CHARGER_EEPROM_LENGTH 2
#define SMART_CHARGER_DET 0x01
......
src/motion_manager.c
+ 20
− 17
View file @ f0fab19e
......@@ -11,7 +11,6 @@
#include "imu.h"
#include "smart_charger.h"
#include "dyn_battery.h"
#include "gpio.h" //
#define MANAGER_TIMER TIM5
#define ENABLE_MANAGER_TIMER_CLK __HAL_RCC_TIM5_CLK_ENABLE()
......@@ -177,7 +176,7 @@ 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
......@@ -185,7 +184,7 @@ void MANAGER_TIMER_IRQHandler(void)
// send the motion commands to the servos
manager_send_motion_command();
// get the disabled servos position
// manager_get_current_position();
// manager_get_current_position();
}
}
}
......@@ -327,12 +326,14 @@ void manager_init(uint16_t period_us)
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
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]);
break;
//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;
}
}
......@@ -384,17 +385,19 @@ void manager_init(uint16_t period_us)
//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_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;
}
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;
}
}
......
src/smart_charger.c
+ 20
− 9
View file @ f0fab19e
......@@ -2,6 +2,7 @@
#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
......@@ -91,8 +92,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;
//uint8_t i;
uint16_t period,i;
//Enable/Disable smart charger
if(ram_in_range(DARWIN_SMART_CHARGER_CNTRL,address,length))
......@@ -109,21 +110,27 @@ void smart_charger_process_write_cmd(unsigned short int address,unsigned short i
smart_charger_set_period(period);
}
//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))
//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];
infifo=&write_fifo[0]; //go to first position of fifo
if(numdata<fifosize) //free space in fifo
{
for(i=BATTERY_INPUT_MAX_CURRENT_L;i<=BATTERY_INPUT_MAX_CURRENT_H;i++)
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++; */
}
}
}
......@@ -133,13 +140,13 @@ void smart_charger_process_write_cmd(unsigned short int address,unsigned short i
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];
outfifo=&write_fifo[0]; //go to first position of fifo
else
outfifo=outfifo+2;
numdata=numdata-2;
......@@ -151,7 +158,11 @@ void smart_charger_process(void)
if(smart_charger_detected && smart_charger_enabled && counter==smart_charger_count)
{
counter = 0;
dyn_master_read_table(dyn_battery_master,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_SMART_CHARGER_AVG_TIME_EMPTY_L]);
if(error==DYN_SUCCESS)
gpio_set_led(LED_3);//verde
else
gpio_set_led(LED_4);
}
}
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