파이션으로 만드는 2D differential 드라이브 코드 예제.

# File name : difdrive.py
# Link : https://www.youtube.com/watch?v=zHboXMY45YU
# Download Car image & rename to DDR.png : https://imgur.com/SpeVm6L 
# x1 = v*cos(theta)
# y1 = v*sin(theta)
# Theta1 = diff(v) / W

import pygame
import math

class Envir:
   def __init__(self, dimensions):
      # colours
      self.black = (0, 0, 0)
      self.white = (255, 255, 255)
      self.green = (0, 255, 0)
      self.blue  = (0, 0, 255)
      self.red   = (255, 0, 0)
      self.yel   = (70, 70, 70)
      # map dimensions
      self.height = dimensions[0]
      self.width  = dimensions[1]
      # window settings
      pygame.display.set_caption("Differential drive robot")
      self.map = pygame.display.set_mode((self.width, self.height))
      # text variables
      self.font = pygame.font.Font('freesansbold.ttf', 50)
      self.text = self.font.render('default', True, self.white, self.black)
      self.textRect = self.text.get_rect()
      self.textRect.center = (dimensions[1] - 600,
                              dimensions[0] - 100)
      #trail
      self.trail_set=[]                        
                              
   def write_info(self, Vl, Vr, theta):
      txt = f"Vl = {Vl} Vr = {Vr}  theta = {int(math.degrees(theta))}"
      self.text = self.font.render(txt, True, self.white, self.black)
      self.map.blit(self.text, self.textRect)
      
   def trail(self, pos):
      for i in range(0, len(self.trail_set)-1):
         pygame.draw.line(self.map, self.yel, 
            (self.trail_set[i][0], self.trail_set[i][1]),
            (self.trail_set[i+1][0], self.trail_set[i+1][1]))
      if self.trail_set.__sizeof__() > 10000:
         self.trail_set.pop(0)
      self.trail_set.append(pos)   
      
   def robot_frame(self, pos, rotation):
      n = 80
      
      centerx, centery = pos
      x_axis = (centerx + n * math.cos(-rotation), centery + n * math.sin(-rotation))
      y_axis = (centerx + n * math.cos(-rotation + math.pi/2), 
                centery + n * math.sin(-rotation + math.pi/2))
      pygame.draw.line(self.map, self.red, (centerx, centery), x_axis, 3)
      pygame.draw.line(self.map, self.green, (centerx, centery), y_axis, 3)      
                 
class Robot:
   def __init__(self, startpos, robotImg, width):
      self.m2p = 3779.52  # meters to pixels ratio
      # robot dimensions
      self.w = width
      self.x = startpos[0]
      self.y = startpos[1]
      self.theta = 0
      self.vl = 0.01 * self.m2p  # meters / second
      self.vr = 0.01 * self.m2p  # meters / second
      self.maxspeed = 0.02 * self.m2p
      self.minspeed = -0.02 * self.m2p
      # graphics
      self.img = pygame.image.load(robotImg)
      self.rotated = self.img
      self.rect=self.rotated.get_rect(center=(self.x, self.y))
      
   def draw(self, map):
      map.blit(self.rotated, self.rect)   
      
   def move(self, event=None):
      if event is not None:
         if event.type == pygame.KEYDOWN:
            if event.key == pygame.K_KP4:  # Left Arrow in Numpad     
               self.vl += 0.001 * self.m2p
            elif event.key == pygame.K_KP1:  # 1  
               self.vl -= 0.001 * self.m2p    
            elif event.key == pygame.K_KP6:  # Right Arrow in Numpad  
               self.vr += 0.001 * self.m2p
            elif event.key == pygame.K_KP3:  # 3  
               self.vr -= 0.001 * self.m2p
               
      # v=(Vr+Vl)/2                  
      self.x += ((self.vl + self.vr) / 2) * math.cos(self.theta) * dt   
      self.y -= ((self.vl + self.vr) / 2) * math.sin(self.theta) * dt
      self.theta += (self.vr - self.vl) / self.w * dt
      # reset theta
      if self.theta > 2 * math.pi or self.theta < -2 * math.pi :
         self.theta = 0
      # set max speed
      self.vr = min(self.vr, self.maxspeed)
      self.vl = min(self.vl, self.maxspeed)
      # set min speed      
      self.vr = max(self.vr, self.minspeed)
      self.vl = max(self.vl, self.minspeed)
      
      self.rotated = pygame.transform.rotozoom(self.img,
         math.degrees(self.theta), 1)         
      self.rect = self.rotated.get_rect(center=(self.x, self.y))
      
         
      
# initialization
pygame.init()

# start postion
start = (200, 200)

# dimension
dims=(600, 1200)

# running or not
running = True

# the envir
environment = Envir(dims)

# the Robot (begining location, image of robot, width)
robot = Robot(start, "./DDR.png", 0.01 * 3779.52)

# dt (Change in time)
dt = 0
lasttime = pygame.time.get_ticks()

# simulation loop
while running:
   for event in pygame.event.get():
      if event.type == pygame.QUIT:
         running = False
      robot.move(event)
       
   dt = (pygame.time.get_ticks()-lasttime) / 1000     
   lasttime = pygame.time.get_ticks()
   pygame.display.update()
   environment.map.fill(environment.black)
   robot.move()
   environment.write_info(int(robot.vl),
                          int(robot.vr),
                          robot.theta)
   robot.draw(environment.map)          
   environment.trail((robot.x, robot.y))             
   environment.robot_frame((robot.x, robot.y), robot.theta)
                              
   #if pygame.mouse.get_focused():
      #sensorOn = True