print msg_empty
print msg_empty
##create the publishers for force mode
pub = rospy.Publisher('/g500/thrusters_input', Float64MultiArray, queue_size=1)
rospy.init_node('keyboardCommand')
##Start force command loop
counter_wrong = 0
print msg_thrust
print msg_command
while not rospy.is_shutdown():
msg = Float64MultiArray()
#to lock the vehicle position
msg.data = [0,0,0,0,0]
try:
c = sys.stdin.read(1)
##Depending on the character set the proper speeds
if c=='\n':
start()
print "Benchmarking Started!"
elif c==' ':
stop()
print "Benchmark finished!"
##commands to guide the robot
else:
#go up-down
if c=='w' or c=='s':
msg.twist.angular.z = rot_c[2]
pub.publish(msg)
else:
max_vel = 0.5
min_dist = 0.03 #[m]
down_scale = 4
msg = Float64MultiArray()
#to lock the vehicle position
q = 0
w = 0
e = 0
r = 0
t = 0
msg.data = [q, w, e, r, t]
#PD CONTROL (position error)
# I want the pole in (-pole) / K_p; K_d = ??? (by test: K_p=0.4 - K_d=1.6)
# input to vehicle system: thrust [(rho*A*(v)^2)/a]
# output from vehicle system: position
K_p = 0.4
K_d = 1.6
#front velocity X - scaled down
#Uncomment the following line if you use the turns scenario
#vTp[0] = vTp[0]/down_scale
pos_err = vTp
err[:, 0] = err[:, 1]
err[0:3, 1] = nq.transpose(pos_err)
err[3, 1] = tan_err
#error derivate
