 #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"

	    ||
	     (req_motor_left == 0 && req_motor_right == 0)
	) {
		//remove correction, use only estimated speeds for new velocity command
		old_motor_left = req_motor_left;
		old_motor_right = req_motor_right;
		millis_since_cmd = millis();


}

//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]
