def run(iterations, time, controller, mytrack, mycar, speed):
pos_out = []
sense_out = []
filt = filters.single_pole_IIR(.1, 0.1)
past_points = numpy.zeros([5,2])
shift_down = numpy.matrix('[0 0 0 0 0, 1 0 0 0 0, 0 1 0 0 0, 0 0 1 0 0, 0 0 0 1 0, 0 0 0 0 1]')
for i in range(iterations):
#sensing
line_sensed, CTE, abs_pt, true_cte = mycar.sense(mytrack)
#past_points = shift_down * past_points
#past_points[0,0] = abs_pt[0,0]
#past_points[0,1] = abs_pt[0,1]
if line_sensed:
sense_out.append("%f, %f" % (abs_pt[0,0], abs_pt[0,1]))
#control
steering = controller.run(CTE)
# update the car's position
mycar = mycar.move(steering, speed*time)
#output the position of the sensor (the front of the car
pos = mycar.get_sensor_pos()
pos_out.append("%f, %f, %f, %f, %f, %f" % (pos[0], pos[1], mycar.orientation, speed, steering, mycar.steering_angle))
return (pos_out, sense_out)
import filters f = filters.single_pole_IIR(0.5) for i in range(10): print(f.step(1))
