CurrentController.ino

/*
Name:		CurrentController.ino
Created:	5/23/2018 5:14:18 PM
Author:	tomea
*/
#include <Servo.h>
#include "Battery.h"
#include <Wire.h>
#include <Adafruit_ADS1015.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);
Adafruit_ADS1115 ads1115;

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

// the loop function runs over and over again until power down or reset
void loop() {
	ads1115.readADC_SingleEnded(0);
	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;
		}
	}
}