def printStats(self):
LogPrinter ( "Mean time between calls: ", "{:.2f}".format( self.intervalMean/1000 ),
" ms. (Should be ~0.2 ms)" )
LogPrinter ( "Standard deviation of time between calls in percent: ",
"{:.2f}".format( 100* sqrt( (self.intervalMeanSq/self.callCount -
self.intervalMean*self.intervalMean / (self.callCount * self.callCount ) ) ) / self.intervalMean ),
" %. (Should be ~1%)" )
def commandParser(self, command: str):
"""Parses a single input command. Should return immediately"""
commandTokens = str.split(command)
#LogPrinter ( commandTokens )
if (commandTokens[0] in self.commands):
arguments = [float(i) for i in commandTokens[1:]]
#LogPrinter( arguments )
self.commands[commandTokens[0]](arguments)
else:
LogPrinter("Unknown command: ", command)
LogPrinter("Supported commands:")
for key in self.commands.keys():
LogPrinter("- ", key)
def run(self):
LogPrinter("Running loop")
reader = NonBlockingRead(self.commandParser)
while True:
reader()
self.taskScheduler.run()
def cmdMoveAtConstantSpeed(self, args):
LogPrinter("Moving at constant speed")
speed = 2000
Kp = 0.0003
Ki = 0
if (len(args) >= 3):
speed = args[0]
Kp = args[1]
Ki = args[2]
self.homingEncoder1.forceHomed() # Force homing.
self.controller1.setParams(Kp, Ki)
self.controller1.setPoint(speed)
self.controller1.enable()
LogPrinter("Speed setpoint: ", speed)
def stopFun():
self.controller1.disable()
self.driver1.setMotorPWM(0)
LogPrinter("Done miving at constant speed")
self.taskScheduler.addTask(
IFutureTask(int(1e6), stopFun, self.taskScheduler))
def cmdSimpleMove(self, args):
LogPrinter("Doing a simple move")
self.homingEncoder1.forceHomed() # Force homing.
self.homingEncoder1.verbose = True
power = 0.25
if (len(args) > 0):
power = args[0]
self.driver1.setMotorPWM(power)
def stopFun():
self.driver1.setMotorPWM(0)
LogPrinter("Done with simple move")
LogPrinter("Speed: ", self.homingEncoder1.speed_cps)
self.homingEncoder1.verbose = False
self.taskScheduler.addTask(
IFutureTask(int(5e6), stopFun, self.taskScheduler))
def run( self ):
LogPrinter ( "IRecurringTask needs to be inherited and the run() shall be overridden." )
def cmdInfo(self, args):
LogPrinter("This is the info")
def run(self):
LogPrinter("Beacon!")
def stopFun():
self.controller1.disable()
self.driver1.setMotorPWM(0)
LogPrinter("Done miving at constant speed")
def stopFun():
self.driver1.setMotorPWM(0)
LogPrinter("Done with simple move")
LogPrinter("Speed: ", self.homingEncoder1.speed_cps)
self.homingEncoder1.verbose = False