def roboControl(data):
## Decisions ##
if data.cmdType == 'movefast':
globals.xy_limit = globals.fast_limit
if data.x == data.x and data.y == data.y and data.theta == data.theta: # check for NaN
count = 0
vx, vy, omega = pid.robot_ctrl(data)
if vx != vx or vy != vy or omega != omega:
print("ERROR with NaN")
pid.reset()
return
# store last valid values in case of NaN
vx_valid = vx
vy_valid = vy
omega_valid = omega
# Go, baby, go
mlib.goXYOmegaWorld(vx, vy, omega, mlib.deg2rad(data.theta))
else: # we got a NaN
count = count + 1
if count < 50: # Only try and go backwards for a little bit
mlib.goXYOmegaWorld(-vx_valid, -vy_valid, -omega_valid,
mlib.deg2rad(data.theta))
else: # if you get NaNs for a while, just stop moving. There clearly a bigger issue.
mlib.stop()
elif data.cmdType == 'moveslow':
globals.xy_limit = globals.slow_limit
if data.x == data.x and data.y == data.y and data.theta == data.theta: # check for NaN
count = 0
vx, vy, omega = pid.robot_ctrl(data)
if vx != vx or vy != vy or omega != omega:
print("ERROR with NaN")
pid.reset()
return
# store last valid values in case of NaN
vx_valid = vx
vy_valid = vy
omega_valid = omega
# Go, baby, go
mlib.goXYOmegaWorld(vx, vy, omega, mlib.deg2rad(data.theta))
else:
count = count + 1
if count < 50: # Only try and go backwards for a little bit
mlib.goXYOmegaWorld(-vx_valid, -vy_valid, -omega_valid,
mlib.deg2rad(data.theta))
else: # if you get NaNs for a while, just stop moving. There clearly a bigger issue.
mlib.stop()
elif data.cmdType == 'kick':
if data.x == data.x and data.y == data.y and data.theta == data.theta: # check for NaN
mlib.kick()
elif data.cmdType == 'idle':
mlib.stop()
def roboControl(data):
## Decisions ##
if data.cmdType == 'movefast':
globals.xy_limit = globals.fast_limit
if data.x == data.x and data.y == data.y and data.theta == data.theta: # check for NaN
count = 0
vx, vy, omega = pid.robot_ctrl(data)
if vx != vx or vy != vy or omega != omega:
print("ERROR with NaN")
pid.reset()
return
# store last valid values in case of NaN
vx_valid = vx
vy_valid = vy
omega_valid = omega
# Go, baby, go
mlib.goXYOmegaWorld(vx,vy,omega,mlib.deg2rad(data.theta))
else: # we got a NaN
count = count + 1
if count < 50: # Only try and go backwards for a little bit
mlib.goXYOmegaWorld(-vx_valid,-vy_valid,-omega_valid,mlib.deg2rad(data.theta))
else: # if you get NaNs for a while, just stop moving. There clearly a bigger issue.
mlib.stop()
elif data.cmdType == 'moveslow':
globals.xy_limit = globals.slow_limit
if data.x == data.x and data.y == data.y and data.theta == data.theta: # check for NaN
count = 0
vx, vy, omega = pid.robot_ctrl(data)
if vx != vx or vy != vy or omega != omega:
print("ERROR with NaN")
pid.reset()
return
# store last valid values in case of NaN
vx_valid = vx
vy_valid = vy
omega_valid = omega
# Go, baby, go
mlib.goXYOmegaWorld(vx,vy,omega,mlib.deg2rad(data.theta))
else:
count = count + 1
if count < 50: # Only try and go backwards for a little bit
mlib.goXYOmegaWorld(-vx_valid,-vy_valid,-omega_valid,mlib.deg2rad(data.theta))
else: # if you get NaNs for a while, just stop moving. There clearly a bigger issue.
mlib.stop()
elif data.cmdType == 'kick':
if data.x == data.x and data.y == data.y and data.theta == data.theta: # check for NaN
mlib.kick()
elif data.cmdType == 'idle':
mlib.stop()
def callback2(data):
# print data
if data.cmdType == 'mov' and data.x == data.x:
msg = Vector3()
msg.x, msg.y, msg.z = pid.robot_ctrl(data)
P2.pub.publish(msg)
def callback2(data):
# print data
if data.cmdType == "mov" and data.x == data.x:
msg = Vector3()
msg.x, msg.y, msg.z = pid.robot_ctrl(data)
P2.pub.publish(msg)
