Exemplo n.º 1
0
  def move(self, period):
    d_theta = self.state[1]*pi/180*period
    d_x = self.state[0]*cos(d_theta)*period
    d_y = self.state[0]*sin(d_theta)*period

    if self.actuated:
      self.actuated.x += (d_x*cos(self.actuated.yaw*pi/180)-
        d_y*sin(self.actuated.yaw*pi/180))
      self.actuated.y += (d_x*sin(self.actuated.yaw*pi/180)+
        d_y*cos(self.actuated.yaw*pi/180))
      self.actuated.yaw += d_theta*180/pi    

    for i in range(len(self.casterCranks)):
      self.casterCranks[i].yaw += self.casterRates[i]*period
        
    WheelDrive.move(self, period)
    
Exemplo n.º 2
0
  def move(self, period):
    if abs(self.state[1]) >= self.epsilon:
      radius = self.wheelBase/tan(self.state[1]*pi/180)

      d_theta = self.state[0]*period/radius
      d_x = radius*sin(d_theta)
      d_y = radius*(1-cos(d_theta))
    else:
      d_theta = 0
      d_x = self.state[0]*period
      d_y = 0

    if self.actuated:
      self.actuated.x += (d_x*cos(self.actuated.yaw*pi/180)-
        d_y*sin(self.actuated.yaw*pi/180))
      self.actuated.y += (d_x*sin(self.actuated.yaw*pi/180)+
        d_y*cos(self.actuated.yaw*pi/180))
      self.actuated.yaw += d_theta*180/pi

    for i in [0, 1]:
      self.wheels[i].yaw += self.wheelSteeringRates[i]*period
        
    WheelDrive.move(self, period)