import sys, rone
## velocity_controller constants
ENCODER_MM_PER_TICKS = 0.0625
K_FEEDFORWARD = 0.0
K_FEEDFORWARD_OFFSET = 0.0
K_PROPORTIONAL = 0.0
K_INTEGRAL = 0.0
######## part a: open loop control ###############################################
def encoder_delta_ticks(new, old):
## Takes two encoder values, a new one and an old one, and returns
## the difference. Handles the wraparound of the encoders.
diff = new - old
if diff > 32768:
diff = diff - 65536
elif diff < -32768:
diff = diff + 65536
return diff
def compute_distance(ticks_new, ticks_old):
## start student code
distance = 0 # placeholder code - replace with your code
# end student code
return distance
def compute_velocity(distance, time):
## Takes distance in mm, time in msec
## returns velocity in mm/sec
## Note: cast distance and time to floats before using them
# Note 2: Don't divide by zero! Return 0 if you divide by zero
return 0 # placeholder code - replace with your code
def characterize_motors():
for pwm in range(0, 101, 10):
## start student code - comments are for hints
# Turn on the left and right motors with the current pwm
# measure the starting encoder positions
# run the motors for one second (sleep)
# measure the new encoder positions
# compute the distance
# compute the velocity
vel_left = 0 # placeholder code - replace with your code
vel_right = 0 # placeholder code - replace with your code
# end student code
print 'pwm = ', pwm, 'vel_left = ', vel_left, 'vel_right = ', vel_right
rone.motor_set_pwm('l',0)
rone.motor_set_pwm('r',0)
def bound(val, val_max):
## start student code - comments are for hints
val = 0 # placeholder code - replace with your code
# end student code
return val
def feedforward_compute(goal_vel):
# returns the PWM value computed by the feed-forward term
# NOTE: This function needs to return an *int* between -100 and 100
## to be written by the student
pwm = 0 # placeholder code - replace with your code
return pwm
def open_loop_motion(vel_left, vel_right, time):
## vel_left, vel_right are the goal wheel velocities
## time is the running time in milliseconds.
## Run the open loop controller for time milliseconds.
## use feedforward_compute(0 to compute the pwm values to send to each motor
## use sys.time() to know how long to run
## turn off the motors when you are done
pass ## to be written by the student
######## part b: closed loop control ############################################
def initialize(motor):
## motor arg is either 'l' or 'r'
ticks_old = rone.encoder_get_ticks(motor)
time_old = sys.time()
iterm_old = 0.0
distance_old = 0.0
return (ticks_old, time_old, iterm_old, distance_old)
def proportional_compute(vel_goal, vel_current):
## Takes the goal velocity and current velocity (mm/sec) as input
## Returns the p_term computed for the closed-loop controller
return 0 # placeholder code - replace with your code
def integral_compute(vel_goal, vel_current, i_term_curr):
## Takes the goal velocity, current velocity (mm/sec), and current
## i_term as input
## Returns the new i_term computed for the closed-loop controller
return 0 # placeholder code - replace with your code
def velocity_controller(motor, vel_goal, ticks_old, time_old, iterm_old, distance_old):
## vel_goal in mm/s
## motor either 'l' or 'r'
## iterm_old passed in from previous function runs (sum of the errors * ki)
## ticks_old is the old position of the given wheel
## time_old is the time of the last reading of the encoders
# placeholder code. Use these variable names, but delete this code once you finish this function
ticks_new = 0 # placeholder code
distance = 0 # placeholder code
time_new = 0 # placeholder code
time_delta = 0 # placeholder code
velocity = 0 # placeholder code
integral_term = 0 # placeholder code
pwm = 0 # placeholder code
# NOTE: the pwm value given to rone.motor_set_pwm() must be an int
# get encoder ticks
# compute distance
# compute velocity
# compute feedback terms
# student code end
# Some example debugging output. Don't print this always, it will slow things down too much
#print 'motor=%s ff_term=%5.1f i_term=%5.1f pwm=%3d' % (motor, float(feedforward_term), float(integral_term), pwm)
# update old values
ticks_old = ticks_new
time_old = time_new
iterm_old = integral_term
distance_old = distance_old + distance
return (ticks_old, time_old, iterm_old, distance_old)
def closed_loop_motion(vel_goal_l, vel_goal_r, time):
## goal velocities vel_goal_l and vel_goal_r (left and right) in mm/s,
## time in msec
## get starting positions for left and right:
(ticks_l, time_l, iterm_l, distance_l) = initialize('l')
(ticks_r, time_r, iterm_r, distance_r) = initialize('r')
## run loop for a certain amount of time
time_start = sys.time()
while sys.time() < (time_start + time):
## call velocity_controller controller for both wheels:
(ticks_l, time_l, iterm_l, distance_l) = velocity_controller('l', vel_goal_l, ticks_l, time_l, iterm_l, distance_l)
(ticks_r, time_r, iterm_r, distance_r) = velocity_controller('r', vel_goal_r, ticks_r, time_r, iterm_r, distance_r)
sys.sleep(30) ## sleep for a set time
## once out of the loop, turn off the motors (set pwm to 0):
rone.motor_set_pwm('r', 0)
rone.motor_set_pwm('l', 0)
######## misc: helper functions ############################################
def controller_test(controller, vel_left, vel_right, time, trials):
for i in range(0, trials):
while rone.button_get_value('r') == False:
rone.led_set_group('r',40)
rone.led_set_group('g',0)
rone.led_set_group('r',0)
rone.led_set_group('g',40)
controller(vel_left, vel_right, time)
rone.led_set_group('r',0)
rone.led_set_group('g',0)
print 'All done'
# Robot moves fast and may move in any direction - on its block or on the ground!
#characterize_motors()
#open_loop_motion(100, 100, 10000)
#closed_loop_motion(100, 100, 10000)
#controller_test(open_loop_motion, 100, 100, 10000, 10)
#controller_test(closed_loop_motion, 100, 100, 10000, 10)