Experiencing Strange Lag on Nano 3.0

[skhan72]

Hello everyone! I hope that my post is within community guidelines; I read the FAQ and posting guide, but please let me know if I have overlooked anything.

My post is for a project for a school robotics team. I am working on the software for a device for underwater data collection that is run by a Nano with the HC05 bluetooth module, along with a GravityRTC, a depth sensor, and a Sparkfun External EEPROM memory chip. I've suddenly started experiencing a strange lag at startup and key stages of the device's cycle. Specifically at startup it takes a noticeable amount of time to begin the initial process (about 15 secs), which is simply transmitting 2 data packets consisting of sensor data. When I run the main state machine, there is a ton of lag (about 25 secs) before it begins its cycle, which is a downward descent until it reaches the bottom and then ascent until it reaches the surface, while collecting temperature, depth and pressure data throughout. The data is stored on the memory chip and then retrieved when it's time to transmit. I experience another 15-20 sec lag before the transmission even begins to run and the data is blank, even though in the startup data transmission, the same functions operate as expected.

I have been testing thoroughly throughout the development process and didn't experience any lag on the relevant functions until just recently. Initially everything I described worked just fine. I have been tracing through previous versions of the software, but admittedly my version control has a gap and so I haven't been able to isolate the issue that way. I am still just a student, so I would really appreciate some help with this. Please let me know if there is any more information I can give that would help.

//---------------------------------------------------------------------------------------------------------------------------
//  File name:  Floater2024_T0209
//  Version 1.0
//  Date: 02-09-2024
//  Program purpose: Thorpedo functioning and controls based on incoming bluetooth communication via Arduino Nano. 
//  Disclaimer: Bearing witness to the Thorpedo may cause involuntary ecstatic spasms.
//  Revision history:
//   Date                  Programmer                    Change ID   Description
//   06/16/24              Sarah Khan                    0005        Initial implementation
//-------------------------------------------------------------------------------------------------------------------------------

#include <SoftwareSerial.h>   // This is needed to talk to the Bluetooth HC-05 and leaving pins 0 & 1 free for debug
#include <Wire.h>  //i2c communication library
#include "GravityRtc.h"  //Real Time Clock library
#include "MS5837.h" //depth sensor library
#include <Adafruit_NeoPixel.h> //NeoPixel LED library
#include "SparkFun_External_EEPROM.h" // Click here to get the library: http://librarymanager/All#SparkFun_External_EEPROM

//NeoPixels pin connection to Arduino
#define LED_PIN 4

//Number of NeoPixels attached to Arduino
#define LED_COUNT 24
#define ledMaxLoops 10

//COMPANY_NUM for data packet **Change to assigned company number**
//char COMPANY_NUM = "EX03";

// constants won't change. They're used here to set pin numbers:
const uint8_t RXBT = 2;
const uint8_t TXBT = 8;
const uint8_t LED1 = 4;
const uint8_t LED2 = 5;
const uint8_t LED3 = 6;
const uint8_t LED4 = 7;
const uint8_t MTR_IN1 = 9;
const uint8_t MTR_IN2 = 10;

//led variables
uint8_t ledPosition = 0;
unsigned long ledLoop = 0;

//Enums for floater state
enum State{IDLE, START, DOWN, FREEFALL, BOTTOM, UP, FLOAT, TRANSMITTING_DATA, TEST, SHUT_DOWN};
//Set initial state at idle
State floater_state;

//************Q: Should I change these to floats? We're reading from the rtc clock now; at most we'll 
//have a few values past the decimal (ie: 0.123)******************
//********Q: Is there a better way to track time lapses for each state?*****************

//Variables for tracking time
unsigned long cycle_start_time = 0.0; //variable to control data collection
unsigned long motor_trigger_time = 0.0;
unsigned long motor_cut_time = 0.0;
unsigned long present_time = 0.0; //Variable to capture real time since start time
unsigned long cycle_elapsed_time = 0.0; //Variable for computing elapsed time
unsigned long last_data_catch_time = 0.0;
unsigned long motor_time_lapse = 0.0;
unsigned long bottom_state_time = 0.0;


//Variables for pump bag
const int MAX_PUMP_FILL_TIME = 45000; //45 secs MAX BAG FILL time **DO NOT EXCEED**
const int FILL_TO_SINK_TIME = 15000; //15 secs Amount time it takes to fill bag to sink the floater.
const int FREEFALL_TIME = 15000; //15 secs of freefall to bottom
const int EMPTY_FLOAT_TIME = 20000; //51 secs to fully empty bag when full. Will ensure bag totally empties.
const int SIT_AT_BOTTOM_TIME = 10000; //10 secs to sit at bottom; also insurance for reaching bottom
const int SURFACE_ASCENT_TIME = 15000; //15secs for natural bouyancy (bag emptied) to reach surface.

const int DATA_CYCLE_TIME = 1000; //1 sec intervals for data collection 

//Variables for capturing data
float captured_depth_data = 0.0;
float captured_pressure_data = 0.0;
float captured_temperature_data = 0.0;
String captured_rtc_time;

//Constant variable for max depth
const float MAX_DEPTH = 2.0; //Set to maximum depth (in meters?)

//Variable for idle depth reading
float idle_depth = 0.2;

//data storage
bool storing_data = false;

// Set up a new SoftwareSerial object
//serial port for bluetooth communication
SoftwareSerial btSerial =  SoftwareSerial(RXBT, TXBT);


//Create ExternalEEPROM memory object
ExternalEEPROM thorpMem;
uint8_t eepromAddress = 0x50;

// create a Real Time Clock object
GravityRtc rtc;     //RTC 

// Depth Sensor Object
MS5837 sensor;

// variables will change:
int btData = 0;  // variable for reading bluetooth serial data

//Declaration NeoPixel strip object
Adafruit_NeoPixel strip(LED_COUNT, LED_PIN, NEO_GRB + NEO_KHZ800);

//Function declarations
void ThorpedoDown();
void ThorpedoStop();
void ThorpedoUp();
void StoreData();
void GenerateData();
void TransmitData();
void ColorLoop();
float CalculateSlope();


#define RTC_TIME_LENGTH 15
#define COMPANY_NUMBER_LENGTH 5


//DataPacket structure for capturing data values
struct DataPacket {
  State floater_state;
  char company_number[COMPANY_NUMBER_LENGTH];
  float depth;
  float pressure;
  float temperature;
  char rtc_time_log[RTC_TIME_LENGTH];
  float time_lapse; 
};

//DataPacket structure default settings for each variable
//**Change to assigned company number**
DataPacket data = {
  .floater_state = IDLE,
  .company_number = "EX03",
  .depth = 00.00,
  .pressure = 00.00,
  .temperature = 00.00,
  .rtc_time_log = "00:00:00 UTC-8",
  .time_lapse = 00.00,
};

//Data entry index
int data_index = 1; ///
int transmit_index = 1; ///

//DataPacket struct size in bytes for address calculation
int const data_size = sizeof(DataPacket); 

//Slope calculation variables
float const SLOPE_THRESHOLD = 0.1;
float calculated_slope = 0.0;
int slope_index_count = 0;
int slope_calc_count = 0;

void setup() {
  // initialize the LED pins as an outputs and turn OFF
  pinMode(LED1,OUTPUT);    // LED 1
  pinMode(LED2,OUTPUT);    // LED 2
  pinMode(LED3,OUTPUT);    // LED 3
  pinMode(LED4,OUTPUT);    // LED 4
  digitalWrite(LED1,LOW);
  digitalWrite(LED2,LOW);
  digitalWrite(LED3,LOW);
  digitalWrite(LED4,LOW);

  // initalize the Motor control pins as outputs and turn OFF
  pinMode(MTR_IN1,OUTPUT);
  pinMode(MTR_IN1,OUTPUT);
  digitalWrite(MTR_IN1,LOW);
  digitalWrite(MTR_IN2,LOW);
  
  // Start the software serial monitor
  btSerial.begin(9600);

  // Start the main serial port for debugging
  Serial.begin(9600);

  // start the I2C service
  Wire.begin();

// setup the Real Time Clock
  rtc.setup();
  rtc.adjustRtc(F(__DATE__), F(__TIME__));

  floater_state = IDLE;

  //Setting memory 
  thorpMem.setMemoryType(2048);
  if (thorpMem.isConnected() == false){
    Serial.println(F("Memory NOT detected.  Freezing."));
    while(true);
  }
  else{
    Serial.println(F("Memory detected!"));
  }

  Serial.println(F("sizeof(DataPacket) // data_size var Value: "));
  Serial.println(data_size);

 //Depth Sensor Setup
/*while (!sensor.init()) {
  Serial.println(F("Init failed!"));
  Serial.println(F("Are SDA/SCL connected correctly?"));
  Serial.println(F("Blue Robotics Bar30: White=SDA, Green=SCL"));
  delay(500);
}
sensor.setModel(MS5837::MS5837_30BA);
sensor.setFluidDensity(997); //Fluid Density = kg/m^3 (freshwater, 1029 for seawater)
*/

//NeoPixel Setup
strip.begin();
strip.show();
strip.setBrightness(50);

//Wipe memory at setup
thorpMem.erase();

// end of all the setup. 
Serial.println(F("Setup Complete"));
btSerial.println("Setup Complete");
}


void loop() {
 // go get the current time from the RTC
  rtc.read();   

  // Update pressure and temperature readings
  sensor.read();

  //present_time variable to be used to get difference from cycle_start_time
  present_time = millis();

  
  //While bluetooth data or data via the serial port is available, read that data.
  while((btSerial.available()>0)||(Serial.available()>0)){
    if (btSerial.available()>0){
      btData=btSerial.read();  // get the command
    }
    else{
      btData=Serial.read();
    }
      // echo the received command back to debug and bluetooth
     Serial.println(F("\nBT Data:"));
     Serial.println(btData);
     btSerial.println("\nBT Data:");
     btSerial.println(btData);
  switch (btData) {
    case 67:  //"C" for Connect Message
      floater_state = IDLE;
      //Send Connect Message with sample data Packet
      Serial.println(F("Behold! The Thorpedo is Alive!"));
      btSerial.println("Behold! The Thorpedo is Alive!");
      break;
    case 68:   //"D" for Thorpedo Down  // testing mode only
      floater_state = TEST;
      //Transmit data back to GUI
      Serial.println(F("Thorpedo DOWN"));
      btSerial.println("Thorpedo DOWMN");
      ThorpedoDown();
      break;
    case 71:   //"G" for Go, Start the cycle
      floater_state = START;
      Serial.println(F("Start Profile Cycle"));
      btSerial.println("Start Profile Cycle");
      break;
    case 83:  // "S" for stop pump //Testing only
      floater_state = IDLE;
      ThorpedoStop();
      Serial.println(F("Stop Test Cycle"));
      btSerial.println("Stop Test Cycle");
      break;
    case 85: //"U" for UP  //testing mode only
      floater_state = TEST;
      Serial.println(F("Thorpedo UP"));
      btSerial.println("Thorpedo UP");
      ThorpedoUp();
      break;

    case 84: //"T" for Transmit Data
      floater_state = TRANSMITTING_DATA;
      Serial.println(F("Thorpedo TRANSMIT DATA"));
      btSerial.println("Thorpedo TRANSMIT DATA");
      break;
    case 82: //"R" for stoRing data
      floater_state = IDLE;     
      storing_data = true;
      Serial.println(F("\nThorpedo STORING DATA"));
      btSerial.println("\nThorpedo STORING DATA");
      break;
    case 80: //"P" for stoP (storing data)
      floater_state = IDLE;
      storing_data = false;
      Serial.println(F("Thorpedo STOPPED STORING DATA"));
      btSerial.println("Thorpedo STOPPED STORING DATA");
    case 13: // lf
      break;
    default:
      Serial.println(F("Command Not Decoded"));
      btSerial.println("Command Not Decoded");
      break;
    }
  }   // end of btData parsing
  
  //State machine
  switch (floater_state) {
    
    case IDLE:
      if(data_index <= 2){ ///
        StoreData();
        if (data_index == 3){
          Serial.println(F("***Behold! The Thorpedo Collects DATA!!***\n"));
          floater_state = TRANSMITTING_DATA;
        }
      }
      break;

    case START:
      storing_data = true;
      ThorpedoDown();
      slope_calc_count = 0;
      rtc.read();
      cycle_start_time = millis(); //Cycle_start_time for data cycle start
      motor_trigger_time = millis(); //Catching the time the motor is started
      ColorLoop();
      floater_state = DOWN;

      //Echoing cycle time and motor trigger time
      Serial.println(F("Thorpedo Vertical Profile Start: "));
      btSerial.println("Thorpedo Vertical Profile Start: ");
      Serial.print(cycle_start_time);
      btSerial.print(cycle_start_time);
      Serial.print(F(" msecs"));
      btSerial.print(" msecs");
      break;
    
    case DOWN:
      //Motor stays on until FILL_TO_SINK_TIME is reached
      ColorLoop();
      motor_time_lapse = present_time - motor_trigger_time;
      if (motor_time_lapse >= FILL_TO_SINK_TIME){
        motor_cut_time = millis();
        ThorpedoStop();
        floater_state = FREEFALL;
      }
      break;

    case FREEFALL:
      ColorLoop();
      if(present_time - motor_cut_time >= FREEFALL_TIME){
        bottom_state_time = millis();
        floater_state = BOTTOM;
      }
      break;
    
    case BOTTOM:
      ColorLoop(); 
      if (present_time - bottom_state_time >= SIT_AT_BOTTOM_TIME){
        motor_trigger_time = millis();
        floater_state = UP;
        ThorpedoUp();
      }
      break;
    
    case UP:
      ColorLoop();
      if (present_time - motor_trigger_time >= EMPTY_FLOAT_TIME){
        ThorpedoStop();
        motor_cut_time = millis();
        floater_state = FLOAT;
      }
      break;
    case FLOAT:
      ColorLoop();
      if (present_time - motor_cut_time >= SURFACE_ASCENT_TIME){
        ThorpedoStop();
        Serial.println(F("Thorpedo profile FINISHED!"));
        btSerial.println("Thorpedo profile FINISHED! \nReady for Data Transfer!");
        floater_state = IDLE;
        storing_data = false;
        ColorLoop();
      }
      break;
    
    case TRANSMITTING_DATA:
      if(transmit_index < data_index){
        ColorLoop();
        TransmitData();
      }
      else{
        btSerial.println("STATUS_CHANGE:2");
        floater_state = IDLE;
        ColorLoop();
      }
      
      break;
    //********Q: What should the default for the state machine be? 
    default://*********************
      break; //************
  }


  ledLoop++;
  if (ledLoop > ledMaxLoops){
    ledLoop = 0;
    ledPosition++;
    
    if (ledPosition == 6){
      ledPosition = 0;
    }
    ColorLoop();
  }

  cycle_elapsed_time = present_time - cycle_start_time;

  if ((present_time - last_data_catch_time) >= DATA_CYCLE_TIME && storing_data){
    StoreData();
    last_data_catch_time = millis();
  }
}

 //StoreData on SparkFun External EEPROM 
 //data location calculated with data_index * data_size const
void StoreData(){
  uint32_t test_read = 0;
  int mem_location = data_index * data_size;

  if(!sensor.init()){
    GenerateData();
  }
  else{
    captured_depth_data = sensor.depth();
    captured_pressure_data = sensor.pressure();
    captured_temperature_data = sensor.temperature();
  }
  data.depth = captured_depth_data;
  data.pressure = captured_pressure_data;
  data.temperature = captured_temperature_data;
  snprintf(data.rtc_time_log, RTC_TIME_LENGTH, "%02d:%02d:%02d UTC-8", rtc.hour, rtc.minute, rtc.second);
  data.rtc_time_log[15] = '\0';
  data.time_lapse = cycle_elapsed_time;

  if(thorpMem.isBusy()!=true){ 
    Serial.println("DATA RETRIEVED FROM MEMORY:");
    thorpMem.get(mem_location, data);
    delay(500);
    ++data_index;
    
    Serial.println(F("\nThorpedo STORE DATA!"));
    Serial.println(F("\ndata_index: "));
    Serial.println(data_index);
    btSerial.println("Thorpedo STORE DATA!");
  }
  else{
    Serial.println(F("Error saving data!"));
    btSerial.println("Error saving data!");
  }
}

//Generates data when sensor data not available
void GenerateData(){
  float distance = 1.0; //meter?/5 secs
  const float initial_depth = 0.20; //initial depth (in cm) of sensor on floater when in water
  const float delta_temp = 0.02; //deg C
  const float initial_temp = 25.0; //deg C
  const float atmos_pressure = 101325; //Pa (standard atmospheric pressure)
  const float water_density = 997.0; //kg/m^3 (density)
  const float gravity = 9.81; //m/s^2 (acceleration due to gravity)

  switch(floater_state){
    case DOWN:
      captured_depth_data = cycle_elapsed_time * distance;
      ++distance;
      break;
    case BOTTOM:
      captured_depth_data = MAX_DEPTH; 
    case UP:
      captured_depth_data = MAX_DEPTH - (cycle_elapsed_time * distance);
      --distance;
    case IDLE:
      captured_depth_data = initial_depth;
  }

  captured_temperature_data = initial_temp + (captured_depth_data * delta_temp);
  captured_pressure_data = (atmos_pressure + (water_density * gravity * captured_depth_data))/1000;

  Serial.println(F("\nThorpedo GENERATED DATA!!!"));
  btSerial.println("\nThorpedo GENERATED DATA!!!");
}


void TransmitData(){
  int mem_location;
  String data_packet = "";
  data_packet.reserve(200); //
  int check_sum = 55;
  mem_location = transmit_index * data_size;
  thorpMem.get(mem_location, data);

  //****TO DO: Switch commented/uncommented****
  data_packet += "dp1::"; 
  data_packet += String(data.rtc_time_log); 
  data_packet += ";";
  data_packet += "dp2::" + String(data.company_number) + ";";
  data_packet += "dp3::" + String(data.depth, 2) + ";";
  data_packet += "dp4::" + String(data.pressure, 2) + ";";
  data_packet += "dp5::" + String(data.temperature, 2) + ";";
  data_packet += "dp6::" + String(data.time_lapse) + ";";
  data_packet += "dp7::" + String(check_sum) + "$";
  
  btSerial.println("STATUS_CHANGE:1");
  btSerial.println("data_packet string");
  btSerial.println(data_packet);
  btSerial.println("\n");
  Serial.println("Data Transmitted:\n");
  Serial.println(data_packet);
  delay(1000);
  ++transmit_index;

  Serial.println(F("\nThorpedo TRANSMIT DATA!"));
  btSerial.println("\nThorpedo TRANSMIT DATA!");
}


//Motor turns on/fills bag
void ThorpedoDown(){
  digitalWrite(MTR_IN1, HIGH);
  digitalWrite(MTR_IN2, LOW);
  digitalWrite(LED3, LOW);
  digitalWrite(LED4, HIGH);
  Serial.println(F("Thorpedo DOWN"));
}

//Motor turns on/pumps water out of bag
void ThorpedoUp(){
  digitalWrite(MTR_IN1, LOW);
  digitalWrite(MTR_IN2, HIGH);
  digitalWrite(LED3, HIGH);
  digitalWrite(LED4, LOW);
  Serial.println(F("Thorpedo UP"));
}

//Turns motor off
void ThorpedoStop(){
  digitalWrite(MTR_IN1, LOW);
  digitalWrite(MTR_IN2, LOW);
  digitalWrite(LED3, LOW);
  digitalWrite(LED4, LOW);
  Serial.println(F("Thorpedo OFF"));
}

//ColorLoop function sets floater_state corresponding colors
void ColorLoop() {
   uint32_t ledColor;

   int i;
   ledColor = strip.Color(0,0,0);
  for (i=0; i<24; i++){
      strip.setPixelColor(i,ledColor);
  }
  // set the colors based upon the current floater mode.
  // the colors come from the following color picker
  // https://www.rapidtables.com/web/color/RGB_Color.html
  
  switch (floater_state){              
    case START: 
      ledColor = strip.Color(77,255,0);     // green
      break;
    case TRANSMITTING_DATA: 
      ledColor = strip.Color(0,205,255);  // turquoise
      break;
    case DOWN:  
      ledColor = strip.Color(77,255,0);  // green
      break;
    case FREEFALL:
      ledColor = strip.Color(204, 0, 102); // pink
      break;
    case BOTTOM:  
      ledColor = strip.Color(255,0,0);  // red
      break;
    case UP:  
      ledColor = strip.Color(255,255,0);  // yellow
      break;
    case FLOAT:
      ledColor = strip.Color(0, 0, 255); //blue
      break;
    case IDLE:  
      ledColor = strip.Color(127,0,255);  // purple
      break;   
    case TEST:
      ledColor = strip.Color(0, 255, 255);
      break;
    }

    strip.setPixelColor(ledPosition,ledColor);
    strip.setPixelColor(ledPosition+6,ledColor);
    strip.setPixelColor(ledPosition+12,ledColor);
    strip.setPixelColor(ledPosition+18,ledColor);    
    strip.show();                           
}

Edit: I've managed to resolve a fair amount of the lag, but I'm experiencing some pretty bad data truncation.

[groundFungus]

Water is a great stopper of radiation. You will have no luck trying to communicate through water

[gilshultz]

You will need to keep the Antenna in the air. Your problem is why most underwater vehicles use an umbilical cord.

If you want to do it wireless you will need to go maybe sonic or lower in frequency. When doing that your response will get slow because of the communications lag.

[skhan72]

Yes! That is something we have accommodated in the device's design. So (sorry I should have mentioned this) the Nano is positioned above water level and that communication both tethered and wireless lags in the same way. We expect to lose connection during operation when it's submerged, and then when it resurfaces, we're expected to re-establish connection and transmit the data that was obtained and stored on the eprom during the run. I'm experiencing the same lag regardless of the connection type and when I'm testing software on the machine when its sitting on a desktop, and so I think it must be in the software itself.

[skhan72]

Yes! We have it designed so that the Nano's bluetooth module sits above water level at the start of the operation. The lag that I'm struggling with happens even when the device is sitting on a desk, regardless of connection type, via cable or wireless.

[jremington]

The problem is probably in the code that you forgot to post. Please read and follow the instructions in the "How to get the best out of this forum" post.

[gilshultz]

@skhan72

The lag that I'm struggling with happens even when the device is sitting on a desk,

I would start by removing the delay(XXX) from the code as you can see on line???

[camsysca]

No code, no attention. Simple equation.

[xfpd]

@skhan72

managed to resolve a fair amount of the lag

Like what?

@skhan72

rtc.adjustRtc(F(__DATE__), F(__TIME__));

You only need this to run once to load the RTC with the system time, then comment-out this line until the RTC battery dies.

@skhan72

delay(500);

Half second delay every data rx?

@skhan72

delay(1000);

One second delay every data tx?

@skhan72

ledColor = strip.Color(0, 255, 255); // CYAN

CYAN