   else {digitalWrite(11, LOW); }
 }
 
+// Ensure 1100 <= pwm <= 1900
 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;
-}
-

  Wire.write(valid(cmd.pwm.surge_stbd_lo));
  Wire.endTransmission();

  Wire.requestFrom(ESC_BOARD[1], ONE_PACKET);
  state.surge_port_lo.checksum ^= Wire.read();
  state.surge_stbd_lo.checksum ^= Wire.read();
  state.surge_port_lo.temp = Wire.read() | Wire.read() << 8;
  state.surge_port_lo.current = get_current();
  state.surge_stbd_lo.temp = Wire.read() | Wire.read() << 8;
  state.surge_stbd_lo.current = get_current();

  Wire.beginTransmission(ESC_BOARD[2]);
  Wire.write(valid(cmd.pwm.sway_fwd) >> 8);

#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};

