TinyCircuits

Show Posts

This section allows you to view all posts made by this member. Note that you can only see posts made in areas you currently have access to.


Topics - jakehenry41kc

Pages: [1]
1
TinyDuino / Bluetooth BLE ST shield sketch loop locks up
« on: January 09, 2019, 09:28:18 AM »
I have a TinyDuino project where I am trying to operate 2 of the micro EMAX servos offered on TinyCircuits website over BLE using a smartphone app that I've developed. My problem is that if I send servo pulses to the BLE shield too fast, the entire sketch loop ceases to function and I have to hard reset the TinyDuino processor to get the sketch to run again. This issue does not appear to be related to servo libraries or attempting to move the servos. When I comment out all servo functionality inside the main loop() function the sketch still locks up if I send integers to the BLE ST shield too fast.

Stack:
  • TinyDuino Processor Shield
  • TinyDuino USB Shield
  • TinyDuino TinyDuino BLE ST Shield
  • TinyDuinos Servo Controller Shield

Sketch "BLEServoDriver.ino":
Code: [Select]
#include <SPI.h>
#include <STBLE.h>
#include <Wire.h>
#include <ServoDriver.h>
                               
//Debug output adds extra flash and memory requirements!
#ifndef BLE_DEBUG
#define BLE_DEBUG true
#endif

#if defined (ARDUINO_ARCH_AVR)
#define SerialMonitorInterface Serial
#elif defined(ARDUINO_ARCH_SAMD)
#define SerialMonitorInterface SerialUSB
#endif

uint8_t ble_rx_buffer[21];
uint8_t ble_rx_buffer_len = 0;
uint8_t ble_connection_state = false;
#define PIPE_UART_OVER_BTLE_UART_TX_TX 0
ServoDriver servoDriver(NO_R_REMOVED);//this value affects the I2C address, which can be changed by
                                //removing resistors R1-R3. Then the corresponding R1_REMOVED,
                                //R2_REMOVED, R1_R2_REMOVED, R1_R4_REMOVED and so on can be set.
                                //Default is NO_R_REMOVED

double range;
double midpoint;
double upper;
double lower;
double speed;

uint16_t oldPos = -1;

/* This method is called once at arduino boot time and is used for setup type tasks that must be carried out */
void setup()
{
  SerialMonitorInterface.begin(57600);
 
  range = 2000.0;
  midpoint = 1500.0;
  upper = midpoint + (range/2.0);
  lower = midpoint - (range/2.0);
  speed = 0.09/((range / 180.0) * 60.0);
 
  // Load Bluetooth drivers first
  loadBluetoothDrivers();
  // Load servo drivers second
  loadServoDrivers();
}

/* This method is called repeatedly by the arduino processor */
void loop()
{
  //Process any ACI commands or events from the NRF8001- main BLE handler, must run often. Keep main loop short.
  aci_loop();
 
  //Check if data is available
  if (ble_rx_buffer_len)
  {
    uint16_t pos;
    sscanf((char*)ble_rx_buffer, "%d", &pos);
   
    if(oldPos == -1)
    {
      moveAllServos(lower, pos);     
    }
    else
    {
      moveAllServos(oldPos, pos);     
    }   
   
    SerialMonitorInterface.println(pos);

    oldPos = pos;
    ble_rx_buffer_len = 0;//clear afer reading
  }
}

/* This method is a delegate method to the BLEsetup function included in the STBLE.zip drivers.
   This is for readability during the setup phase. */
void loadBluetoothDrivers()
{
   BLEsetup();
}

/* This method loads the drivers for the servo controller shield for communication with up to 4 servo motors */
void loadServoDrivers()
{
  Wire.begin();
  pinMode(9, OUTPUT); //Pin 9 is the reset pin for the servo controller TinyShield
 
  // THE FOLLOWING 2 LINES MUST BE COMMENTED OUT FOR COMPATIBILITY WITH BLUETOOTH SHIELD
  // BECAUSE DROPPING THE POWER LOW ON PIN 9 RESETS THE BLUETOOTH AND IT WILL NOT RECOVER
  // digitalWrite(9, LOW);
  // delay(10);
 
  digitalWrite(9, HIGH);
  delay(100);
 
  //Set the period to 20000us or 20ms, correct for driving most servos
  if(servoDriver.begin(20000) != 0)
  {
    SerialMonitorInterface.println("Motor driver not detected!");
    while(1);
  }
}

/* This method is a convienience method to set all servo motors to the same position */
void moveAllServos(int from, int to)
{
  servoDriver.setServo(1, to);
  servoDriver.setServo(2, to);
  servoDriver.setServo(3, to);
  servoDriver.setServo(4, to);
 
  /* Time it takes servo to move to specified position based on servo speed rating at 60 deg turn */
  long delayMS = long(1000.0 * speed * abs(from-to));
  SerialMonitorInterface.print("Delay ");
  SerialMonitorInterface.print(delayMS);
  SerialMonitorInterface.println(" ms");
  delay(delayMS);
}

File UART.ino taken directly from UARTPassthrough example project (changed BLE broadcast name only):
Code: [Select]
#if BLE_DEBUG
#include <stdio.h>
char sprintbuff[100];
#define PRINTF(...) {sprintf(sprintbuff,__VA_ARGS__);SerialMonitorInterface.print(sprintbuff);}
#else
#define PRINTF(...)
#endif


volatile uint8_t set_connectable = 1;
uint16_t connection_handle = 0;


#define  ADV_INTERVAL_MIN_MS  50
#define  ADV_INTERVAL_MAX_MS  100


int connected = FALSE;


int BLEsetup() {
  int ret;

  HCI_Init();
  /* Init SPI interface */
  BNRG_SPI_Init();
  /* Reset BlueNRG/BlueNRG-MS SPI interface */
  BlueNRG_RST();

  uint8_t bdaddr[] = {0x12, 0x34, 0x00, 0xE1, 0x80, 0x02};

  ret = aci_hal_write_config_data(CONFIG_DATA_PUBADDR_OFFSET, CONFIG_DATA_PUBADDR_LEN, bdaddr);

  if (ret) {
    PRINTF("Setting BD_ADDR failed.\n");
  }

  ret = aci_gatt_init();

  if (ret) {
    PRINTF("GATT_Init failed.\n");
  }

  uint16_t service_handle, dev_name_char_handle, appearance_char_handle;
  ret = aci_gap_init_IDB05A1(GAP_PERIPHERAL_ROLE_IDB05A1, 0, 0x07, &service_handle, &dev_name_char_handle, &appearance_char_handle);

  if (ret) {
    PRINTF("GAP_Init failed.\n");
  }

  const char *name = "BlueNRG";

  ret = aci_gatt_update_char_value(service_handle, dev_name_char_handle, 0, strlen(name), (uint8_t *)name);

  if (ret) {
    PRINTF("aci_gatt_update_char_value failed.\n");
  } else {
    PRINTF("BLE Stack Initialized.\n");
  }

  ret = Add_UART_Service();

  if (ret == BLE_STATUS_SUCCESS) {
    PRINTF("UART service added successfully.\n");
  } else {
    PRINTF("Error while adding UART service.\n");
  }

  /* +4 dBm output power */
  ret = aci_hal_set_tx_power_level(1, 3);
}

void aci_loop() {
  HCI_Process();
  ble_connection_state = connected;
  if (set_connectable) {
    setConnectable();
    set_connectable = 0;
  }
  if (HCI_Queue_Empty()) {
    //Enter_LP_Sleep_Mode();
  }
}

#define COPY_UUID_128(uuid_struct, uuid_15, uuid_14, uuid_13, uuid_12, uuid_11, uuid_10, uuid_9, uuid_8, uuid_7, uuid_6, uuid_5, uuid_4, uuid_3, uuid_2, uuid_1, uuid_0) \
  do {\
    uuid_struct[0] = uuid_0; uuid_struct[1] = uuid_1; uuid_struct[2] = uuid_2; uuid_struct[3] = uuid_3; \
    uuid_struct[4] = uuid_4; uuid_struct[5] = uuid_5; uuid_struct[6] = uuid_6; uuid_struct[7] = uuid_7; \
    uuid_struct[8] = uuid_8; uuid_struct[9] = uuid_9; uuid_struct[10] = uuid_10; uuid_struct[11] = uuid_11; \
    uuid_struct[12] = uuid_12; uuid_struct[13] = uuid_13; uuid_struct[14] = uuid_14; uuid_struct[15] = uuid_15; \
  }while(0)

#define COPY_UART_SERVICE_UUID(uuid_struct)  COPY_UUID_128(uuid_struct,0x6E, 0x40, 0x00, 0x01, 0xB5, 0xA3, 0xF3, 0x93, 0xE0, 0xA9, 0xE5, 0x0E, 0x24, 0xDC, 0xCA, 0x9E)
#define COPY_UART_TX_CHAR_UUID(uuid_struct)  COPY_UUID_128(uuid_struct,0x6E, 0x40, 0x00, 0x02, 0xB5, 0xA3, 0xF3, 0x93, 0xE0, 0xA9, 0xE5, 0x0E, 0x24, 0xDC, 0xCA, 0x9E)
#define COPY_UART_RX_CHAR_UUID(uuid_struct)  COPY_UUID_128(uuid_struct,0x6E, 0x40, 0x00, 0x03, 0xB5, 0xA3, 0xF3, 0x93, 0xE0, 0xA9, 0xE5, 0x0E, 0x24, 0xDC, 0xCA, 0x9E)

uint16_t UARTServHandle, UARTTXCharHandle, UARTRXCharHandle;


uint8_t Add_UART_Service(void)
{
  tBleStatus ret;
  uint8_t uuid[16];

  COPY_UART_SERVICE_UUID(uuid);
  ret = aci_gatt_add_serv(UUID_TYPE_128,  uuid, PRIMARY_SERVICE, 7, &UARTServHandle);
  if (ret != BLE_STATUS_SUCCESS) goto fail;

  COPY_UART_TX_CHAR_UUID(uuid);
  ret =  aci_gatt_add_char(UARTServHandle, UUID_TYPE_128, uuid, 20, CHAR_PROP_WRITE_WITHOUT_RESP, ATTR_PERMISSION_NONE, GATT_NOTIFY_ATTRIBUTE_WRITE,
                           16, 1, &UARTTXCharHandle);
  if (ret != BLE_STATUS_SUCCESS) goto fail;

  COPY_UART_RX_CHAR_UUID(uuid);
  ret =  aci_gatt_add_char(UARTServHandle, UUID_TYPE_128, uuid, 20, CHAR_PROP_NOTIFY, ATTR_PERMISSION_NONE, 0,
                           16, 1, &UARTRXCharHandle);
  if (ret != BLE_STATUS_SUCCESS) goto fail;

  return BLE_STATUS_SUCCESS;

fail:
  PRINTF("Error while adding UART service.\n");
  return BLE_STATUS_ERROR ;

}



uint8_t lib_aci_send_data(uint8_t ignore, uint8_t* sendBuffer, uint8_t sendLength) {
  return !Write_UART_TX((char*)sendBuffer, sendLength);
}

uint8_t Write_UART_TX(char* TXdata, uint8_t datasize)
{
  tBleStatus ret;

  ret = aci_gatt_update_char_value(UARTServHandle, UARTRXCharHandle, 0, datasize, (uint8_t *)TXdata);

  if (ret != BLE_STATUS_SUCCESS) {
    PRINTF("Error while updating UART characteristic.\n") ;
    return BLE_STATUS_ERROR ;
  }
  return BLE_STATUS_SUCCESS;

}


void Read_Request_CB(uint16_t handle)
{
  /*if(handle == UARTTXCharHandle + 1)
    {

    }
    else if(handle == UARTRXCharHandle + 1)
    {


    }*/

  if (connection_handle != 0)
    aci_gatt_allow_read(connection_handle);
}


void setConnectable(void)
{
  tBleStatus ret;

  // const char local_name[] = {AD_TYPE_COMPLETE_LOCAL_NAME, 'B', 'l', 'u', 'e', 'N', 'R', 'G'};
  const char local_name[] = {AD_TYPE_COMPLETE_LOCAL_NAME, 'D', 'u' ,'n' ,'c' ,'a' ,'n'};

  hci_le_set_scan_resp_data(0, NULL);
  PRINTF("General Discoverable Mode.\n");

  ret = aci_gap_set_discoverable(ADV_IND,
                                 (ADV_INTERVAL_MIN_MS * 1000) / 625, (ADV_INTERVAL_MAX_MS * 1000) / 625,
                                 STATIC_RANDOM_ADDR, NO_WHITE_LIST_USE,
                                 sizeof(local_name), local_name, 0, NULL, 0, 0);

  if (ret != BLE_STATUS_SUCCESS)
    PRINTF("%d\n", (uint8_t)ret);

}

void Attribute_Modified_CB(uint16_t handle, uint8_t data_length, uint8_t *att_data)
{
  if (handle == UARTTXCharHandle + 1) {
    int i;
    for (i = 0; i < data_length; i++) {
      ble_rx_buffer[i] = att_data[i];
    }
    ble_rx_buffer[i] = '\0';
    ble_rx_buffer_len = data_length;
  }
}

void GAP_ConnectionComplete_CB(uint8_t addr[6], uint16_t handle) {

  connected = TRUE;
  connection_handle = handle;

  PRINTF("Connected to device:");
  for (int i = 5; i > 0; i--) {
    PRINTF("%02X-", addr[i]);
  }
  PRINTF("%02X\r\n", addr[0]);
}

void GAP_DisconnectionComplete_CB(void) {
  connected = FALSE;
  PRINTF("Disconnected\n");
  /* Make the device connectable again. */
  set_connectable = TRUE;
}



void HCI_Event_CB(void *pckt)
{
  hci_uart_pckt *hci_pckt = (hci_uart_pckt *)pckt;
  hci_event_pckt *event_pckt = (hci_event_pckt*)hci_pckt->data;

  if (hci_pckt->type != HCI_EVENT_PKT)
    return;

  switch (event_pckt->evt) {

    case EVT_DISCONN_COMPLETE:
      {
        //evt_disconn_complete *evt = (void *)event_pckt->data;
        GAP_DisconnectionComplete_CB();
      }
      break;

    case EVT_LE_META_EVENT:
      {
        evt_le_meta_event *evt = (evt_le_meta_event *)event_pckt->data;

        switch (evt->subevent) {
          case EVT_LE_CONN_COMPLETE:
            {
              evt_le_connection_complete *cc = (evt_le_connection_complete *)evt->data;
              GAP_ConnectionComplete_CB(cc->peer_bdaddr, cc->handle);
            }
            break;
        }
      }
      break;

    case EVT_VENDOR:
      {
        evt_blue_aci *blue_evt = (evt_blue_aci *)event_pckt->data;
        switch (blue_evt->ecode) {

          case EVT_BLUE_GATT_READ_PERMIT_REQ:
            {
              evt_gatt_read_permit_req *pr = (evt_gatt_read_permit_req *)blue_evt->data;
              Read_Request_CB(pr->attr_handle);
            }
            break;

          case EVT_BLUE_GATT_ATTRIBUTE_MODIFIED:
            {
              evt_gatt_attr_modified_IDB05A1 *evt = (evt_gatt_attr_modified_IDB05A1*)blue_evt->data;
              Attribute_Modified_CB(evt->attr_handle, evt->data_length, evt->att_data);
            }
            break;
        }
      }
      break;
  }

}

I am simply connecting to the BLE shield via an Android app similar to nRF UART 2.0 (nordic semi) and sending servo pulse micro second values to the BLE shield as a string, once its received in the Arduino sketch, I convert it back to an integer and write that value to the servos. The minimum sized message I send to the BLE shield from the app would be a string "510" and the max value sent would be the string "2540". I have implemented a virtual joystick in the android app I am using and if i send pulse updates to the sketch too fast the BLE comms end up freezing the whole app into a non-recoverable state. If I lower the send interval to around 10 times/sec I don't experience sketch freezes as frequently but it is still and issue. I'd like to understand the root cause of this issue so I can implement an applicable solution where I can get the highest frequency servo pulse updates possible. I want to have smooth servo operation with little to no jittering. The lower the frequency I am restricted to the more jittering I experience in the servos for smaller pulse movements.

Please advise.

Thank you.

Pages: [1]