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)
Beispiel #2
0
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)
Beispiel #3
0
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()