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