Skip to content

Commit

Permalink
Merge pull request #138 from UbiquityRobotics/battery-fix
Browse files Browse the repository at this point in the history 
Battery percentage calculator
  • Loading branch information
@MoffKalast
MoffKalast authored Aug 30, 2021
2 parents 5b920c2 + 91c17d9 commit e3e071e
Show file tree
Hide file tree
Showing 2 changed files with 101 additions and 36 deletions.
3 changes: 2 additions & 1 deletion include/ubiquity_motor/motor_hardware.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -122,7 +122,8 @@ class MotorHardware : public hardware_interface::RobotHW {
void clearCommands();
void readInputs();
void writeSpeeds();
void writeSpeedsInRadians(double left_radians, double right_radians);
void writeSpeedsInRadians(double left_radians, double right_radians);
float calculateBatteryPercentage(float voltage, int cells, const float* type);
void requestFirmwareVersion();
void requestFirmwareDate();
void setParams(FirmwareParams firmware_params);
Expand Down
134 changes: 99 additions & 35 deletions src/motor_hardware.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,6 +61,37 @@ int32_t g_odomLeft = 0;
int32_t g_odomRight = 0;
int32_t g_odomEvent = 0;


//lead acid battery percentage levels for a single cell
const static float SLA_AGM[11] = {
1.8, // 0
1.89, // 10
1.93, // 20
1.96, // 30
1.98, // 40
2.01, // 50
2.03, // 60
2.05, // 70
2.07, // 80
2.08, // 90
2.175, // 100
};

//li-ion battery percentage levels for a single cell
const static float LI_ION[11] = {
3.3, // 0
3.49, // 10
3.53, // 20
3.55, // 30
3.60, // 40
3.64, // 50
3.70, // 60
3.80, // 70
3.85, // 80
4.05, // 90
4.20, // 100
};

// This utility opens and reads 1 or more bytes from a device on an I2C bus
// This method was taken on it's own from a big I2C class we may choose to use later
static int i2c_BufferRead(const char *i2cDevFile, uint8_t i2cAddr,
Expand Down Expand Up @@ -214,15 +245,15 @@ void MotorHardware::readInputs() {
} else {
ROS_INFO_ONCE("Firmware version %d", mm.getData());
firmware_version = mm.getData();
motor_diag_.firmware_version = firmware_version;
motor_diag_.firmware_version = firmware_version;
}
break;

case MotorMessage::REG_FIRMWARE_DATE:
// Firmware date is only supported as of fw version MIN_FW_FIRMWARE_DATE
ROS_INFO_ONCE("Firmware date 0x%x (format 0xYYYYMMDD)", mm.getData());
firmware_date = mm.getData();
motor_diag_.firmware_date = firmware_date;
motor_diag_.firmware_date = firmware_date;
break;

case MotorMessage::REG_BOTH_ODOM: {
Expand All @@ -248,7 +279,7 @@ void MotorHardware::readInputs() {
joints_[WheelJointLocation::Left].position += (odomLeft / ticks_per_radian);
joints_[WheelJointLocation::Right].position += (odomRight / ticks_per_radian);

motor_diag_.odom_update_status.tick(); // Let diag know we got odom
motor_diag_.odom_update_status.tick(); // Let diag know we got odom
break;
}
case MotorMessage::REG_BOTH_ERROR: {
Expand All @@ -274,28 +305,28 @@ void MotorHardware::readInputs() {
// Set radians per encoder tic based on encoder specifics
if (data & MotorMessage::OPT_ENC_6_STATE) {
ROS_WARN_ONCE("Encoder Resolution: 'Enhanced'");
fw_params.hw_options |= MotorMessage::OPT_ENC_6_STATE;
fw_params.hw_options |= MotorMessage::OPT_ENC_6_STATE;
ticks_per_radian = TICKS_PER_RADIAN_ENC_3_STATE * 2;
} else {
ROS_WARN_ONCE("Encoder Resolution: 'Standard'");
fw_params.hw_options &= ~MotorMessage::OPT_ENC_6_STATE;
fw_params.hw_options &= ~MotorMessage::OPT_ENC_6_STATE;
ticks_per_radian = TICKS_PER_RADIAN_ENC_3_STATE;
}

if (data & MotorMessage::OPT_WHEEL_TYPE_THIN) {
ROS_WARN_ONCE("Wheel type is: 'thin'");
fw_params.hw_options |= MotorMessage::OPT_WHEEL_TYPE_THIN;
fw_params.hw_options |= MotorMessage::OPT_WHEEL_TYPE_THIN;
} else {
ROS_WARN_ONCE("Wheel type is: 'standard'");
fw_params.hw_options &= ~MotorMessage::OPT_WHEEL_TYPE_THIN;
fw_params.hw_options &= ~MotorMessage::OPT_WHEEL_TYPE_THIN;
}

if (data & MotorMessage::OPT_WHEEL_DIR_REVERSE) {
ROS_WARN_ONCE("Wheel direction is: 'reverse'");
fw_params.hw_options |= MotorMessage::OPT_WHEEL_DIR_REVERSE;
fw_params.hw_options |= MotorMessage::OPT_WHEEL_DIR_REVERSE;
} else {
ROS_WARN_ONCE("Wheel direction is: 'standard'");
fw_params.hw_options &= ~MotorMessage::OPT_WHEEL_DIR_REVERSE;
fw_params.hw_options &= ~MotorMessage::OPT_WHEEL_DIR_REVERSE;
}
break;
}
Expand All @@ -305,31 +336,31 @@ void MotorHardware::readInputs() {

if (data & MotorMessage::LIM_M1_PWM) {
ROS_WARN("left PWM limit reached");
motor_diag_.left_pwm_limit = true;
motor_diag_.left_pwm_limit = true;
}
if (data & MotorMessage::LIM_M2_PWM) {
ROS_WARN("right PWM limit reached");
motor_diag_.right_pwm_limit = true;
motor_diag_.right_pwm_limit = true;
}
if (data & MotorMessage::LIM_M1_INTEGRAL) {
ROS_DEBUG("left Integral limit reached");
motor_diag_.left_integral_limit = true;
motor_diag_.left_integral_limit = true;
}
if (data & MotorMessage::LIM_M2_INTEGRAL) {
ROS_DEBUG("right Integral limit reached");
motor_diag_.right_integral_limit = true;
motor_diag_.right_integral_limit = true;
}
if (data & MotorMessage::LIM_M1_MAX_SPD) {
ROS_WARN("left Maximum speed reached");
motor_diag_.left_max_speed_limit = true;
motor_diag_.left_max_speed_limit = true;
}
if (data & MotorMessage::LIM_M2_MAX_SPD) {
ROS_WARN("right Maximum speed reached");
motor_diag_.right_max_speed_limit = true;
motor_diag_.right_max_speed_limit = true;
}
if (data & MotorMessage::LIM_PARAM_LIMIT) {
ROS_WARN_ONCE("parameter limit in firmware");
motor_diag_.param_limit_in_firmware = true;
motor_diag_.param_limit_in_firmware = true;
}
break;
}
Expand All @@ -342,36 +373,38 @@ void MotorHardware::readInputs() {
bstate.charge = std::numeric_limits<float>::quiet_NaN();
bstate.capacity = std::numeric_limits<float>::quiet_NaN();
bstate.design_capacity = std::numeric_limits<float>::quiet_NaN();
bstate.percentage = std::max(0.0, std::min(1.0, (bstate.voltage - 20.0) * 0.125));

// Hardcoded to a sealed lead acid 12S battery, but adjustable for future use
bstate.percentage = calculateBatteryPercentage(bstate.voltage, 12, SLA_AGM);
bstate.power_supply_status = sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN;
bstate.power_supply_health = sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN;
bstate.power_supply_technology = sensor_msgs::BatteryState::POWER_SUPPLY_TECHNOLOGY_UNKNOWN;
battery_state.publish(bstate);

motor_diag_.battery_voltage = bstate.voltage;
motor_diag_.battery_voltage_low_level = MotorHardware::fw_params.battery_voltage_low_level;
motor_diag_.battery_voltage_critical = MotorHardware::fw_params.battery_voltage_critical;
motor_diag_.battery_voltage = bstate.voltage;
motor_diag_.battery_voltage_low_level = MotorHardware::fw_params.battery_voltage_low_level;
motor_diag_.battery_voltage_critical = MotorHardware::fw_params.battery_voltage_critical;
break;
}
case MotorMessage::REG_MOT_PWR_ACTIVE: { // Starting with rev 5.0 board we can see power state
int32_t data = mm.getData();

if (data & MotorMessage::MOT_POW_ACTIVE) {
if (estop_motor_power_off == true) {
if (estop_motor_power_off == true) {
ROS_WARN("Motor power has gone from inactive to active. Most likely from ESTOP switch");
}
estop_motor_power_off = false;
estop_motor_power_off = false;
} else {
if (estop_motor_power_off == false) {
if (estop_motor_power_off == false) {
ROS_WARN("Motor power has gone inactive. Most likely from ESTOP switch active");
}
estop_motor_power_off = true;
estop_motor_power_off = true;
}
motor_diag_.estop_motor_power_off = estop_motor_power_off; // A copy for diagnostics topic

std_msgs::Bool estop_message;
estop_message.data = !estop_motor_power_off;
motor_power_active.publish(estop_message);
std_msgs::Bool estop_message;
estop_message.data = !estop_motor_power_off;
motor_power_active.publish(estop_message);
}

case MotorMessage::REG_TINT_BOTH_WHLS: { // As of v41 show time between wheel enc edges
Expand Down Expand Up @@ -406,13 +439,44 @@ void MotorHardware::readInputs() {
}
}

// calculateBatteryPercentage() takes in battery voltage, number of cells, and type; returns approximate percentage
//
// A battery type is defined by an array of 11 values, each corresponding to one 10% sized step from 0% to 100%.
// If the values fall between the steps, they are linearly interpolated to give a more accurate reading.
//
float MotorHardware::calculateBatteryPercentage(float voltage, int cells, const float* type) {
float onecell = voltage / (float)cells;

if(onecell >= type[10])
return 1.0;
else if(onecell <= type[0])
return 0.0;

int upper = 0;
int lower = 0;

for(int i = 0; i < 11; i++){
if(onecell > type[i]){
lower = i;
}else{
upper = i;
break;
}
}

float deltavoltage = type[upper] - type[lower];
float between_percent = (onecell - type[lower]) / deltavoltage;

return (float)lower * 0.1 + between_percent * 0.1;
}

// writeSpeedsInRadians() Take in radians per sec for wheels and send in message to controller
//
// A direct write speeds that allows caller setting speeds in radians
// This interface allows maintaining of system speed in state but override to zero
// which is of value for such a case as ESTOP implementation
//
void MotorHardware::writeSpeedsInRadians(double left_radians, double right_radians) {
void MotorHardware::writeSpeedsInRadians(double left_radians, double right_radians) {
MotorMessage both;
both.setRegister(MotorMessage::REG_BOTH_SPEED_SET);
both.setType(MotorMessage::TYPE_WRITE);
Expand Down Expand Up @@ -852,32 +916,32 @@ void MotorDiagnostics::limit_status(DiagnosticStatusWrapper &stat) {
stat.summary(DiagnosticStatus::OK, "Limits reached:");
if (left_pwm_limit) {
stat.mergeSummary(DiagnosticStatusWrapper::ERROR, " left pwm,");
left_pwm_limit = false;
left_pwm_limit = false;
}
if (right_pwm_limit) {
stat.mergeSummary(DiagnosticStatusWrapper::ERROR, " right pwm,");
right_pwm_limit = false;
right_pwm_limit = false;
}
if (left_integral_limit) {
stat.mergeSummary(DiagnosticStatusWrapper::WARN, " left integral,");
left_integral_limit = false;
left_integral_limit = false;
}
if (right_integral_limit) {
stat.mergeSummary(DiagnosticStatusWrapper::WARN, " right integral,");
right_integral_limit = false;
right_integral_limit = false;
}
if (left_max_speed_limit) {
stat.mergeSummary(DiagnosticStatusWrapper::WARN, " left speed,");
left_max_speed_limit = false;
left_max_speed_limit = false;
}
if (right_max_speed_limit) {
stat.mergeSummary(DiagnosticStatusWrapper::WARN, " right speed,");
right_max_speed_limit = false;
right_max_speed_limit = false;
}
if (param_limit_in_firmware) {
// A parameter was sent to firmware that was out of limits for the firmware register
stat.mergeSummary(DiagnosticStatusWrapper::WARN, " firmware limit,");
param_limit_in_firmware = false;
param_limit_in_firmware = false;
}
}

Expand Down

0 comments on commit e3e071e

Please sign in to comment.