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
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'])