# alexband/Udacity-373

twiddle

huanghuang committed Mar 25, 2012
1 parent 8e741ef commit ce2ec7aeb9f4dd03016489d67e73b031224bc868
Showing with 378 additions and 2 deletions.
1. +2 −2 5-11.py
2. +171 −0 5-14.py
3. +205 −0 twiddle.py
 @@ -138,7 +138,7 @@ def __repr__(self): def run(param1, param2): myrobot = robot() myrobot.set(0.0, 1.0, 0.0) - #myrobot.set_steering_drift(10.0 / 180.8 * pi) + myrobot.set_steering_drift(10.0 / 180.8 * pi) speed = 1.0 # motion distance is equal to speed (we assume time = 1) N = 100 cte = myrobot.y @@ -154,7 +154,7 @@ def run(param1, param2): # # Call your function with parameters of 0.2 and 3.0 and print results -run(0.2, 3.0) +run(0.2, 4.0)
171 5-14.py
 @@ -0,0 +1,171 @@ +# ----------- +# User Instructions +# +# Implement a P controller by running 100 iterations +# of robot motion. The steering angle should be set +# by the parameter tau so that: +# +# steering = -tau_p * CTE - tau_d * diff_CTE - tau_i * int_CTE +# +# where the integrated crosstrack error (int_CTE) is +# the sum of all the previous crosstrack errors. +# This term works to cancel out steering drift. +# +# Your code should print a list that looks just like +# the list shown in the video. +# +# Only modify code at the bottom! +# ------------ + +from math import * +import random + + +# ------------------------------------------------ +# +# this is the robot class +# + +class robot: + + # -------- + # init: + # creates robot and initializes location/orientation to 0, 0, 0 + # + + def __init__(self, length = 20.0): + self.x = 0.0 + self.y = 0.0 + self.orientation = 0.0 + self.length = length + self.steering_noise = 0.0 + self.distance_noise = 0.0 + self.steering_drift = 0.0 + + # -------- + # set: + # sets a robot coordinate + # + + def set(self, new_x, new_y, new_orientation): + + self.x = float(new_x) + self.y = float(new_y) + self.orientation = float(new_orientation) % (2.0 * pi) + + + # -------- + # set_noise: + # sets the noise parameters + # + + def set_noise(self, new_s_noise, new_d_noise): + # makes it possible to change the noise parameters + # this is often useful in particle filters + self.steering_noise = float(new_s_noise) + self.distance_noise = float(new_d_noise) + + # -------- + # set_steering_drift: + # sets the systematical steering drift parameter + # + + def set_steering_drift(self, drift): + self.steering_drift = drift + + # -------- + # move: + # steering = front wheel steering angle, limited by max_steering_angle + # distance = total distance driven, most be non-negative + + def move(self, steering, distance, + tolerance = 0.001, max_steering_angle = pi / 4.0): + + if steering > max_steering_angle: + steering = max_steering_angle + if steering < -max_steering_angle: + steering = -max_steering_angle + if distance < 0.0: + distance = 0.0 + + + # make a new copy + res = robot() + res.length = self.length + res.steering_noise = self.steering_noise + res.distance_noise = self.distance_noise + res.steering_drift = self.steering_drift + + # apply noise + steering2 = random.gauss(steering, self.steering_noise) + distance2 = random.gauss(distance, self.distance_noise) + + # apply steering drift + steering2 += self.steering_drift + + # Execute motion + turn = tan(steering2) * distance2 / res.length + + if abs(turn) < tolerance: + + # approximate by straight line motion + + res.x = self.x + (distance2 * cos(self.orientation)) + res.y = self.y + (distance2 * sin(self.orientation)) + res.orientation = (self.orientation + turn) % (2.0 * pi) + + else: + + # approximate bicycle model for motion + + radius = distance2 / turn + cx = self.x - (sin(self.orientation) * radius) + cy = self.y + (cos(self.orientation) * radius) + res.orientation = (self.orientation + turn) % (2.0 * pi) + res.x = cx + (sin(res.orientation) * radius) + res.y = cy - (cos(res.orientation) * radius) + + return res + + + + + def __repr__(self): + return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation) + + + + +############## ADD / MODIFY CODE BELOW #################### + +# ------------------------------------------------------------------------ +# +# run - does a single control run. + + +def run(param1, param2, param3): + myrobot = robot() + myrobot.set(0.0, 1.0, 0.0) + speed = 1.0 # motion distance is equal to speed (we assume time = 1) + N = 100 + myrobot.set_steering_drift(10.0 / 180.0 * pi) # 10 degree bias, this will be added in by the move function, you do not need to add it below! + cte = myrobot.y + I = 0 + for i in range(N): + cte_now = myrobot.y + p = cte + d = cte_now - cte + I += cte + steer = -param1 * p - param2 * d - param3 * I + myrobot = myrobot.move(steer, speed) + cte = cte_now + print myrobot, steer + + +# Call your function with parameters of (0.2, 3.0, and 0.004) +run(0.2, 3.0, 0.004) + + + + +