Esempio n. 1
0
def getAvgSpeed(motor_id,speed):

	# Stop all motors and reset encoder counts
	w.kill()
	w.ResetEncoders(motor_id)

	# Move forward
	w.Forward(motor_id, speed)

	# Let the motor ramp up and get stable
	time.sleep(1)

	# how many samples should I get?
	#N = 10

	# how many seconds to sample over?
	Tmax = 5

	Ts = 0.100 # 100 ms, sample rate

	# Based on that, how many samples will I be getting?
	N = int(math.floor(Tmax/Ts))

	samples = []
	while len(samples)<N:
		samples.append(s(addr)[1])
		time.sleep(Ts)
	
	avg = sum(samples)/N

	# Stop motors
	w.kill()

	return (avg,N)
Esempio n. 2
0
def getAvgSpeed(motor_id, speed):

    # Stop all motors and reset encoder counts
    w.kill()
    w.ResetEncoders(motor_id)

    # Move forward
    w.Forward(motor_id, speed)

    # Let the motor ramp up and get stable
    time.sleep(1)

    # how many samples should I get?
    #N = 10

    # how many seconds to sample over?
    Tmax = 5

    Ts = 0.100  # 100 ms, sample rate

    # Based on that, how many samples will I be getting?
    N = int(math.floor(Tmax / Ts))

    samples = []
    while len(samples) < N:
        samples.append(s(addr)[1])
        time.sleep(Ts)

    avg = sum(samples) / N

    # Stop motors
    w.kill()

    return (avg, N)
Esempio n. 3
0
def kill():
    """Kill
    Call this only if you don't care about how the bot stops
    """
    w.kill()
Esempio 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)
Esempio n. 5
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()
Esempio n. 6
0
def kill():
    """Kill
    Call this only if you don't care about how the bot stops
    """
    w.kill()
Esempio n. 7
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()