Esempio n. 1
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()
right_num_revs = 0.0
left_num_revs = 0.0
slave_power = 0.25
master_power = 0.25
target_power = 0.6
alpha = .95
while True:
    if (abs(right_num_revs)+abs(left_num_revs))/2.0 >= dist*d_mod:
        break
    try:    
        print(str(right_num_revs)+"  "+str(left_num_revs))
        while(master_power < target_power and (abs(right_num_revs)+abs(left_num_revs))/2.0 < dist*d_mod):
            master_power = alpha*master_power + (1-alpha)*target_power
            print(str(right_num_revs)+"  "+str(left_num_revs))
			#slave_power = alpha*master_power + (1-alpha)*target_power
            #motors.speed(slave_power, master_power)
            adjustMotorPowers_straight()
            motors.speed(slave_power, master_power)
            readEncoder_straight()
		
        print "got out of loop"
        adjustMotorPowers_straight()
        motors.speed(slave_power, master_power)
        #adjustMotorPowers_straight()
        readEncoder_straight()
    except KeyboardInterrupt:
        break

motors.cleanup()
Esempio n. 3
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()