Exemplo n.º 1
0
def readEncoder():
    global en_left
    global en_right
    global right_num_revs
    global left_num_revs

    new_en_left, new_en_right = encoders.read()
    if new_en_right != en_right or new_en_left != en_left:
        en_right = new_en_right
        right_num_revs += en_right
        en_left = new_en_left
        left_num_revs += en_left
def readEncoder_straight():
	global en_left
	global en_right
	global right_num_revs
	global left_num_revs

	new_en_left, new_en_right = encoders.read()
	if(new_en_right != en_right or new_en_left != en_left):
		en_right = new_en_right
		right_num_revs = en_right
		en_left = new_en_left
		left_num_revs = en_left		
slave_power = -.25
right_num_revs = 0
left_num_revs = 0
kp = .5

#distance modifier
d_mod = .95

turns= angle/360.0*math.pi*diameter/wheel_circumference
dist = distance/wheel_circumference

encoders.init()
encoders.clear()
motors.init()

en_left, en_right = encoders.read()

SETTINGS_FILE = "RTIMULib"

def adjustMotorPowers():
	global slave_power
	global en_left
	global en_right
	global kp

	error = en_right + en_left
	slave_power -= error/kp
	encoders.clear()
	time.sleep(.1)

def readEncoder():
Exemplo n.º 4
0
def go(given_angle, given_distance):
    angle = given_angle
    distance = given_distance
    curr_time = time.time()
    next_state = False

    #constants in feet
    diameter = 11.25
    wheel_circumference = 16.72

    #right is master, left is slave
    global slave_power
    global master_power
    master_power = .25
    slave_power = -.25
    global right_num_revs
    right_num_revs = 0
    global left_num_revs
    left_num_revs = 0
    global kp
    kp = .5

    #distance modifier
    d_mod = .95

    turns = angle / 360.0 * math.pi * diameter / wheel_circumference
    dist = distance / wheel_circumference

    encoders.init()
    encoders.clear()
    motors.init()

    global en_right
    global en_left
    en_left, en_right = encoders.read()

    while True:
        try:
            if (abs(right_num_revs) + abs(left_num_revs)
                ) / 2.0 >= turns * d_mod and not next_state:
                print slave_power
                print master_power
                curr_time = time.time()
                next_state = True

                if time.time() > (curr_time + 0) and next_state:
                    break

            motors.speed(slave_power, master_power)
            adjustMotorPowers()
            readEncoder()
        except KeyboardInterrupt:
            break

    motors.speed(0, 0)
    time.sleep(1)

    kill_constant = 5
    right_num_revs = 0.0
    left_num_revs = 0.0
    slave_power = 0.233
    master_power = 0.25

    while True:
        if (abs(right_num_revs) + abs(left_num_revs)) / 2.0 >= dist * d_mod:
            break
        try:
            if (abs(left_num_revs) - abs(right_num_revs)) > kill_threshold:
                master_power += .95 * master_power + .05 * kill_constant
            print(str(right_num_revs) + "  " + str(left_num_revs))
            motors.speed(slave_power, master_power)
            adjustMotorPowers_straight()
            readEncoder_straight()
            print "Slave Speed: " + str(slave_power), "Master Speed: " + str(
                master_power)
        except KeyboardInterrupt:
            break

    motors.cleanup()
Exemplo n.º 5
0
"""
This file tests the interface for driving the motor encoders.
Filename: test_encoders.py
Author: Matthew Yu
Last Modified: 2/21/20
Notes:
    * Proposed operation:
        setup()
        ...
        shutdown()
"""
import encoders as e

e.setup()
input = input("Press enter to quit\n\n")
e.read(0) # should return an error message
print(str(e.read(1))) # should return 0 since motor isn't moving.
e.reset(0) # should return an error message
e.reset(1) # should work. Read should be 0 now if it wasn't already.
print(str(e.read(1)))
e.shutdown()
Exemplo n.º 6
0
entries = []

# up freq and repeat
for f in range(50, 51):  # 1-10
    # modify freq
    motor_controller.setup(config.MOTOR_PWM_FREQ * f)

    # up duty and repeat
    for d in range(1, 11):  # 0-10
        # modify power
        motor_controller.set_speed(config.MOTOR_PWM_DUTY * d)

        for i in range(4):  # repeat trial 5 times
            # drive forward
            motor_controller.drive_forward(.5)  # 1 second
            print("Ticks: " + str(encoders.read(
                config.FRONT_LEFT)))  # reading from encoder Back Right

            # append to dataset
            entries.append([
                config.MOTOR_PWM_FREQ * f, config.MOTOR_PWM_DUTY * d,
                encoders.read(config.FRONT_LEFT)
            ])

            # reset ticks
            encoders.reset(config.FRONT_LEFT)
            t.sleep(1)

    t.sleep(1)
    # restart motor controller
    motor_controller.shutdown()
Exemplo n.º 7
0
def go(given_angle, given_distance):
	angle = given_angle
	distance = given_distance
	curr_time = time.time()
	next_state = False
	
	#constants in feet
	diameter = 11.25
	wheel_circumference = 16.72

	#right is master, left is slave
	global slave_power
	global master_power
	master_power = .25
	slave_power = -.25
	global right_num_revs
	right_num_revs= 0
	global left_num_revs
	left_num_revs = 0
	global kp
	kp = .5

	#distance modifier
	d_mod = .95

	turns= angle/360.0*math.pi*diameter/wheel_circumference
	dist = distance/wheel_circumference
	
	encoders.init()
	encoders.clear()
	motors.init()
	
	global en_right
	global en_left
	en_left, en_right = encoders.read()


	while True:
		try:
			if(abs(right_num_revs)+abs(left_num_revs))/2.0 >= turns*d_mod and not next_state:
				print slave_power
				print master_power
				curr_time=time.time()
				next_state = True
			
				if time.time()>(curr_time + 0) and next_state:
					break
		
			motors.speed(slave_power, master_power)
			adjustMotorPowers()
			readEncoder()
		except KeyboardInterrupt:
			break

	motors.speed(0,0)
	time.sleep(1)

	kill_constant = 5
	right_num_revs = 0.0
	left_num_revs = 0.0
	slave_power = 0.233
	master_power = 0.25

	while True:
		if (abs(right_num_revs)+abs(left_num_revs))/2.0 >= dist*d_mod:
			break
		try:    
			if(abs(left_num_revs)-abs(right_num_revs)) > kill_threshold:
				master_power += .95*master_power + .05*kill_constant
			print(str(right_num_revs)+"  "+str(left_num_revs))
			motors.speed(slave_power, master_power)
			adjustMotorPowers_straight()
			readEncoder_straight()
			print "Slave Speed: "+str(slave_power), "Master Speed: "+str(master_power)
		except KeyboardInterrupt:
			break

	motors.cleanup()