 
 
 float getBatteryVoltage() {
-  return 5.0 * analogRead(BATTERY_READ_PIN) / 1024.0;
+  // unsigned int batteryVoltage = analogRead(BATTERY_READ_PIN) * 5000L * 3/2 / 1023;
+  // return 5.0 * (float)analogRead(BATTERY_READ_PIN) / 1024.0;
+  return (unsigned int)(analogRead(BATTERY_READ_PIN) * 5000L * 3/2 / 1023);
 }
 
 void cmd_vel_callback( const geometry_msgs::Twist& twist_msg) {

ros::Subscriber<geometry_msgs::Twist> cmd_vel_topic("cmd_vel", &cmd_vel_callback);

// Battery Voltage Publisher
std_msgs::Float32 voltage_msg;
ros::Publisher battery_topic("battery_voltage", &voltage_msg);

void publishBatteryVoltage() {
  voltage_msg.data = getBatteryVoltage();
  battery_topic.publish( &voltage_msg );

#include <ZumoMotors.h>
#include "Timer.h"
#include <geometry_msgs/Twist.h>
#include <std_msgs/UInt16.h>

// Distance between both wheels
#define WHEEL_OFFSET_M 0.1 // 10cm
