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)