Source Code for Script script-demo_calibPlan_py

  1  from joy import * 
  2   
  3 -class CalibrationPlan( Plan ): 
  4 -  def __init__(self,app,mod): 
  5      Plan.__init__(self,app) 
  6      self.mod = mod 
  7      self.clicked = False 
  8     
  9 -  def onEvent( self, evt ): 
 10      if evt.type == KEYDOWN: 
 11          self.clicked = True 
 12      # Always run 
 13      return True 
 14     
 15 -  def behavior( self ): 
 16          #send go limp command 
 17      self.mod.go_slack() 
 18          #command user to move module to limits 
 19      mx = -1e9 
 20      mn = 1e9 
 21          #wait for user input (enter stroke or something) 
 22      self.clicked = False 
 23      while not self.clicked: 
 24            #remember min and max vals 
 25        yield 
 26        adc = self.mod.od.get_rawADC() 
 27        if mx<adc: 
 28          mx = adc 
 29        if mn>adc: 
 30              mn = adc 
 31        print "Move module to both limits then hit a key !!! min adc = " , mn , " max adc = " , mx 
 32      #enter command mode 
 33      self.mod.od.set_calmode(1) 
 34      #send 0 
 35      midVal = 0 
 36      self.mod.od.set_pos(midVal) 
 37      time.sleep(.5) 
 38      #check if feedback is > max or < min and adjust 
 39      while (self.mod.od.get_rawADC() <= mn+10) or (self.mod.od.get_rawADC() >= mx-10): 
 40        #keep checking until successful 
 41        if self.mod.od.get_rawADC() <= mn: 
 42          midVal += 100 
 43        else: 
 44          midVal -= 100 
 45        self.mod.od.set_pos(midVal) 
 46        time.sleep(.1) 
 47        print "!!! Adjusting middle value to" ,midVal 
 48      #slowly increment until maximum pot value is reached 
 49      mxRaw = midVal 
 50      while self.mod.od.get_rawADC() < (mx): 
 51            #remember maximum raw value 
 52        mxRaw += 25 
 53        self.mod.od.set_pos(mxRaw) 
 54        print "!!! Moving to " , mxRaw 
 55        time.sleep(.025) 
 56      #send 0 
 57      self.mod.od.set_pos(midVal) 
 58      #slowly decrement until minimum pot value is reached 
 59      mnRaw = midVal 
 60      while self.mod.od.get_rawADC() > (mn): 
 61        #remember minimum raw value 
 62        mnRaw -= 25 
 63        self.mod.od.set_pos(mnRaw) 
 64        print "!!! Moving to " , mnRaw 
 65        time.sleep(.025) 
 66      print "!!! Position min = ", mnRaw 
 67      print "!!! Position max = ", mxRaw 
 68      print "!!! Feedback min = ", mn 
 69      print "!!! Feedback max = ", mx 
 70      #raw amplitude = maximum raw - minimum raw 
 71      print "!!! Pamp = ", (mxRaw-mnRaw)/2 
 72      self.mod.od.set_calPamp((mxRaw-mnRaw)/2) 
 73      #raw average = (maximum raw + minimum raw)/2 
 74      print "!!! Pctr = ", (mxRaw+mnRaw)/2.0 
 75      self.mod.od.set_calPctr((mxRaw+mnRaw)/2.0) 
 76      #feedback amplitude = maximum feedback - minimum feedback 
 77      print "!!! Famp = ", (mx-mn)/2 
 78      self.mod.od.set_calFamp((mx-mn)/2) 
 79      #feedback average = (maximum feedback + minimum feedback)/2 
 80      print "!!! Fctr = ", (mx+mn)/2.0 
 81      self.mod.od.set_calFctr((mx+mn)/2.0) 
 82      #turn off raw mode 
 83      self.mod.od.set_calmode(0) 
 84      #send module reset command  
 85       
 86      #alert user it's done 
 87       
 88 -class CalibrationApp( JoyApp ): 
 89     
 90 -  def __init__(self,targetID,*arg,**kw): 
 91      print targetID 
 92      JoyApp.__init__(self,robot=dict( 
 93        required=[targetID], # must find the target module 
 94        names={ targetID : 'theModule' }, 
 95        walk=True # and walk its object dictionary 
 96        ),*arg,**kw) 
 97       
 98 -  def onStart( self ): 
 99      self.plan = CalibrationPlan( self, self.robot.at.theModule ) 
100      self.plan.start() 
101     
102 -  def onEvent(self, evt): 
103      if evt.type==KEYDOWN: 
104        JoyApp.onEvent(self,evt) 
105        self.plan.push(evt) 
106        return 
107      if not self.plan.isRunning(): 
108        self.stop() 
109         
110  if __name__=="__main__": 
111    print """ 
112    Calibration tool 
113    ---------------- 
114   
115    Given a module ID (in hex) on the commandline,  
116    calibrate this module. 
117     
118    """ 
119    import sys 
120    if len(sys.argv) != 2: 
121      sys.stderr.write(""" 
122        <<insert usage message here>>> 
123        """) 
124      sys.exit(1) 
125    app=CalibrationApp(int(sys.argv[1],16)) 
126    app.run() 
127