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()
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()
