Try different approach for N64Esp32.cpp, much better, but still not completely reliable...

2 years ago · 3146ebec3a
4 changed files with 614 additions and 115 deletions
--- a/code-format.sh
+++ b/code-format.sh
@ -1,4 +1,4 @@
				@@ -1,4 +1,4 @@
 #!/bin/sh

-find src -type f \( -iname '*.h' -o -iname '*.c' -o -iname '*.cpp' \) -print0 | xargs -0 clang-format -style=file -i -fallback-style=none
+find src test -type f \( -iname '*.h' -o -iname '*.c' -o -iname '*.cpp' \) -print0 | xargs -0 clang-format -style=file -i -fallback-style=none

--- a/platformio.ini
+++ b/platformio.ini
@ -29,17 +29,18 @@ build_flags = -DGAMEPAD_COUNT=2
				@@ -29,17 +29,18 @@ build_flags = -DGAMEPAD_COUNT=2

 [env]
 monitor_speed = 115200
-framework = arduino

 # boards

 [esp32]
 platform = espressif32
 board = esp32dev
+framework = arduino

 [micro]
 platform = atmelavr
 board = micro
+framework = arduino

 # outputs

@ -163,7 +164,7 @@ build_flags = ${in-snes.build_flags} ${out-switchusb.build_flags}
				@@ -163,7 +164,7 @@ build_flags = ${in-snes.build_flags} ${out-switchusb.build_flags}

 [in-n64-esp32]
 src_filter = -<*> +<N64Esp32.cpp>
-build_flags = ${common.build_flags} -DGAMEPAD_INPUT=3 -DGAMEPAD_COUNT=1 -O0
+build_flags = ${common.build_flags} -DGAMEPAD_INPUT=3 -DGAMEPAD_COUNT=1 -fpermissive -O0

 [in-n64-micro]
 src_filter = -<*> +<N64Micro.cpp>
@ -382,3 +383,8 @@ src_filter  = ${ohm.src_filter}
				@@ -382,3 +383,8 @@ src_filter  = ${ohm.src_filter}
 [env:micro-ohm]
 extends     = micro, ohm
 src_filter  = ${ohm.src_filter}
+
+[env:native]
+src_filter = -<*> +<test_N64Esp32.cpp>
+platform = native
+test_ignore = test_embedded
--- a/src/N64Esp32.cpp
+++ b/src/N64Esp32.cpp
@ -16,12 +16,13 @@ PIN # USAGE
				@@ -16,12 +16,13 @@ PIN # USAGE

 #include "pins.h"

-#define DATA_PIN OR_PIN_2
+#define DATA_PIN 23

-#define PRINT_Y_AXIS_VALUES 1
-#define PRINT_X_AXIS_VALUES 1
+//#define PRINT_Y_AXIS_VALUES 1
+//#define PRINT_X_AXIS_VALUES 1
 //#define PLOT_CONSOLE_POLLING 1
 #define DEBUG
+#define PRINT_DATA

 #ifndef GAMEPAD_COUNT
 #define GAMEPAD_COUNT 1
@ -37,13 +38,21 @@ PIN # USAGE
				@@ -37,13 +38,21 @@ PIN # USAGE
 #define LINE_WRITE_HIGH pinMode(DATA_PIN, INPUT_PULLUP)
 #define LINE_WRITE_LOW pinMode(DATA_PIN, OUTPUT)

-#define DATA_SIZE 450  // number of sample points to poll
-#define DATA_OFFSET 0  // number of samples to ignore after staring to poll
-
 #define MAX_INCLINE_AXIS_X 60
 #define MAX_INCLINE_AXIS_Y 60

-#define BIT_THRESHOLD 6
+#define DATA_SIZE 1536	// number of sample points to poll
+#define CALIBRATE_PASSES 5
+
+#define NUM_BITS 32
+
+// buffer to hold data being read from controller
+bool buffer[DATA_SIZE];
+
+// bit resolution and offsets
+int bitOffsets[NUM_BITS];
+int bitResolution = 0;
+int bitsToRead = DATA_SIZE;

 GAMEPAD_CLASS gamepad;

@ -72,12 +81,75 @@ struct ControllerData {
				@@ -72,12 +81,75 @@ struct ControllerData {
 	bool yAxisRaw[8];
 };

-// buffer to hold data being read from controller
-bool buffer[DATA_SIZE + DATA_OFFSET];
+void updateOffsetsAndResolution() {
+	// the current bit counter
+	int bitCounter = 0;

-// bit resolution and offsets
-int bitOffsets[32];
-int bitResolution;
+	// to hold the number of 1's in this bit
+	int thisResolution = 0;
+
+	// current index
+	int i = 0;
+
+	// we might be further refining a previous calibration, if non-zero, remove existing bitResolution
+	for (int i = 0; i < NUM_BITS; i++) {
+		bitOffsets[i] += bitResolution;
+	}
+
+	for (; i < DATA_SIZE; i++) {
+		if (buffer[i] == false) {
+			// we skip all leading 1's
+			break;
+		}
+	}
+
+	// iterate over buffer
+	for (; i < DATA_SIZE - 1 && bitCounter < NUM_BITS; i++) {
+		if (buffer[i] == true) {
+			++thisResolution;
+			// if a falling edge is detected
+			if (buffer[1 + i] == false) {
+				// store bit's earliest possible beginning offsets
+				int thisOffset = i - thisResolution + 1;
+				if (bitOffsets[bitCounter] == 0 || bitOffsets[bitCounter] > thisOffset) {
+					bitOffsets[bitCounter] = thisOffset;
+				}
+
+				// store max resolution in bitResolution
+				if (thisResolution > bitResolution) {
+					bitResolution = thisResolution;
+				}
+
+				// reset thisResolution
+				thisResolution = 0;
+				// increment bitCounter
+				++bitCounter;
+			}
+		}
+	}
+
+	// calculate bit's beginning offsets by subtracting resolution
+	// if this index is 0, button is not pressed, if 1, button is pressed
+	for (int i = 0; i < NUM_BITS; i++) {
+		bitOffsets[i] -= bitResolution;
+	}
+}
+
+void calcBitsToRead() {
+	bitsToRead = bitOffsets[NUM_BITS - 1] + 1;
+	if (bitsToRead < NUM_BITS * 2) {
+		// todo: not enough, error out...
+		while (true) {
+			printf("not enough bitsToRead, increase DATA_SIZE ???\n");
+			delay(5000);
+		}
+	}
+}
+
+/* Function to extract a controller bit from the buffer of returned data */
+void getBit(bool *bit, int offset, bool *data) {
+	*bit = data[offset] == true;
+}

 /** Function to send a Command to the attached N64-Controller.
 *  Must be run from RAM to defy timing differences introduced from
@ -87,8 +159,8 @@ void IRAM_ATTR sendCommand(byte command) {
				@@ -87,8 +159,8 @@ void IRAM_ATTR sendCommand(byte command) {
 	// the current bit to write
 	bool bit;

-	// clear output buffer
-	memset(buffer, 0, DATA_SIZE + DATA_OFFSET);
+	// clear output buffer, todo: really need to do this????
+	//memset(buffer, 0, DATA_SIZE);

 	noInterrupts();

@ -111,38 +183,37 @@ void IRAM_ATTR sendCommand(byte command) {
				@@ -111,38 +183,37 @@ void IRAM_ATTR sendCommand(byte command) {
 	delayMicroseconds(2);

 	// read returned data as fast as possible
-	for (int i = 0; i < DATA_SIZE + DATA_OFFSET; i++) {
-		buffer[i] = digitalRead(DATA_PIN);
+	for (int i = 0; i < bitsToRead; i++) {
+		//buffer[i] = digitalRead(DATA_PIN);
+		// this is faster:
+#if DATA_PIN < 32
+		buffer[i] = (GPIO.in >> DATA_PIN) & 0x1;
+#elif DATA_PIN < 40
+		buffer[i] = (GPIO.in1.val >> (DATA_PIN - 32)) & 0x1;
+#else
+
+#error unsupported DATA_PIN must be <40
+
+#endif
+		//delayMicroseconds(1);
 	}

 	interrupts();

 // plot polling process from controller if unstructed to
 #ifdef PLOT_CONSOLE_POLLING
-	for (int i = 0; i < DATA_SIZE + DATA_OFFSET; i++) {
+	for (int i = 0; i < bitsToRead; i++) {
 		Serial.println(buffer[i] * 2500);
 	}
-#endif
-}
-
-/* Function to extract a controller bit from the buffer of returned data */
-void getBit(bool *bit, int offset, bool *data) {
-	// sanity check offset
-	if (offset < 0)
-		offset = 0;

-	// count
-	short count = 0;
+#endif

-	// get count from offset to offset + length
-	for (int i = offset + DATA_OFFSET; i < offset + bitResolution; i++) {
-		count += *(data + i);
+#ifdef PRINT_DATA
+	for (int i = 0; i < bitsToRead; i++) {
+		printf(buffer[i] ? "1" : "0");
 	}
-
-	// if offset surpasses threshold set bit
-	*bit = false;
-	if (count > BIT_THRESHOLD)
-		*bit = true;
+	printf("\n----------------\n");
+#endif
 }

 /** Function to populate the controller struct if command 0x01 was sent.
@ -153,22 +224,21 @@ void getBit(bool *bit, int offset, bool *data) {
				@@ -153,22 +224,21 @@ void getBit(bool *bit, int offset, bool *data) {
 */
 void populateControllerStruct(ControllerData *data) {
 	// first byte
-	getBit(&(data->buttonA), bitOffsets[0], &buffer[0]);
-	getBit(&(data->buttonB), bitOffsets[1], &buffer[0]);
-	getBit(&(data->buttonZ), bitOffsets[2], &buffer[0]);
-	getBit(&(data->buttonStart), bitOffsets[3], &buffer[0]);
-	getBit(&(data->DPadUp), bitOffsets[4], &buffer[0]);
-	getBit(&(data->DPadDown), bitOffsets[5], &buffer[0]);
-	getBit(&(data->DPadLeft), bitOffsets[6], &buffer[0]);
-	getBit(&(data->DPadRight), bitOffsets[7], &buffer[0]);
+	data->buttonA = buffer[bitOffsets[0]];
+	data->buttonB = buffer[bitOffsets[1]];
+	data->buttonZ = buffer[bitOffsets[2]];
+	data->buttonStart = buffer[bitOffsets[3]];
+	data->DPadUp = buffer[bitOffsets[4]];
+	data->DPadDown = buffer[bitOffsets[5]];
+	data->DPadLeft = buffer[bitOffsets[6]];
+	data->DPadRight = buffer[bitOffsets[7]];

 	// second byte, first two bits are unused
-	getBit(&(data->buttonL), bitOffsets[10], &buffer[0]);
-	getBit(&(data->buttonR), bitOffsets[11], &buffer[0]);
-	getBit(&(data->CUp), bitOffsets[12], &buffer[0]);
-	getBit(&(data->CDown), bitOffsets[13], &buffer[0]);
-	getBit(&(data->CLeft), bitOffsets[14], &buffer[0]);
-	getBit(&(data->CRight), bitOffsets[15], &buffer[0]);
+	data->buttonL = buffer[bitOffsets[10]];
+	data->buttonR = buffer[bitOffsets[11]];
+	data->CUp = buffer[bitOffsets[12]];
+	data->CDown = buffer[bitOffsets[13]];
+	data->CRight = buffer[bitOffsets[14]];

 	// third byte
 	getBit(&(data->xAxisRaw[0]), bitOffsets[16], &buffer[0]);
@ -246,76 +316,18 @@ void populateControllerStruct(ControllerData *data) {
				@@ -246,76 +316,18 @@ void populateControllerStruct(ControllerData *data) {
 	//Serial.printf("xaxis: %-3i yaxis: %-3i \n",data->xAxis,data->yAxis);
 }

-void updateOffsetsAndResolution() {
-	// the current bit counter
-	int bitCounter = 0;
-
-	// to hold the offset of A Button's falling edge
-	int bitAfallingOffset = 0;
-
-	// iterate over buffer
-	for (int i = 0; i < DATA_SIZE + DATA_OFFSET - 1; i++) {
-		// if a falling edge is detected
-		if (buffer[i] == true && buffer[1 + i] == false) {
-			// store bit's end offset
-			bitOffsets[bitCounter] = i + 1;
-
-			// if it's the A button store offset of the falling edge
-			if (bitCounter == 0)
-				bitAfallingOffset = i + 1;
-
-			// if it's the B button calculate the bit Resolution
-			if (bitCounter == 1)
-				bitResolution = (i + 1) - bitAfallingOffset;
-
-			// increment bit counter
-			bitCounter++;
-		}
-	}
-
-	//Serial.printf("Bit resolution is %i \n", bitResolution);
-
-	// calculate bit's beginning offsets by subtracting resolution
-	for (int i = 0; i < 32; i++) {
-		bitOffsets[i] -= bitResolution;
-		//Serial.printf("beginning of bit %i detected @ begin+%i \n", i + 1, bitOffsets[i]);
-	}
-}
-
 ControllerData controller;

-void setup() {
-#ifdef DEBUG
-	Serial.begin(115200);
-#endif
-
-	gamepad.begin();
-
-	// setup io pins
-	//setupIO();
-	// the controller data line
-	LINE_WRITE_HIGH;
-
-#ifdef PLOT_CONSOLE_POLLING
-	delay(5000);
-	sendCommand(0x01);
-	while (true)
-		;
-#endif
-
-	delay(5000);
-
-	sendCommand(0x01);
-	updateOffsetsAndResolution();
-}
-
-void loop() {
+void loope() {
 	// polling must not occur faster than every 20 ms
-	delay(14);
+	//delay(14);
+	delay(30);
+	//delay(2000); // todo: change to above

 	//Serial.println("sending command to n64");
 	// send command 0x01 to n64 controller
 	sendCommand(0x01);
+	//updateOffsetsAndResolution();

 	// store received data in controller struct
 	populateControllerStruct(&controller);
@ -385,3 +397,59 @@ void loop() {
				@@ -385,3 +397,59 @@ void loop() {

 	//delay(500);
 }
+
+void pinned_loop() {
+	while (true) {
+		loope();
+	}
+}
+
+void setup() {
+#ifdef DEBUG
+	Serial.begin(115200);
+	delay(5000);
+#endif
+
+	gamepad.begin();
+
+	// setup io pins
+	//setupIO();
+	// the controller data line
+	LINE_WRITE_HIGH;
+
+#ifdef PLOT_CONSOLE_POLLING
+	sendCommand(0x01);
+	while (true)
+		;
+#endif
+
+	for (int i = 0; i < CALIBRATE_PASSES; ++i) {
+		sendCommand(0x01);
+		updateOffsetsAndResolution();
+		if (i != CALIBRATE_PASSES) {
+			delay(14);
+		}
+	}
+	calcBitsToRead();
+#ifdef DEBUG
+	printf("bitOffsets: ");
+	for (int i = 0; i < NUM_BITS; i++) {
+		printf("%i:%i ", i, bitOffsets[i]);
+	}
+	printf("\n");
+	printf("bitOffsets: ");
+	for (int i = 0; i < NUM_BITS; i++) {
+		printf("%i, ", bitOffsets[i]);
+	}
+	printf("\n");
+	printf("bitResolution: %i\n", bitResolution);
+	delay(5000);
+#endif
+
+	//xTaskCreatePinnedToCore(pinned_loop, "gbuttons", 2048, NULL, 1, NULL, 0);
+	//xTaskCreatePinnedToCore(pinned_loop, "gbuttons", 2048, NULL, 1, NULL, 1);
+}
+
+void loop() {
+	loope();
+}
--- a/test/test_N64Esp32.cpp
+++ b/test/test_N64Esp32.cpp
@ -0,0 +1,425 @@
				@@ -0,0 +1,425 @@
+/*
+ Copyright (c) 2014-present PlatformIO <contact@platformio.org>
+
+ Licensed under the Apache License, Version 2.0 (the "License");
+ you may not use this file except in compliance with the License.
+ You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+ Unless required by applicable law or agreed to in writing, software
+ distributed under the License is distributed on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ See the License for the specific language governing permissions and
+ limitations under the License.
+**/
+
+#include <cstdio>
+
+#include <unity.h>
+
+#define DATA_SIZE 800  // number of sample points to poll
+
+// buffer to hold data being read from controller
+bool buffer[DATA_SIZE];
+
+#define NUM_BITS 32
+
+// bit resolution and offsets
+int bitOffsets[NUM_BITS];
+int bitResolution = 0;
+int bitsToRead = DATA_SIZE;
+
+struct ControllerData {
+	bool buttonA;
+	bool buttonB;
+	bool buttonZ;
+	bool buttonL;
+	bool buttonR;
+	bool buttonStart;
+
+	bool DPadUp;
+	bool DPadDown;
+	bool DPadLeft;
+	bool DPadRight;
+
+	bool CUp;
+	bool CDown;
+	bool CLeft;
+	bool CRight;
+
+	short xAxis;
+	short yAxis;
+
+	bool xAxisRaw[8];
+	bool yAxisRaw[8];
+};
+
+ControllerData controller;
+
+void updateOffsetsAndResolution() {
+	// the current bit counter
+	int bitCounter = 0;
+
+	// to hold the number of 1's in this bit
+	int thisResolution = 0;
+
+	// current index
+	int i = 0;
+
+	// we might be further refining a previous calibration, if non-zero, remove existing bitResolution
+	for (int i = 0; i < NUM_BITS; i++) {
+		bitOffsets[i] += bitResolution;
+	}
+
+	for (; i < DATA_SIZE; i++) {
+		if (buffer[i] == false) {
+			// we skip all leading 1's
+			break;
+		}
+	}
+
+	// iterate over buffer
+	for (; i < DATA_SIZE - 1 && bitCounter < NUM_BITS; i++) {
+		if (buffer[i] == true) {
+			++thisResolution;
+			// if a falling edge is detected
+			if (buffer[1 + i] == false) {
+				// store bit's earliest possible beginning offsets
+				int thisOffset = i - thisResolution + 1;
+				if (bitOffsets[bitCounter] == 0 || bitOffsets[bitCounter] > thisOffset) {
+					bitOffsets[bitCounter] = thisOffset;
+				}
+
+				// store max resolution in bitResolution
+				if (thisResolution > bitResolution) {
+					bitResolution = thisResolution;
+				}
+
+				// reset thisResolution
+				thisResolution = 0;
+				// increment bitCounter
+				++bitCounter;
+			}
+		}
+	}
+
+	// calculate bit's beginning offsets by subtracting resolution
+	// if this index is 0, button is not pressed, if 1, button is pressed
+	for (int i = 0; i < NUM_BITS; i++) {
+		bitOffsets[i] -= bitResolution;
+	}
+}
+
+void calcBitsToRead() {
+	bitsToRead = bitOffsets[NUM_BITS - 1] + 1;
+	if (bitsToRead < NUM_BITS * 2) {
+		// todo: not enough, error out...
+	}
+}
+
+/* Function to extract a controller bit from the buffer of returned data */
+void getBit(bool *bit, int offset, bool *data) {
+	*bit = data[offset];
+}
+
+/** Function to populate the controller struct if command 0x01 was sent.
+ *  Buttons are set according to data in buffer, raw axis data is written,
+ *  Axis Data is correctly decoded from raw axis data by taking two's complement
+ *  and checking if value if below 'MAX_INCLINE_AXIS_X' or 'MAX_INCLINE_AXIS_Y'.
+ *  If values surpass the maximum incline they are set to match those values.
+ */
+void populateControllerStruct(ControllerData *data) {
+	// first byte
+	getBit(&(data->buttonA), bitOffsets[0], &buffer[0]);
+	getBit(&(data->buttonB), bitOffsets[1], &buffer[0]);
+	getBit(&(data->buttonZ), bitOffsets[2], &buffer[0]);
+	getBit(&(data->buttonStart), bitOffsets[3], &buffer[0]);
+	getBit(&(data->DPadUp), bitOffsets[4], &buffer[0]);
+	getBit(&(data->DPadDown), bitOffsets[5], &buffer[0]);
+	getBit(&(data->DPadLeft), bitOffsets[6], &buffer[0]);
+	getBit(&(data->DPadRight), bitOffsets[7], &buffer[0]);
+
+	// second byte, first two bits are unused
+	getBit(&(data->buttonL), bitOffsets[10], &buffer[0]);
+	getBit(&(data->buttonR), bitOffsets[11], &buffer[0]);
+	getBit(&(data->CUp), bitOffsets[12], &buffer[0]);
+	getBit(&(data->CDown), bitOffsets[13], &buffer[0]);
+	getBit(&(data->CLeft), bitOffsets[14], &buffer[0]);
+	getBit(&(data->CRight), bitOffsets[15], &buffer[0]);
+
+	// third byte
+	getBit(&(data->xAxisRaw[0]), bitOffsets[16], &buffer[0]);
+	getBit(&(data->xAxisRaw[1]), bitOffsets[17], &buffer[0]);
+	getBit(&(data->xAxisRaw[2]), bitOffsets[18], &buffer[0]);
+	getBit(&(data->xAxisRaw[3]), bitOffsets[19], &buffer[0]);
+	getBit(&(data->xAxisRaw[4]), bitOffsets[20], &buffer[0]);
+	getBit(&(data->xAxisRaw[5]), bitOffsets[21], &buffer[0]);
+	getBit(&(data->xAxisRaw[6]), bitOffsets[22], &buffer[0]);
+	getBit(&(data->xAxisRaw[7]), bitOffsets[23], &buffer[0]);
+
+	// fourth byte
+	getBit(&(data->yAxisRaw[0]), bitOffsets[24], &buffer[0]);
+	getBit(&(data->yAxisRaw[1]), bitOffsets[25], &buffer[0]);
+	getBit(&(data->yAxisRaw[2]), bitOffsets[26], &buffer[0]);
+	getBit(&(data->yAxisRaw[3]), bitOffsets[27], &buffer[0]);
+	getBit(&(data->yAxisRaw[4]), bitOffsets[28], &buffer[0]);
+	getBit(&(data->yAxisRaw[5]), bitOffsets[29], &buffer[0]);
+	getBit(&(data->yAxisRaw[6]), bitOffsets[30], &buffer[0]);
+	getBit(&(data->yAxisRaw[7]), bitOffsets[31], &buffer[0]);
+
+	// sum up bits to get axis bytes
+	data->xAxis = 0;
+	data->yAxis = 0;
+	for (int i = 0; i < 8; i++) {
+		data->xAxis += (data->xAxisRaw[i] * (0x80 >> (i)));
+		data->yAxis += (data->yAxisRaw[i] * (0x80 >> (i)));
+	}
+
+// print axis values
+#ifdef DEBUG
+	printf("yRaw: %i %i %i %i %i %i %i %i xRaw: %i %i %i %i %i %i %i %i yAxis: %03i xAxis: %03i",
+		   data->yAxisRaw[0],
+		   data->yAxisRaw[1],
+		   data->yAxisRaw[2],
+		   data->yAxisRaw[3],
+		   data->yAxisRaw[4],
+		   data->yAxisRaw[5],
+		   data->yAxisRaw[6],
+		   data->yAxisRaw[7],
+		   data->xAxisRaw[0],
+		   data->xAxisRaw[1],
+		   data->xAxisRaw[2],
+		   data->xAxisRaw[3],
+		   data->xAxisRaw[4],
+		   data->xAxisRaw[5],
+		   data->xAxisRaw[6],
+		   data->xAxisRaw[7],
+		   data->yAxis,
+		   data->xAxis);
+#endif
+
+	// decode xAxis two's complement
+	if (data->xAxis & 0x80) {
+		data->xAxis = -1 * (0xff - data->xAxis);
+	}
+
+	// decode yAxis two's complement
+	if (data->yAxis & 0x80) {
+		data->yAxis = -1 * (0xff - data->yAxis);
+	}
+
+	/*
+	// keep x axis below maxIncline
+	if (data->xAxis > MAX_INCLINE_AXIS_X)
+		data->xAxis = MAX_INCLINE_AXIS_X;
+	if (data->xAxis < -MAX_INCLINE_AXIS_X)
+		data->xAxis = -MAX_INCLINE_AXIS_X;
+
+	// keep y axis below maxIncline
+	if (data->yAxis > MAX_INCLINE_AXIS_Y)
+		data->yAxis = MAX_INCLINE_AXIS_Y;
+	if (data->yAxis < -MAX_INCLINE_AXIS_Y)
+		data->yAxis = -MAX_INCLINE_AXIS_Y;
+    */
+
+	//printf("xaxis: %-3i yaxis: %-3i \n",data->xAxis,data->yAxis);
+}
+
+void load(const char *s) {
+	const char *t;
+	int i = 0;
+
+	// convert test string to buffer
+	for (t = s; *t != '\0'; t++) {
+		buffer[i++] = *t == '1';
+	}
+}
+
+void loadUpdate(const char *s) {
+	load(s);
+
+	updateOffsetsAndResolution();
+
+	/*
+	for (int i = 0; i < DATA_SIZE; i++) {
+		printf(buffer[i] ? "1" : "0");
+	}
+	printf("\n----------------\n");
+	*/
+	printf("bitOffsets: ");
+	for (int i = 0; i < NUM_BITS; i++) {
+		//printf("%i ", bitOffsets[i]);
+		printf("%i:%i ", i, bitOffsets[i]);
+	}
+	printf("\n");
+	printf("bitResolution: %i\n", bitResolution);
+	printf("bit 0: %i\n", buffer[bitOffsets[0]]);
+	printf("bit buffer[14]: %i\n", buffer[14]);
+	printf("bit buffer[15]: %i\n", buffer[15]);
+	printf("bit buffer[16]: %i\n", buffer[16]);
+}
+
+void reset() {
+	// reset all bitOffsets to 0 which is invalid
+	for (int i = 0; i < NUM_BITS; i++) {
+		bitOffsets[i] = 0;
+	}
+	bitResolution = 0;
+	bitsToRead = DATA_SIZE;
+}
+
+void print(const char *s) {
+	printf(s);
+}
+
+// void setUp(void) {
+// // set stuff up here
+// }
+
+// void tearDown(void) {
+// // clean stuff up here
+// }
+
+/*
+*/
+void test_function_updateOffsetsAndResolution(void) {
+	reset();
+	loadUpdate(
+		"0000000000000000111110000000000000000011110000000000000000011111000000000000000011111000000000000000011111000000000000000001111000000000000000001111100000000000000001111100000000000000001111"
+		"1000000000000000001111000000000000000001111100000000000000001111100000000000000000111100000000000000000111100000000000000000111110000000000000000111110000000000000000011110000000000000000011"
+		"1100000000000000000111110000000000000000111110000000000000000011110000000000000000011110000000000000000011111000000000000000011111000000000000000001111000000000000000001111000000000000000001"
+		"1111000000000000000011111000000000000000001111000000000000000001111100000000000000001111100000000000000001111100000000000111111111111111111111111111111111111111111111111111111111111111111111"
+		"1111111111111111111111111111111111111111");
+
+	TEST_ASSERT_EQUAL(5, bitResolution);
+
+	loadUpdate(
+		"1000000000000001111110000000000000000011110000000000000000011111000000000000000011111000000000000000011111000000000000000001111000000000000000001111100000000000000001111100000000000000001111"
+		"1000000000000000001111000000000000000001111100000000000000001111100000000000000000111100000000000000000111100000000000000000111110000000000000000111110000000000000000011110000000000000000011"
+		"1100000000000000000111110000000000000000111110000000000000000011110000000000000000011110000000000000000011111000000000000000011111000000000000000001111000000000000000001111000000000000000001"
+		"1111000000000000000011111000000000000000001111000000000000000001111100000000000000001111100000000000000001111100000000000111111111111111111111111111111111111111111111111111111111111111111111"
+		"1111111111111111111111111111111111111111");
+
+	TEST_ASSERT_EQUAL(6, bitResolution);
+
+	loadUpdate(
+		"1000000000000000001111000000000000000011110000000000000000011111000000000000000011111000000000000000011111000000000000000001111000000000000000001111100000000000000001111100000000000000001111"
+		"1000000000000000001111000000000000000001111100000000000000001111100000000000000000111100000000000000000111100000000000000000111110000000000000000111110000000000000000011110000000000000000011"
+		"1100000000000000000111110000000000000000111110000000000000000011110000000000000000011110000000000000000011111000000000000000011111000000000000000001111000000000000000001111000000000000000001"
+		"1111000000000000000011111000000000000000001111000000000000000001111100000000000000001111100000000000000001111100000000000111111111111111111111111111111111111111111111111111111111111111111111"
+		"1111111111111111111111111111111111111111");
+
+	TEST_ASSERT_EQUAL(6, bitResolution);
+
+	calcBitsToRead();
+	TEST_ASSERT_EQUAL(670, bitsToRead);
+
+	bool buttonA = false;
+
+	getBit(&buttonA, bitOffsets[0], &buffer[0]);
+
+	TEST_ASSERT_EQUAL(false, buttonA);
+
+	load(
+		"0000001111111111111110000000000000000011110000000000000000011111000000000000000011111000000000000000011111000000000000000001111000000000000000001111100000000000000001111100000000000000001111"
+		"1000000000000000001111000000000000000001111100000000000000001111100000000000000000111100000000000000000111100000000000000000111110000000000000000111110000000000000000011110000000000000000011"
+		"1100000000000000000111110000000000000000111110000000000000000011110000000000000000011110000000000000000011111000000000000000011111000000000000000001111000000000000000001111000000000000000001"
+		"1111000000000000000011111000000000000000001111000000000000000001111100000000000000001111100000000000000001111100000000000111111111111111111111111111111111111111111111111111111111111111111111"
+		"1111111111111111111111111111111111111111");
+
+	getBit(&buttonA, bitOffsets[0], &buffer[0]);
+
+	TEST_ASSERT_EQUAL(true, buttonA);
+}
+
+void test_function_realControllerRead(void) {
+	reset();
+	loadUpdate(
+		"0000000000000011111000000000000000111100000000000000001111000000000000000011110000000000000000111100000000000000001111000000000000000111110000000000000001111100000000000000011111000000000000"
+		"0001111100000000000000011111000000000000000111100000000000000001111000000000000000011110000000000000000111100000000000000001111000000000000000011110000000000000000111100000000000000011100000"
+		"0000000000111100000000000000011110000000000000000111100000000000000001111000000000000000111110000000000000001111100000000000000011111000000000000000111110000000000000001111100000000000000011"
+		"1100000000000000001111000000000000000011110000000000000000111100000000000111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111"
+		"1111111111111111111111111111111111111111");
+	loadUpdate(
+		"0000000000000011111000000000000000111110000000000000001111000000000000000011110000000000000000111100000000000000001111000000000000000011110000000000000000111100000000000000001111000000000000"
+		"0000111100000000000000001111000000000000000111110000000000000001111100000000000000011111000000000000000111110000000000000001111100000000000000011110000000000000000111100000000000000001111000"
+		"0000000000000111100000000000000001111000000000000000011110000000000000000111100000000000000001111000000000000000011110000000000000000111100000000000000011111000000000000000111110000000000000"
+		"0011111000000000000000111110000000000000001111000000000000000011110000000000011111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111"
+		"1111111111111111111111111111111111111111");
+	loadUpdate(
+		"0000000000000000111100000000000000001111000000000000000011110000000000000000111100000000000000001111000000000000000011110000000000000000111100000000000000001111000000000000000011110000000000"
+		"0000011111000000000000000111110000000000000001111100000000000000011111000000000000000111100000000000000001111000000000000000011110000000000000000111100000000000000001111000000000000000011110"
+		"0000000000000001111000000000000000011110000000000000000111100000000000000001111000000000000000111110000000000000001111100000000000000011111000000000000000111110000000000000001111100000000000"
+		"0000111100000000000000001111000000000000000011110000000000000000111100000000000111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111"
+		"1111111111111111111111111111111111111111");
+	loadUpdate(
+		"0000000000000001111100000000000000011110000000000000000111100000000000000001111000000000000000011110000000000000000111100000000000000001111000000000000000011110000000000000000111100000000000"
+		"0000011110000000000000000111100000000000000011111000000000000000111110000000000000001111100000000000000011111000000000000000111100000000000000001111000000000000000011110000000000000000111100"
+		"0000000000000011110000000000000000111100000000000000001111000000000000000011110000000000000000111100000000000000001111000000000000000111110000000000000001111100000000000000011111000000000000"
+		"0001111100000000000000011110000000000000000111100000000000000001111000000000001111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111"
+		"1111111111111111111111111111111111111111");
+	loadUpdate(
+		"0000000000000011111000000000000000111110000000000000001111100000000000000011110000000000000000111100000000000000001111000000000000000011110000000000000000111100000000000000001111000000000000"
+		"0000111100000000000000001111000000000000000011110000000000000001111100000000000000011111000000000000000111110000000000000001111100000000000000011111000000000000000111100000000000000001111000"
+		"0000000000000111100000000000000001111000000000000000011110000000000000000111100000000000000001111000000000000000011110000000000000000111100000000000000001111000000000000000111110000000000000"
+		"0011111000000000000000111110000000000000001111100000000000000011110000000000011111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111"
+		"1111111111111111111111111111111111111111");
+
+	TEST_ASSERT_EQUAL(5, bitResolution);
+
+	calcBitsToRead();
+	TEST_ASSERT_EQUAL(624, bitsToRead);
+
+	load(
+		"0000000000000011110000000000000000111100000000000000001111000000000000000011110000000000000000111100000000000000001111000000000000000011110000000000000000111100000000000000011111000000000000"
+		"0001111100000000000000011111000000000000000111110000000000000001111100000000000000011110000000000000000111100000000000000001111000000111111111111110000001111111111111100000011111111111111000"
+		"0000000000000111100000000000000001111000000111111111111110000001111111111111100000000000000011111000000000000000111110000011111111111111100000000000000011111000000000000000111100000000000000"
+		"001111000000111111111111110000001111111111111100000011");
+
+	bool buttonA = true;
+
+	getBit(&buttonA, bitOffsets[0], &buffer[0]);
+
+	TEST_ASSERT_EQUAL(false, buttonA);
+
+	populateControllerStruct(&controller);
+
+	print(" buttons: ");
+	/*
+	*/
+	print(controller.buttonA ? "A" : "-");
+	print(controller.buttonB ? "B" : "-");
+	print(controller.buttonZ ? "Z" : "-");
+	print(controller.buttonL ? "L" : "-");
+	print(controller.buttonR ? "R" : "-");
+	print(controller.buttonStart ? "S" : "-");
+	print(" DPAD: ");
+	print(controller.DPadUp ? "U" : "-");
+	print(controller.DPadDown ? "D" : "-");
+	print(controller.DPadLeft ? "L" : "-");
+	print(controller.DPadRight ? "R" : "-");
+	print(" C: ");
+	print(controller.CUp ? "U" : "-");
+	print(controller.CDown ? "D" : "-");
+	print(controller.CLeft ? "L" : "-");
+	print(controller.CRight ? "R" : "-");
+	print(" Y: ");
+	//print(controller.yAxis);
+	//print(" YT: ");
+	//print(translateAxis(controller.yAxis));
+	print(" X: ");
+	//print(controller.xAxis);
+	//print(" XT: ");
+	//print(translateAxis(controller.xAxis));
+	printf("\n");
+}
+
+int main(int argc, char **argv) {
+	UNITY_BEGIN();
+	//RUN_TEST(test_function_updateOffsetsAndResolution);
+	RUN_TEST(test_function_realControllerRead);
+	UNITY_END();
+
+	return 0;
+}