561 lines
16 KiB
C++
561 lines
16 KiB
C++
/*
|
|
Adept MobileRobots Robotics Interface for Applications (ARIA)
|
|
Copyright (C) 2004, 2005 ActivMedia Robotics LLC
|
|
Copyright (C) 2006, 2007, 2008, 2009, 2010 MobileRobots Inc.
|
|
Copyright (C) 2011, 2012, 2013 Adept Technology
|
|
|
|
This program is free software; you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation; either version 2 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program; if not, write to the Free Software
|
|
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
|
|
If you wish to redistribute ARIA under different terms, contact
|
|
Adept MobileRobots for information about a commercial version of ARIA at
|
|
robots@mobilerobots.com or
|
|
Adept MobileRobots, 10 Columbia Drive, Amherst, NH 03031; +1-603-881-7960
|
|
*/
|
|
#ifndef ARBATTERYMTX_H
|
|
#define ARBATTERYMTX_H
|
|
|
|
#include "ariaTypedefs.h"
|
|
#include "ArRangeDevice.h"
|
|
#include "ArFunctor.h"
|
|
#include "ArRobot.h"
|
|
#include "ArRobotPacket.h"
|
|
#include "ArRobotConnector.h"
|
|
|
|
|
|
|
|
// Packets are in the format of
|
|
// 2 bytes header (0xfa 0xba)
|
|
// 1 byte length
|
|
// 1 byte command
|
|
// xx bytes command specific / args
|
|
// 2 bytes checksum
|
|
//
|
|
|
|
|
|
/**
|
|
@since 2.8.0
|
|
*/
|
|
class ArBatteryMTX : public ArASyncTask
|
|
{
|
|
public:
|
|
/// Constructor
|
|
AREXPORT ArBatteryMTX(
|
|
int batteryBoardNum = 0,
|
|
const char * name = "MTXBattery",
|
|
ArDeviceConnection *conn = NULL,
|
|
ArRobot *robot = NULL);
|
|
/// Destructor
|
|
AREXPORT virtual ~ArBatteryMTX();
|
|
// Grabs the new readings from the robot and adds them to the buffers
|
|
// (Primarily for internal use.)
|
|
//AREXPORT void processReadings(void);
|
|
|
|
int getBoardNum(void)
|
|
{ return myBoardNum; }
|
|
|
|
/// Sets the robot pointer, also attaches its process function to the
|
|
/// robot as a Sensor Interpretation task.
|
|
AREXPORT virtual void setRobot(ArRobot *robot);
|
|
|
|
/// Very Internal call that gets the packet sender, shouldn't be used
|
|
ArRobotPacketSender *getPacketSender(void)
|
|
{ return mySender; }
|
|
/// Very Internal call that gets the packet sender, shouldn't be used
|
|
ArRobotPacketReceiver *getPacketReceiver(void)
|
|
{ return myReceiver; }
|
|
|
|
/// Sets the device this instance receives packets from
|
|
AREXPORT void setDeviceConnection(ArDeviceConnection *conn);
|
|
/// Gets the device this instance receives packets from
|
|
AREXPORT ArDeviceConnection *getDeviceConnection(void);
|
|
|
|
AREXPORT int getAsyncConnectState(void);
|
|
|
|
ArRobotPacket getCellPacket()
|
|
{ return myCellPacket; }
|
|
|
|
AREXPORT virtual bool blockingConnect(bool sendTracking, bool recvTracking);
|
|
AREXPORT virtual bool disconnect(void);
|
|
virtual bool isConnected(void) { return myIsConnected; }
|
|
virtual bool isTryingToConnect(void)
|
|
{
|
|
if (myStartConnect)
|
|
return true;
|
|
else if (myTryingToConnect)
|
|
return true;
|
|
else
|
|
return false;
|
|
}
|
|
|
|
/// Lock this device
|
|
virtual int lockDevice() { return(myDeviceMutex.lock());}
|
|
/// Try to lock this device
|
|
virtual int tryLockDevice() {return(myDeviceMutex.tryLock());}
|
|
/// Unlock this device
|
|
virtual int unlockDevice() {return(myDeviceMutex.unlock());}
|
|
|
|
AREXPORT void logBatteryInfo(ArLog::LogLevel level = ArLog::Normal);
|
|
AREXPORT void logCellInfo(ArLog::LogLevel level = ArLog::Normal);
|
|
void log(ArLog::LogLevel level = ArLog::Normal)
|
|
{
|
|
logBatteryInfo(level);
|
|
logCellInfo(level);
|
|
}
|
|
|
|
AREXPORT bool sendPowerOff();
|
|
AREXPORT bool sendPowerOffCancel();
|
|
AREXPORT bool sendStopCharging();
|
|
AREXPORT bool sendStartCharging();
|
|
AREXPORT bool sendSetPowerOffDelay(unsigned int msDelay);
|
|
AREXPORT bool sendSetRealTimeClock(unsigned int secSinceEpoch);
|
|
AREXPORT bool sendResetCellData();
|
|
AREXPORT bool sendSetReserveValue(unsigned short hundredthOfPercent);
|
|
AREXPORT bool sendSetBalanceValue(unsigned short hundredthOfPercent);
|
|
AREXPORT bool sendEmergencyPowerOff();
|
|
AREXPORT bool sendSystemInfo(unsigned char dataValue);
|
|
AREXPORT bool sendCellInfo(unsigned char dataValue);
|
|
AREXPORT bool sendBasicInfo(unsigned char dataValue);
|
|
|
|
AREXPORT void updateSystemInfo(unsigned char *buf);
|
|
AREXPORT void updateCellInfo(unsigned char *buf);
|
|
AREXPORT void updateBasicInfo(unsigned char *buf);
|
|
|
|
// need to figure out how to pass back the system and cell info
|
|
//AREXPORT bool fetchSystemInfo();
|
|
//AREXPORT bool fetchCellInfo();
|
|
|
|
// basic info
|
|
/// Charge estimate (in percentage, 0-100)
|
|
double getChargeEstimate(void) const
|
|
{ return myChargeEstimate; }
|
|
/// Current draw (amps, negative is charging)
|
|
double getCurrentDraw(void) const
|
|
{ return myCurrentDraw; }
|
|
/// volts
|
|
double getPackVoltage(void) const
|
|
{ return myPackVoltage; }
|
|
int getStatusFlags(void) const
|
|
{ return myStatusFlags; }
|
|
int getErrorFlags(void) const
|
|
{ return myErrorFlags; }
|
|
|
|
bool onCharger(void) const
|
|
{ return myOnCharger; }
|
|
ArRobot::ChargeState getChargeState(void) const
|
|
{ return myChargeState; }
|
|
int getChargeStateAsInt(void) const
|
|
{ return myChargeState; }
|
|
|
|
// system info
|
|
int getId(void) const
|
|
{ return myId; }
|
|
int getFirmwareVersion(void) const
|
|
{ return myFirmwareVersion; }
|
|
int getSerialNumber(void) const
|
|
{ return mySerialNumber; }
|
|
//int getCurrentTime(void) const
|
|
// { return myCurrentTime; }
|
|
long long getCurrentTime(void) const
|
|
{ return myCurrentTime; }
|
|
long long getLastChargeTime(void) const
|
|
{ return myLastChargeTime; }
|
|
int getChargeRemainingEstimate(void) const
|
|
{ return myChargeRemainingEstimate; }
|
|
int getCapacityEstimate(void) const
|
|
{ return myCapacityEstimate; }
|
|
double getDelay(void) const
|
|
{ return myDelay; }
|
|
int getCycleCount(void) const
|
|
{ return myCycleCount; }
|
|
double getTemperature(void) const
|
|
{ return myTemperature; }
|
|
double getPaddleVolts(void) const
|
|
{ return myPaddleVolts; }
|
|
double getVoltage(void) const
|
|
{ return myVoltage; }
|
|
double getFuseVoltage(void) const
|
|
{ return myFuseVoltage; }
|
|
double getChargeCurrent(void) const
|
|
{ return myChargeCurrent; }
|
|
double getDisChargeCurrent(void) const
|
|
{ return myDisChargeCurrent; }
|
|
double getCellImbalance(void) const
|
|
{ return myCellImbalance; }
|
|
double getImbalanceQuality(void) const
|
|
{ return myImbalanceQuality; }
|
|
double getReserveChargeValue(void) const
|
|
{ return myReserveChargeValue; }
|
|
|
|
// cell info
|
|
int getNumCells(void) const
|
|
{ return myNumCells; }
|
|
|
|
|
|
int getCellFlag(int cellNum) const
|
|
{
|
|
std::map<int, CellInfo *>::const_iterator iter =
|
|
myCellNumToInfoMap.find(cellNum);
|
|
if (iter == myCellNumToInfoMap.end())
|
|
return -1;
|
|
else {
|
|
CellInfo *info = iter->second;
|
|
return(info->myCellFlags);
|
|
} }
|
|
|
|
int getCellCapacity(int cellNum) const
|
|
{
|
|
std::map<int, CellInfo *>::const_iterator iter =
|
|
myCellNumToInfoMap.find(cellNum);
|
|
if (iter == myCellNumToInfoMap.end())
|
|
return -1;
|
|
else {
|
|
CellInfo *info = iter->second;
|
|
return(info->myCellCapacity);
|
|
} }
|
|
|
|
int getCellCharge(int cellNum) const
|
|
{
|
|
std::map<int, CellInfo *>::const_iterator iter =
|
|
myCellNumToInfoMap.find(cellNum);
|
|
if (iter == myCellNumToInfoMap.end())
|
|
return -1;
|
|
else {
|
|
CellInfo *info = iter->second;
|
|
return(info->myCellCharge);
|
|
} }
|
|
|
|
double getCellVoltage(int cellNum) const
|
|
{
|
|
std::map<int, CellInfo *>::const_iterator iter =
|
|
myCellNumToInfoMap.find(cellNum);
|
|
if (iter == myCellNumToInfoMap.end())
|
|
return -1;
|
|
else {
|
|
CellInfo *info = iter->second;
|
|
return(info->myCellVoltage);
|
|
} }
|
|
|
|
/// Request a continous stream of packets
|
|
AREXPORT void requestContinuousSysInfoPackets(void);
|
|
/// Stop the stream of packets
|
|
AREXPORT void stopSysInfoPackets(void);
|
|
/// See if we've requested packets
|
|
AREXPORT bool haveRequestedSysInfoPackets(void);
|
|
|
|
/// Request a continous stream of packets
|
|
AREXPORT void requestContinuousCellInfoPackets(void);
|
|
/// Stop the stream of packets
|
|
AREXPORT void stopCellInfoPackets(void);
|
|
/// See if we've requested packets
|
|
AREXPORT bool haveRequestedCellInfoPackets(void);
|
|
|
|
AREXPORT virtual const char *getName(void) const;
|
|
|
|
void setInfoLogLevel(ArLog::LogLevel infoLogLevel)
|
|
{ myInfoLogLevel = infoLogLevel; }
|
|
|
|
/// Gets the default port type for the battery
|
|
const char *getDefaultPortType(void) { return myDefaultPortType.c_str(); }
|
|
|
|
/// Gets the default port type for the battery
|
|
const char *getDefaultTcpPort(void) { return myDefaultTcpPort.c_str(); }
|
|
|
|
/// Sets the numter of seconds without a response until connection assumed lost
|
|
virtual void setConnectionTimeoutSeconds(double seconds)
|
|
{ ArLog::log(ArLog::Normal,
|
|
"%s::setConnectionTimeoutSeconds: Setting timeout to %g secs",
|
|
getName(), seconds);
|
|
myTimeoutSeconds = seconds; }
|
|
/// Gets the number of seconds without a response until connection assumed lost
|
|
virtual double getConnectionTimeoutSeconds(void)
|
|
{return myTimeoutSeconds; }
|
|
/// check for lost connections
|
|
AREXPORT bool checkLostConnection(void);
|
|
/// disconnect
|
|
AREXPORT void disconnectOnError(void);
|
|
/// Gets the time data was last receieved
|
|
ArTime getLastReadingTime(void) { return myLastReading; }
|
|
/// Gets the number of battery readings received in the last second
|
|
AREXPORT int getReadingCount(void);
|
|
// Function called in sensorInterp to indicate that a
|
|
// reading was received
|
|
AREXPORT virtual void internalGotReading(void);
|
|
|
|
/// Adds a callback for when disconnection happens because of an error
|
|
void addDisconnectOnErrorCB(ArFunctor *functor,
|
|
int position = 51)
|
|
{ myDisconnectOnErrorCBList.addCallback(functor, position); }
|
|
|
|
/// Removes a callback for when disconnection happens because of an error
|
|
void remDisconnectOnErrorCB(ArFunctor *functor)
|
|
{ myDisconnectOnErrorCBList.remCallback(functor); }
|
|
|
|
|
|
/// Adds a callback for when the battery is powering off
|
|
void addBatteryPoweringOffCB(ArFunctor *functor,
|
|
int position = 51)
|
|
{ myBatteryPoweringOffCBList.addCallback(functor, position); }
|
|
|
|
/// Removes a callback for when the battery is powering off
|
|
void remBatteryPoweringOffCB(ArFunctor *functor)
|
|
{ myBatteryPoweringOffCBList.remCallback(functor); }
|
|
|
|
/// Adds a callback for when the battery is powering off
|
|
void addBatteryPoweringOffCancelledCB(ArFunctor *functor,
|
|
int position = 51)
|
|
{ myBatteryPoweringOffCancelledCBList.addCallback(functor, position); }
|
|
|
|
/// Removes a callback for when the battery is powering off
|
|
void remBatteryPoweringOffCancelledCB(ArFunctor *functor)
|
|
{ myBatteryPoweringOffCancelledCBList.remCallback(functor); }
|
|
|
|
// myStatusFlags
|
|
enum StatusFlags {
|
|
STATUS_ON_CHARGER=0x0001,
|
|
STATUS_CHARGING=0x0002,
|
|
STATUS_BALANCING_ENGAGED=0x0004,
|
|
STATUS_CHARGER_ON=0x0008,
|
|
STATUS_BATTERY_POWERING_OFF=0x0010,
|
|
STATUS_MASTER_SWITCH_ON=0x0020,
|
|
STATUS_CHARGE_SWITCH_ON=0x0040,
|
|
STATUS_COMMANDED_SHUTDOWN=0x0080,
|
|
STATUS_OFF_BUTTON_PRESSED=0x0100,
|
|
STATUS_ON_BUTTON_PRESSED=0x0200,
|
|
STATUS_USER_BUTTON_PRESSED=0x0400
|
|
};
|
|
|
|
// myErrorFlags (if this is updated also change the code in interpBasicInfo
|
|
enum ErrorFlags {
|
|
ERROR_BATTERY_OVERVOLTAGE=0x0001,
|
|
ERROR_BATTERY_UNDERVOLTAGE=0x0002,
|
|
ERROR_OVERCURRENT=0x0004,
|
|
ERROR_BLOWNFUSE=0x0008,
|
|
ERROR_RTC_ERROR=0x0010,
|
|
ERROR_OVER_TEMPERATURE=0x0020,
|
|
ERROR_MASTER_SWITCH_FAULT=0x0040,
|
|
ERROR_SRAM_ERROR=0x0080,
|
|
ERROR_CHARGER_OUT_OF_VOLTAGE_RANGE=0x0100,
|
|
ERROR_CHARGER_CIRCUIT_FAULT=0x0200
|
|
};
|
|
|
|
enum Headers {
|
|
HEADER1=0xfa,
|
|
HEADER2=0xba
|
|
};
|
|
|
|
|
|
protected:
|
|
ArDeviceConnection *myConn;
|
|
int myAsyncConnectState;
|
|
std::string myName;
|
|
std::string myDefaultPortType;
|
|
std::string myDefaultTcpPort;
|
|
|
|
double myTimeoutSeconds;
|
|
bool myRobotRunningAndConnected;
|
|
|
|
ArTime myLastReading;
|
|
|
|
// packet count
|
|
time_t myTimeLastReading;
|
|
int myReadingCurrentCount;
|
|
int myReadingCount;
|
|
|
|
ArCallbackList myDisconnectOnErrorCBList;
|
|
ArCallbackList myBatteryPoweringOffCBList;
|
|
ArCallbackList myBatteryPoweringOffCancelledCBList;
|
|
|
|
ArRobot *myRobot;
|
|
ArFunctorC<ArBatteryMTX> myProcessCB;
|
|
|
|
AREXPORT virtual void batterySetName(const char *name);
|
|
AREXPORT virtual void * runThread(void *arg);
|
|
|
|
|
|
AREXPORT bool getSystemInfo();
|
|
AREXPORT bool getCellInfo();
|
|
AREXPORT bool getBasicInfo();
|
|
|
|
void interpBasicInfo(void);
|
|
void interpErrors(void);
|
|
void checkAndSetCurrentErrors(ErrorFlags errorFlag, const char *errorString);
|
|
|
|
// PS - need this because of debug log - battery won't send continuous cell
|
|
ArRobotPacket myCellPacket;
|
|
|
|
void sensorInterp(void);
|
|
void failedToConnect(void);
|
|
void clear(void);
|
|
bool myIsConnected;
|
|
bool myTryingToConnect;
|
|
bool myStartConnect;
|
|
|
|
bool myOnCharger;
|
|
ArRobot::ChargeState myChargeState;
|
|
|
|
int myBoardNum;
|
|
unsigned char myVersion;
|
|
|
|
ArLog::LogLevel myLogLevel;
|
|
|
|
//ArBatteryMTXPacketReceiver myReceiver;
|
|
|
|
|
|
ArRobotPacketReceiver *myReceiver;
|
|
ArRobotPacketSender *mySender;
|
|
|
|
ArMutex myPacketsMutex;
|
|
ArMutex myDataMutex;
|
|
ArMutex myDeviceMutex;
|
|
|
|
ArLog::LogLevel myInfoLogLevel;
|
|
|
|
//std::list<ArBatteryMTXPacket *> myPackets;
|
|
std::list<ArRobotPacket *> myPackets;
|
|
|
|
ArTime myPrevBatteryIntTime;
|
|
|
|
bool myRequestedSysInfoBatteryPackets;
|
|
bool myRequestedCellInfoBatteryPackets;
|
|
|
|
bool mySendTracking;
|
|
bool myRecvTracking;
|
|
|
|
// Protocol Commands
|
|
|
|
|
|
enum Commands {
|
|
BASIC_INFO=0x00,
|
|
SYSTEM_INFO=0x01,
|
|
CELL_INFO=0x02,
|
|
POWER_OFF_REQUEST=0x10,
|
|
POWER_OFF_CANCEL=0x11,
|
|
STOP_CHARGING=0x12,
|
|
START_CHARGING=0x13,
|
|
SET_POWER_OFF_DELAY=0x20,
|
|
SET_REAL_TIME_CLOCK=0x21,
|
|
RESET_CELL_DATA=0x22,
|
|
SET_RESERVE_VALUE=0x23,
|
|
SET_BALANCE_VALUE=0x24,
|
|
EMERGENCY_OFF=0xff
|
|
};
|
|
|
|
// SYSTEM_INFO and CELL_INFO Data
|
|
enum Data {
|
|
STOP_SENDING=0x00,
|
|
SEND_ONCE=0x01,
|
|
SEND_CONTINUOUS=0x02
|
|
};
|
|
|
|
// Length fields -
|
|
enum Sizes {
|
|
BASIC_INFO_SIZE=16,
|
|
SYSTEM_INFO_SIZE=60,
|
|
CELL_INFO_SIZE=95 // this is for 8 cells
|
|
};
|
|
|
|
// System Info
|
|
unsigned char myId;
|
|
unsigned char myFirmwareVersion;
|
|
unsigned int mySerialNumber;
|
|
long long myCurrentTime;
|
|
//unsigned int myCurrentTime;
|
|
//unsigned int myLastChargeTime;
|
|
long long myLastChargeTime;
|
|
unsigned int myChargeRemainingEstimate;
|
|
unsigned int myCapacityEstimate;
|
|
unsigned int myRawDelay;
|
|
double myDelay;
|
|
unsigned int myCycleCount;
|
|
unsigned short myRawTemperature;
|
|
double myTemperature;
|
|
unsigned short myRawPaddleVolts;
|
|
double myPaddleVolts;
|
|
unsigned short myRawVoltage;
|
|
double myVoltage;
|
|
unsigned short myRawFuseVoltage;
|
|
double myFuseVoltage;
|
|
unsigned short myRawChargeCurrent;
|
|
double myChargeCurrent;
|
|
unsigned short myRawDisChargeCurrent;
|
|
double myDisChargeCurrent;
|
|
unsigned short myRawCellImbalance;
|
|
double myCellImbalance;
|
|
unsigned short myRawImbalanceQuality;
|
|
double myImbalanceQuality;
|
|
unsigned short myRawReserveChargeValue;
|
|
double myReserveChargeValue;
|
|
|
|
// end system info
|
|
|
|
// Cell Info
|
|
|
|
// myCellFlags defines
|
|
|
|
enum CellFlags {
|
|
BALANCER_IS_ON=0x01,
|
|
OVER_VOLTAGE=0x02,
|
|
UNDER_VOLTAGE=0x04
|
|
};
|
|
|
|
|
|
struct CellInfo {
|
|
unsigned char myCellFlags;
|
|
unsigned short myRawCellVoltage;
|
|
double myCellVoltage;
|
|
unsigned short myCellCharge;
|
|
unsigned short myCellCapacity;
|
|
};
|
|
|
|
unsigned char myNumCells;
|
|
std::map <int, CellInfo *> myCellNumToInfoMap;
|
|
|
|
// end cell info
|
|
|
|
// Basic Info
|
|
|
|
unsigned short myRawChargeEstimate;
|
|
double myChargeEstimate;
|
|
unsigned short myRawCurrentDraw;
|
|
double myCurrentDraw;
|
|
unsigned short myRawPackVoltage;
|
|
double myPackVoltage;
|
|
unsigned short myStatusFlags;
|
|
unsigned short myErrorFlags;
|
|
|
|
bool myHaveSetRTC;
|
|
|
|
int myLastStatusFlags;
|
|
|
|
bool myFirstErrorFlagsCheck;
|
|
unsigned short myLastErrorFlags;
|
|
std::string myErrorString;
|
|
int myErrorCount;
|
|
std::string myLastErrorString;
|
|
int myLastErrorCount;
|
|
|
|
|
|
|
|
// end basic info
|
|
|
|
ArFunctorC<ArBatteryMTX> mySensorInterpTask;
|
|
ArRetFunctorC<bool, ArBatteryMTX> myAriaExitCB;
|
|
|
|
};
|
|
|
|
|
|
|
|
#endif // ARBATTERYMTX_H
|