 #include "CalLib.h"
 #include <EEPROM.h>
 #include <ros.h>
-#include "geometry_msgs/Twist.h"
 #include "robik/GenericStatus.h"
 #include "robik/GenericControl.h"
 #include "robik/ArmControl.h"
+#include "robik/VelocityControl.h"
 #include "robik.h"
 #include "robot_config.h"
 #include "robik_api.h"


void genericMessageListener(const robik::GenericControl& msg);
void armMessageListener(const robik::ArmControl& msg);
void velocityMessageListener(const geometry_msgs::Twist& msg);
ros::Subscriber<robik::GenericControl> sub_generic_control("robik_generic_control", &genericMessageListener);
ros::Subscriber<robik::ArmControl> sub_arm_control("robik_arm_control", &armMessageListener);
ros::Subscriber<geometry_msgs::Twist> sub_velocity_control("robik_velocity_control", &velocityMessageListener);
void setArmPower(bool status);
bool getArmPower();



}

//Input received from diff_drive_controller
void velocityMessageListener(const robik::VelocityControl& msg) {
	req_motor_left = constrain(msg.motor_left, -MAX_VEL, MAX_VEL);
	req_motor_right = constrain(msg.motor_right, -MAX_VEL, MAX_VEL);
	velocity_control_LR();
}

// vel: linear velocity [m/sec]
