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 run(self,ang,gait): if self.s_run == 0: espeak.synth("I'm off for a run") self.s_run = 1 serial_out.setgait(gait) serial_out.state(0,1,1) nav.odo(1.5,ang) print ('run') serial_out.travel(ang,100,0)
def turnL(self): x = 0 while x < 3: serial_out.travel(0,0,-90) self.commit() print ('left') time.sleep(0.1) x +=1 priorturn = 'l' turn_time = time.time()
def turnR(self): x = 0 while x < 3: serial_out.travel(0,0,90) self.commit() print ('right') time.sleep(0.1) x +=1 priorturn = 'r' turn_time = time.time()
def turn(self,t): serial_out.travel(0,100,t) self.commit() print ('turn ', t)
def walk(self,ang,gait): serial_out.setgait(gait) serial_out.state(0,0,1) serial_out.travel(ang,100,0) nav.odo(1,ang) print ('walk ', ang)