def __init__(self, tle, maxSpeed): self.tle = tle self.speed = 0 self.estimatedPosition = Position(0) self.targetRelativePosition = Position(0) self.lastKnownPosition = Position(0) self.targetRelativeSpeed = 0 self.error = PositionError() self.maxSpeed = maxSpeed self.fractionalErrorSwitchPoint = 0.05
class Model: def __init__(self, tle, maxSpeed): self.tle = tle self.speed = 0 self.estimatedPosition = Position(0) self.targetRelativePosition = Position(0) self.lastKnownPosition = Position(0) self.targetRelativeSpeed = 0 self.error = PositionError() self.maxSpeed = maxSpeed self.fractionalErrorSwitchPoint = 0.05 def nextPosition(self): return self.lastKnownPosition.get() + SPACING_RADIANS def distanceToNext(self): return abs(self.nextPosition() - self.estimatedPosition.get()) def tick(self, t): self.tle.update() self.targetRelativePosition.set( self.tle.get_azimuth(False) ) self.estimatedPosition.add( -self.speed * t ) ## Speed is +ve going anti-clockwise self.error.update(self.targetRelativePosition.get(), self.estimatedPosition.get()) self.targetRelativeSpeed = self.tle.azimuth_speed(False) self.setNewSpeed() def fractionalError(self): return self.error.magnitude / RADIANS_PER_TURN def useFastMode(self): return self.fractionalError() > self.fractionalErrorSwitchPoint def setNewSpeed(self): if self.useFastMode(): # Run at fastest possible speed to catch up if self.error.leading: self.speed = -self.maxSpeed # Ahead by a long way - run in reverse else: self.speed = +self.maxSpeed else: if self.error.leading: # Reduce speed to let target catch up self.speed = self.targetRelativeSpeed * 0.8 else: # Increase speed to catch up # Set catch-up speed based on magnitude of error speed_diff = (self.maxSpeed - self.targetRelativeSpeed) * self.fractionalError() / self.fractionalErrorSwitchPoint self.speed = (self.targetRelativeSpeed + speed_diff) def printTitles(self): print( "Target RP\tTarget Speed\tEst. P\tError\tSpeed\tLag\Lead" ) def printVars(self): print( "{0:.3f}\t\t{1:.3f}\t\t{2:.3f}\t{3:.3f}\t{4:.3f}\t{5}({6})".format( self.targetRelativePosition.get(), self.targetRelativeSpeed, self.estimatedPosition.get(), self.error.magnitude, self.speed, "Leading" if self.error.leading else "Lagging", "Fast" if self.useFastMode() else "Track" ) ) def run(self): self.printTitles() lastPrintTime = int(time.time()) self.printVars() while(1): self.tick(0.001) timeNow = int(time.time()) if (timeNow > lastPrintTime): #if 1: self.printVars() lastPrintTime = timeNow time.sleep(0.001)