Commit 2950b8dd authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Merge branch 'smart_charger' into 'master'

Smart charger

See merge request !1
parents 59d66822 82e82c60
......@@ -616,7 +616,7 @@ typedef enum {
*/
//añadidos
DARWIN_SMART_CHARGER_ID = 0x0247,
DARWIN_SMART_CHARGER_STATUS = 0x0248,
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,
......
......@@ -6,11 +6,15 @@
#include "imu.h"
#include "darwin_time.h"
#include "darwin_dyn_slave.h"
#include "darwin_dyn_master.h"
#include "motion_manager.h"
#include "action.h"
#include "action_id.h"
int main(void)
{
uint16_t eeprom_data,period;
uint8_t i;
/* initialize the HAL module */
HAL_Init();
......
......@@ -12,12 +12,14 @@ 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
uint8_t init_regs[6]={0xFF,0xFF,0xFF,0xFF,0xFF,0xFF};
#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 current[2];
//---------------------------------------
......@@ -32,11 +34,11 @@ void smart_charger_init(uint16_t period_us)
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_count = (smart_charger_period*1000)/period_us;
smart_charger_write = 0x00;
infifo=outfifo=&write_fifo[0];
numdata=0; //fifo empty
ram_write_table(DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L, 6, init_regs);
}
inline void smart_charger_set_version(TDynamixelMaster *master)
......@@ -113,9 +115,11 @@ void smart_charger_process_write_cmd(unsigned short int address,unsigned short i
//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;
current[0]=data[DARWIN_SMART_CHARGER_LIMIT_CURRENT_L-address];
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
{
......@@ -125,13 +129,15 @@ void smart_charger_process_write_cmd(unsigned short int address,unsigned short i
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++; */
}
// }
smart_charger_write = 0x01;
}
}
......@@ -139,30 +145,38 @@ 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;
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);
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])
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;
*/
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)
if(error!=DYN_SUCCESS){
ram_write_table(DARWIN_SMART_CHARGER_AVG_TIME_EMPTY_L, 6, init_regs);
gpio_set_led(LED_RX); //naranja
}else{
gpio_set_led(LED_3);//verde
else
gpio_set_led(LED_4);
}
counter = 0;
}
}
......@@ -11,6 +11,8 @@
* Enable smart charger
* Enable motion manager timer
* Read battery status of smart charger
To test only smart charger module, comment th eother modules in the motion manager timer
*/
#include "stm32f1xx_hal.h"
......@@ -60,6 +62,8 @@ int main(void)
unsigned char data_write;
*/
////SCAN
unsigned char num;
unsigned char servo_ids[254];
......@@ -160,6 +164,20 @@ int main(void)
gpio_set_led(LED_3);
*/
//HABILITAR POWER, MOTION MANAGER, SERVOS, DEFINIRLO AL MODULO ACTION, LEVANTAR ROBOT
// darwin_dyn_master_enable_power();
// for(i=0;i<31;i++)
// {
// manager_set_module(i,MM_ACTION);
// manager_enable_servo(i);
// }
// manager_enable();
//
// action_load_page(WALK_READY);
// action_start_page();
gpio_set_led(LED_RX); //naranja
while(1);/* main function does not return */
......
Supports Markdown
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