# alexband/Udacity-373

Fetching contributors…
Cannot retrieve contributors at this time
172 lines (124 sloc) 4.41 KB
 # ----------- # 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 = 200 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)