   Wire.write(valid(cmd.pwm.heave_stbd_aft));
   Wire.endTransmission();
 
-  Wire.requestFrom(ESC_BOARD[4], ONE_PACKET);
-  state.heave_port_aft.checksum ^= Wire.read();
-  state.heave_stbd_aft.checksum ^= Wire.read();
-  state.heave_port_aft.temp = Wire.read() | Wire.read() << 8;
-  state.heave_port_aft.current = get_current();
-  state.heave_stbd_aft.temp = Wire.read() | Wire.read() << 8;
-  state.heave_stbd_aft.current = get_current();
-
-  // Publish checksum results
   state_pub.publish(&state);
 
   // Red LEDs

  else {digitalWrite(11, LOW); }
}

int16_t valid(int16_t pwm)
{
  pwm = pwm > 1900 ? 1900 : pwm;
  pwm = pwm < 1100 ? 1100 : pwm;
  return pwm;
}

float get_current()
{
  return ((Wire.read() | Wire.read() << 8) - 96.0) / 36.0;
}


#include <Wire.h>
#include <ros.h>
#include <riptide_msgs/PwmStamped.h>
#include <std_msgs/Int8.h>

// Thrusters off!
int STOP = 1500;
// Checksum size
int ONE_BYTE = 1;
// Addresses
int ESC_BOARD[] = {1, 2, 4, 8, 16};

