Failing to get data from 2 Morphs simultaneously on a Teensy

Hello,

I have a Teensy 3.2 microcontroller board. I have 2 Morphs connected to the board over 2 developer’s cables on serial ports 2 and 3. I have adapted the code from a former Arduino sketch which was running fine on a single Morph. However now I can’t get data from both Morphs simultaneously. If I print the data on the serial 1 port, I have nothing coming from one of the devices (the first one addressed).

I have checked all the hardware: the Morphs, the cables and the serial ports are fine. If I swap anything, including the serial ports, the data missing are always those from the device named “Sensel A” in my code. So the issue is obviously within the code. I think that I have duplicated and adapted to each device all the code which should have been. However there must be something common remaining and probably something related to the second Morph erases data from the first one.

Also, if I comment all the lines related to device B in the main sketch I still do not have the data from device A.

Any help on this issue would be highly appreciated!

Here’s the main sketch:

#include "sensel.h"

//Frame struct for containing the contact array and number of contacts
SenselFrameA frameA;
SenselFrameB frameB;

//Whether the Sensel device has been initialized
bool sensel_readyA = false;
bool sensel_readyB = false;

void setup() {
  //swith Teensy LED on
  pinMode(13, OUTPUT); 
  digitalWrite(13, HIGH);
  
  //Open serial for SenselSerial declared in sensel.h
  senselOpenA();
  senselOpenB();
  
  //Set frame content to scan. No pressure or label support.
  senselSetFrameContentA(SENSEL_REG_CONTACTS_FLAG);
  senselSetFrameContentB(SENSEL_REG_CONTACTS_FLAG);

  //Start scanning the Sensel device
  senselStartScanningA();
  senselStartScanningB();

  //Mark the Sensel device as ready
  sensel_readyA = true;
  sensel_readyB = true;

  //ADD CODE TO LIGHT LEDs ON BOTH MORPHS HERE
}

void loop() {
  //When A ready, start reading frames
  if(sensel_readyA)
  {
    //Read the frame of contacts from the Sensel device
    senselGetFrameA(&frameA);

    //Print the frame of contact data to SenselDebugSerial if defined
    senselPrintFrameA(&frameA);
  }
   //When B ready, start reading frames
  if(sensel_readyB)
  {
    //Read the frame of contacts from the Sensel device
    senselGetFrameB(&frameB);

    //Print the frame of contact data to SenselDebugSerial if defined
    senselPrintFrameB(&frameB);
  }
  //delay(5);
  // ADD CODE TO SEND MIDI DATA HERE
}

Here’s the modified sensel.h:

#ifndef SENSEL_H
#define SENSEL_H

//Serial of the Sensel device
#define SenselSerialA Serial2 // set for Teensy 3.2
#define SenselSerialB Serial3 // set for Teensy 3.2

//Serial for debug information
#define SenselDebugSerial Serial

//Max size for Sensel RX buffer
#define SENSEL_RX_BUFFER_SIZE 512

//Struct for Sensel contact information - A and B
typedef struct __attribute__((__packed__))
{
    byte id;
    byte type;
    float x_pos;
    float y_pos;
    float total_force;
    float area;
    float orientation;
    float major_axis;
    float minor_axis;
} SenselContact;

//Struct for Sensel frame, only contains contacts - A
typedef struct __attribute__((__packed__))
{
  byte n_contacts;
  SenselContact contacts[16];
}SenselFrameA;

//Struct for Sensel frame, only contains contacts - B
typedef struct __attribute__((__packed__))
{
  byte n_contacts;
  SenselContact contacts[16];
}SenselFrameB;


//Flag for enabling contact scanning
const byte SENSEL_REG_CONTACTS_FLAG = 0x04;

//Ack for read register
const byte SENSEL_PT_READ_ACK = 1;

//Ack for read variable size register
const byte SENSEL_PT_RVS_ACK = 3;

//Ack for write register
const byte SENSEL_PT_WRITE_ACK = 5;

const byte SENSEL_CONTACT_TYPE_INVALID = 0;
const byte SENSEL_CONTACT_TYPE_START = 1;
const byte SENSEL_CONTACT_TYPE_MOVE = 2;
const byte SENSEL_CONTACT_TYPE_END = 3;

#endif

Here’s the modified sensel file:

#include "sensel.h"
#include "sensel_register_map.h"

#define SenselRate 115200
#define DebugRate 250000

// MIDI params - NOY USED YET
// the MIDI channel number to send messages
const int channel = 1;

// the MIDI virtual cable to use
int cable = 0;

// the variables to represent the data to be sent over MIDI (14 bits split in two CC messages)
int xpos_MSB_A, xpos_LSB_A, ypos_MSB_A, ypos_LSB_A, force_MSB_A, force_LSB_A, orient_MSB_A, orient_LSB_A;
int xpos_MSB_B, xpos_LSB_B, ypos_MSB_B, ypos_LSB_B, force_MSB_B, force_LSB_B, orient_MSB_B, orient_LSB_B;

//RX buffer for Sensel serial
byte rx_bufA[SENSEL_RX_BUFFER_SIZE];
byte rx_bufB[SENSEL_RX_BUFFER_SIZE];

//Counter for RX buffer - A
unsigned int counterA = 0;

//Counter for RX buffer - B
unsigned int counterB = 0;

//Open Sensel device on SenselSerial - A
void senselOpenA()
{
  SenselSerialA.begin(SenselRate);
  #ifdef SenselDebugSerial
    SenselDebugSerial.begin(DebugRate);
  #endif
  delay(3000);
  #ifdef SenselDebugSerial
    SenselDebugSerial.println("Sensel A Open Complete!");
  #endif
}

//Open Sensel device on SenselSerial - B
void senselOpenB()
{
  SenselSerialB.begin(SenselRate);
  #ifdef SenselDebugSerial
    SenselDebugSerial.begin(DebugRate);
  #endif
  delay(3000);
  #ifdef SenselDebugSerial
    SenselDebugSerial.println("Sensel B Open Complete!");
  #endif
}

//Set frame content for Sensel device, supports SENSEL_REG_CONTACTS_FLAG - A
void senselSetFrameContentA(byte content)
{
  senselWriteRegA(SENSEL_REG_FRAME_CONTENT_CONTROL, 1, content);
}

//Set frame content for Sensel device, supports SENSEL_REG_CONTACTS_FLAG - B
void senselSetFrameContentB(byte content)
{
  senselWriteRegB(SENSEL_REG_FRAME_CONTENT_CONTROL, 1, content);
}

//Start scanning on Sensel device - A
void senselStartScanningA()
{
  senselWriteRegA(SENSEL_REG_SCAN_ENABLED, 1, 1);
}

//Start scanning on Sensel device - B
void senselStartScanningB()
{
  senselWriteRegB(SENSEL_REG_SCAN_ENABLED, 1, 1);
}

//Stop scanning on a Sensel device - A
void senselStopScanningA()
{
  senselWriteRegA(SENSEL_REG_SCAN_ENABLED, 1, 0);
}

//Stop scanning on a Sensel device - B
void senselStopScanningB()
{
  senselWriteRegB(SENSEL_REG_SCAN_ENABLED, 1, 0);
}

//Read all available data from SenselSerial - A
void senselReadAvailableA() {
  int len = SenselSerialA.available();
  if (len > 0) {
    SenselSerialA.readBytes(&rx_bufA[counterA%SENSEL_RX_BUFFER_SIZE], len);
    counterA = (counterA + len);
  }
}

//Read all available data from SenselSerial - B
void senselReadAvailableB() {
  int len = SenselSerialB.available();
  if (len > 0) {
    SenselSerialB.readBytes(&rx_bufB[counterB%SENSEL_RX_BUFFER_SIZE], len);
    counterB = (counterB + len);
  }
}

//Write byte to register on Sensel device - A
void senselWriteRegA(byte addr, byte sizeVar, byte data)
{
  SenselSerialA.write(0x01);
  SenselSerialA.write(addr);
  SenselSerialA.write(sizeVar);
  SenselSerialA.write(data);
  SenselSerialA.write(data);
  SenselSerialA.readBytes(rx_bufA, 2);
  if (rx_bufA[0] != SENSEL_PT_WRITE_ACK) {
    #ifdef SenselDebugSerial
      SenselDebugSerial.println("A - FAILED TO RECEIVE ACK ON WRITE");
    #endif
  }
}

//Write byte to register on Sensel device - B
void senselWriteRegB(byte addr, byte sizeVar, byte data)
{
  SenselSerialB.write(0x01);
  SenselSerialB.write(addr);
  SenselSerialB.write(sizeVar);
  SenselSerialB.write(data);
  SenselSerialB.write(data);
  SenselSerialB.readBytes(rx_bufB, 2);
  if (rx_bufB[0] != SENSEL_PT_WRITE_ACK) {
    #ifdef SenselDebugSerial
      SenselDebugSerial.println("B - FAILED TO RECEIVE ACK ON WRITE");
    #endif
  }
}

//Read byte array from register on Sensel device - A
void senselReadRegA(byte addr, byte sizeVar, byte* buf)
{
  byte checksum = 0;
  SenselSerialA.write(0x81);
  SenselSerialA.write(addr);
  SenselSerialA.write(sizeVar);
  SenselSerialA.readBytes(rx_bufA, 4);
  if (rx_bufA[0] != SENSEL_PT_READ_ACK) {
    #ifdef SenselDebugSerial
      SenselDebugSerial.println("FAILED TO RECEIVE ACK ON READ");
    #endif
    _senselFlushA();
    return;
  }
  unsigned int resp_size = _convertBytesToU16(rx_bufA[2], rx_bufA[3]);
  SenselSerialA.readBytes(buf, resp_size);
  SenselSerialA.readBytes(&checksum, 1);
}

//Read byte array from register on Sensel device - B
void senselReadRegB(byte addr, byte sizeVar, byte* buf)
{
  byte checksum = 0;
  SenselSerialB.write(0x81);
  SenselSerialB.write(addr);
  SenselSerialB.write(sizeVar);
  SenselSerialB.readBytes(rx_bufB, 4);
  if (rx_bufB[0] != SENSEL_PT_READ_ACK) {
    #ifdef SenselDebugSerial
      SenselDebugSerial.println("FAILED TO RECEIVE ACK ON READ");
    #endif
    _senselFlushB();
    return;
  }
  unsigned int resp_size = _convertBytesToU16(rx_bufB[2], rx_bufB[3]);
  SenselSerialB.readBytes(buf, resp_size);
  SenselSerialB.readBytes(&checksum, 1);
}

//Convert 4 bytes to a unsigned long
unsigned long _convertBytesToU32(byte b0, byte b1, byte b2, byte b3)
{
  return ((((unsigned long)b3) & 0xff) << 24) | ((((unsigned long)b2) & 0xff) << 16) | ((((unsigned long)b1) & 0xff) << 8) | (((unsigned long)b0) & 0xff);
}

//Convert 2 bytes to an unsigned int
unsigned int _convertBytesToU16(byte b0, byte b1)
{
  return ((((unsigned int)b1) & 0xff) << 8) | (((unsigned int)b0) & 0xff);
}

//Convert 2 bytes to a signed int
int _convertBytesToS16(byte b0, byte b1)
{
  return ((((int)b1)) << 8) | (((int)b0) & 0xff);
}

//Flush the SenselSerial of all data - A
void _senselFlushA()
{
  while(SenselSerialA.available() > 0) {
    SenselSerialA.read();
  delay(1);
  }
  SenselSerialA.flush();
}

//Flush the SenselSerial of all data - B
void _senselFlushB()
{
  while(SenselSerialB.available() > 0) {
    SenselSerialB.read();
  delay(1);
  }
  SenselSerialB.flush();
}

//Read contact frame data from SenselSerial - A
void senselGetFrameA(SenselFrameA *frameA)
{
  counterA = 0;
  SenselSerialA.write(0x81);
  SenselSerialA.write(SENSEL_REG_SCAN_READ_FRAME);
  SenselSerialA.write((byte)0x00);
  frameA->n_contacts = 0;
  int i;
  int contact_size = 16;
  int timeout = 20;
  while(counterA < 5 && timeout > 0){
    senselReadAvailableA();
    delay(1);
    timeout--;
  }
  if(timeout == 0 || rx_bufA[0] != SENSEL_PT_RVS_ACK){
    _senselFlushA();
    return;
  }
  unsigned int resp_size = _convertBytesToU16(rx_bufA[3], rx_bufA[4]);
  timeout = 50;
  while(counterA < resp_size+6 && timeout > 0){
    senselReadAvailableA();
    delay(1);
    timeout--;
  }
  if(timeout == 0){
    _senselFlushA();
    return;
  }
  if(rx_bufA[5] == SENSEL_REG_CONTACTS_FLAG && (unsigned int)(frameA->n_contacts*contact_size) == resp_size-8){
    for(i = 0; i < frameA->n_contacts; i++){
      int offset = 13+i*contact_size;
      frameA->contacts[i].id = rx_bufA[offset+0];
      frameA->contacts[i].type = rx_bufA[offset+1];
      frameA->contacts[i].x_pos = _convertBytesToU16(rx_bufA[offset+2],rx_bufA[offset+3])/256.0f;
      frameA->contacts[i].y_pos = _convertBytesToU16(rx_bufA[offset+4],rx_bufA[offset+5])/256.0f;
      frameA->contacts[i].total_force = _convertBytesToU16(rx_bufA[offset+6],rx_bufA[offset+7])/8.0f;
      //frameA->contacts[i].area = _convertBytesToU16(rx_bufA[offset+8],rx_bufA[offset+9])/1.0f;
      frameA->contacts[i].orientation = _convertBytesToS16(rx_bufA[offset+10],rx_bufA[offset+11])/16.0f;
      //frameA->contacts[i].major_axis = _convertBytesToU16(rx_bufA[offset+12],rx_bufA[offset+13])/256.0f;
      //frameA->contacts[i].minor_axis = _convertBytesToU16(rx_bufA[offset+14],rx_bufA[offset+15])/256.0f;
    }
  }
  else{
    _senselFlushA();
  }
}

//Read contact frame data from SenselSerial - B
void senselGetFrameB(SenselFrameB *frameB)
{
  counterB = 0;
  SenselSerialB.write(0x81);
  SenselSerialB.write(SENSEL_REG_SCAN_READ_FRAME);
  SenselSerialB.write((byte)0x00);
  frameB->n_contacts = 0;
  int i;
  int contact_size = 16;
  int timeout = 20;
  while(counterB < 5 && timeout > 0){
    senselReadAvailableB();
    delay(1);
    timeout--;
  }
  if(timeout == 0 || rx_bufB[0] != SENSEL_PT_RVS_ACK){
    _senselFlushB();
    return;
  }
  unsigned int resp_size = _convertBytesToU16(rx_bufB[3], rx_bufB[4]);
  timeout = 50;
  while(counterB < resp_size+6 && timeout > 0){
    senselReadAvailableB();
    delay(1);
    timeout--;
  }
  if(timeout == 0){
    _senselFlushB();
    return;
  }
  frameB->n_contacts = rx_bufB[12];
  if(rx_bufB[5] == SENSEL_REG_CONTACTS_FLAG && (unsigned int)(frameB->n_contacts*contact_size) == resp_size-8){
    for(i = 0; i < frameB->n_contacts; i++){
      int offset = 13+i*contact_size;
      frameB->contacts[i].id = rx_bufB[offset+0];
      frameB->contacts[i].type = rx_bufB[offset+1];
      frameB->contacts[i].x_pos = _convertBytesToU16(rx_bufB[offset+2],rx_bufB[offset+3])/256.0f;
      frameB->contacts[i].y_pos = _convertBytesToU16(rx_bufB[offset+4],rx_bufB[offset+5])/256.0f;
      frameB->contacts[i].total_force = _convertBytesToU16(rx_bufB[offset+6],rx_bufB[offset+7])/8.0f;
      frameB->contacts[i].area = _convertBytesToU16(rx_bufB[offset+8],rx_bufB[offset+9])/1.0f;
      frameB->contacts[i].orientation = _convertBytesToS16(rx_bufB[offset+10],rx_bufB[offset+11])/16.0f;
      //frameB->contacts[i].major_axis = _convertBytesToU16(rx_bufB[offset+12],rx_bufB[offset+13])/256.0f;
      //frameB->contacts[i].minor_axis = _convertBytesToU16(rx_bufB[offset+14],rx_bufB[offset+15])/256.0f;
    }
  }
  else{
    _senselFlushB();
  }
}


//Print SenselFrame contact information - A
void senselPrintFrameA(SenselFrameA *frameA){
  #ifdef SenselDebugSerial
    if (frameA->n_contacts > 0) {
      SenselDebugSerial.print("NumContacts A ");
      SenselDebugSerial.println(frameA->n_contacts);
    }
    for(int i = 0; i < frameA->n_contacts; i++){
      SenselDebugSerial.print("ID\t");
      SenselDebugSerial.print(frameA->contacts[i].id);
      SenselDebugSerial.print("\t");
    }
    SenselDebugSerial.print("\n");
    for(int i = 0; i < frameA->n_contacts; i++){
      SenselDebugSerial.print("A_x_pos ");
      SenselDebugSerial.print(frameA->contacts[i].x_pos);
      SenselDebugSerial.print("\t");
    }
    SenselDebugSerial.print("\n");
    for(int i = 0; i < frameA->n_contacts; i++){
      SenselDebugSerial.print("A_y_pos ");
      SenselDebugSerial.print(frameA->contacts[i].y_pos);
      SenselDebugSerial.print("\t");
    }
    SenselDebugSerial.print("\n");
    for(int i = 0; i < frameA->n_contacts; i++){
      SenselDebugSerial.print("A_force ");
    SenselDebugSerial.print(frameA->contacts[i].total_force);
      SenselDebugSerial.print("\t");
    }
    SenselDebugSerial.print("\n");
    for(int i = 0; i < frameA->n_contacts; i++){
      SenselDebugSerial.print("a_area ");
      SenselDebugSerial.print(frameA->contacts[i].area);
      SenselDebugSerial.print("\t");
    }
    SenselDebugSerial.print("\n");
    for(int i = 0; i < frameA->n_contacts; i++){
      SenselDebugSerial.print("A_ori ");
      SenselDebugSerial.print(frameA->contacts[i].orientation);
      SenselDebugSerial.print("\t");
    }
    SenselDebugSerial.print("\n");
  #endif
}

//Print SenselFrame contact information - B
void senselPrintFrameB(SenselFrameB *frameB){
  #ifdef SenselDebugSerial
    if (frameB->n_contacts > 0) {
      SenselDebugSerial.print("NumContacts B ");
      SenselDebugSerial.println(frameB->n_contacts);
    }
    for(int i = 0; i < frameB->n_contacts; i++){
      SenselDebugSerial.print("ID\t");
      SenselDebugSerial.print(frameB->contacts[i].id);
      SenselDebugSerial.print("\t");
    }
    SenselDebugSerial.print("\n");
    for(int i = 0; i < frameB->n_contacts; i++){
      SenselDebugSerial.print("B_x_pos ");
      SenselDebugSerial.print(frameB->contacts[i].x_pos);
      SenselDebugSerial.print("\t");
    }
    SenselDebugSerial.print("\n");
    for(int i = 0; i < frameB->n_contacts; i++){
      SenselDebugSerial.print("B_y_pos ");
      SenselDebugSerial.print(frameB->contacts[i].y_pos);
      SenselDebugSerial.print("\t");
    }
    SenselDebugSerial.print("\n");
    for(int i = 0; i < frameB->n_contacts; i++){
      SenselDebugSerial.print("B_force ");
      SenselDebugSerial.print(frameB->contacts[i].total_force);
      SenselDebugSerial.print("\t");
    }
    SenselDebugSerial.print("\n");
    for(int i = 0; i < frameB->n_contacts; i++){
      SenselDebugSerial.print("B_area ");
      SenselDebugSerial.print(frameB->contacts[i].area);
      SenselDebugSerial.print("\t");
    }
    SenselDebugSerial.print("\n");
    for(int i = 0; i < frameB->n_contacts; i++){
      SenselDebugSerial.print("B_ori ");
      SenselDebugSerial.print(frameB->contacts[i].orientation);
      SenselDebugSerial.print("\t");
    }
    SenselDebugSerial.print("\n");
  #endif
}

The sensel_register_map.h has been left unmodified:

#ifndef SENSEL_REGISTER_MAP_H
#define SENSEL_REGISTER_MAP_H

#define SENSEL_REG_MAP_PROTOCOL_VERSION 1
#define SENSEL_REG_MAP_MAJOR_VERSION 0
#define SENSEL_REG_MAP_MINOR_VERSION 7
#define SENSEL_REG_MAP_BUILD_VERSION 60

// Sensel Register Addresses:

#define SENSEL_REG_MAGIC                                    0x00
#define SENSEL_REG_FW_VERSION_PROTOCOL                      0x06
#define SENSEL_REG_FW_VERSION_MAJOR                         0x07
#define SENSEL_REG_FW_VERSION_MINOR                         0x08
#define SENSEL_REG_FW_VERSION_BUILD                         0x09
#define SENSEL_REG_FW_VERSION_RELEASE                       0x0B
#define SENSEL_REG_DEVICE_ID                                0x0C
#define SENSEL_REG_DEVICE_REVISION                          0x0E
#define SENSEL_REG_DEVICE_SERIAL_NUMBER                     0x0F
#define SENSEL_REG_SENSOR_NUM_COLS                          0x10
#define SENSEL_REG_SENSOR_NUM_ROWS                          0x12
#define SENSEL_REG_SENSOR_ACTIVE_AREA_WIDTH_UM              0x14
#define SENSEL_REG_SENSOR_ACTIVE_AREA_HEIGHT_UM             0x18
#define SENSEL_REG_COMPRESSION_METADATA                     0x1C
#define SENSEL_REG_SCAN_FRAME_RATE                          0x20
#define SENSEL_REG_SCAN_BUFFER_CONTROL                      0x22
#define SENSEL_REG_SCAN_DETAIL_CONTROL                      0x23
#define SENSEL_REG_FRAME_CONTENT_CONTROL                    0x24
#define SENSEL_REG_SCAN_ENABLED                             0x25
#define SENSEL_REG_SCAN_READ_FRAME                          0x26
#define SENSEL_REG_FRAME_CONTENT_SUPPORTED                  0x28
#define SENSEL_REG_CONTACTS_MAX_COUNT                       0x40
#define SENSEL_REG_CONTACTS_ENABLE_BLOB_MERGE               0x41
#define SENSEL_REG_CONTACTS_MIN_FORCE                       0x47
#define SENSEL_REG_CONTACTS_MASK                            0x4B
#define SENSEL_REG_BASELINE_ENABLED                         0x50
#define SENSEL_REG_BASELINE_INCREASE_RATE                   0x51
#define SENSEL_REG_BASELINE_DECREASE_RATE                   0x53
#define SENSEL_REG_BASELINE_DYNAMIC_ENABLED                 0x57
#define SENSEL_REG_ACCEL_X                                  0x60
#define SENSEL_REG_ACCEL_Y                                  0x62
#define SENSEL_REG_ACCEL_Z                                  0x64
#define SENSEL_REG_BATTERY_STATUS                           0x70
#define SENSEL_REG_BATTERY_PERCENTAGE                       0x71
#define SENSEL_REG_POWER_BUTTON_PRESSED                     0x72
#define SENSEL_REG_LED_BRIGHTNESS                           0x80
#define SENSEL_REG_LED_BRIGHTNESS_SIZE                      0x81
#define SENSEL_REG_LED_BRIGHTNESS_MAX                       0x82
#define SENSEL_REG_LED_COUNT                                0x84
#define SENSEL_REG_UNIT_SHIFT_DIMS                          0xA0
#define SENSEL_REG_UNIT_SHIFT_FORCE                         0xA1
#define SENSEL_REG_UNIT_SHIFT_AREA                          0xA2
#define SENSEL_REG_UNIT_SHIFT_ANGLE                         0xA3
#define SENSEL_REG_UNIT_SHIFT_TIME                          0xA4
#define SENSEL_REG_DEVICE_OPEN                              0xD0
#define SENSEL_REG_SOFT_RESET                               0xE0
#define SENSEL_REG_ERROR_CODE                               0xEC


// Sensel Register Sizes:

#define SENSEL_REG_SIZE_MAGIC                               6
#define SENSEL_REG_SIZE_FW_VERSION_PROTOCOL                 1
#define SENSEL_REG_SIZE_FW_VERSION_MAJOR                    1
#define SENSEL_REG_SIZE_FW_VERSION_MINOR                    1
#define SENSEL_REG_SIZE_FW_VERSION_BUILD                    2
#define SENSEL_REG_SIZE_FW_VERSION_RELEASE                  1
#define SENSEL_REG_SIZE_DEVICE_ID                           2
#define SENSEL_REG_SIZE_DEVICE_REVISION                     1
#define SENSEL_REG_SIZE_DEVICE_SERIAL_NUMBER                1
#define SENSEL_REG_SIZE_SENSOR_NUM_COLS                     2
#define SENSEL_REG_SIZE_SENSOR_NUM_ROWS                     2
#define SENSEL_REG_SIZE_SENSOR_ACTIVE_AREA_WIDTH_UM         4
#define SENSEL_REG_SIZE_SENSOR_ACTIVE_AREA_HEIGHT_UM        4
#define SENSEL_REG_SIZE_SCAN_FRAME_RATE                     2
#define SENSEL_REG_SIZE_SCAN_BUFFER_CONTROL                 1
#define SENSEL_REG_SIZE_SCAN_DETAIL_CONTROL                 1
#define SENSEL_REG_SIZE_FRAME_CONTENT_CONTROL               1
#define SENSEL_REG_SIZE_SCAN_ENABLED                        1
#define SENSEL_REG_SIZE_SCAN_READ_FRAME                     1
#define SENSEL_REG_SIZE_FRAME_CONTENT_SUPPORTED             1
#define SENSEL_REG_SIZE_CONTACTS_MAX_COUNT                  1
#define SENSEL_REG_SIZE_CONTACTS_ENABLE_BLOB_MERGE          1
#define SENSEL_REG_SIZE_CONTACTS_MIN_FORCE                  2
#define SENSEL_REG_SIZE_CONTACTS_MASK                       1
#define SENSEL_REG_SIZE_BASELINE_ENABLED                    1
#define SENSEL_REG_SIZE_BASELINE_INCREASE_RATE              2
#define SENSEL_REG_SIZE_BASELINE_DECREASE_RATE              2
#define SENSEL_REG_SIZE_BASELINE_DYNAMIC_ENABLED            1
#define SENSEL_REG_SIZE_ACCEL_X                             2
#define SENSEL_REG_SIZE_ACCEL_Y                             2
#define SENSEL_REG_SIZE_ACCEL_Z                             2
#define SENSEL_REG_SIZE_BATTERY_STATUS                      1
#define SENSEL_REG_SIZE_BATTERY_PERCENTAGE                  1
#define SENSEL_REG_SIZE_POWER_BUTTON_PRESSED                1
#define SENSEL_REG_SIZE_LED_BRIGHTNESS                      1
#define SENSEL_REG_SIZE_LED_BRIGHTNESS_SIZE                 1
#define SENSEL_REG_SIZE_LED_BRIGHTNESS_MAX                  2
#define SENSEL_REG_SIZE_LED_COUNT                           1
#define SENSEL_REG_SIZE_UNIT_SHIFT_DIMS                     1
#define SENSEL_REG_SIZE_UNIT_SHIFT_FORCE                    1
#define SENSEL_REG_SIZE_UNIT_SHIFT_AREA                     1
#define SENSEL_REG_SIZE_UNIT_SHIFT_ANGLE                    1
#define SENSEL_REG_SIZE_UNIT_SHIFT_TIME                     1
#define SENSEL_REG_SIZE_DEVICE_OPEN                         1
#define SENSEL_REG_SIZE_SOFT_RESET                          1
#define SENSEL_REG_SIZE_ERROR_CODE                          1

#endif //SENSEL_REGISTER_MAP_H

Can you confirm that you can successfully get either the first Morph or the second Morph connected and working at different times by changing only the code you upload?

Duplicating code can become a source of headaches when it comes to editing each of the duplicates. Instead of creating duplicates of the hardware specific function calls, had you considered changing the functions to take in the Morph specific serial port as an input parameter?

Indeed. if I modify the following lines in sensel.h I can choose from which device I get the data (the one associated with the SenselSerialB variable):

//Serial of the Sensel device
#define SenselSerialA Serial2 // set for Teensy 3.2
#define SenselSerialB Serial3 // set for Teensy 3.2

-> I get data from device connected to Serial3.

//Serial of the Sensel device
#define SenselSerialA Serial3 // set for Teensy 3.2
#define SenselSerialB Serial2 // set for Teensy 3.2

-> I get data from device connected to Serial2.

No, I haven’t considered this option. I am not so good in C programming. Do you think it’s worth exploring that, as much of the duplication work has been done yet?

It is already some time since I played around with this. But as far as I see in the code from back then it is possible to connect to several Morphs via senselOpenDeviceByComPort via device ID:


Device IDs are as far as I remember unique per Morph - this was “serial” in the config file of the this application:

Very interesting, but does it function with serial connections and with the API for Arduino?

I don’t know, sorry. Only tested this code on Mac and Raspberry Pi.

After having checked I can confirm that the API for Arduino are much simpler than the desktop API and there’s no senselOpenDeviceByComPort function in the Arduino API.

Ok, sorry then, didn’t know there is a difference API-wise.
Perhaps you could look at the implementation of both and extend the Arduino one accordintly?

Hmmm… after looking at your altered code I noticed that there was two unexpected differences within void senselGetFrameN(SenselFrameN *frameN)

If that was deliberate, then I wouldn’t advise it.
If it wasn’t deliberate, that’s one of the key reasons not to duplicate code.

I would suggest fixing the differences and see if it resolves your issue.

1 Like

Thank you Galen! I am not sure which differences you were talking about, but it led me to compare senseGetFrameA() and senselGetFrameB() and obviously there was one line missing in the first one.

frameA->n_contacts = rx_bufA[12];

was missing in there:

void senselGetFrameA(SenselFrameA *frameA)
{
  counterA = 0;
  SenselSerialA.write(0x81);
  SenselSerialA.write(SENSEL_REG_SCAN_READ_FRAME);
  SenselSerialA.write((byte)0x00);
  frameA->n_contacts = 0;
  int i;
  int contact_size = 16;
  int timeout = 20;
  while(counterA < 5 && timeout > 0){
    senselReadAvailableA();
    delay(1);
    timeout--;
  }
  if(timeout == 0 || rx_bufA[0] != SENSEL_PT_RVS_ACK){
    _senselFlushA();
    return;
  }
  unsigned int resp_size = _convertBytesToU16(rx_bufA[3], rx_bufA[4]);
  timeout = 50;
  while(counterA < resp_size+6 && timeout > 0){
    senselReadAvailableA();
    delay(1);
    timeout--;
  }
  if(timeout == 0){
    _senselFlushA();
    return;
  }
  frameA->n_contacts = rx_bufA[12];
  if(rx_bufA[5] == SENSEL_REG_CONTACTS_FLAG && (unsigned int)(frameA->n_contacts*contact_size) == resp_size-8){
    for(i = 0; i < frameA->n_contacts; i++){
      int offset = 13+i*contact_size;
      frameA->contacts[i].id = rx_bufA[offset+0];
      frameA->contacts[i].type = rx_bufA[offset+1];
      frameA->contacts[i].x_pos = _convertBytesToU16(rx_bufA[offset+2],rx_bufA[offset+3])/256.0f;
      frameA->contacts[i].y_pos = _convertBytesToU16(rx_bufA[offset+4],rx_bufA[offset+5])/256.0f;
      frameA->contacts[i].total_force = _convertBytesToU16(rx_bufA[offset+6],rx_bufA[offset+7])/8.0f;
      //frameA->contacts[i].area = _convertBytesToU16(rx_bufA[offset+8],rx_bufA[offset+9])/1.0f;
      frameA->contacts[i].orientation = _convertBytesToS16(rx_bufA[offset+10],rx_bufA[offset+11])/16.0f;
      //frameA->contacts[i].major_axis = _convertBytesToU16(rx_bufA[offset+12],rx_bufA[offset+13])/256.0f;
      //frameA->contacts[i].minor_axis = _convertBytesToU16(rx_bufA[offset+14],rx_bufA[offset+15])/256.0f;
    }
  }
  else{
    _senselFlushA();
  }
}

Thank you everyone for the help. Everything’s OK now! :blush: :blush: :blush:

Awesome! Glad to hear things are working now.

Yup, that was the missing line. And the other difference I believe was the commented out:

//frameA->contacts[i].area = _convertBytesToU16(rx_bufA[offset+8],rx_bufA[offset+9])/1.0f;

Not sure why you had it commented out in one and not the other.

And in the unlikely case you wanted to clean up the code, I’d suggest wrapping the Sensel code in a Class. That way you can create as many Sensel Morph “Objects”, with each one connecting to a different Serial port. If you take the time to learn what Classes are it should be relatively trivial to wrap things up. And it will eliminate any duplicated code. However I’m guessing that you’re happy the way things are, which is okay too.

Best of luck in your project :+1:

1 Like
Twitter RSS Facebook