diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..a71b9a4 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,6 @@ +{ + "files.associations": { + "string_view": "cpp", + "regex": "cpp" + } +} \ No newline at end of file diff --git a/src/Filter.h b/src/Filter.h deleted file mode 100644 index ef36fbf..0000000 --- a/src/Filter.h +++ /dev/null @@ -1,90 +0,0 @@ -#include -#include -#include "MedianFilter.h" - -// Create median filter objects, once for each sensor -MedianFilter mf0; -MedianFilter mf1; -//MedianFilter mf2; -//MedianFilter mf3; -//MedianFilter mf4; -//MedianFilter mf5; -//MedianFilter mf6; -//MedianFilter mf7; -//MedianFilter mf8; -//MedianFilter mf9; - -float getFilteredAngle(float fingerAngles,int i){ - - //thumbIP_angle, - if (i == 0){ - mf0.addSample(fingerAngles); - float FilterAngle0 = mf0.getMedian(); - return FilterAngle0; - } - - // thumbMCP_angle - else if (i == 1){ - mf1.addSample(fingerAngles); - float FilterAngle1 = mf1.getMedian(); - return FilterAngle1; - } - - /* - // f1PIP_angle - else if (i == 2){ - mf1.addSample(fingerAngles); - float FilterAngle2 = mf1.getMedian(); - return FilterAngle2; - } - - // f1MCP_angle - else if (i == 3){ - mf1.addSample(fingerAngles); - float FilterAngle1 = mf1.getMedian(); - return FilterAngle1; - } - - // f2PIP_angle - else if (i == 4){ - mf1.addSample(fingerAngles); - float FilterAngle1 = mf1.getMedian(); - return FilterAngle1; - } - - // f2MCP_angle - else if (i == 5){ - mf1.addSample(fingerAngles); - float FilterAngle1 = mf1.getMedian(); - return FilterAngle1; - } - - // f3PIP_angle - else if (i == 6){ - mf1.addSample(fingerAngles); - float FilterAngle1 = mf1.getMedian(); - return FilterAngle1; - } - - // f3MCP_angle - else if (i == 7){ - mf1.addSample(fingerAngles); - float FilterAngle1 = mf1.getMedian(); - return FilterAngle1; - } - - // f4PIP_angle - else if (i == 8){ - mf1.addSample(fingerAngles); - float FilterAngle1 = mf1.getMedian(); - return FilterAngle1; - } - - // f4MCP_angle - else if (i == 9){ - mf1.addSample(fingerAngles); - float FilterAngle1 = mf1.getMedian(); - return FilterAngle1; - } - */ -} \ No newline at end of file diff --git a/src/FilterTest.cpp b/src/FilterTest.cpp deleted file mode 100644 index c83c842..0000000 --- a/src/FilterTest.cpp +++ /dev/null @@ -1,47 +0,0 @@ -#include -#include "Filter.h" - - -const int flexPin1 = 4; // Pin connected to voltage divider output -const int flexPin2 = 0; // Pin connected to voltage divider output -float fingerAngles[2] = {0,0}; -float fingerAnglesFiltered[2] = {0,0}; -const int sizeList = 2; - -// Change these constants according to your project's design -const float VCC = 5; // voltage at Ardunio 5V line -const float R_DIV = 10000.0; // resistor used to create a voltage divider - -void setup() { - Serial.begin(115200); - pinMode(flexPin1, INPUT); - pinMode(flexPin2, INPUT); -} - - -void loop() { - - // Calculate angle of fingers - int ADCflex0 = analogRead(flexPin1); - int ADCflex1 = analogRead(flexPin2); - - float Vflex0 = ADCflex0 * VCC / 4095.0; //1023.0 - float Vflex1 = ADCflex1 * VCC / 4095.0; //1023.0 - - float Rflex0 = R_DIV * (VCC / Vflex0 - 1.0); - float Rflex1 = R_DIV * (VCC / Vflex1 - 1.0); - - float angle0 = Rflex0 * 2.6716/1000 - 30.2045 -20; - float angle1 = Rflex1 * 2.6716/1000 - 30.2045 -20; - - fingerAngles[0] = angle0; - fingerAngles[1] = angle1; - - - //Läser alla sensorer och lägger i en lista som sedan filtreras - for(int i = 0; i +#include + +#ifdef __AVR__ +#include +#include +#elif defined(ESP8266) +#include +#undef PROGMEM +#define PROGMEM STORE_ATTR +#elif defined(__IMXRT1052__) || defined(__IMXRT1062__) +// PROGMEM is defefind for T4 to place data in specific memory section +#undef PROGMEM +#define PROGMEM +#else +#define PROGMEM +#endif + +const uint8_t FreeMono12pt7bBitmaps[] PROGMEM = { + 0x00, 0x49, 0x24, 0x92, 0x48, 0x01, 0xF8, 0xE7, 0xE7, 0x67, 0x42, 0x42, + 0x42, 0x42, 0x09, 0x02, 0x41, 0x10, 0x44, 0x11, 0x1F, 0xF1, 0x10, 0x4C, + 0x12, 0x3F, 0xE1, 0x20, 0x48, 0x12, 0x04, 0x81, 0x20, 0x48, 0x04, 0x07, + 0xA2, 0x19, 0x02, 0x40, 0x10, 0x03, 0x80, 0x3C, 0x00, 0x80, 0x10, 0x06, + 0x01, 0xE0, 0xAF, 0xC0, 0x40, 0x10, 0x04, 0x00, 0x3C, 0x19, 0x84, 0x21, + 0x08, 0x66, 0x0F, 0x00, 0x0C, 0x1C, 0x78, 0x01, 0xE0, 0xCC, 0x21, 0x08, + 0x43, 0x30, 0x78, 0x3E, 0x30, 0x10, 0x08, 0x02, 0x03, 0x03, 0x47, 0x14, + 0x8A, 0x43, 0x31, 0x8F, 0x60, 0xFD, 0xA4, 0x90, 0x05, 0x25, 0x24, 0x92, + 0x48, 0x92, 0x24, 0x11, 0x24, 0x89, 0x24, 0x92, 0x92, 0x90, 0x00, 0x04, + 0x02, 0x11, 0x07, 0xF0, 0xC0, 0x50, 0x48, 0x42, 0x00, 0x08, 0x04, 0x02, + 0x01, 0x00, 0x87, 0xFC, 0x20, 0x10, 0x08, 0x04, 0x02, 0x00, 0x3B, 0x9C, + 0xCE, 0x62, 0x00, 0xFF, 0xE0, 0xFF, 0x80, 0x00, 0x80, 0xC0, 0x40, 0x20, + 0x20, 0x10, 0x10, 0x08, 0x08, 0x04, 0x04, 0x02, 0x02, 0x01, 0x01, 0x00, + 0x80, 0x80, 0x40, 0x00, 0x1C, 0x31, 0x90, 0x58, 0x38, 0x0C, 0x06, 0x03, + 0x01, 0x80, 0xC0, 0x60, 0x38, 0x34, 0x13, 0x18, 0x70, 0x30, 0xE1, 0x44, + 0x81, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x81, 0x1F, 0xC0, 0x1E, 0x10, + 0x90, 0x68, 0x10, 0x08, 0x0C, 0x04, 0x04, 0x04, 0x06, 0x06, 0x06, 0x06, + 0x0E, 0x07, 0xFE, 0x3E, 0x10, 0x40, 0x08, 0x02, 0x00, 0x80, 0x40, 0xE0, + 0x04, 0x00, 0x80, 0x10, 0x04, 0x01, 0x00, 0x98, 0x63, 0xE0, 0x06, 0x0A, + 0x0A, 0x12, 0x22, 0x22, 0x42, 0x42, 0x82, 0x82, 0xFF, 0x02, 0x02, 0x02, + 0x0F, 0x7F, 0x20, 0x10, 0x08, 0x04, 0x02, 0xF1, 0x8C, 0x03, 0x00, 0x80, + 0x40, 0x20, 0x18, 0x16, 0x18, 0xF0, 0x0F, 0x8C, 0x08, 0x08, 0x04, 0x04, + 0x02, 0x79, 0x46, 0xC1, 0xE0, 0x60, 0x28, 0x14, 0x19, 0x18, 0x78, 0xFF, + 0x81, 0x81, 0x03, 0x02, 0x02, 0x02, 0x04, 0x04, 0x04, 0x04, 0x08, 0x08, + 0x08, 0x08, 0x3E, 0x31, 0xA0, 0x70, 0x18, 0x0C, 0x05, 0x8C, 0x38, 0x63, + 0x40, 0x60, 0x30, 0x18, 0x1B, 0x18, 0xF8, 0x3C, 0x31, 0x30, 0x50, 0x28, + 0x0C, 0x0F, 0x06, 0x85, 0x3C, 0x80, 0x40, 0x40, 0x20, 0x20, 0x63, 0xE0, + 0xFF, 0x80, 0x07, 0xFC, 0x39, 0xCE, 0x00, 0x00, 0x06, 0x33, 0x98, 0xC4, + 0x00, 0x00, 0xC0, 0x60, 0x18, 0x0C, 0x06, 0x01, 0x80, 0x0C, 0x00, 0x60, + 0x03, 0x00, 0x30, 0x01, 0x00, 0xFF, 0xF0, 0x00, 0x00, 0x0F, 0xFF, 0xC0, + 0x06, 0x00, 0x30, 0x01, 0x80, 0x18, 0x01, 0x80, 0xC0, 0x30, 0x18, 0x0C, + 0x02, 0x00, 0x00, 0x3E, 0x60, 0xA0, 0x20, 0x10, 0x08, 0x08, 0x18, 0x10, + 0x08, 0x00, 0x00, 0x00, 0x01, 0xC0, 0xE0, 0x1C, 0x31, 0x10, 0x50, 0x28, + 0x14, 0x3A, 0x25, 0x22, 0x91, 0x4C, 0xA3, 0xF0, 0x08, 0x02, 0x01, 0x80, + 0x7C, 0x3F, 0x00, 0x0C, 0x00, 0x48, 0x01, 0x20, 0x04, 0x40, 0x21, 0x00, + 0x84, 0x04, 0x08, 0x1F, 0xE0, 0x40, 0x82, 0x01, 0x08, 0x04, 0x20, 0x13, + 0xE1, 0xF0, 0xFF, 0x08, 0x11, 0x01, 0x20, 0x24, 0x04, 0x81, 0x1F, 0xC2, + 0x06, 0x40, 0x68, 0x05, 0x00, 0xA0, 0x14, 0x05, 0xFF, 0x00, 0x1E, 0x48, + 0x74, 0x05, 0x01, 0x80, 0x20, 0x08, 0x02, 0x00, 0x80, 0x20, 0x04, 0x01, + 0x01, 0x30, 0x87, 0xC0, 0xFE, 0x10, 0x44, 0x09, 0x03, 0x40, 0x50, 0x14, + 0x05, 0x01, 0x40, 0x50, 0x14, 0x0D, 0x02, 0x41, 0x3F, 0x80, 0xFF, 0xC8, + 0x09, 0x01, 0x20, 0x04, 0x00, 0x88, 0x1F, 0x02, 0x20, 0x40, 0x08, 0x01, + 0x00, 0xA0, 0x14, 0x03, 0xFF, 0xC0, 0xFF, 0xE8, 0x05, 0x00, 0xA0, 0x04, + 0x00, 0x88, 0x1F, 0x02, 0x20, 0x40, 0x08, 0x01, 0x00, 0x20, 0x04, 0x01, + 0xF0, 0x00, 0x1F, 0x46, 0x19, 0x01, 0x60, 0x28, 0x01, 0x00, 0x20, 0x04, + 0x00, 0x83, 0xF0, 0x0B, 0x01, 0x20, 0x23, 0x0C, 0x3E, 0x00, 0xE1, 0xD0, + 0x24, 0x09, 0x02, 0x40, 0x90, 0x27, 0xF9, 0x02, 0x40, 0x90, 0x24, 0x09, + 0x02, 0x40, 0xB8, 0x70, 0xFE, 0x20, 0x40, 0x81, 0x02, 0x04, 0x08, 0x10, + 0x20, 0x40, 0x81, 0x1F, 0xC0, 0x0F, 0xE0, 0x10, 0x02, 0x00, 0x40, 0x08, + 0x01, 0x00, 0x20, 0x04, 0x80, 0x90, 0x12, 0x02, 0x40, 0xC6, 0x30, 0x7C, + 0x00, 0xF1, 0xE4, 0x0C, 0x41, 0x04, 0x20, 0x44, 0x04, 0x80, 0x5C, 0x06, + 0x60, 0x43, 0x04, 0x10, 0x40, 0x84, 0x08, 0x40, 0xCF, 0x07, 0xF8, 0x04, + 0x00, 0x80, 0x10, 0x02, 0x00, 0x40, 0x08, 0x01, 0x00, 0x20, 0x04, 0x04, + 0x80, 0x90, 0x12, 0x03, 0xFF, 0xC0, 0xE0, 0x3B, 0x01, 0x94, 0x14, 0xA0, + 0xA4, 0x89, 0x24, 0x49, 0x14, 0x48, 0xA2, 0x45, 0x12, 0x10, 0x90, 0x04, + 0x80, 0x24, 0x01, 0x78, 0x3C, 0xE0, 0xF6, 0x02, 0x50, 0x25, 0x02, 0x48, + 0x24, 0xC2, 0x44, 0x24, 0x22, 0x43, 0x24, 0x12, 0x40, 0xA4, 0x0A, 0x40, + 0x6F, 0x06, 0x0F, 0x03, 0x0C, 0x60, 0x64, 0x02, 0x80, 0x18, 0x01, 0x80, + 0x18, 0x01, 0x80, 0x18, 0x01, 0x40, 0x26, 0x06, 0x30, 0xC0, 0xF0, 0xFF, + 0x10, 0x64, 0x05, 0x01, 0x40, 0x50, 0x34, 0x19, 0xF8, 0x40, 0x10, 0x04, + 0x01, 0x00, 0x40, 0x3E, 0x00, 0x0F, 0x03, 0x0C, 0x60, 0x64, 0x02, 0x80, + 0x18, 0x01, 0x80, 0x18, 0x01, 0x80, 0x18, 0x01, 0x40, 0x26, 0x06, 0x30, + 0xC1, 0xF0, 0x0C, 0x01, 0xF1, 0x30, 0xE0, 0xFF, 0x04, 0x18, 0x40, 0x44, + 0x04, 0x40, 0x44, 0x0C, 0x41, 0x87, 0xE0, 0x43, 0x04, 0x18, 0x40, 0x84, + 0x04, 0x40, 0x4F, 0x03, 0x1F, 0x48, 0x34, 0x05, 0x01, 0x40, 0x08, 0x01, + 0xC0, 0x0E, 0x00, 0x40, 0x18, 0x07, 0x01, 0xE1, 0xA7, 0xC0, 0xFF, 0xF0, + 0x86, 0x10, 0x82, 0x00, 0x40, 0x08, 0x01, 0x00, 0x20, 0x04, 0x00, 0x80, + 0x10, 0x02, 0x00, 0x40, 0x7F, 0x00, 0xF0, 0xF4, 0x02, 0x40, 0x24, 0x02, + 0x40, 0x24, 0x02, 0x40, 0x24, 0x02, 0x40, 0x24, 0x02, 0x40, 0x22, 0x04, + 0x30, 0xC0, 0xF0, 0xF8, 0x7C, 0x80, 0x22, 0x01, 0x04, 0x04, 0x10, 0x20, + 0x40, 0x80, 0x82, 0x02, 0x10, 0x08, 0x40, 0x11, 0x00, 0x48, 0x01, 0xA0, + 0x03, 0x00, 0x0C, 0x00, 0xF8, 0x7C, 0x80, 0x22, 0x00, 0x88, 0xC2, 0x23, + 0x10, 0x8E, 0x42, 0x29, 0x09, 0x24, 0x24, 0x90, 0x91, 0x41, 0x85, 0x06, + 0x14, 0x18, 0x70, 0x60, 0x80, 0xF0, 0xF2, 0x06, 0x30, 0x41, 0x08, 0x09, + 0x80, 0x50, 0x06, 0x00, 0x60, 0x0D, 0x00, 0x88, 0x10, 0xC2, 0x04, 0x60, + 0x2F, 0x0F, 0xF0, 0xF2, 0x02, 0x10, 0x41, 0x04, 0x08, 0x80, 0x50, 0x05, + 0x00, 0x20, 0x02, 0x00, 0x20, 0x02, 0x00, 0x20, 0x02, 0x01, 0xFC, 0xFF, + 0x40, 0xA0, 0x90, 0x40, 0x40, 0x40, 0x20, 0x20, 0x20, 0x10, 0x50, 0x30, + 0x18, 0x0F, 0xFC, 0xF2, 0x49, 0x24, 0x92, 0x49, 0x24, 0x9C, 0x80, 0x60, + 0x10, 0x08, 0x02, 0x01, 0x00, 0x40, 0x20, 0x08, 0x04, 0x01, 0x00, 0x80, + 0x20, 0x10, 0x04, 0x02, 0x00, 0x80, 0x40, 0xE4, 0x92, 0x49, 0x24, 0x92, + 0x49, 0x3C, 0x08, 0x0C, 0x09, 0x0C, 0x4C, 0x14, 0x04, 0xFF, 0xFC, 0x84, + 0x21, 0x3E, 0x00, 0x60, 0x08, 0x02, 0x3F, 0x98, 0x28, 0x0A, 0x02, 0xC3, + 0x9F, 0x30, 0xE0, 0x01, 0x00, 0x08, 0x00, 0x40, 0x02, 0x00, 0x13, 0xE0, + 0xA0, 0x86, 0x02, 0x20, 0x09, 0x00, 0x48, 0x02, 0x40, 0x13, 0x01, 0x14, + 0x13, 0x9F, 0x00, 0x1F, 0x4C, 0x19, 0x01, 0x40, 0x28, 0x01, 0x00, 0x20, + 0x02, 0x00, 0x60, 0x43, 0xF0, 0x00, 0xC0, 0x08, 0x01, 0x00, 0x20, 0x04, + 0x3C, 0x98, 0x52, 0x06, 0x80, 0x50, 0x0A, 0x01, 0x40, 0x24, 0x0C, 0xC2, + 0x87, 0x98, 0x3F, 0x18, 0x68, 0x06, 0x01, 0xFF, 0xE0, 0x08, 0x03, 0x00, + 0x60, 0xC7, 0xC0, 0x0F, 0x98, 0x08, 0x04, 0x02, 0x07, 0xF8, 0x80, 0x40, + 0x20, 0x10, 0x08, 0x04, 0x02, 0x01, 0x03, 0xF8, 0x1E, 0x6C, 0x39, 0x03, + 0x40, 0x28, 0x05, 0x00, 0xA0, 0x12, 0x06, 0x61, 0x43, 0xC8, 0x01, 0x00, + 0x20, 0x08, 0x3E, 0x00, 0xC0, 0x10, 0x04, 0x01, 0x00, 0x40, 0x13, 0x87, + 0x11, 0x82, 0x40, 0x90, 0x24, 0x09, 0x02, 0x40, 0x90, 0x2E, 0x1C, 0x08, + 0x04, 0x02, 0x00, 0x00, 0x03, 0xC0, 0x20, 0x10, 0x08, 0x04, 0x02, 0x01, + 0x00, 0x80, 0x43, 0xFE, 0x04, 0x08, 0x10, 0x00, 0x1F, 0xC0, 0x81, 0x02, + 0x04, 0x08, 0x10, 0x20, 0x40, 0x81, 0x02, 0x0B, 0xE0, 0xE0, 0x02, 0x00, + 0x20, 0x02, 0x00, 0x20, 0x02, 0x3C, 0x21, 0x02, 0x60, 0x2C, 0x03, 0x80, + 0x24, 0x02, 0x20, 0x21, 0x02, 0x08, 0xE1, 0xF0, 0x78, 0x04, 0x02, 0x01, + 0x00, 0x80, 0x40, 0x20, 0x10, 0x08, 0x04, 0x02, 0x01, 0x00, 0x80, 0x43, + 0xFE, 0xDC, 0xE3, 0x39, 0x90, 0x84, 0x84, 0x24, 0x21, 0x21, 0x09, 0x08, + 0x48, 0x42, 0x42, 0x17, 0x18, 0xC0, 0x67, 0x83, 0x84, 0x20, 0x22, 0x02, + 0x20, 0x22, 0x02, 0x20, 0x22, 0x02, 0x20, 0x2F, 0x07, 0x1F, 0x04, 0x11, + 0x01, 0x40, 0x18, 0x03, 0x00, 0x60, 0x0A, 0x02, 0x20, 0x83, 0xE0, 0xCF, + 0x85, 0x06, 0x60, 0x24, 0x01, 0x40, 0x14, 0x01, 0x40, 0x16, 0x02, 0x50, + 0x44, 0xF8, 0x40, 0x04, 0x00, 0x40, 0x0F, 0x00, 0x1E, 0x6C, 0x3B, 0x03, + 0x40, 0x28, 0x05, 0x00, 0xA0, 0x12, 0x06, 0x61, 0x43, 0xC8, 0x01, 0x00, + 0x20, 0x04, 0x03, 0xC0, 0xE3, 0x8B, 0x13, 0x00, 0x80, 0x20, 0x08, 0x02, + 0x00, 0x80, 0x20, 0x3F, 0x80, 0x1F, 0x58, 0x34, 0x05, 0x80, 0x1E, 0x00, + 0x60, 0x06, 0x01, 0xC0, 0xA7, 0xC0, 0x20, 0x04, 0x00, 0x80, 0x10, 0x0F, + 0xF0, 0x40, 0x08, 0x01, 0x00, 0x20, 0x04, 0x00, 0x80, 0x10, 0x03, 0x04, + 0x3F, 0x00, 0xC1, 0xC8, 0x09, 0x01, 0x20, 0x24, 0x04, 0x80, 0x90, 0x12, + 0x02, 0x61, 0xC7, 0xCC, 0xF8, 0xF9, 0x01, 0x08, 0x10, 0x60, 0x81, 0x0C, + 0x08, 0x40, 0x22, 0x01, 0x20, 0x05, 0x00, 0x30, 0x00, 0xF0, 0x7A, 0x01, + 0x10, 0x08, 0x8C, 0x42, 0x62, 0x12, 0x90, 0xA5, 0x05, 0x38, 0x28, 0xC0, + 0x86, 0x00, 0x78, 0xF3, 0x04, 0x18, 0x80, 0xD0, 0x06, 0x00, 0x70, 0x09, + 0x81, 0x0C, 0x20, 0x6F, 0x8F, 0xF0, 0xF2, 0x02, 0x20, 0x41, 0x04, 0x10, + 0x80, 0x88, 0x09, 0x00, 0x50, 0x06, 0x00, 0x20, 0x04, 0x00, 0x40, 0x08, + 0x0F, 0xE0, 0xFF, 0x41, 0x00, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x40, + 0xBF, 0xC0, 0x19, 0x08, 0x42, 0x10, 0x84, 0x44, 0x18, 0x42, 0x10, 0x84, + 0x20, 0xC0, 0xFF, 0xFF, 0xC0, 0xC1, 0x08, 0x42, 0x10, 0x84, 0x30, 0x4C, + 0x42, 0x10, 0x84, 0x26, 0x00, 0x38, 0x13, 0x38, 0x38 }; + +const GFXglyph FreeMono12pt7bGlyphs[] PROGMEM = { + { 0, 1, 1, 14, 0, 0 }, // 0x20 ' ' + { 1, 3, 15, 14, 6, -14 }, // 0x21 '!' + { 7, 8, 7, 14, 3, -14 }, // 0x22 '"' + { 14, 10, 16, 14, 2, -14 }, // 0x23 '#' + { 34, 10, 17, 14, 2, -14 }, // 0x24 '$' + { 56, 10, 15, 14, 2, -14 }, // 0x25 '%' + { 75, 9, 12, 14, 3, -11 }, // 0x26 '&' + { 89, 3, 7, 14, 5, -14 }, // 0x27 ''' + { 92, 3, 18, 14, 7, -14 }, // 0x28 '(' + { 99, 3, 18, 14, 4, -14 }, // 0x29 ')' + { 106, 9, 9, 14, 3, -14 }, // 0x2A '*' + { 117, 9, 11, 14, 3, -11 }, // 0x2B '+' + { 130, 5, 7, 14, 3, -3 }, // 0x2C ',' + { 135, 11, 1, 14, 2, -6 }, // 0x2D '-' + { 137, 3, 3, 14, 5, -2 }, // 0x2E '.' + { 139, 9, 18, 14, 3, -15 }, // 0x2F '/' + { 160, 9, 15, 14, 3, -14 }, // 0x30 '0' + { 177, 7, 14, 14, 4, -13 }, // 0x31 '1' + { 190, 9, 15, 14, 2, -14 }, // 0x32 '2' + { 207, 10, 15, 14, 2, -14 }, // 0x33 '3' + { 226, 8, 15, 14, 3, -14 }, // 0x34 '4' + { 241, 9, 15, 14, 3, -14 }, // 0x35 '5' + { 258, 9, 15, 14, 3, -14 }, // 0x36 '6' + { 275, 8, 15, 14, 3, -14 }, // 0x37 '7' + { 290, 9, 15, 14, 3, -14 }, // 0x38 '8' + { 307, 9, 15, 14, 3, -14 }, // 0x39 '9' + { 324, 3, 10, 14, 5, -9 }, // 0x3A ':' + { 328, 5, 13, 14, 3, -9 }, // 0x3B ';' + { 337, 11, 11, 14, 2, -11 }, // 0x3C '<' + { 353, 12, 4, 14, 1, -8 }, // 0x3D '=' + { 359, 11, 11, 14, 2, -11 }, // 0x3E '>' + { 375, 9, 14, 14, 3, -13 }, // 0x3F '?' + { 391, 9, 16, 14, 3, -14 }, // 0x40 '@' + { 409, 14, 14, 14, 0, -13 }, // 0x41 'A' + { 434, 11, 14, 14, 2, -13 }, // 0x42 'B' + { 454, 10, 14, 14, 2, -13 }, // 0x43 'C' + { 472, 10, 14, 14, 2, -13 }, // 0x44 'D' + { 490, 11, 14, 14, 2, -13 }, // 0x45 'E' + { 510, 11, 14, 14, 2, -13 }, // 0x46 'F' + { 530, 11, 14, 14, 2, -13 }, // 0x47 'G' + { 550, 10, 14, 14, 2, -13 }, // 0x48 'H' + { 568, 7, 14, 14, 4, -13 }, // 0x49 'I' + { 581, 11, 14, 14, 2, -13 }, // 0x4A 'J' + { 601, 12, 14, 14, 2, -13 }, // 0x4B 'K' + { 622, 11, 14, 14, 2, -13 }, // 0x4C 'L' + { 642, 13, 14, 14, 1, -13 }, // 0x4D 'M' + { 665, 12, 14, 14, 1, -13 }, // 0x4E 'N' + { 686, 12, 14, 14, 1, -13 }, // 0x4F 'O' + { 707, 10, 14, 14, 2, -13 }, // 0x50 'P' + { 725, 12, 17, 14, 1, -13 }, // 0x51 'Q' + { 751, 12, 14, 14, 2, -13 }, // 0x52 'R' + { 772, 10, 14, 14, 2, -13 }, // 0x53 'S' + { 790, 11, 14, 14, 2, -13 }, // 0x54 'T' + { 810, 12, 14, 14, 1, -13 }, // 0x55 'U' + { 831, 14, 14, 14, 0, -13 }, // 0x56 'V' + { 856, 14, 14, 14, 0, -13 }, // 0x57 'W' + { 881, 12, 14, 14, 1, -13 }, // 0x58 'X' + { 902, 12, 14, 14, 1, -13 }, // 0x59 'Y' + { 923, 9, 14, 14, 3, -13 }, // 0x5A 'Z' + { 939, 3, 18, 14, 7, -14 }, // 0x5B '[' + { 946, 9, 18, 14, 3, -15 }, // 0x5C '\' + { 967, 3, 18, 14, 5, -14 }, // 0x5D ']' + { 974, 9, 6, 14, 3, -14 }, // 0x5E '^' + { 981, 14, 1, 14, 0, 3 }, // 0x5F '_' + { 983, 4, 4, 14, 4, -15 }, // 0x60 '`' + { 985, 10, 10, 14, 2, -9 }, // 0x61 'a' + { 998, 13, 15, 14, 0, -14 }, // 0x62 'b' + { 1023, 11, 10, 14, 2, -9 }, // 0x63 'c' + { 1037, 11, 15, 14, 2, -14 }, // 0x64 'd' + { 1058, 10, 10, 14, 2, -9 }, // 0x65 'e' + { 1071, 9, 15, 14, 4, -14 }, // 0x66 'f' + { 1088, 11, 14, 14, 2, -9 }, // 0x67 'g' + { 1108, 10, 15, 14, 2, -14 }, // 0x68 'h' + { 1127, 9, 15, 14, 3, -14 }, // 0x69 'i' + { 1144, 7, 19, 14, 3, -14 }, // 0x6A 'j' + { 1161, 12, 15, 14, 1, -14 }, // 0x6B 'k' + { 1184, 9, 15, 14, 3, -14 }, // 0x6C 'l' + { 1201, 13, 10, 14, 1, -9 }, // 0x6D 'm' + { 1218, 12, 10, 14, 1, -9 }, // 0x6E 'n' + { 1233, 11, 10, 14, 2, -9 }, // 0x6F 'o' + { 1247, 12, 14, 14, 1, -9 }, // 0x70 'p' + { 1268, 11, 14, 14, 2, -9 }, // 0x71 'q' + { 1288, 10, 10, 14, 3, -9 }, // 0x72 'r' + { 1301, 10, 10, 14, 2, -9 }, // 0x73 's' + { 1314, 11, 14, 14, 1, -13 }, // 0x74 't' + { 1334, 11, 10, 14, 2, -9 }, // 0x75 'u' + { 1348, 13, 10, 14, 1, -9 }, // 0x76 'v' + { 1365, 13, 10, 14, 1, -9 }, // 0x77 'w' + { 1382, 12, 10, 14, 1, -9 }, // 0x78 'x' + { 1397, 12, 14, 14, 1, -9 }, // 0x79 'y' + { 1418, 9, 10, 14, 3, -9 }, // 0x7A 'z' + { 1430, 5, 18, 14, 5, -14 }, // 0x7B '{' + { 1442, 1, 18, 14, 7, -14 }, // 0x7C '|' + { 1445, 5, 18, 14, 5, -14 }, // 0x7D '}' + { 1457, 10, 3, 14, 2, -7 } }; // 0x7E '~' + +const GFXfont FreeMono12pt7b PROGMEM = { + (uint8_t *)FreeMono12pt7bBitmaps, + (GFXglyph *)FreeMono12pt7bGlyphs, + 0x20, 0x7E, 24 }; + +// Approx. 2133 bytes + +#endif // FreeMono12pt7b_H diff --git a/src/config.h b/src/config.h new file mode 100644 index 0000000..46fa15e --- /dev/null +++ b/src/config.h @@ -0,0 +1,25 @@ +/** + * Configuration file for the entire project. + * Define and change all constant variables here! +*/ +#pragma once + +// Connection for pins on multiplexer +#define SIG_PIN 36 +#define s0 26 +#define s1 27 +#define s2 14 +#define s3 12 + +// Connection for pins on display +#define DISP_RES 1 +#define DISP_DC 15 +#define DISP_CS 5 +#define DISP_SCK 18 +#define DISP_MOSI 23 + +// Number of samples for the median filter +const int SAMPLES = 7; + +// Pin to the push button that turns the controller on +#define INTERUPT_PIN 17 \ No newline at end of file diff --git a/src/display.h b/src/display.h new file mode 100644 index 0000000..edf2ddc --- /dev/null +++ b/src/display.h @@ -0,0 +1,58 @@ +/* + * This header file is for seting up the LCD-display and creating methods to easily control the display from the main class. + */ +#include +#include +#include "FreeMono12pt7b.h" +#include "config.h" + +// Screen setup +Arduino_DataBus *bus = new Arduino_ESP32SPI(DISP_DC /* DC */, DISP_CS /* CS */, DISP_SCK /* SCK */, DISP_MOSI /* MOSI */, -1 /* MISO */, VSPI /* spi_num */); +//Arduino_DataBus *bus = new Arduino_ESP32SPI(33 /* DC */, 5 /* CS */, 18 /* SCK */, 23 /* MOSI */, 19 /* MISO */); +Arduino_GFX *gfx = new Arduino_ST7789(bus, 7 /* RST */, 0 /* rotation */, true /* IPS */, + 240 /* width */, 240 /* height */, 0 /* col offset 1 */, 0 /* row offset 1 */); + +// Initialises the display, must be called before using other methods. +void disp_initialize(){ + // Display setup + pinMode(DISP_RES, OUTPUT); + pinMode(DISP_DC, OUTPUT); + digitalWrite(DISP_RES, HIGH); // Keep reset pin high + digitalWrite(DISP_RES, HIGH); // Set data mode to "Command" + gfx->begin(); // Start the screen object + gfx->fillScreen(BLACK); // "Clear" the screen + gfx->setCursor(5, 15); // Set print pointer to top left corner + gfx->setFont(&FreeMono12pt7b); // Set font +} + +// Clear display and reset cursor +void disp_clr(){ + gfx->fillScreen(BLACK); // "Clear" the display + gfx->setCursor(5, 15); // Set print pointer to top left corner +} + +// Print line of text on display +void disp_println(String string){ + gfx->println(string); +} +void disp_println(int num){ + gfx->println(num); +} + +// Print text on display +void disp_print(String string){ + gfx->print(string); +} +void disp_print(int num){ + gfx->print(num); +} + +// Set cursor location on display +void disp_setCursor(int16_t x, int16_t y){ + gfx->setCursor(x,y); +} + +// Set colour of text on display +void disp_setTextColor(int16_t COLOR){ + gfx->setTextColor(COLOR); +} \ No newline at end of file diff --git a/src/flexSensor.cpp b/src/flexSensor.cpp new file mode 100644 index 0000000..0ef8cb7 --- /dev/null +++ b/src/flexSensor.cpp @@ -0,0 +1,153 @@ +/** + * This class file is used to create flex sensor objects that handle all reading + * and filtering of each sensor signal.cha +*/ + +#include +#include +#include "Config.h" +#include "flexSensor.h" + +/** + * Constructor + * @param channel the channel to which the flex sensor is connected to. + * @param minimum_angle the minimum angle of flex sensor. + * @param maximum_angle the maximum angle of flex sensor. + * @param multipl boolean that states whether the flex sensor is connected to the multiplexer or directly to the esp32. +*/ +flexSensor::flexSensor(int channel, int minimum_angle, int maximum_angle){ + multipl = true; + m_channel = channel; + min_angle = minimum_angle; + max_angle = maximum_angle; +} +flexSensor::flexSensor(int channel){ + multipl = true; + m_channel = channel; + min_angle = 0; + max_angle = 90; +} +flexSensor::flexSensor(bool multiplexer, int pin, int minimum_angle, int maximum_angle){ + multipl = multiplexer; + m_channel = pin; + min_angle = minimum_angle; + max_angle = maximum_angle; +} +flexSensor::flexSensor(bool multiplexer, int pin){ + multipl = multiplexer; + m_channel = pin; + min_angle = 0; + max_angle = 90; +} + +/** + * Changes multiplexer to read from int channel and then returns the read value. + * @return analog reading from multiplexer in input channel. + * this method is taken from http://adam-meyer.com/arduino/CD74HC4067 under MIT-License. +*/ +int flexSensor::readMux(int channel){ + int controlPin[] = {s0, s1, s2, s3}; + + int muxChannel[16][4]={ + {0,0,0,0}, //channel 0 + {1,0,0,0}, //channel 1 + {0,1,0,0}, //channel 2 + {1,1,0,0}, //channel 3 + {0,0,1,0}, //channel 4 + {1,0,1,0}, //channel 5 + {0,1,1,0}, //channel 6 + {1,1,1,0}, //channel 7 + {0,0,0,1}, //channel 8 + {1,0,0,1}, //channel 9 + {0,1,0,1}, //channel 10 + {1,1,0,1}, //channel 11 + {0,0,1,1}, //channel 12 + {1,0,1,1}, //channel 13 + {0,1,1,1}, //channel 14 + {1,1,1,1} //channel 15 + }; + + //loop through the 4 sig + for(int i = 0; i < 4; i ++){ + digitalWrite(controlPin[i], muxChannel[channel][i]); + } + + //read the value at the SIG pin + int val = analogRead(SIG_PIN); + + //return the value + return val; +} + +int flexSensor::getValue(){ + if(multipl){ + return readMux(m_channel); + } + else{ + return analogRead(m_channel); + } + +} + + +/** + * Performs a measurement of the sensor and adds it to the median filter. + * @return a float with the median angle value. +*/ +float flexSensor::getAngle(){ + float angle; + if(multipl){ + angle = map(readMux(m_channel),calibrateOpen,calibrateClosed,min_angle,max_angle); + } + else{ + angle = map(analogRead(m_channel),calibrateOpen,calibrateClosed,min_angle,max_angle); + } + m_f.addSample(angle); + return m_f.getMedian(); +} + +/** + * Calibrates the flex sensor, either the open or closed state depending on input + * @return void +*/ +void flexSensor::calibrate(bool state){ + + if(multipl){ + for(int i = 0; i < 2*SAMPLES; i++){ + m_f.addSample(readMux(m_channel)); + } + } + else{ + for(int i = 0; i < 2*SAMPLES; i++){ + m_f.addSample(analogRead(m_channel)); + } + } + + if(!state){ + calibrateOpen = m_f.getMedian(); + } + else{ + calibrateClosed = m_f.getMedian(); + } +} +/** + * @returns value of calibrateClosed +*/ +int flexSensor::getCalibrateClosed(){ + return calibrateClosed; +} + +/** + * @returns value of calibrateOpen +*/ +int flexSensor::getCalibrateOpen(){ + return calibrateOpen; +} + +void flexSensor::setCalibrateClosed(int value){ + calibrateClosed = value; +} + +void flexSensor::setCalibrateOpen(int value){ + calibrateOpen = value; +} \ No newline at end of file diff --git a/src/flexSensor.h b/src/flexSensor.h new file mode 100644 index 0000000..654742e --- /dev/null +++ b/src/flexSensor.h @@ -0,0 +1,40 @@ +/** + * This class file is used to create flex sensor objects that handle all reading + * and filtering of each sensor signal. +*/ +#pragma once + +#include +#include "Config.h" +#include "MedianFilter.h" + +/** + * Main class +*/ +class flexSensor{ +private: + // Variables and objects + int m_channel; + int min_angle; + int max_angle; + int calibrateOpen; + int calibrateClosed; + bool multipl; + // Number of samples are defined in config.h + MedianFilter m_f; + // Methods + int readMux(int); +public: + flexSensor(int channel, int minimum_angle, int maximum_angle); + flexSensor(int channel); + flexSensor(bool multiplexer, int pin, int minimum_angle, int maximum_angle); + flexSensor(bool multiplexer, int pin); + // Methods + int getValue(); + float getAngle(); + void calibrate(bool state); + int getCalibrateOpen(); + int getCalibrateClosed(); + void setCalibrateOpen(int value); + void setCalibrateClosed(int value); +}; \ No newline at end of file diff --git a/src/flexsensorClass.cpp b/src/flexsensorClass.cpp deleted file mode 100644 index 8bd2eb1..0000000 --- a/src/flexsensorClass.cpp +++ /dev/null @@ -1,37 +0,0 @@ -/* - This class file is used to create flex sensor objects that handle all reading - and filtering of each sensor signal. -*/ -#include "flexsensorClass.h" -#include "readFingers.h" -/* - Constructor -*/ -flexsensor::flexsensor(int pin, String location){ - m_pin = pin; - m_location = location; - - initFlexSensor(m_pin); -} - -/* - Performs a measurement of the sensor and adds it to the median filter. -*/ -void flexsensor::read(){ - float m = analogRead(m_pin); - int a = getAngle(m); - m_f.addSample(a); -} -/* - Returns the median sensor reading value. -*/ -float flexsensor::getMedian(){ - return m_f.getMedian(); -} - -/* - Returns the location of the sensor. -*/ -String flexsensor::getlocation(){ - return m_location; -} \ No newline at end of file diff --git a/src/flexsensorClass.h b/src/flexsensorClass.h deleted file mode 100644 index 2e951e4..0000000 --- a/src/flexsensorClass.h +++ /dev/null @@ -1,29 +0,0 @@ -/* - This class file is used to create flex sensor objects that handle all reading - and filtering of each sensor signal. -*/ -#pragma once - -#include -#include "config.h" -#include "MedianFilter.h" - -/* - Main class -*/ -class flexsensor{ - // Variables -private: - int m_pin; - String m_location; - MedianFilter m_f; - -public: - flexsensor(int pin, String location){}; - // Methods - // ... Gets - float getMedian(); - String getlocation(); - // ... Sets - void read(); -}; \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index d816d39..c26f875 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,45 +1,109 @@ #include -#include -#include - -#define interuptPin 17 - -// Pin connected to voltage divider output. used for analogRead to get resistance of the flexsensors -const int thumbIP_Pin = 36; -const int thumbMCP_Pin = 39; -const int f1PIP_Pin = 32; -const int f1MCP_Pin = 33; -const int f2PIP_Pin = 34; -const int f2MCP_Pin = 35; -const int f3PIP_Pin = ads1118.AIN_2; -const int f3MCP_Pin = ads1118.AIN_3; -const int f4PIP_Pin = ads1118.AIN_0; -const int f4MCP_Pin = ads1118.AIN_1; -const int pinList[] = {thumbIP_Pin, thumbMCP_Pin, f1PIP_Pin, f1MCP_Pin, f2PIP_Pin, f2MCP_Pin, f3PIP_Pin, f3MCP_Pin, f4PIP_Pin, f4MCP_Pin}; -const int sizeList = sizeof(pinList)/sizeof(int); -float fingerAngles[10] = {0,0,0,0,0,0,0,0,0,0}; +#include "TalkyBoi.h" +#include "config.h" +#include "flexSensor.h" +#include "potentiometer.h" +#include "display.h" + int sendID = 0; -int o = 0; int buttonRun = 0; int state = 0; +int calibrateState = 0; + +// Declaring all flexSensor objects +flexSensor thumbIP = flexSensor(8); +flexSensor thumbMCP = flexSensor(9); +flexSensor f1PIP = flexSensor(0); +flexSensor f1MCP = flexSensor(1); +flexSensor f2PIP = flexSensor(2); +flexSensor f2MCP = flexSensor(3); +flexSensor f3PIP = flexSensor(4); +flexSensor f3MCP = flexSensor(5); +flexSensor f4PIP = flexSensor(6); +flexSensor f4MCP = flexSensor(7); +flexSensor opposition = flexSensor(10); + +// Declaring all potentiometer objects +potentiometer f1AD = potentiometer(11); +potentiometer f2AD = potentiometer(12); +potentiometer f3AD = potentiometer(13); +potentiometer f4AD = potentiometer(14); +potentiometer tAD = potentiometer(15); + +// Creating list of flexSensor and list of potetiometer objects +flexSensor pinList[] = {thumbIP, thumbMCP, f1PIP, f1MCP, f2PIP, f2MCP, f3PIP, f3MCP, f4PIP, f4MCP, opposition}; +potentiometer potList[4] = {f1AD,f2AD,f3AD,f4AD}; +const int sizeList = sizeof(pinList)/sizeof(int); +// List of angles of all joints in fingers +float fingerAngles[] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; + // Initiates the multiplexer. void initBoard(){ - ads1118.begin(); // Initialize board - ads1118.setSamplingRate(ads1118.RATE_860SPS); // highest sampling rate possible - ads1118.setFullScaleRange(ads1118.FSR_4096); // 12 bit + pinMode(s0, OUTPUT); + pinMode(s1, OUTPUT); + pinMode(s2, OUTPUT); + pinMode(s3, OUTPUT); + + digitalWrite(s0, LOW); + digitalWrite(s1, LOW); + digitalWrite(s2, LOW); + digitalWrite(s3, LOW); } -void initAnalogPin(){ - pinMode(thumbIP_Pin, INPUT); // change later! - pinMode(thumbMCP_Pin, INPUT); - pinMode(f1PIP_Pin, INPUT); - pinMode(f1MCP_Pin, INPUT); - pinMode(f2PIP_Pin, INPUT); - pinMode(f2MCP_Pin, INPUT); +/* + * Takes in a given channel number and outputs a multiplexer reading from that given channel. + */ +int readMux(int channel){ + int controlPin[] = {s0, s1, s2, s3}; + + int muxChannel[16][4]={ + {0,0,0,0}, //channel 0 + {1,0,0,0}, //channel 1 + {0,1,0,0}, //channel 2 + {1,1,0,0}, //channel 3 + {0,0,1,0}, //channel 4 + {1,0,1,0}, //channel 5 + {0,1,1,0}, //channel 6 + {1,1,1,0}, //channel 7 + {0,0,0,1}, //channel 8 + {1,0,0,1}, //channel 9 + {0,1,0,1}, //channel 10 + {1,1,0,1}, //channel 11 + {0,0,1,1}, //channel 12 + {1,0,1,1}, //channel 13 + {0,1,1,1}, //channel 14 + {1,1,1,1} //channel 15 + }; + + //loop through the 4 sig + for(int i = 0; i < 4; i ++){ + digitalWrite(controlPin[i], muxChannel[channel][i]); + } + + //read the value at the SIG pin + int val = analogRead(SIG_PIN); + //return the value + return val; } +/** + * Interupt function for the initial calibration. +*/ +void interuptCalibrate(){ + static unsigned long last_interuptTime = 0; + unsigned long interupt_time = millis(); + if((interupt_time - last_interuptTime) > 200){ + Serial.println("PUSH"); + calibrateState++; + } + last_interuptTime = interupt_time; +} + +/** + * Toggles the state of the microcontroller. +*/ void interuptFunc(){ static unsigned long last_interuptTime = 0; unsigned long interupt_time = millis(); @@ -51,49 +115,208 @@ void interuptFunc(){ buttonRun = 1; state = 1; } + //Serial.println("Button"); } last_interuptTime = interupt_time; + } +void display_angle(potentiometer sensor){ + disp_setTextColor(WHITE); + disp_println(sensor.getAngle()); +} +void display_angle(flexSensor sensor){ + disp_setTextColor(WHITE); + disp_println(sensor.getAngle()); +} - -void setup() { - Serial.begin(115200); +/*Calibrates all 10 flex sensors for flexion movement. First hold hand open until OK + is printed and then have hand closed until OK is printed again */ +void calibrateFlexion(){ + attachInterrupt(INTERUPT_PIN,interuptCalibrate,RISING); + Serial.println("Flexion Calibr."); + disp_clr(); + disp_setTextColor(WHITE); + disp_println("Flexion Calibr."); + disp_println("----------------"); + Serial.println("HAND OPEN"); + disp_print("HAND OPEN"); + while(calibrateState < 1){delay(50);} + for(int i = 0; i<11 ; i++){ + pinList[i].calibrate(false); + } + Serial.println("OK"); + disp_setTextColor(GREEN); + disp_println(" OK"); + delay(100); + disp_setTextColor(WHITE); + disp_println("-----"); + Serial.println("HAND CLOSED"); + disp_print("HAND CLOSED"); + while(calibrateState < 2){delay(50);} + for(int i = 0; i<11 ; i++){ + pinList[i].calibrate(true); + } + Serial.println("OK"); + disp_setTextColor(GREEN); + disp_println(" OK"); delay(100); - getMACAdress(); //MAC adress är vad som körs för att WIFI ska funkar, den ANDRA bärands MAC adress ska skrivas in i denna koden och tvärtom. - init_wifi(); // Initiate ESP_NOW - initBoard(); // Initiate breakout board - initAnalogPin(); - // attachInterrupt(17, interuptFunc, HIGH); // interupt för start/stopp knapp + disp_setTextColor(WHITE); + disp_println("-----"); + delay(100); + disp_println("CALIBRATION"); + disp_println("COMPLETE"); + while(calibrateState < 3){delay(50);} + calibrateState = 0; + disp_clr(); + detachInterrupt(INTERUPT_PIN); } -void loop() { - //while(!state){} //Ta bort kommentar för att ha en knapp som låser/öppnar programmet när det körs. Behöver en debounce för knappen innan det funkar. +/* Calibrates potentiometers for measuring abduction and adduction of fingers. + Each finger is calibrated individually with instructions printed on the display */ +/*Change calibration for abduction and adduction potentiometers +By calibrating a "middle value" and theneither add a value +- to that middle value. +or calibrate a motion to one side and then mirror that motion to the otehr side.*/ - for(int i = 0; i>>>>>> de77f9514885061b4545c7e60c6282d2ad28d74c - sendID++; + potList[i].calibrate(false); + disp_setTextColor(GREEN); + Serial.println("OK"); + disp_println("OK"); + delay(100); + disp_setTextColor(WHITE); + disp_println("-----"); + disp_print("keep F"); + disp_print(i+1); + disp_println(" right"); + Serial.print("keep F"); + Serial.print(i+1); + Serial.print(" right"); + while(calibrateState < i+2){delay(50);} + + potList[i].calibrate(true); + disp_setTextColor(GREEN); + disp_println("OK"); + Serial.println("OK"); + delay(100); + disp_setTextColor(WHITE); + disp_println("-----"); + delay(500); + } + disp_println("CALIBRATION"); + disp_println("COMPLETE"); + Serial.println("CALIBRATION COMPLETE"); + while(calibrateState < 3){delay(50);} + calibrateState = 0; + disp_clr(); + detachInterrupt(INTERUPT_PIN); +} - send(sendID, fingerAngles[0], fingerAngles[1], fingerAngles[2], fingerAngles[3], fingerAngles[4], fingerAngles[5], fingerAngles[6], fingerAngles[7], fingerAngles[8], fingerAngles[9]); -<<<<<<< HEAD +void calibrateAbduction2(){ + attachInterrupt(INTERUPT_PIN,interuptCalibrate,RISING); - sendToModel(o,1.0,1.0); - o++; - delay(1000); - -======= - delay(10); - delay(10); ->>>>>>> de77f9514885061b4545c7e60c6282d2ad28d74c + Serial.println("Put hand in neutral position"); + while(calibrateState < 1){delay(50);} + int pot0_mean = potList[0].getValue(); + int pot1_mean = potList[1].getValue(); + int pot2_mean = potList[2].getValue(); + int pot3_mean = potList[3].getValue(); + Serial.println("DONE"); + delay(100); + + Serial.println("Put hand in abducted position"); + while(calibrateState < 2){delay(50);} + potList[0].calibrate(false); + potList[1].calibrate(false); + potList[2].calibrate(true); + potList[3].calibrate(true); + + potList[0].setCalibrateMax(2*pot0_mean - potList[0].getCalibrateMin()); + potList[1].setCalibrateMax(2*pot0_mean - potList[1].getCalibrateMin()); + potList[2].setCalibrateMin(2*pot0_mean - potList[2].getCalibrateMax()); + potList[3].setCalibrateMin(2*pot0_mean - potList[3].getCalibrateMax()); + Serial.println("Calibration complete"); + calibrateState = 0; + detachInterrupt(INTERUPT_PIN); } +void setup() { + Serial.begin(115200); + delay(100); + getMACAdress(); //MAC adress is run for the WIFI to work, the OTHER wearers MAC adress shall be written in this code and vice versa. + init_wifi(); // Initiate ESP_NOW + initBoard(); // Initiate breakout board + + disp_initialize(); // Initialise display + pinMode(INTERUPT_PIN,INPUT); + // interupt for start/stop button + calibrateFlexion(); + calibrateAbduction2(); // Calibrate flextion movement of fingers + calibrateAbduction(); // Calibrate abduction and adduction movement of fingers + + attachInterrupt(INTERUPT_PIN, interuptFunc, RISING); + potList[1].setCalibrateMin(50); + potList[1].setCalibrateMax(105); + potList[2].setCalibrateMin(110); + potList[2].setCalibrateMax(240); + potList[3].setCalibrateMin(130); + potList[3].setCalibrateMax(200); +} + +void loop() { + delay(20); + sendID++; + send(sendID, pinList[0].getAngle(),pinList[1].getAngle(),pinList[2].getAngle(),pinList[3].getAngle(), + pinList[4].getAngle(),pinList[5].getAngle(),pinList[6].getAngle(),pinList[7].getAngle(), + pinList[8].getAngle(),pinList[9].getAngle(),pinList[10].getAngle(),potList[0].getAngle(),0, + 0,0); + + Serial.print(pinList[0].getAngle()); + Serial.print(","); + Serial.print(pinList[1].getAngle()); + Serial.print(","); + Serial.print(pinList[2].getAngle()); + Serial.print(","); + Serial.print(pinList[3].getAngle()); + Serial.print(","); + Serial.print(pinList[4].getAngle()); + Serial.print(","); + Serial.print(pinList[5].getAngle()); + Serial.print(","); + Serial.print(pinList[6].getAngle()); + Serial.print(","); + Serial.print(pinList[7].getAngle()); + Serial.print(","); + Serial.print(pinList[8].getAngle()); + Serial.print(","); + Serial.print(pinList[9].getAngle()); + Serial.print(","); + Serial.print(pinList[10].getAngle()); + Serial.print(","); + Serial.print(potList[0].getAngle()); + Serial.print(","); + Serial.print(0); + Serial.print(","); + Serial.print(0); + Serial.print(","); + Serial.println(0); +} diff --git a/src/potentiometer.cpp b/src/potentiometer.cpp new file mode 100644 index 0000000..db3d492 --- /dev/null +++ b/src/potentiometer.cpp @@ -0,0 +1,176 @@ +/** + * This class file is used to create potentiometer objects that handle all reading + * and filtering of each sensor signal. +*/ + +#include +#include +#include "potentiometer.h" +#include "Config.h" + + +/** + * Constructor + * @param channel the channel to which the flex sensor is connected to. + * @param minimum_angle the minimum angle of flex sensor. + * @param maximum_angle the maximum angle of flex sensor. + * @param multipl boolean that states whether the potentiometer is connected to the multiplexer or directly to the esp32. +*/ +potentiometer::potentiometer(int channel, int minimum_angle, int maximum_angle){ + multipl = true; + m_channel = channel; + min_angle = minimum_angle; + max_angle = maximum_angle; + calibrateM = 0; +} +potentiometer::potentiometer(int channel){ + multipl = true; + m_channel = channel; + min_angle = -20; + max_angle = 20; + calibrateM = 0; +} +potentiometer::potentiometer(bool multiplexer, int pin, int minimum_angle, int maximum_angle){ + multipl = multiplexer; + m_channel = pin; + min_angle = minimum_angle; + max_angle = maximum_angle; + calibrateM = 0; +} +potentiometer::potentiometer(bool multiplexer, int pin){ + multipl = multiplexer; + m_channel = pin; + min_angle = -15; + max_angle = 15; + calibrateM = 0; +} + +/** + * Changes multiplexer to read from int channel and then returns the read value. + * @return analog reading from multiplexer in input channel. + * this method is taken from http://adam-meyer.com/arduino/CD74HC4067 under MIT-License. +*/ +int potentiometer::readMux(int channel){ + int controlPin[] = {s0, s1, s2, s3}; + + int muxChannel[16][4]={ + {0,0,0,0}, //channel 0 + {1,0,0,0}, //channel 1 + {0,1,0,0}, //channel 2 + {1,1,0,0}, //channel 3 + {0,0,1,0}, //channel 4 + {1,0,1,0}, //channel 5 + {0,1,1,0}, //channel 6 + {1,1,1,0}, //channel 7 + {0,0,0,1}, //channel 8 + {1,0,0,1}, //channel 9 + {0,1,0,1}, //channel 10 + {1,1,0,1}, //channel 11 + {0,0,1,1}, //channel 12 + {1,0,1,1}, //channel 13 + {0,1,1,1}, //channel 14 + {1,1,1,1} //channel 15 + }; + + //loop through the 4 sig + for(int i = 0; i < 4; i ++){ + digitalWrite(controlPin[i], muxChannel[channel][i]); + } + + //read the value at the SIG pin + int val = analogRead(SIG_PIN); + + //return the value + return val; +} + +// Returns value read from multiplexer at given channel (for debugging purposes mainly) +int potentiometer::getValue(){ + if(multipl){ + return readMux(m_channel); + } + else{ + return analogRead(m_channel); + } +} + + +/** + * Performs a measurement of the sensor and adds it to the median filter. + * @return a float with the median angle value. +*/ +float potentiometer::getAngle(){ + float angle; + if(multipl){ + angle = map(readMux(m_channel), calibrateMin, calibrateMax, min_angle, max_angle); + } + else{ + angle = map(analogRead(m_channel), calibrateMin, calibrateMax, min_angle, max_angle); + } + m_f.addSample(angle); + return m_f.getMedian(); +} + + +/** + * Calibrates the potentiometer, either the min or max state depending on input. + * @return void +*/ +void potentiometer::calibrate(bool state){ + if(multipl){ + for(int i = 0; i < 2*SAMPLES; i++){ + m_f.addSample(readMux(m_channel)); + } + } + else{ + for(int i = 0; i < 2*SAMPLES; i++){ + m_f.addSample(analogRead(m_channel)); + } + } + + if(!state){ + calibrateMin = m_f.getMedian(); + } + + else{ + calibrateMin = m_f.getMedian(); + } + +} + +/** + * @returns value of calibrateMax +*/ +int potentiometer::getCalibrateMax(){ + return calibrateMax; +} + +/** + * @returns value of calibrateMin +*/ +int potentiometer::getCalibrateMin(){ + return calibrateMin; +} + +/** + * sets value of calibrateMax +*/ +void potentiometer::setCalibrateMax(int val){ + calibrateMax = val; +} + +/** + * sets value of calibrateMin +*/ +void potentiometer::setCalibrateMin(int val){ + calibrateMin = val; +} + +void potentiometer::calibrateMean(int delta_min, int delta_max){ + for(int i = 0; i < 2*SAMPLES; i++){ + m_f.addSample(readMux(m_channel)); + } + calibrateM = m_f.getMedian(); + calibrateMin = calibrateMax - delta_min; + calibrateMax = calibrateMin - delta_max; +} \ No newline at end of file diff --git a/src/potentiometer.h b/src/potentiometer.h new file mode 100644 index 0000000..96fb679 --- /dev/null +++ b/src/potentiometer.h @@ -0,0 +1,42 @@ +/** + * This class file is used to create flex sensor objects that handle all reading + * and filtering of each sensor signal. +*/ +#pragma once + +#include +#include "Config.h" +#include "MedianFilter.h" + +/** + * Main class +*/ +class potentiometer{ +private: + // Variables and objects + int m_channel; + int min_angle; + int max_angle; + int calibrateMin; + int calibrateMax; + int calibrateM; + bool multipl; + // Number of samples are defined in config.h + MedianFilter m_f; + // Methods + int readMux(int); +public: + potentiometer(int channel, int minimum_angle, int maximum_angle); + potentiometer(int channel); + potentiometer(bool multiplexer, int pin, int minimum_angle, int maximum_angle); + potentiometer(bool multuplexer, int pin); + // Methods + int getValue(); + float getAngle(); + void calibrate(bool state); + int getCalibrateMin(); + int getCalibrateMax(); + void setCalibrateMax(int val); + void setCalibrateMin(int val); + void calibrateMean(int delta_min, int delta_max); +}; \ No newline at end of file diff --git a/src/readFingers.h b/src/readFingers.h deleted file mode 100644 index 5d6a209..0000000 --- a/src/readFingers.h +++ /dev/null @@ -1,31 +0,0 @@ -#include -#include -#include - -// Defining breakout board, multiplexter -#define CS 5 -ADS1118 ads1118(CS); - -// Change these constants according to your project's design -const float VCC = 3.3; // voltage at Ardunio 5V line -const float R_DIV = 100000.0; // resistor used to create a voltage divider - -float readResistance(int pin, int type){ // 0<=type<6 -> on esp board, 6= on multiplexer - float Vflex = 0; - if(type < 6){ - int ADCflex = analogRead(pin); - Vflex = ADCflex * VCC / 4095.0; // 12 bit gives 4095 values - } - else if (type >= 6){ - Vflex = ads1118.getMilliVolts(pin) / 1000.0; - } - - float Rflex = R_DIV * (VCC / Vflex - 1.0); - return Rflex; -} - -float getAngle(int pin, int type){ - float angle = readResistance(pin, type) * 2.6716/1000 - 30.2045 - 60; //From Matlab calibration - return angle; -} - diff --git a/src/sensorClass.cpp b/src/sensorClass.cpp new file mode 100644 index 0000000..f4df918 --- /dev/null +++ b/src/sensorClass.cpp @@ -0,0 +1,54 @@ +/** + * This class file is used to create flex sensor objects that handle all reading + * and filtering of each sensor signal. +*/ + +#include +#include +#include "SensorClass.h" +#include "Config.h" + + +/** + * Constructor + * @param pin the pin to which the flex sensor is connected to. + * @param ads the ADC breakout board object which the flexsensor is connected to. +*/ + +sensor::sensor(int pin){ + m_pin = pin; +} + + +/** + * Performs a measurement of the sensor and adds it to the median filter. + * @return a float with the median angle value. +*/ + +float sensor::getAngle(){ + float angle = getResistance(m_pin) * 2.6716/1000 - 30.2045 - 60; //From Matlab calibration + + m_f.addSample(angle); + + return m_f.getMedian(); +} + + +/** + * Helper method for getAngle(). + * Takes a reading at the voltage divider and returns a resistance. + * @param pin the pin to take a reading from. + * @return a float with the resistance of the flex sensor. +*/ + +float sensor::getResistance(int pin){ + float Vflex = 0.0; + + //Vflex = ads1118.getMilliVolts(pin) / 1000.0; + + //float Rflex = R_DIV * (VCC / Vflex - 1.0); + float Rflex = 0.0; + return Rflex; +} + + diff --git a/src/sensorClass.h b/src/sensorClass.h new file mode 100644 index 0000000..eb24068 --- /dev/null +++ b/src/sensorClass.h @@ -0,0 +1,27 @@ +/** + * This class file is used to create flex sensor objects that handle all reading + * and filtering of each sensor signal. +*/ +#pragma once + +#include +#include +#include "Config.h" +#include "MedianFilter.h" + +/** + * Main class +*/ +class sensor{ +private: + // Variables and objects + int m_pin; + // Number of samples are defined in config.h + MedianFilter m_f; + // Methods + float getResistance(int); +public: + sensor(int pin); + // Methods + float getAngle(); +}; \ No newline at end of file diff --git a/src/talkyBoi.h b/src/talkyBoi.h index 6a4bfdc..051d222 100644 --- a/src/talkyBoi.h +++ b/src/talkyBoi.h @@ -2,10 +2,12 @@ #include #include -//uint8_t broadcastAdress[] = {0x94,0xB9,0x7E,0xE6,0x79,0x9C}; //MAC-adress till den svarta -uint8_t broadcastAdress[] = {0x94,0xB9,0x7E,0xE5,0x31,0xD8}; //MAC-adress till den silver +uint8_t broadcastAdress[] = {0x94,0xB9,0x7E,0xE6,0x79,0x9C}; //MAC-adress till den svarta/andra gruppen +//uint8_t broadcastAdress[] = {0x94,0xB9,0x7E,0xE5,0x31,0xD8}; //MAC-adress till den silver //uint8_t broadcastAdress[] = {0x7C,0x9E,0xBD,0x60,0xD1,0x8C}; //MAC till den med maskering -//uint8_t broadcastAdressModel[] = {0X7C,0X9E,0XBD,0X61,0X58,0XF4}; //MAC till den med vit tejp +//uint8_t broadcastAdress[] = {0X7C,0X9E,0XBD,0X61,0X58,0XF4}; //MAC till den med vit tejp +//uint8_t broadcastAdress[] = {0x94,0xB9,0x7E,0xE4,0x84,0x34}; + int recID = 0; int error = 0; @@ -24,10 +26,11 @@ typedef struct struct_message{ float finger4PIP; float finger4MCP; float thumbOpp; - float test12; - float test13; - float test14; - float test15; + float finger1Pot; + float finger2Pot; + float finger3Pot; + float finger4Pot; + float opposition; }struct_message; struct_message msg_to_send; @@ -36,7 +39,7 @@ struct_message msg_incoming; // Callback when data is sent, triggas när något skickas void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) { Serial.print("\r\nLast Packet Send Status:\t"); - //Serial.write((String)mac_addr)); + /*//Serial.write((String)mac_addr)); Serial.print(mac_addr[0], HEX); Serial.print(","); Serial.print(mac_addr[1], HEX); @@ -47,25 +50,72 @@ void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) { Serial.print(","); Serial.print(mac_addr[4], HEX); Serial.print(","); - Serial.println(mac_addr[5], HEX); - Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail"); + Serial.println(mac_addr[5], HEX);*/ + Serial.print(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success: " : "Delivery Fail: "); if(status == ESP_NOW_SEND_FAIL){ error++; }else{ succ++; } + Serial.println(error); +} +void send(float sendID, float thumbIP, float thumbMCP, float finger1PIP, float finger1MCP, float finger2PIP, float finger2MCP, float finger3PIP, float finger3MCP, float finger4PIP, float finger4MCP,float opposition,float pot1,float pot2,float pot3,float pot4){ + msg_to_send.sendID = sendID; + msg_to_send.thumbIP = thumbIP; + msg_to_send.thumbMCP = thumbMCP; + msg_to_send.finger1PIP = finger1PIP; + msg_to_send.finger1MCP = finger1MCP; + msg_to_send.finger2PIP = finger2PIP; + msg_to_send.finger2MCP = finger2MCP; + msg_to_send.finger3PIP = finger3PIP; + msg_to_send.finger3MCP = finger3MCP; + msg_to_send.finger4PIP = finger4PIP; + msg_to_send.finger4MCP = finger4MCP; + msg_to_send.opposition = opposition; + msg_to_send.finger1Pot = pot1; + msg_to_send.finger2Pot = pot2; + msg_to_send.finger3Pot = pot3; + msg_to_send.finger4Pot = pot4; + + esp_err_t result = esp_now_send(broadcastAdress, (uint8_t *) &msg_to_send, sizeof(msg_to_send)); } // Callback when data is received, triggas när något mottas (används ej) void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) { memcpy(&msg_incoming, incomingData, sizeof(msg_incoming)); - recID = msg_incoming.sendID; - Serial.print(recID); + Serial.print(msg_incoming.thumbIP); Serial.print(","); - Serial.print(recID); + Serial.print(msg_incoming.thumbMCP); Serial.print(","); - Serial.println(recID); - + Serial.print(msg_incoming.finger1PIP); + Serial.print(","); + Serial.print(msg_incoming.finger1MCP); + Serial.print(","); + Serial.print(msg_incoming.finger2PIP); + Serial.print(","); + Serial.print(msg_incoming.finger2MCP); + Serial.print(","); + Serial.print(msg_incoming.finger3PIP); + Serial.print(","); + Serial.print(msg_incoming.finger3MCP); + Serial.print(","); + Serial.print(msg_incoming.finger4PIP); + Serial.print(","); + Serial.print(msg_incoming.finger4MCP); + Serial.print(","); + Serial.print(msg_incoming.opposition); + Serial.print(","); + Serial.print(msg_incoming.finger1Pot); + Serial.print(","); + Serial.print(0); + Serial.print(","); + Serial.print(0); + Serial.print(","); + Serial.println(0); + send(1, msg_incoming.thumbIP,msg_incoming.thumbMCP,msg_incoming.finger1PIP,msg_incoming.finger1MCP, + msg_incoming.finger2PIP,msg_incoming.finger2MCP,msg_incoming.finger3PIP,msg_incoming.finger3MCP, + msg_incoming.finger4PIP,msg_incoming.finger4MCP,msg_incoming.opposition,msg_incoming.finger1Pot,0, + 0,0); } void getMACAdress(){ @@ -95,28 +145,13 @@ void init_wifi (){ esp_now_register_recv_cb(OnDataRecv); } -void send (float sendID, float thumbIP, float thumbMCP, float finger1PIP, float finger1MCP, float finger2PIP, float finger2MCP, float finger3PIP, float finger3MCP, float finger4PIP, float finger4MCP){ - uint8_t broadcastAdress[] = {0x94,0xB9,0x7E,0xE5,0x31,0xD8}; - msg_to_send.sendID = sendID; - msg_to_send.thumbIP = thumbIP; - msg_to_send.thumbMCP = thumbMCP; - msg_to_send.finger1PIP = finger1PIP; - msg_to_send.finger1MCP = finger1MCP; - msg_to_send.finger2PIP = finger2PIP; - msg_to_send.finger2MCP = finger2MCP; - msg_to_send.finger3PIP = finger3PIP; - msg_to_send.finger3MCP = finger3MCP; - msg_to_send.finger4PIP = finger4PIP; - msg_to_send.finger4MCP = finger4MCP; - - esp_err_t result = esp_now_send(broadcastAdress, (uint8_t *) &msg_to_send, sizeof(msg_to_send)); -} void sendToModel(int sendID, float thumbIP, float thumbMCP){ Serial.println("Send to model : "); uint8_t broadcastAdress[] = {0X7C,0X9E,0XBD,0X61,0X58,0XF4}; msg_to_send.sendID = sendID; esp_err_t result = esp_now_send(broadcastAdress,(uint8_t *) &msg_to_send, sizeof(msg_to_send)); } + void recieve () { } \ No newline at end of file