class Car(object):
def __init__(self, x=0, y=0, theta=0):
self.x = x
self.y = y
self.theta = radians(theta)
self.velLin = 0
self.velAng = 0
self.distFront = 0
self.distRight = 0
self.distLeft = 0
self.sensors = Sensors()
self.control = False
def initControl(self, KpAng, KpLin, circuitWeight):
self.control = ControlDistWall(KpAng, KpLin, circuitWeight)
def sense(self, wallsPoints, maxDist):
retPoints = []
for p in self.sensors.calcDistanceToWall(self.theta, wallsPoints,
maxDist, self):
retPoints.append(p)
if len(retPoints) == 3:
self.distFront = retPoints[0][3]
self.distRight = retPoints[1][3]
self.distLeft = retPoints[2][3]
return retPoints
def move(self, velLin, velAng):
self.velLin = velLin
self.velAng = velAng
self.theta = self.theta + velAng
self.x = self.x + velLin * cos(self.theta)
self.y = self.y + velLin * sin(self.theta)
#print "x = "+str(self.x)+" y = "+str(self.y)+" theta = "+ str(self.theta) +" velLin = "+str(velLin)+" velAng = "+str(velAng)
def controlCar(self):
self.move(self.control.controlLinVel(self.distFront),
self.control.controlAngVel(self.distRight, self.distLeft))
def display(self):
stroke(255, 0, 0)
velLin = self.velLin
if velLin < 30:
velLin = 30
line(self.x, self.y, self.x + velLin * cos(self.theta),
self.y + velLin * sin(self.theta))
fill(255)
ellipse(self.x, self.y, 10, 10)
