# udacity373_code / unit5_9.py

 ``` 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152``` ```# ----------- # 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 * crosstrack_error # # Note that tau is called "param" in the function # run to be completed below. # # Your code should print output that looks like # the output shown in the video. That is, at each step: # print myrobot, steering # # 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(param): 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 # Add Code Here for i in range(N): crosstrack_error = myrobot.y steer = -param * crosstrack_error myrobot = myrobot.move(steer, speed) print myrobot, steer run(0.1) # call function with parameter tau of 0.1 and print results ```