 Servo port_thruster;
 Servo stbd_thruster;
 
-const float ANGLE_CONVERSION = 4096.0/360.0;
-
 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);

#include <ros.h>
#include <std_msgs/Float64.h>

#include "ax12.h"
#include <Servo.h>

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

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

int port_servo_cmd = 150;
int stbd_servo_cmd = 150;
int port_servo_pos = 150;
int stbd_servo_pos = 150;

const int PORT_SERVO = 18;
const int STBD_SERVO = 15;

const float ANGLE_CONVERSION = 4096.0/360.0;

void callback(const std_msgs::Int32MultiArray& cmd)
{
  port_servo_cmd = cmd.data[0];
  stbd_servo_cmd = cmd.data[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::Int32MultiArray> cmd_sub("command", &callback);

const int AFT_THRUSTER = 13;
const int PORT_THRUSTER = 12;
const int STBD_THRUSTER = 14;

int port_thrust;
int stbd_thrust;
int aft_thrust;

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