def move_backward(time):
# student code start
rone.motor_set_pwm('l', -MOTOR_PWM)
rone.motor_set_pwm('r', -MOTOR_PWM)
sys.sleep(time)
rone.motor_brake('l')
rone.motor_brake('r')
def move_forward(time):
# student code start
if time < 0:
rone.motor_set_pwm('r', 0 - MOTOR_PWM)
rone.motor_set_pwm('l', 0 - MOTOR_PWM)
else:
rone.motor_set_pwm('r', MOTOR_PWM)
rone.motor_set_pwm('l', MOTOR_PWM)
sys.sleep(abs(time))
def _velocity(motor):
## 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
vel_goal = _vcstate[motor]['goalvel']
iterm_old = _vcstate[motor]['iterm']
ticks_old = _vcstate[motor]['ticks']
time_old = _vcstate[motor]['time']
# compute distance
ticks_new = rone.encoder_get_ticks(motor)
distance = _compute_distance(ticks_new, ticks_old)
# compute velocity_controller
time_new = sys.time()
time_delta = time_new - time_old
velocity = _compute_velocity(distance, time_delta)
_vcstate[motor]['vel'] = velocity
# some debug printing. Don't leave it in, it slows down the computer
# compute feedback terms
error = vel_goal - velocity
feedforward_term = _feedforward_compute(vel_goal)
proportional_term = _proportional_compute(error)
integral_term = _integral_compute(error, iterm_old)
pwm = feedforward_term + proportional_term + integral_term
pwm = int(pwm)
pwm = clamp(pwm, 100)
rone.motor_set_pwm(motor, pwm)
#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
_vcstate[motor]['ticks'] = ticks_new
_vcstate[motor]['time'] = time_old = time_new
_vcstate[motor]['iterm'] = integral_term
def _velocity(motor):
## 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
vel_goal = _vcstate[motor]["goalvel"]
iterm_old = _vcstate[motor]["iterm"]
ticks_old = _vcstate[motor]["ticks"]
time_old = _vcstate[motor]["time"]
# compute distance
ticks_new = rone.encoder_get_ticks(motor)
distance = _compute_distance(ticks_new, ticks_old)
# compute velocity_controller
time_new = sys.time()
time_delta = time_new - time_old
velocity = _compute_velocity(distance, time_delta)
_vcstate[motor]["vel"] = velocity
# some debug printing. Don't leave it in, it slows down the computer
# compute feedback terms
error = vel_goal - velocity
feedforward_term = _feedforward_compute(vel_goal)
integral_term = _integral_compute(error, iterm_old)
proportional_term = _proportional_compute(error)
pwm = feedforward_term + proportional_term + integral_term
pwm = int(pwm)
pwm = clamp(pwm, 100)
rone.motor_set_pwm(motor, pwm)
# 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
_vcstate[motor]["ticks"] = ticks_new
_vcstate[motor]["time"] = time_old = time_new
_vcstate[motor]["iterm"] = integral_term
def back_rotate_left(time):
# student code start
rone.motor_set_pwm('r', -MOTOR_PWM)
rone.motor_brake('l')
sys.sleep(time)
rone.motor_brake('r')
def move_rotate_right(time):
# student code start
rone.motor_set_pwm('l', MOTOR_PWM)
rone.motor_brake('r')
sys.sleep(time)
rone.motor_brake('l')
def waypoint_motion():
velocity.init(0.22, 40, 0.5, 0.1)
leds.init()
poseX.init(pose_update)
motionX.init(compute_goal_distance_and_heading, motion_controller_tv,
motion_controller_rv)
pose_estimator_print_time = sys.time()
mode = MODE_INACTIVE
pose_old = (0.0, 0.0, 0.0)
waypoint_list = []
while True:
# update the LED animations
leds.update()
# update the pose estimator
poseX.update()
# update the motion controller
(tv, rv) = motionX.update()
velocity.set_tvrv(tv, rv)
# update the velocity controller if you are active, otherwise coast so the robot can be pushed
if mode == MODE_ACTIVE:
velocity.update()
else:
rone.motor_set_pwm('l', 0)
rone.motor_set_pwm('r', 0)
# print status every 500ms
current_time = sys.time()
if sys.time() > pose_estimator_print_time:
pose_estimator_print_time += 250
print 'goal', motionX.get_goal(), 'pose', poseX.get_pose(
), 'odo', poseX.get_odometer()
if mode == MODE_INACTIVE:
if (math2.pose_subtract(poseX.get_pose(), pose_old) !=
(0.0, 0.0, 0.0)):
# We're moving! Yay! Blink excitedly!
leds.set_pattern('r', 'blink_fast',
int(LED_BRIGHTNESS * 1.5))
else:
# not moving. sad face.
leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
pose_old = poseX.get_pose()
# check the buttons. If the red button is pressed, load the waypoint list
if rone.button_get_value('r'):
if mode == MODE_INACTIVE:
poseX.set_pose(0, 0, 0)
#waypoint_list = [(608, 0), (608, 304), (0, 304), (0, 0)]
waypoint_list = [(700, 0), (700, 700), (0, 700), (0, 0)]
mode = MODE_ACTIVE
# check to see if you are at your waypoint. If so, go to the next one
if mode == MODE_ACTIVE:
leds.set_pattern('g', 'blink_fast', LED_BRIGHTNESS)
if motionX.is_done():
## Do we have another waypoint?
if len(waypoint_list) > 0:
leds.set_pattern('rgb', 'group', LED_BRIGHTNESS)
sys.sleep(250)
waypoint = waypoint_list.pop(0)
print 'waypoint', waypoint
motionX.set_goal(waypoint, MOTION_TV)
else:
print 'waypoint list empty'
mode = MODE_INACTIVE
velocity.set_tvrv(0, 0)
def move_rotate_left(time):
# student code start
rone.motor_set_pwm('r', MOTOR_PWM)
rone.motor_set_pwm('l', 0)
sys.sleep(abs(time))
def move_stop(time):
# student code start
rone.motor_set_pwm('r', 0)
rone.motor_set_pwm('l', 0)
sys.sleep(abs(time))
