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)