From 21ec2b425f484ddd1cbfa9d3a4e8cd764e9b3b72 Mon Sep 17 00:00:00 2001
From: leo <leo.lemaire007@orange.fr>
Date: Mon, 21 Nov 2022 20:56:47 +0100
Subject: [PATCH] simulation

---
 .gitignore    |  2 ++
 robot.py      | 31 +++++++++++++++++++++++++++++++
 simulateur.py | 31 +++++++++++++++++++++++++++++++
 3 files changed, 64 insertions(+)
 create mode 100644 .gitignore
 create mode 100644 robot.py
 create mode 100644 simulateur.py

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..b2b6d48
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,2 @@
+__pycache__/
+build/
diff --git a/robot.py b/robot.py
new file mode 100644
index 0000000..ff8e9a2
--- /dev/null
+++ b/robot.py
@@ -0,0 +1,31 @@
+from math import cos, sin, pi
+
+class Robot:
+    # position
+    pos=(0,1.5)
+    # orientation
+    rot=pi/2
+    # velocite
+    v=0
+    # velocite angulaire
+    angv=0
+    # velocite des roues (non angulaire)
+    wv=(0.1,0.1)
+    # espace entre les deux roues
+    l=0.1
+    # time
+    t = 0
+
+    def __init__(self):
+        print("yo")
+        
+    def update(self, dt):
+        self.t += dt
+        # https://robotics.stackexchange.com/questions/106/what-is-a-suitable-model-for-two-wheeled-robots
+        self.v = 1/2*(self.wv[0]+self.wv[1])
+        self.angv = 1/self.l*(self.wv[1]-self.wv[0])
+        self.rot += self.angv*dt
+        self.pos = (self.pos[0]+self.v*sin(self.rot)*dt,self.pos[1]+self.v*cos(self.rot)*dt)
+        if(self.pos[0]>=10):
+            print(self.t)
+            sys.exit()
\ No newline at end of file
diff --git a/simulateur.py b/simulateur.py
new file mode 100644
index 0000000..f3aff55
--- /dev/null
+++ b/simulateur.py
@@ -0,0 +1,31 @@
+from robot import Robot
+
+from math import degrees
+import sys, pygame
+from pygame.locals import *
+pygame.init()
+
+size = width, height = 1000, 300
+black = 0, 0, 0
+
+screen = pygame.display.set_mode(size)
+
+r = Robot()
+
+while True:
+    for event in pygame.event.get():
+        if event.type == pygame.QUIT: sys.exit()
+        if event.type == pygame.KEYDOWN:
+            if event.key == K_q:
+                sys.exit()
+    
+    screen.fill(black)
+    r.update(1/60)
+    robotShape = pygame.Rect(0,0, 10,20);
+    rsurf = pygame.Surface((10,20)).convert_alpha();
+    pygame.draw.rect(rsurf, (255,255,255), robotShape);
+    rsurf_r = pygame.transform.rotate(rsurf, degrees(r.rot));
+    center = rsurf_r.get_rect().center
+    screen.blit(rsurf_r, (r.pos[0]*100-center[0],r.pos[1]*100-center[1]))
+    pygame.display.flip()
+