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