Program hangs if line 24 of Battery.cpp is uncommented.

Battery.cpp

#include "Battery.h"


// Initilisation function for battery class.
// Params:
// float pdFactor      - factor to convert divided voltage to actual voltage. Needs to be measured manually.
// int averagingWindow - How many samples to use when averaging
// int voltagePin      - Pin number for reading voltage (Must be an analog pin)
// int currentPin      - Pin number for reading current (Must be an analog pin)
Battery::Battery(float pdFactor, int averagingWindow, int voltagePin, int currentPin) : pdFactor(pdFactor), averagingWindow(averagingWindow), voltagePin(voltagePin), currentPin(currentPin) {
	update();
	currentScale = 0.5f / current_v;
	ads1115.begin();
}

// Updates voltage and current variables.
void Battery::update() {
	getData();
}

// Averages ADC data of a window of averagingWindow size, and then updates voltage and current class variables.
void Battery::getData() {
	Serial.println("test");
	ads1115.readADC_SingleEnded(0);
	// averaging loop
	for (int i = 0; i <= averagingWindow; i++) {
		voltage_adc += analogRead(voltagePin);
		current_adc += analogRead(currentPin);
	}

	// convert ADC reading into voltage
	// voltage_adc and current_adc are being converted to floats to hopefully get better accuracy from this calculation. Untested!!
	voltage = ((float)voltage_adc / averagingWindow) * (5.0 / 1023.0) * pdFactor;
	current_v = ((float)current_adc / averagingWindow) * (5.0 / 1023.0) * currentScale;

	// reset ADC variables
	voltage_adc = 0;
	current_adc = 0;

	// Voltage range checks
	// --------------------
	// if ACS770 voltage < 0.5 something has gone wrong, so clamp to 0.5V if voltage goes below
	if (current_v < 0.5f) {
		current_v = 0.5f;
	}
	// if battery voltage goes below 10V (assuming its a 4S battery) something has gone wrong.
	if (voltage < 10.0f) {
		//Serial.println("Battery voltage below 10V. Check connections.");
	}
	// if battery voltage goes below 0V something has gone wrong, so clamp to 0V if it goes below. ADC is broken if this goes below 0.
	if (voltage < 0.0f) {
		//Serial.println("Battery voltage below 0V. Something has gone wrong!");
		voltage = 0.0f;
	}

	// convert current sense voltage into current using mV/A factor found in ACS770 datasheet.
	// Take away 0.5 from current sense voltage as 0.5V is voltage at 0A.
	current = (current_v - 0.5f) / 40e-3;
}

Battery.h

#pragma once
#if defined(ARDUINO) && ARDUINO >= 100
	#include "arduino.h"
#else
	#include "WProgram.h"
#endif
#include <Adafruit_ADS1015.h>

class Battery
{
public:
	const float pdFactor;
	float voltage, current;
	int averagingWindow, voltagePin, currentPin;
	Battery(float pdFactor, int averagingWindow, int voltagePin, int currentPin);
	void update();
private:
	Adafruit_ADS1115 ads1115;
	int voltage_adc, current_adc, current_v;
	float currentScale;
	void getData();
};

CurrentController.ino

/*
Name:		CurrentController.ino
Created:	5/23/2018 5:14:18 PM
Author:	tomea
*/
#include <Servo.h>
#include "Battery.h"
#include <Wire.h>



// global variables
const float PD_FACTOR = 3.504943957968f; // Uses resistor divider in ratio ~3.55. This allows for error in resistors and ADC.
bool shouldSpin = 0;
int incomingByte;
int escVal = 5;
// Current lookup table. First column is current, second is ESC value.
float iLUT[22] = {
	0,		10,
	2.37,	1100,
	7.11,	1200,
	12.71,	1300,
	17.95,	1400,
	23.04,	1500,
	27.56,	1600,
	31.76,	1700,
	37.42,	1800,
	47.42,	1900,
	49.06,	2000
};

// function declarations
void readSerial();
void sendData();
int getESC(float current);
int setESC(int value);

// Class declaration
Servo esc;
Battery battery(3.504943957968f, 1000, A1, A0);

// the setup function runs once when you press reset or power the board
void setup() {
	Serial.begin(9600);
	Serial.println("Started");
	delay(5000); // RUNAWAY TIME!!! :p
}

// the loop function runs over and over again until power down or reset
void loop() {
	readSerial();
	setESC(escVal);
	battery.update();
	sendData();
}

// Returns an ESC PWM value which draw approximately the current in the function parameter.
// Use the LUT and interpolates between those values in the LUT.
int getESC(float current) {
	int min = 0;
	int max = 18;
	for (int i = 0; i < 19; i += 2) {
		if (iLUT[i] == current) {
			return iLUT[i + 1];
		}
		else if (iLUT[i] > current) {
			max = i;
			min = i - 2;
			return map(current, iLUT[min], iLUT[max], iLUT[min + 1], iLUT[max + 1]);
		}
		else if (current < iLUT[0]) {
			max = 2;
			min = 0;
			return map(current, iLUT[min], iLUT[max], iLUT[min + 1], iLUT[max + 1]);
		}
	}
}

// Outputs the requested PWM signal on pin 9 to the ESC.
int setESC(int value) {
	if (shouldSpin) {
		esc.writeMicroseconds(value);
	}
	else {
		esc.writeMicroseconds(1000); // 1000 = off / idle
	}
}

// Sends voltage and current data via UART
// TODO: Send value being written to ESC with this data
void sendData() {
	Serial.print(battery.voltage, 4);
	Serial.print(",");
	Serial.print(battery.current, 4);
}

// Reads byte stored in UART buffer and if it corresponds to a command does an action.
// 83  = S (Start the motor)
// 115 = s (Stop the motor)
// 43  = + (Increase ESC output by 5%
// 45  = - (Decrease ESC output by 5%
void readSerial() {
	// check is Serial data available
	if (Serial.available() > 0) {
		// read incoming byte
		incomingByte = Serial.read();
		if (incomingByte == 83)
			shouldSpin = 1;
		if (incomingByte == 115)
			shouldSpin = 0;
		if (incomingByte == 43) {
			escVal += 5;
			//Serial.println(val);
			if (escVal > 100)
				escVal = 100;
		}
		if (incomingByte == 45) {
			escVal -= 5;
			if (escVal < 0)
				escVal = 0;
		}
	}
}