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

Servo port_thruster;
Servo stbd_thruster;

const float ANGLE_CONVERSION = 4096.0/360.0;

void setup()
{
  nh.initNode();
  nh.advertise(state_pub);

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

    stbd_thruster.writeMicroseconds(ZERO_THRUST);
    aft_thruster.writeMicroseconds(ZERO_THRUST);
  }
  state.data[0] = port_servo_pos;
  state.data[1] = stbd_servo_pos;
  state.data[2] = GetSpeed(PORT_SERVO);
  state.data[3] = GetSpeed(STBD_SERVO);
  state.data[4] = GetLoad(PORT_SERVO);
  state.data[5] = GetLoad(STBD_SERVO);
  state_pub.publish(&state);
  delay(33);
}
