Crated de2bot class, added arcs lmao

.gitignore

@@ -0,0 +1,2 @@
+.idea
+__pycache__

README.md

@@ -0,0 +1,10 @@
+# DE2sim
+
+This is a simulator for the Fall 2019 ECE 2031 final project.
+
+## DE2 Robot
+### Drivetrain
+The DE2bot is a differential drive robot
+
+### Sensors
+It has a total of 8 sonar sensors

de2bot.py

@@ -0,0 +1,42 @@
+import numpy as np
+
+
+class State:
+    def __init__(self, pose=np.asmatrix([0.0, 0.0, 0.0]).T, twist=np.asmatrix([0.0, 0.0]).T):
+        self.pose = pose
+        self.twist = twist
+
+
+class DE2Config:
+    def __init__(self, axle_length: float = 0.15, drag_coeff: float = 1.0):
+        self.axle_length = axle_length
+        self.drag_coeff = drag_coeff
+
+    def __str__(self):
+        return f"axle_length: {self.axle_length}\ndrag_coeff: {self.drag_coeff}"
+
+    def __repr__(self):
+        return f"axle_length: {self.axle_length}\ndrag_coeff: {self.drag_coeff}"
+
+
+class DE2Bot:
+    def __init__(self, state: State = State(), config: DE2Config = DE2Config()):
+        self.state = state
+        self.config = config
+
+        self.G = np.asmatrix([
+            [1.0, 1.0],
+            [1 / self.config.axle_length, -1 / self.config.axle_length]
+        ])
+
+    def apply(self, u: np.ndarray, dt: float):
+        drag = -self.config.drag_coeff * self.state.twist
+        f = 0.1 * self.G * u + drag
+
+        linear, angular = self.state.twist[0, 0], self.state.twist[1, 0]
+        heading = self.state.pose[2, 0]
+
+        self.state.pose += dt * np.asmatrix([[linear * np.cos(heading),
+                                              linear * np.sin(heading),
+                                              angular]]).T
+        self.state.twist += dt * f

sim.py

@@ -3,6 +3,9 @@
 import random
 import vis
 
+from de2bot import DE2Bot
+
+
 def meas_prob(x, o, meas_angle):
     ddir = o - x[:2, :]
     dist = np.linalg.norm(ddir)
@@ -15,6 +18,7 @@ def meas_prob(x, o, meas_angle):
     cos = np.arccos(cos_th)
     return np.exp(-15 * dist * cos[0, 0] * cos[0, 0])
 
+
 def sensor_model(x, obstacles, meas_angle):
     by_prox = sorted(obstacles, key=lambda o: np.linalg.norm(x[:2, :] - o))
 
@@ -24,25 +28,18 @@ def sensor_model(x, obstacles, meas_angle):
             return np.linalg.norm(obs - x[:2, 0])
     return None
 
-print(sensor_model(np.asmatrix([[0, 0, 6.6]]).T, [np.asmatrix([[1, 0]]).T], 2.5))
 
 def main():
     clock = pygame.time.Clock()
 
-    pos = np.asmatrix([0., 0, 0]).T
-    vel = np.asmatrix([0, 0.]).T
     visualizer = vis.Visualizer()
 
     fps = 60
     dt = 1.0 / fps
     t = 0.0
     i = 0
 
-    r = 0.15
-    G = np.asmatrix([
-        [1.0, 1.0],
-        [1 / r, -1 / r]
-    ])
+    robot = DE2Bot()
 
     obstacles = [
         np.asmatrix([[0.5, 0]]).T,
@@ -57,6 +54,8 @@ def main():
         angles_deg = [-144, -90, -44, -12, 12, 44, 90, 144]
         angles_rad = [np.deg2rad(a) for a in angles_deg]
 
+        pos = robot.state.pose
+
         hits = []
         for a in angles_rad:
             dist = sensor_model(pos, obstacles, a)
@@ -65,14 +64,12 @@ def main():
                 hits.append((pos[0, 0] + dist * np.cos(angle), pos[1, 0] + dist * np.sin(angle)))
 
         u = np.asmatrix([1.0, 2.0]).T
+        robot.apply(u, dt)
 
-        f = 0.1 * G * u - vel
-        pos += dt * np.asmatrix([[vel[0, 0] * np.cos(pos[2, 0]), vel[0, 0] * np.sin(pos[2, 0]), vel[1, 0]]]).T
-        vel += dt * f
-
-        visualizer.draw(pos, pos, hits, [(mat[0, 0], mat[1, 0]) for mat in obstacles])
+        visualizer.draw(robot.state.pose, robot.state.pose, hits, [(mat[0, 0], mat[1, 0]) for mat in obstacles])
 
         clock.tick(60)
 
+
 if __name__ == '__main__':
     main()

vis.py

@@ -1,5 +1,7 @@
 import pygame
 import numpy as np
+import math
+
 
 class Visualizer:
     def __init__(self):
@@ -84,6 +86,26 @@ def draw(self, robot_coords, goal_point, ping_coords, obs_coords):
                            rscreen_coords, 5)
 
         for ping in ping_coords:
+            ping_pos = self.to_screen_coords(ping)
+            robot_screen_coords = self.to_screen_coords(robot_coords[:, 0])
+
+            dx = ping[0] - robot_coords[0, 0]
+            dy = ping[1] - robot_coords[1, 0]
+
+            line_angle = math.atan2(dy, dx)
+
+            dy = ping_pos[0] - robot_screen_coords[0]
+            dx = ping_pos[1] - robot_screen_coords[1]
+
+            line_length = math.hypot(dx, dy)
+
+            start_angle = line_angle - 10 * np.pi / 180
+            end_angle = line_angle + 10 * np.pi / 180
+
+            rect = pygame.Rect(robot_screen_coords[0] - line_length, robot_screen_coords[1] - line_length,
+                               line_length * 2, line_length * 2)
+            # pygame.draw.rect(self.display, (255, 255, 255), rect, 1)
+            pygame.draw.arc(self.display, (255, 255, 255), rect, start_angle, end_angle)
             pygame.draw.aaline(self.display, (255, 255, 255),
                                rscreen_coords, self.to_screen_coords(ping), 10)