Esempio n. 1
0
 def hdgchange(self,temp,timer):
     currenthdg = hmcmag.adj_heading()
     if time.time()-timer > 5:
       temp = 0
     targethdg = self.getangle(coords['i_CurPos'],coords['i_TarPos']) + temp
     #print( coords['i_CurPos'],":",coords['i_TarPos'],":",temp)
     if targethdg > 360:
        targethdg -= 360
     if targethdg < 0:
        targethdg += 360   
     #print ("current:",currenthdg,"  target:", targethdg)
     error = targethdg - currenthdg
     while abs(error) > self.deadzone:
       currenthdg = hmcmag.adj_heading()
       #print ("current:",currenthdg,"  target:", targethdg)
       error = targethdg - currenthdg
       error_delta = error - self.previous_error
       if (abs(error)< self.deadzone):
           error = 0
       elif(error > 180):
           error -= 360
       elif (error < -180):
           error += 360
       error_delta = error - self.previous_error    
       self.previous_error = error
       if error > 0: 
          change = min(90,int(((error * self.P_gain + error_delta * self.D_gain)/100)+50))
       else:
            change = max(-90,int(((error * self.P_gain + error_delta * self.D_gain)/100)-50))
       #print change
       serial_out.travel(0,0,change)
     self.gridhdg = currenthdg 
Esempio n. 2
0
 def offset (self):
     coords['i_Offset'] = - hmcmag.heading()
     time.sleep(0.5)
     print ('heading:  ',hmcmag.heading(), '  adj_heading:  ',hmcmag.adj_heading(), '  offset:  ',coords['i_Offset'])