Try different approach for N64Esp32.cpp, much better, but still not completely reliable...
This commit is contained in:
parent
966f1c7e36
commit
3146ebec3a
|
@ -1,4 +1,4 @@
|
||||||
#!/bin/sh
|
#!/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
|
||||||
|
|
||||||
|
|
|
@ -29,17 +29,18 @@ build_flags = -DGAMEPAD_COUNT=2
|
||||||
|
|
||||||
[env]
|
[env]
|
||||||
monitor_speed = 115200
|
monitor_speed = 115200
|
||||||
framework = arduino
|
|
||||||
|
|
||||||
# boards
|
# boards
|
||||||
|
|
||||||
[esp32]
|
[esp32]
|
||||||
platform = espressif32
|
platform = espressif32
|
||||||
board = esp32dev
|
board = esp32dev
|
||||||
|
framework = arduino
|
||||||
|
|
||||||
[micro]
|
[micro]
|
||||||
platform = atmelavr
|
platform = atmelavr
|
||||||
board = micro
|
board = micro
|
||||||
|
framework = arduino
|
||||||
|
|
||||||
# outputs
|
# outputs
|
||||||
|
|
||||||
|
@ -163,7 +164,7 @@ build_flags = ${in-snes.build_flags} ${out-switchusb.build_flags}
|
||||||
|
|
||||||
[in-n64-esp32]
|
[in-n64-esp32]
|
||||||
src_filter = -<*> +<N64Esp32.cpp>
|
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]
|
[in-n64-micro]
|
||||||
src_filter = -<*> +<N64Micro.cpp>
|
src_filter = -<*> +<N64Micro.cpp>
|
||||||
|
@ -382,3 +383,8 @@ src_filter = ${ohm.src_filter}
|
||||||
[env:micro-ohm]
|
[env:micro-ohm]
|
||||||
extends = micro, ohm
|
extends = micro, ohm
|
||||||
src_filter = ${ohm.src_filter}
|
src_filter = ${ohm.src_filter}
|
||||||
|
|
||||||
|
[env:native]
|
||||||
|
src_filter = -<*> +<test_N64Esp32.cpp>
|
||||||
|
platform = native
|
||||||
|
test_ignore = test_embedded
|
||||||
|
|
292
src/N64Esp32.cpp
292
src/N64Esp32.cpp
|
@ -16,12 +16,13 @@ PIN # USAGE
|
||||||
|
|
||||||
#include "pins.h"
|
#include "pins.h"
|
||||||
|
|
||||||
#define DATA_PIN OR_PIN_2
|
#define DATA_PIN 23
|
||||||
|
|
||||||
#define PRINT_Y_AXIS_VALUES 1
|
//#define PRINT_Y_AXIS_VALUES 1
|
||||||
#define PRINT_X_AXIS_VALUES 1
|
//#define PRINT_X_AXIS_VALUES 1
|
||||||
//#define PLOT_CONSOLE_POLLING 1
|
//#define PLOT_CONSOLE_POLLING 1
|
||||||
#define DEBUG
|
#define DEBUG
|
||||||
|
#define PRINT_DATA
|
||||||
|
|
||||||
#ifndef GAMEPAD_COUNT
|
#ifndef GAMEPAD_COUNT
|
||||||
#define GAMEPAD_COUNT 1
|
#define GAMEPAD_COUNT 1
|
||||||
|
@ -37,13 +38,21 @@ PIN # USAGE
|
||||||
#define LINE_WRITE_HIGH pinMode(DATA_PIN, INPUT_PULLUP)
|
#define LINE_WRITE_HIGH pinMode(DATA_PIN, INPUT_PULLUP)
|
||||||
#define LINE_WRITE_LOW pinMode(DATA_PIN, OUTPUT)
|
#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_X 60
|
||||||
#define MAX_INCLINE_AXIS_Y 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;
|
GAMEPAD_CLASS gamepad;
|
||||||
|
|
||||||
|
@ -72,12 +81,75 @@ struct ControllerData {
|
||||||
bool yAxisRaw[8];
|
bool yAxisRaw[8];
|
||||||
};
|
};
|
||||||
|
|
||||||
// buffer to hold data being read from controller
|
void updateOffsetsAndResolution() {
|
||||||
bool buffer[DATA_SIZE + DATA_OFFSET];
|
// the current bit counter
|
||||||
|
int bitCounter = 0;
|
||||||
|
|
||||||
// bit resolution and offsets
|
// to hold the number of 1's in this bit
|
||||||
int bitOffsets[32];
|
int thisResolution = 0;
|
||||||
int bitResolution;
|
|
||||||
|
// 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.
|
/** Function to send a Command to the attached N64-Controller.
|
||||||
* Must be run from RAM to defy timing differences introduced from
|
* Must be run from RAM to defy timing differences introduced from
|
||||||
|
@ -87,8 +159,8 @@ void IRAM_ATTR sendCommand(byte command) {
|
||||||
// the current bit to write
|
// the current bit to write
|
||||||
bool bit;
|
bool bit;
|
||||||
|
|
||||||
// clear output buffer
|
// clear output buffer, todo: really need to do this????
|
||||||
memset(buffer, 0, DATA_SIZE + DATA_OFFSET);
|
//memset(buffer, 0, DATA_SIZE);
|
||||||
|
|
||||||
noInterrupts();
|
noInterrupts();
|
||||||
|
|
||||||
|
@ -111,38 +183,37 @@ void IRAM_ATTR sendCommand(byte command) {
|
||||||
delayMicroseconds(2);
|
delayMicroseconds(2);
|
||||||
|
|
||||||
// read returned data as fast as possible
|
// read returned data as fast as possible
|
||||||
for (int i = 0; i < DATA_SIZE + DATA_OFFSET; i++) {
|
for (int i = 0; i < bitsToRead; i++) {
|
||||||
buffer[i] = digitalRead(DATA_PIN);
|
//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();
|
interrupts();
|
||||||
|
|
||||||
// plot polling process from controller if unstructed to
|
// plot polling process from controller if unstructed to
|
||||||
#ifdef PLOT_CONSOLE_POLLING
|
#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);
|
Serial.println(buffer[i] * 2500);
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
}
|
|
||||||
|
|
||||||
/* Function to extract a controller bit from the buffer of returned data */
|
#ifdef PRINT_DATA
|
||||||
void getBit(bool *bit, int offset, bool *data) {
|
for (int i = 0; i < bitsToRead; i++) {
|
||||||
// sanity check offset
|
printf(buffer[i] ? "1" : "0");
|
||||||
if (offset < 0)
|
|
||||||
offset = 0;
|
|
||||||
|
|
||||||
// count
|
|
||||||
short count = 0;
|
|
||||||
|
|
||||||
// get count from offset to offset + length
|
|
||||||
for (int i = offset + DATA_OFFSET; i < offset + bitResolution; i++) {
|
|
||||||
count += *(data + i);
|
|
||||||
}
|
}
|
||||||
|
printf("\n----------------\n");
|
||||||
// if offset surpasses threshold set bit
|
#endif
|
||||||
*bit = false;
|
|
||||||
if (count > BIT_THRESHOLD)
|
|
||||||
*bit = true;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/** Function to populate the controller struct if command 0x01 was sent.
|
/** Function to populate the controller struct if command 0x01 was sent.
|
||||||
|
@ -153,22 +224,21 @@ void getBit(bool *bit, int offset, bool *data) {
|
||||||
*/
|
*/
|
||||||
void populateControllerStruct(ControllerData *data) {
|
void populateControllerStruct(ControllerData *data) {
|
||||||
// first byte
|
// first byte
|
||||||
getBit(&(data->buttonA), bitOffsets[0], &buffer[0]);
|
data->buttonA = buffer[bitOffsets[0]];
|
||||||
getBit(&(data->buttonB), bitOffsets[1], &buffer[0]);
|
data->buttonB = buffer[bitOffsets[1]];
|
||||||
getBit(&(data->buttonZ), bitOffsets[2], &buffer[0]);
|
data->buttonZ = buffer[bitOffsets[2]];
|
||||||
getBit(&(data->buttonStart), bitOffsets[3], &buffer[0]);
|
data->buttonStart = buffer[bitOffsets[3]];
|
||||||
getBit(&(data->DPadUp), bitOffsets[4], &buffer[0]);
|
data->DPadUp = buffer[bitOffsets[4]];
|
||||||
getBit(&(data->DPadDown), bitOffsets[5], &buffer[0]);
|
data->DPadDown = buffer[bitOffsets[5]];
|
||||||
getBit(&(data->DPadLeft), bitOffsets[6], &buffer[0]);
|
data->DPadLeft = buffer[bitOffsets[6]];
|
||||||
getBit(&(data->DPadRight), bitOffsets[7], &buffer[0]);
|
data->DPadRight = buffer[bitOffsets[7]];
|
||||||
|
|
||||||
// second byte, first two bits are unused
|
// second byte, first two bits are unused
|
||||||
getBit(&(data->buttonL), bitOffsets[10], &buffer[0]);
|
data->buttonL = buffer[bitOffsets[10]];
|
||||||
getBit(&(data->buttonR), bitOffsets[11], &buffer[0]);
|
data->buttonR = buffer[bitOffsets[11]];
|
||||||
getBit(&(data->CUp), bitOffsets[12], &buffer[0]);
|
data->CUp = buffer[bitOffsets[12]];
|
||||||
getBit(&(data->CDown), bitOffsets[13], &buffer[0]);
|
data->CDown = buffer[bitOffsets[13]];
|
||||||
getBit(&(data->CLeft), bitOffsets[14], &buffer[0]);
|
data->CRight = buffer[bitOffsets[14]];
|
||||||
getBit(&(data->CRight), bitOffsets[15], &buffer[0]);
|
|
||||||
|
|
||||||
// third byte
|
// third byte
|
||||||
getBit(&(data->xAxisRaw[0]), bitOffsets[16], &buffer[0]);
|
getBit(&(data->xAxisRaw[0]), bitOffsets[16], &buffer[0]);
|
||||||
|
@ -246,76 +316,18 @@ void populateControllerStruct(ControllerData *data) {
|
||||||
//Serial.printf("xaxis: %-3i yaxis: %-3i \n",data->xAxis,data->yAxis);
|
//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;
|
ControllerData controller;
|
||||||
|
|
||||||
void setup() {
|
void loope() {
|
||||||
#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() {
|
|
||||||
// polling must not occur faster than every 20 ms
|
// 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");
|
//Serial.println("sending command to n64");
|
||||||
// send command 0x01 to n64 controller
|
// send command 0x01 to n64 controller
|
||||||
sendCommand(0x01);
|
sendCommand(0x01);
|
||||||
|
//updateOffsetsAndResolution();
|
||||||
|
|
||||||
// store received data in controller struct
|
// store received data in controller struct
|
||||||
populateControllerStruct(&controller);
|
populateControllerStruct(&controller);
|
||||||
|
@ -385,3 +397,59 @@ void loop() {
|
||||||
|
|
||||||
//delay(500);
|
//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();
|
||||||
|
}
|
||||||
|
|
|
@ -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;
|
||||||
|
}
|
Loading…
Reference in New Issue