 #include <ros.h>
-#include <std_msgs/Float64.h>
+#include <std_msgs/Int32MultiArray.h>
 
 #include "ax12.h"
 #include <Servo.h>

#define GetLoad(id) (ax12GetRegister(id, AX_PRESENT_LOAD_L, 2))

ros::NodeHandle nh;
std_msgs::Float64 state;
std_msgs::Float64 cmd;

int port_servo_cmd = 150;
int stbd_servo_cmd = 150;
int port_servo_pos = 150;
int stbd_servo_pos = 150;
int state[] = {150, 150, 0, 0, 0, 0};

void callback(const std_msgs::Float64 cmd);
{
  port_servo_cmd = cmd[0];
  stbd_servo_cmd = cmd[1];
  SetPosition(PORT_SERVO, port_servo_cmd * ANGLE_CONVERSION);
  SetPosition(STBD_SERVO, stbd_servo_cmd * ANGLE_CONVERSION);
  // set -- port, stbd, aft -- thrusters
}

ros::Publisher state_pub("state", &state);
ros::Subscriber<std_msgs::Float64> cmd_sub("command", &callback);

const int PORT_SERVO = 18;
const int STBD_SERVO = 15;
const int AFT_THRUSTER = 13;
const int PORT_THRUSTER = 12;
const int STBD_THRUSTER = 14;

const int MIN_THRUST = 1000;
const int ZERO_THRUST = 1500;

Servo port_thruster;
Servo stbd_thruster;

void setup()
{
  nh.initNode();
  nh.advertise(state_pub);
  state.layout.data_offset = 0;
  state.layout.dim[0].label = "command";
  state.layout.dim[0].size = 2;
  state.layout.dim[0].stride = 2;

  ax12Init(1000000);
  aft_thruster.attach(AFT_THRUSTER);
