   */
 
 
+  if(millis() > led_time){
+      led_state = (led_state == HIGH) ? LOW : HIGH;
+      digitalWrite(LED_PIN, led_state);
+      led_time = millis() + LED_BLINK_LEN;
+  }
 
   counter += 1;
   delay(1);


#include <Wire.h>
#include "MS5837.h"
#include "PCA9685.h"
#include "ADC121.h"
#include "Adafruit_INA219.h"

#define BUFFER_SIZE 64 //may need to change
#define NUM_THRUSTERS 8

#define THRUSTER_NEUTRAL 1285
/// THRUSTER_NEUTRAL - 256
#define THRUSTER_MIN 1029U
// THRUSTER_NETURAL + 256
#define THRUSTER_MAX 1541U

// Time(ms) arduino waits without hearing from jetson before turning off thrusters
#define ALIVE_TIMEOUT 5000


void thrustersNeutral() {
  for ( int i = 0; i < NUM_THRUSTERS; i++ ) {
      /* pca.thrusterSet(i, THRUSTER_NEUTRAL); */
      esc[i].writeMicroseconds(THRUSTER_NEUTRAL);
  }
}

void thrustersOff() {
    for ( int i = 0; i < NUM_THRUSTERS; i++ ) {
        /* pca.thrusterSet(i, THRUSTER_NEUTRAL); */
        esc[i].writeMicroseconds(0);
    }
}

// Placeholder, needs to get depth from I2C, then println it to serial
void getDepth() {
