Beispiel #1
0
def _handle_motion_timer():
    global _set_speed, _velocityhat
    if _set_speed:
        motion.drive(*_velocities, smooth=_smooth, theta=_thetahat)
        _set_speed = False

    (vx, vy, w, s1, s2, s3) = motion.get_velocities(theta=_thetahat)
    _velocityhat = (vx, vy, w, s1, s2, s3)
Beispiel #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()
Beispiel #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()
Beispiel #4
0
def square():
    motion.drive(.5, 0, 0)
    time.sleep(1)

    motion.stop()
    time.sleep(0.5)

    motion.drive(0, .5, 0)
    time.sleep(1)

    motion.stop()
    time.sleep(0.5)

    motion.drive(-.5, 0, 0)
    time.sleep(1)

    motion.stop()
    time.sleep(0.5)

    motion.drive(0, -.5, 0)
    time.sleep(1)

    motion.stop()
Beispiel #5
0
def square():
	motion.drive(.5, 0, 0)
	time.sleep(1)

	motion.stop()
	time.sleep(0.5)

	motion.drive(0, .5, 0)
	time.sleep(1)

	motion.stop()
	time.sleep(0.5)

	motion.drive(-.5, 0, 0)
	time.sleep(1)

	motion.stop()
	time.sleep(0.5)

	motion.drive(0, -.5, 0)
	time.sleep(1)

	motion.stop()
Beispiel #6
0
def _go_home():
    # Get current robot position, rounded to 1 decimal place
    xhat = round(_xhat, 1)
    yhat = round(_yhat, 1)
    thetahat = round(_thetahat, 1)

    # Set a tolerance
    tolerance = 0.1

    if abs(xhat) > tolerance:
        dt = abs(xhat / _vx)
        sign = -1 if xhat > 0 else 1
        print("motion.drive({},{},{}) for {} s\r".format(sign*_vx, 0, 0, dt))
        motion.drive(sign*_vx, 0, 0)
        time.sleep(dt)
        motion.stop()
        time.sleep(0.5)

    if abs(yhat) > tolerance:
        dt = abs(yhat / _vy)
        sign = -1 if yhat > 0 else 1
        print("motion.drive({},{},{}) for {} s\r".format(0, sign*_vy, 0, dt))
        motion.drive(0, sign*_vy, 0)
        time.sleep(dt)
        motion.stop()
        time.sleep(0.5)

    if abs(thetahat) > tolerance*100:
        # since it's periodic...
#        thetahat = round(thetahat%(360) ,2)
        dt = abs(thetahat / _w)
        sign = -1 if thetahat > 0 else 1
        print("motion.drive({},{},{}) for {} s\r".format(0, 0, sign*_w, dt))
        motion.drive(0, 0, sign*_w)
        time.sleep(dt)
        motion.stop()
        time.sleep(0.5)
Beispiel #7
0
def spin():
    motion.drive(0, 0, 2 * np.pi)

    time.sleep(3)

    motion.stop()
Beispiel #8
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()
Beispiel #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()
Beispiel #10
0
def spin():
	motion.drive(0, 0, 2*np.pi)

	time.sleep(3)

	motion.stop()
Beispiel #11
0
def _handle_velocity_command(msg):
    velocities = (msg.linear.x, msg.linear.y, msg.angular.z)
    motion.drive(*velocities, smooth=_smooth, theta=_theta)