def couple(self, vehicles): """ Checks if another vehicle is within 1 unit (meter) range of our hitch. If yes, it will be connected to us. """ if self.hitchConstraint == None: ourHitchAbsolute = Util.getAbsolutePos(self.getTrailerHitchPoint(), self.getChassis().getBodyNp()) for vehicle in vehicles: otherHitchAbsolute = Util.getAbsolutePos(vehicle.getTrailerHitchPoint(), vehicle.getChassis().getBodyNp()) if not self == vehicle and vehicle.getType() == "trailer" \ and Util.getDistance(ourHitchAbsolute, otherHitchAbsolute) <= 1: self.hitchConstraint = self.createCouplingJoint(self.getTrailerHitchPoint(), vehicle.getTrailerHitchPoint(), vehicle.getChassis().getBodyNp().node()) self.hitchedTrailer = vehicle else: self.world.removeConstraint(self.hitchConstraint) self.hitchConstraint = None self.hitchedTrailer = None
def update(self, task):
# Calculate our desired position, then use a PID-controller to get there
absTargetPos = Util.getAbsolutePos(self.relTargetPos, self.target)
# http://en.wikipedia.org/wiki/PID_controller
# http://www.engin.umich.edu/group/ctm/PID/PID.html
# Kx: modifying factors: proportional, integral and derivative
# error = targetPos - curPos
# dt: the time difference
# u = Kp * error + Ki * integrate(error, dt) + Kd * (de / dt)
Kp = 1 #0.05
Ki = 0. #0.08
Kd = 0. #.001
lastError = self.error
self.error = absTargetPos - self.cam.getPos()
# linear pid, x axis
prop = Kp * self.error[0]
intgr = Ki * self.error[0] * globalClock.getDt()
derv = Kd * (lastError[0] - self.error[0]) / globalClock.getDt()
corrForce = Vec3(prop + intgr + derv, 0, 0)
# y axis
prop = Kp * self.error[1]
intgr = Ki * self.error[1] * globalClock.getDt()
derv = Kd * (lastError[1] - self.error[1]) / globalClock.getDt()
corrForce[1] = prop + intgr + derv
# z axis
prop = Kp * self.error[2]
intgr = Ki * self.error[2] * globalClock.getDt()
derv = Kd * (lastError[2] - self.error[2]) / globalClock.getDt()
corrForce[2] = prop + intgr + derv
newCamPos = self.cam.getPos() + corrForce
#print self.cam.getPos(self.target) # prints correct relative coordinates, just FYI
self.cam.setPos(newCamPos)
relToTarget = Util.getAbsolutePos(self.relLookAtPos, self.cam)
self.cam.lookAt(Util.getAbsolutePos(relToTarget, self.target))
self.cam.setR(self.target.getR())
return task.cont
def update(self, task):
# Calculate our desired position, then use a PID-controller to get there
relTargetPos = Vec3(0, -self.distance, self.height)
absTargetPos = Util.getAbsolutePos(relTargetPos, self.target)
# http://en.wikipedia.org/wiki/PID_controller
# http://www.engin.umich.edu/group/ctm/PID/PID.html
# Kx: modifying factors: proportional, integral and derivative
# error = targetPos - curPos
# dt: the time difference
# u = Kp * error + Ki * integrate(error, dt) + Kd * (de / dt)
#self._printAsInt(absTargetPos)
Kp = 0.05
Ki = 0.05
Kd = .001
lastError = self.error
self.error = absTargetPos - self.cam.getPos()
# linear pid, x axis
prop = Kp * self.error[0]
intgr = Ki * self.error[0] * globalClock.getDt()
derv = Kd * (lastError[0] - self.error[0]) / globalClock.getDt()
corrForce = Vec3(prop + intgr + derv, 0, 0)
# y axis
prop = Kp * self.error[1]
intgr = Ki * self.error[1] * globalClock.getDt()
derv = Kd * (lastError[1] - self.error[1]) / globalClock.getDt()
corrForce[1] = prop + intgr + derv
# z axis
# more or less statically set as targetheight + our height offset
newCamPos = self.cam.getPos() + corrForce
newCamPos[2] = self.target.getPos()[2] + self.height
#print self.cam.getPos(self.target) # prints correct relative coordinates, just FYI
self.cam.setPos(newCamPos)
self.cam.lookAt(self.target)
#self.cam.lookAt(self.target.getPos(self.cam) + Vec3(0, 0, 1.5)) # works
return task.cont
