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 main():
#default speed
speed = .1
while 1:
action = get_action()
if action == 'UP':
mlib.goXYOmegaWorld(speed,0)
elif action == 'LEFT':
mlib.goXYOmegaWorld(0,speed)
elif action == 'DOWN':
mlib.goXYOmegaWorld(-speed,0)
elif action == 'RIGHT':
mlib.goXYOmegaWorld(0,-speed)
elif action == 'SPIN_CCW':
mlib.goXYOmegaWorld(0,0,1+speed)
elif action == 'SPIN_CW':
mlib.goXYOmegaWorld(0,0,-1-speed)
elif action == 'STOP':
mlib.stop()
elif action == 'SPEED_DEC':
speed = speed - .1
print("Speed: ",speed)
elif action == 'SPEED_INC':
speed = speed + .1
print("Speed: ",speed)
elif action == 'KILL':
return
elif action == 'KICK':
mlib.kick()
sys.exit(1)
def main():
#default speed
speed = .1
while 1:
action = get_action()
if action == 'UP':
mlib.goXYOmegaWorld(speed, 0)
elif action == 'LEFT':
mlib.goXYOmegaWorld(0, speed)
elif action == 'DOWN':
mlib.goXYOmegaWorld(-speed, 0)
elif action == 'RIGHT':
mlib.goXYOmegaWorld(0, -speed)
elif action == 'SPIN_CCW':
mlib.goXYOmegaWorld(0, 0, 1 + speed)
elif action == 'SPIN_CW':
mlib.goXYOmegaWorld(0, 0, -1 - speed)
elif action == 'STOP':
mlib.stop()
elif action == 'SPEED_DEC':
speed = speed - .1
print("Speed: ", speed)
elif action == 'SPEED_INC':
speed = speed + .1
print("Speed: ", speed)
elif action == 'KILL':
return
elif action == 'KICK':
mlib.kick()
sys.exit(1)
