Exemplo n.º 1
0
def main():
    rospy.init_node('motion', anonymous=False)

    if rospy.get_param('/simulation_mode', False):
        print "[motion] Bye!"
        return

    # Get this robot's motor's QPPS from rosparam
    m1 = rospy.get_param('M1QPPS', None)
    m2 = rospy.get_param('M2QPPS', None)
    m3 = rospy.get_param('M3QPPS', None)

    # Does this robot use v3 or v5 RoboClaws?
    use_rcv3 = rospy.get_param('use_rcv3', True)

    # init wheelbase
    wheelbase.init(m1qpps=m1, m2qpps=m2, m3qpps=m3, use_rcv3=use_rcv3)

    # Register a shutdown hook to kill motion
    rospy.on_shutdown(_shutdown_hook)

    rospy.Subscriber('vel_cmds', Twist, _handle_velocity_command)
    pub = rospy.Publisher('encoder_estimates', EncoderEstimates, queue_size=10)

    # Services
    rospy.Service('battery', RoboClawRPC, _get_battery_voltage)
    rospy.Service('kick', Trigger, _kick)

    # So that we know the robot's theta
    rospy.Subscriber('robot_state', RobotState, _handle_theta)

    rospy.spin()
    return

    rate = rospy.Rate(10) # 100hz
    while not rospy.is_shutdown():


        (vx, vy, w, s1, s2, s3) = motion.get_velocities(theta=_theta)
        estimate = EncoderEstimates()
        estimate.world_velocities.vx = vx
        estimate.world_velocities.vy = vy
        estimate.world_velocities.w  = w
        estimate.pulses.s1 = s1
        estimate.pulses.s2 = s2
        estimate.pulses.s3 = s3
        pub.publish(estimate)
            
        rate.sleep()

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()
Exemplo n.º 2
0
def test_square_calibration(velocity=0.6,
                            sleep_time=1.5,
                            M1QPPS=None,
                            M2QPPS=None,
                            M3QPPS=None):
    global wheelbase_configured

    # Make sure everything is init'd
    if not wheelbase_configured:
        w.init()
        wheelbase_configured = True

    _m1qpps = M1QPPS if M1QPPS is not None else qppsM1
    _m2qpps = M2QPPS if M2QPPS is not None else qppsM2
    _m3qpps = M3QPPS if M3QPPS is not None else qppsM3

    if _m1qpps is not None and _m2qpps is not None and _m3qpps is not None:
        w.SetVelocityPID(w.M1, kp, ki, kd, _m1qpps)
        w.SetVelocityPID(w.M2, kp, ki, kd, _m2qpps)
        w.SetVelocityPID(w.M3, kp, ki, kd, _m3qpps)

    # Right
    motion.drive(velocity, 0, 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Up
    motion.drive(0, velocity, 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Left
    motion.drive(-velocity, 0, 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Down
    motion.drive(0, -velocity, 0)
    time.sleep(sleep_time)

    motion.stop()
Exemplo n.º 3
0
def test_square_calibration(velocity=0.6, sleep_time=1.5, M1QPPS=None, M2QPPS=None, M3QPPS=None):
    global wheelbase_configured

    # Make sure everything is init'd
    if not wheelbase_configured:
        w.init()
        wheelbase_configured = True

    _m1qpps = M1QPPS if M1QPPS is not None else qppsM1
    _m2qpps = M2QPPS if M2QPPS is not None else qppsM2
    _m3qpps = M3QPPS if M3QPPS is not None else qppsM3

    if _m1qpps is not None and _m2qpps is not None and _m3qpps is not None:
        w.SetVelocityPID(w.M1, kp, ki, kd, _m1qpps)
        w.SetVelocityPID(w.M2, kp, ki, kd, _m2qpps)
        w.SetVelocityPID(w.M3, kp, ki, kd, _m3qpps)

    # Right
    motion.drive(velocity, 0, 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Up
    motion.drive(0, velocity, 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Left
    motion.drive(-velocity, 0, 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Down
    motion.drive(0, -velocity, 0)
    time.sleep(sleep_time)

    motion.stop()
Exemplo n.º 4
0
def main():
    global _motion_timer
    _motion_timer = RepeatedTimer(_motion_timer_period, _handle_motion_timer)
    _motion_timer.start()

    global _odom_timer
    _odom_timer = RepeatedTimer(_odom_timer_period, _handle_odom_timer)
    _odom_timer.start()

    global _ctrl_timer
    _ctrl_timer = RepeatedTimer(_ctrl_timer_period, _handle_ctrl_timer)
    _ctrl_timer.start()

    use_rcv3 = os.environ['USE_RCV3'] == 'true'
    w.init(use_rcv3=use_rcv3)

    # TODO: Okay, so Controller.init(None) will automagically load in some PID 
    # values for each of the x, y, theta position controllers. However, since
    # `teleop` is run in real life on different robots, really it should read
    # the correct YAML robot config file and load in the PID constants and
    # pass them into Controller.init(gains) as in the controller_node.py.
    # Two ways to do this: (1) do like os.environ['USE_RCV3'], where the odroid_setup.bash
    # script loads up the PID constants as environment variables (this seems messy).
    # (2) find a python YAML reading library and just read the file directly from here
    # based on which robot this is (you can use os.environ['ROBOT'])
    Controller.init()
    
    print 'Started.'


    global _velocities, _set_speed, _smooth, _odom_on, _previous_action, _ctrl_on

    while(1):
        action = get_action()
        if action == 'UP':
            _set_speed = True
            _velocities = (0, _vy, 0)

        elif action == 'DOWN':
            _set_speed = True
            _velocities = (0, -_vy, 0)

        elif action == 'RIGHT':
            _set_speed = True
            _velocities = (_vx, 0, 0)

        elif action == 'LEFT':
            _set_speed = True
            _velocities = (-_vx, 0, 0)

        elif action == 'SPIN_CW':
            _set_speed = True
            _velocities = (0, 0, _w)

        elif action == 'SPIN_CCW':
            _set_speed = True
            _velocities = (0, 0, -_w)

        elif action == 'SET_HOME':
            Odometry.init()

        elif action == 'GO_HOME':
            _motion_timer.stop()
            motion.stop()
            time.sleep(1)

            _go_home()

            time.sleep(1)
            _set_speed = True
            _velocities = (0, 0, 0)
            _motion_timer.start()

        elif action == 'GO_TO_POINT':
            _toggle_timers(False)
            motion.stop()
            time.sleep(1)

            _ask_for_point()

            time.sleep(1)
            _ctrl_on = True
            _odom_on = True
            _toggle_timers(True)

        elif action == 'TOGGLE_CNTRL':
            _ctrl_on = not _ctrl_on
            print("Controller: {}".format(_ctrl_on))

        elif action == 'TOGGLE_SMOOTH':
            _smooth = not _smooth
            print("Smooth: {}".format(_smooth))

        elif action == 'TOGGLE_ODOM':
            _odom_on = not _odom_on
            print("Odom: {}".format(_odom_on))

        elif action == 'BATTERY':
            print("\n\r*** Battery: {} ***\n\r".format(get_battery()))

        elif action == 'BREAKPOINT':
            _toggle_timers(False)
            import ipdb; ipdb.set_trace()
            _toggle_timers(True)

        elif action == 'CALIBRATION_MODE':
            _toggle_timers(False)

            _deal_with_calibration()

            time.sleep(1)
            _toggle_timers(True)

        elif action == 'DIE':
            _toggle_timers(False)
            motion.stop()
            w.kill()
            return sys.exit(0)

        else:
            _set_speed = True
            _velocities = (0, 0, 0)

        # handle stopping before changing directions
        if _action_requires_stop(action):
            _set_speed = False
            motion.stop()
            time.sleep(0.4)
            _set_speed = True

        _previous_action = action

        print "{}\r".format(_velocities)
Exemplo n.º 5
0
def init():
    w.init(set_PID=False)
Exemplo n.º 6
0
def calibrate(speed=48, sleep_time=2, set_PID=True):
    speedM1Forward = 0
    speedM1Backward = 0
    speedM2Forward = 0
    speedM2Backward = 0
    speedM3Forward = 0
    speedM3Backward = 0

    # Should I use rcv3 or rcv5?
    use_rcv3 = os.environ['USE_RCV3'] == 'true'

    # Initialize wheelbase with PID
    w.init(use_rcv3=use_rcv3, set_PID=False)
    global wheelbase_configured
    wheelbase_configured = True

    w.kill()
    _print_stats()

    speedM1_f = 60
    speedM1_b = 50
    speedM2_f = 55
    speedM2_b = 55
    speedM3_f = 55
    speedM3_b = 55

    # Forward (at 0 degrees)
    w.Backward(w.M1, speedM1_b)  # M1 backward sample 1
    w.Forward(w.M3, speedM3_f)  # M3 forward sample 1
    time.sleep(sleep_time)

    speedM1Backward = speedM1Backward + _read(w.M1)
    speedM3Forward = speedM3Forward + _read(w.M3)

    w.kill()
    time.sleep(1)

    # Backward (at 180 degrees)
    w.Forward(w.M1, speedM1_f)  # M1 forward sample 1
    w.Backward(w.M3, speedM3_b)  # M3 backward sample 1
    time.sleep(sleep_time)

    speedM1Forward = speedM1Forward + _read(w.M1)
    speedM3Backward = speedM3Backward + _read(w.M3)

    w.kill()
    time.sleep(1)

    # Right back (at -120 degrees)
    w.Backward(w.M3, speedM3_b)  # M3 backward sample 2
    w.Forward(w.M2, speedM2_f)  # M2 forward sample 1
    time.sleep(sleep_time)

    speedM3Backward = speedM3Backward + _read(w.M3)
    # speedM3Backward = speedM3Backward/2
    speedM2Forward = speedM2Forward + _read(w.M2)

    w.kill()
    time.sleep(1)

    # Right forward (at 120 degrees)
    w.Forward(w.M3, speedM3_f)  # M3 forward sample 2
    w.Backward(w.M2, speedM2_b)  # M2 backward sample 1
    time.sleep(sleep_time)

    speedM3Forward = speedM3Forward + _read(w.M3)
    # speedM3Forward  = speedM3Forward/2
    speedM2Backward = speedM2Backward + _read(w.M2)

    w.kill()
    time.sleep(1)

    # Left Back (at 120 degrees)
    w.Forward(w.M1, speedM1_f)  # M1 forward sample 2
    w.Backward(w.M2, speedM2_b)  # M2 backward sample 2
    time.sleep(sleep_time)

    speedM1Forward = speedM1Forward + _read(w.M1)
    # speedM1Forward  = speedM1Forward/2
    speedM2Backward = speedM2Backward + _read(w.M2)
    # speedM2Backward = speedM2Backward/2

    w.kill()
    time.sleep(1)

    # Left Forward (at -120 degrees)
    w.Backward(w.M1, speedM1_b)  # M1 backward sample 2
    w.Forward(w.M2, speedM2_f)  # M2 forward sample 2
    time.sleep(sleep_time)

    speedM1Backward = speedM1Backward + _read(w.M1)
    # speedM1Backward = speedM1Backward/2
    speedM2Forward = speedM2Forward + _read(w.M2)
    # speedM2Forward  = speedM2Forward/2

    w.kill()

    speedM1Forward = (speedM1Forward * MAX_PWM) / speedM1_f
    speedM1Backward = (speedM1Backward * MAX_PWM) / speedM1_b
    speedM2Forward = (speedM2Forward * MAX_PWM) / speedM2_f
    speedM2Backward = (speedM2Backward * MAX_PWM) / speedM2_b
    speedM3Forward = (speedM3Forward * MAX_PWM) / speedM3_f
    speedM3Backward = (speedM3Backward * MAX_PWM) / speedM3_b

    global qppsM1
    global qppsM2
    global qppsM3

    qppsM1 = (speedM1Forward + abs(speedM1Backward)) / 2
    qppsM2 = (speedM2Forward + abs(speedM2Backward)) / 2
    qppsM3 = (speedM3Forward + abs(speedM3Backward)) / 2

    print "M1QPPS={},M2QPPS={},M3QPPS={}".format(qppsM1, qppsM2, qppsM3)

    if set_PID:
        w.SetVelocityPID(w.M1, kp, ki, kd, qppsM1)
        w.SetVelocityPID(w.M2, kp, ki, kd, qppsM2)
        w.SetVelocityPID(w.M3, kp, ki, kd, qppsM3)

        _print_stats()
Exemplo n.º 7
0
def test_calibration(velocity=0.6,
                     sleep_time=1.5,
                     M1QPPS=None,
                     M2QPPS=None,
                     M3QPPS=None):
    global wheelbase_configured

    # Make sure everything is init'd
    if not wheelbase_configured:
        w.init()
        wheelbase_configured = True

    _m1qpps = M1QPPS if M1QPPS is not None else qppsM1
    _m2qpps = M2QPPS if M2QPPS is not None else qppsM2
    _m3qpps = M3QPPS if M3QPPS is not None else qppsM3

    if _m1qpps is not None and _m2qpps is not None and _m3qpps is not None:
        w.UpdateVelocityPID(w.M1, q=_m1qpps)
        w.UpdateVelocityPID(w.M2, q=_m2qpps)
        w.UpdateVelocityPID(w.M3, q=_m3qpps)

    _print_stats()

    print("velocity: {}\r".format(velocity))

    # Forward
    motion.drive(velocity, 0, 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Backward
    motion.drive(-velocity, 0, 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Left back
    motion.drive(velocity * np.cos(2 * np.pi / 3),
                 velocity * np.sin(2 * np.pi / 3), 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Right forward
    motion.drive(-np.cos(2 * np.pi / 3) * velocity,
                 -np.sin(2 * np.pi / 3) * velocity, 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Right back
    motion.drive(
        np.cos(-2 * np.pi / 3) * velocity,
        np.sin(-2 * np.pi / 3) * velocity, 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Right Forward
    motion.drive(-np.cos(-2 * np.pi / 3) * velocity,
                 -np.sin(-2 * np.pi / 3) * velocity, 0)
    time.sleep(sleep_time)

    motion.stop()
Exemplo n.º 8
0
def calibrate(speed=48, sleep_time=2, set_PID=True):
    speedM1Forward = 0
    speedM1Backward = 0
    speedM2Forward = 0
    speedM2Backward = 0
    speedM3Forward = 0
    speedM3Backward = 0

    # Should I use rcv3 or rcv5?
    use_rcv3 = os.environ["USE_RCV3"] == "true"

    # Initialize wheelbase with PID
    w.init(use_rcv3=use_rcv3, set_PID=False)
    global wheelbase_configured
    wheelbase_configured = True

    w.kill()
    _print_stats()

    speedM1_f = 60
    speedM1_b = 50
    speedM2_f = 55
    speedM2_b = 55
    speedM3_f = 55
    speedM3_b = 55

    # Forward (at 0 degrees)
    w.Backward(w.M1, speedM1_b)  # M1 backward sample 1
    w.Forward(w.M3, speedM3_f)  # M3 forward sample 1
    time.sleep(sleep_time)

    speedM1Backward = speedM1Backward + _read(w.M1)
    speedM3Forward = speedM3Forward + _read(w.M3)

    w.kill()
    time.sleep(1)

    # Backward (at 180 degrees)
    w.Forward(w.M1, speedM1_f)  # M1 forward sample 1
    w.Backward(w.M3, speedM3_b)  # M3 backward sample 1
    time.sleep(sleep_time)

    speedM1Forward = speedM1Forward + _read(w.M1)
    speedM3Backward = speedM3Backward + _read(w.M3)

    w.kill()
    time.sleep(1)

    # Right back (at -120 degrees)
    w.Backward(w.M3, speedM3_b)  # M3 backward sample 2
    w.Forward(w.M2, speedM2_f)  # M2 forward sample 1
    time.sleep(sleep_time)

    speedM3Backward = speedM3Backward + _read(w.M3)
    # speedM3Backward = speedM3Backward/2
    speedM2Forward = speedM2Forward + _read(w.M2)

    w.kill()
    time.sleep(1)

    # Right forward (at 120 degrees)
    w.Forward(w.M3, speedM3_f)  # M3 forward sample 2
    w.Backward(w.M2, speedM2_b)  # M2 backward sample 1
    time.sleep(sleep_time)

    speedM3Forward = speedM3Forward + _read(w.M3)
    # speedM3Forward  = speedM3Forward/2
    speedM2Backward = speedM2Backward + _read(w.M2)

    w.kill()
    time.sleep(1)

    # Left Back (at 120 degrees)
    w.Forward(w.M1, speedM1_f)  # M1 forward sample 2
    w.Backward(w.M2, speedM2_b)  # M2 backward sample 2
    time.sleep(sleep_time)

    speedM1Forward = speedM1Forward + _read(w.M1)
    # speedM1Forward  = speedM1Forward/2
    speedM2Backward = speedM2Backward + _read(w.M2)
    # speedM2Backward = speedM2Backward/2

    w.kill()
    time.sleep(1)

    # Left Forward (at -120 degrees)
    w.Backward(w.M1, speedM1_b)  # M1 backward sample 2
    w.Forward(w.M2, speedM2_f)  # M2 forward sample 2
    time.sleep(sleep_time)

    speedM1Backward = speedM1Backward + _read(w.M1)
    # speedM1Backward = speedM1Backward/2
    speedM2Forward = speedM2Forward + _read(w.M2)
    # speedM2Forward  = speedM2Forward/2

    w.kill()

    speedM1Forward = (speedM1Forward * MAX_PWM) / speedM1_f
    speedM1Backward = (speedM1Backward * MAX_PWM) / speedM1_b
    speedM2Forward = (speedM2Forward * MAX_PWM) / speedM2_f
    speedM2Backward = (speedM2Backward * MAX_PWM) / speedM2_b
    speedM3Forward = (speedM3Forward * MAX_PWM) / speedM3_f
    speedM3Backward = (speedM3Backward * MAX_PWM) / speedM3_b

    global qppsM1
    global qppsM2
    global qppsM3

    qppsM1 = (speedM1Forward + abs(speedM1Backward)) / 2
    qppsM2 = (speedM2Forward + abs(speedM2Backward)) / 2
    qppsM3 = (speedM3Forward + abs(speedM3Backward)) / 2

    print "M1QPPS={},M2QPPS={},M3QPPS={}".format(qppsM1, qppsM2, qppsM3)

    if set_PID:
        w.SetVelocityPID(w.M1, kp, ki, kd, qppsM1)
        w.SetVelocityPID(w.M2, kp, ki, kd, qppsM2)
        w.SetVelocityPID(w.M3, kp, ki, kd, qppsM3)

        _print_stats()
Exemplo n.º 9
0
def test_calibration(velocity=0.6, sleep_time=1.5, M1QPPS=None, M2QPPS=None, M3QPPS=None):
    global wheelbase_configured

    # Make sure everything is init'd
    if not wheelbase_configured:
        w.init()
        wheelbase_configured = True

    _m1qpps = M1QPPS if M1QPPS is not None else qppsM1
    _m2qpps = M2QPPS if M2QPPS is not None else qppsM2
    _m3qpps = M3QPPS if M3QPPS is not None else qppsM3

    if _m1qpps is not None and _m2qpps is not None and _m3qpps is not None:
        w.UpdateVelocityPID(w.M1, q=_m1qpps)
        w.UpdateVelocityPID(w.M2, q=_m2qpps)
        w.UpdateVelocityPID(w.M3, q=_m3qpps)

    _print_stats()

    print ("velocity: {}\r".format(velocity))

    # Forward
    motion.drive(velocity, 0, 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Backward
    motion.drive(-velocity, 0, 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Left back
    motion.drive(velocity * np.cos(2 * np.pi / 3), velocity * np.sin(2 * np.pi / 3), 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Right forward
    motion.drive(-np.cos(2 * np.pi / 3) * velocity, -np.sin(2 * np.pi / 3) * velocity, 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Right back
    motion.drive(np.cos(-2 * np.pi / 3) * velocity, np.sin(-2 * np.pi / 3) * velocity, 0)
    time.sleep(sleep_time)

    motion.stop()
    time.sleep(sleep_time)

    # Right Forward
    motion.drive(-np.cos(-2 * np.pi / 3) * velocity, -np.sin(-2 * np.pi / 3) * velocity, 0)
    time.sleep(sleep_time)

    motion.stop()
Exemplo n.º 10
0
def init():
    w.init(set_PID=False)