Implement P controller

import random
import numpy as np
import matplotlib.pyplot as plt

class Robot(object):
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

def set(self, x, y, orientation):
self.x = x
self.y = y
self.orientation = orientation % (2.0 * np.pi)

def set_noise(self, steering_noise, distance_noise):
self.steering_noise = steering_noise
self.distance_noise = distance_noise

def set_steering_drift(self, drift):
self.steering_drift = drift

def move(self, steering, distance, tolerance=0.001, max_steering_angle=np.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 steering2 = random.gauss(steering, self.steering_noise) distance2 = random.gauss(distance, self.distance_noise) steering2 += self.steering_drift turn = np.tan(steering2) * distance2 / self.length if abs(turn) < tolerance: self.x += distance2 * np.cos(self.orientation) self.y += distance2 * np.sin(self.orientation) self.orientation = (self.orientation + turn) % (2.0 * np.pi) else: radius = distance2 / turn cx = self.x - (np.sin(self.orientation) * radius) cy = self.y + (np.cos(self.orientation) * radius) self.orientation = (self.orietation + turn) % (2.0 + np.pi) self.x = cx + (np.sin(self.orientation) * raidus) self.y = cy - (np.cos(self.orientation) * raidus) def __repr__(self): return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation) [/python]