Пример #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 
Пример #2
0
 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)
Пример #3
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()                  
Пример #4
0
 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()  
Пример #5
0
 def turn(self,t):
     serial_out.travel(0,100,t)
     self.commit()
     print ('turn ', t)    
Пример #6
0
 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)