Commit cca6cf66 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Almost complete version of the firmware. Problems with too much data in the SMBus.

Completed the charger part of the PC driver.
parent af9f919e
# driver source files
SET(sources battery_charger.cpp)
SET(sources battery_charger.cpp battery_charger_exceptions.cpp)
# application header files
SET(headers battery_charger.h)
SET(headers battery_charger.h battery_charger_exceptions.h ../../../stm32_code/include/dyn_battery.h)
# locate the necessary dependencies
FIND_PACKAGE(iriutils REQUIRED)
FIND_PACKAGE(comm REQUIRED)
FIND_PACKAGE(dynamixel REQUIRED)
# add the necessary include directories
INCLUDE_DIRECTORIES(.)
INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${comm_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${dynamixel_INCLUDE_DIR})
# create the shared library
ADD_LIBRARY(battery_charger SHARED ${sources})
# link necessary libraries
TARGET_LINK_LIBRARIES(battery_charger ${iriutils_LIBRARY})
TARGET_LINK_LIBRARIES(battery_charger ${comm_LIBRARY})
TARGET_LINK_LIBRARIES(battery_charger ${dynamixel_LIBRARY})
TARGET_LINK_LIBRARIES(battery_charger pthread)
INSTALL(TARGETS battery_charger
RUNTIME DESTINATION bin
LIBRARY DESTINATION lib/iridrivers
......
#include "battery_charger.h"
#include "../../../stm32_code/include/dyn_battery.h"
#include "exceptions.h"
#include "battery_charger_exceptions.h"
CBattery_Charger::CBattery_Charger()
CBattery_Charger::CBattery_Charger(std::string& battery_id,unsigned char bus_id,int baudrate,unsigned char dev_id)
{
this->dyn_server=CDynamixelServer::instance();
this->dyn_device=NULL;
try{
this->dyn_server->config_bus(bus_id,baudrate);
this->dyn_device=this->dyn_server->get_device(dev_id);
this->battery_id=battery_id;
}catch(CException &e){
/* handle exceptions */
if(this->dyn_device!=NULL)
delete this->dyn_device;
this->dyn_device=NULL;
throw;
}
}
CBattery_Charger::CBattery_Charger(std::string& battery_id,std::string& bus_id,int baudrate,unsigned char dev_id)
{
this->dyn_server=CDynamixelServer::instance();
this->dyn_device=NULL;
try{
this->dyn_server->config_bus(bus_id,baudrate);
this->dyn_device=this->dyn_server->get_device(dev_id);
this->battery_id=battery_id;
}catch(CException &e){
/* handle exceptions */
if(this->dyn_device!=NULL)
delete this->dyn_device;
this->dyn_device=NULL;
throw;
}
}
void CBattery_Charger::check_model(void)
{
unsigned short int model;
unsigned char fw_ver;
if(this->dyn_device==NULL)
{
/* handle exceptions */
throw CBatteryChargerException(_HERE_,"The Dynamixel device is not properly configured.");
}
else
{
this->dyn_device->read_byte_register(BATTERY_VERSION,&fw_ver);
this->dyn_device->read_word_register(BATTERY_MODEL_NUMBER_L,&model);
if(model!=DYN_BATTERY_MODEL)
throw CBatteryChargerException(_HERE_,"The connected device is not a battery charger module.");
}
}
/* charger functions */
void CBattery_Charger::get_no_battery_output(double *voltage_v,double *current_a)
{
unsigned short int data[2];
this->dyn_device->read_registers(BATTERY_OUTPUT_CURRENT_L,(unsigned char *)data,4);
*voltage_v=data[1]/1000.0;
*current_a=data[0]/1000.0;
}
void CBattery_Charger::set_no_battery_output(double voltage_v,double current_a)
{
unsigned short int current,voltage;
unsigned char data[4];
current=current_a*1000;
voltage=voltage_v*1000;
data[0]=current&0x00FF;
data[1]=(current>>8)&0x00FF;
data[2]=voltage&0x00FF;
data[3]=(voltage>>8)&0x00FF;
/* write byte by byte because of the low speed of the EEPROM */
this->dyn_device->write_byte_register(BATTERY_OUTPUT_CURRENT_L,data[0]);
this->dyn_device->write_byte_register(BATTERY_OUTPUT_CURRENT_H,data[1]);
this->dyn_device->write_byte_register(BATTERY_OUTPUT_VOLTAGE_L,data[2]);
this->dyn_device->write_byte_register(BATTERY_OUTPUT_VOLTAGE_H,data[3]);
}
double CBattery_Charger::get_max_input_current(void)
{
unsigned short int current;
this->dyn_device->read_word_register(BATTERY_INPUT_MAX_CURRENT_L,&current);
return current/500.0;
}
void CBattery_Charger::set_max_input_current(double current_a)
{
unsigned short int current;
current=current_a*500;
this->dyn_device->write_word_register(BATTERY_INPUT_MAX_CURRENT_L,current);
}
double CBattery_Charger::get_input_current(void)
{
unsigned short int current;
this->dyn_device->read_word_register(BATTERY_INPUT_CURRENT_L,&current);
return current/1000.0;
}
void CBattery_Charger::get_charger_output(double *voltage_v,double *current_a)
{
unsigned short int data[2];
this->dyn_device->read_registers(BATTERY_CHARGE_CURRENT_L,(unsigned char *)data,4);
*voltage_v=data[1]/1000.0;
*current_a=data[0]/1000.0;
}
bool CBattery_Charger::is_ac_input_present(void)
{
unsigned char status;
this->dyn_device->read_byte_register(BATTERY_CHARGER_STATUS,&status);
if(status&0x02)
return true;
else
return false;
}
bool CBattery_Charger::is_battery_present(void)
{
unsigned char status;
this->dyn_device->read_byte_register(BATTERY_CHARGER_STATUS,&status);
if(status&0x04)
return true;
else
return false;
}
bool CBattery_Charger::is_charger_enabled(void)
{
unsigned char status;
this->dyn_device->read_byte_register(BATTERY_CHARGER_STATUS,&status);
if(status&0x01)
return true;
else
return false;
}
/* fuel gauge functions */
CBattery_Charger::~CBattery_Charger()
{
if(this->dyn_device!=NULL)
{
this->dyn_server->free_device(this->dyn_device->get_id());
delete this->dyn_device;
this->dyn_device=NULL;
}
}
#ifndef _BATTERY_CHARGER_H
#define _BATTERY_CHARGER_H
#include "dynamixelserver.h"
#include "dynamixel.h"
#include "threadserver.h"
#include "eventserver.h"
class CBattery_Charger
{
private:
std::string battery_id;
CDynamixelServer *dyn_server;
CDynamixel *dyn_device;
protected:
void check_model(void);
public:
CBattery_Charger();
CBattery_Charger(std::string& cont_id,unsigned char bus_id,int baudrate,unsigned char dev_id);
CBattery_Charger(std::string& cont_id,std::string& bus_id,int baudrate,unsigned char dev_id);
/* charger functions */
void get_no_battery_output(double *voltage_v,double *current_a);
void set_no_battery_output(double voltage_v,double current_a);
double get_max_input_current(void);
void set_max_input_current(double current_a);
double get_input_current(void);
void get_charger_output(double *voltage_v,double *current_a);
bool is_ac_input_present(void);
bool is_battery_present(void);
bool is_charger_enabled(void);
/* fuel gauge functions */
~CBattery_Charger();
};
......
#include "battery_charger_exceptions.h"
#include <sstream>
#include <string.h>
#include <stdio.h>
const std::string battery_charger_error_message="Battery_Charger error - ";
CBatteryChargerException::CBatteryChargerException(const std::string& where,const std::string &error_msg):CException(where,battery_charger_error_message)
{
std::stringstream text;
this->error_msg+=error_msg;
}
#ifndef _BATTERY_CHARGER_EXCEPTIONS
#define _BATTERY_CHARGER_EXCEPTIONS
#include "exceptions.h"
/**
* \brief Battery Charger exception
*
* This class is used to report general errors of the battery charger device. These
* errors are normally caused by invalid parameters passed to the functions.
* In this case the error message is used to identify which error has ocuurred.
*
*/
class CBatteryChargerException : public CException
{
public:
/**
* \brief
*/
CBatteryChargerException(const std::string& where,const std::string &error_msg);
};
#endif
# create an example application
ADD_EXECUTABLE(battery_charger_test battery_charger_test.cpp)
# link necessary libraries
TARGET_LINK_LIBRARIES(battery_charger_test battery_charger)
TARGET_LINK_LIBRARIES(battery_charger_test battery_charger ${iriutils_LIBRARY} ${comm_LIBRARY} ${dynamixel_LIBRARY} pthread)
#include "battery_charger.h"
#include "exceptions.h"
#include <iostream>
std::string battery_name="battery";
std::string battery_serial="A401293W";
int main(int argc, char *argv[])
{
double voltage,current;
int i=0;
try{
CBattery_Charger battery(battery_name,battery_serial,57600,0xC0);
for(i=0;i<100;i++)
{
battery.get_no_battery_output(&voltage,&current);
std::cout << "Voltage: " << voltage << " V, Current: " << current << " A" << std::endl;
current=battery.get_max_input_current();
std::cout << "Max. input current: " << current << " A" << std::endl;
current=battery.get_input_current();
std::cout << "Input current: " << current << " A" << std::endl;
battery.get_charger_output(&voltage,&current);
std::cout << "Actual Voltage: " << voltage << " V, Actual Current: " << current << " A" << std::endl;
sleep(1);
}
}catch(CException &e){
std::cout << e.what() << std::endl;
}
}
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