closest angle graph

src/main/python/modeling/shooter_model.py

@@ -5,10 +5,15 @@
 import matplotlib.pyplot as plt
 
 @dataclasses.dataclass
-class VelocityVector():
+class Vector():
   angle: float  # Degrees
   velocity: float  # Meters / Sec
 
+  def getY(self):
+    return self.velocity * math.sin(self.angle)
+  def getX(self):
+    return self.velocity * math.cos(self.angle)
+
 @dataclasses.dataclass
 class Point():
   x: float  # Meters
@@ -44,19 +49,45 @@ def get_angle(self):
     calculated_angle = math.atan((-b + math.sqrt(math.pow(b,2) - 4 * a * c))/(2 * a))
     return calculated_angle
 
+  def getvector(self) -> Vector:
+    return Vector(self.get_angle,self.get_vel(self.rpm))
+
+def prunepoints(points: []) -> Point:
+  s = -1
+  for i in range(1,len(points)):
+    if points[i].y < points[i-1].y:
+      s=i
+      break
+  return points[ :s]
+
 class ProjectileMotion:
   g = 9.8
-  def __init__(self, dt: float, range):
+  def __init__(self, dt: float):
     self.dt = dt
-    self.range = range
-  def getPoints(self, initial_vel, angle, exit: Point):
+
+  def getpoints(self, vector: Vector, exit: Point) -> []:
     points = []
-    for i in range(self.range):
-      t = i * self.dt
-      x = self.vel*t*math.cos(angle)+exit.x
-      y = (-self.g/2)*math.pow(t,2) + self.vel*t*math.sin(angle) + exit.y
-      points.append(Point(x,y))
-
+    t = 0
+    note_position = [exit,exit]
+    note_velocity = [Point(vector.getX(),vector.getY()), Point(vector.getX(),vector.getY())]
+
+    for i in range(100):
+
+      angle = math.atan2(note_position[-1].x - note_position[-2].x,note_position[-1].y - note_position[-2].y)
+      acceleration = Point((-0.0015*(math.pow(note_velocity[-1].x,2) + math.pow(note_velocity[-1].y,2)))*math.cos(angle),(-9.8)+(-0.0015*(math.pow(note_velocity[-1].x,2) + math.pow(note_velocity[-1].y,2))*math.sin(angle)))
+      velocity = Point(acceleration.x * self.dt + note_velocity[-2].x, acceleration.y * self.dt + note_velocity[-2].y)
+      position = Point(note_position[-2].x + velocity.x * self.dt, note_position[-2].y + velocity.y * self.dt)
+
+
+      note_velocity.append(velocity)
+      note_position.append(position)
+      t = t + self.dt
+
+      points.append(position)
+      if points[-1].y < 0:
+        break
+    return note_position
+
 
 
 class ShooterModel:
@@ -66,7 +97,7 @@ def graph_note(self, model: Model):
     rpos = model.rpos
     gpos = model.gpos
     angle = model.get_angle()
-    vel = Point(model.get_vel(model.rpm) * math.cos(angle), model.get_vel(model.rpm) * math.sin(angle))
+    vel = Point(model.getvector().getX(), model.getvector().getY())
 
     print(angle * (180/math.pi))
 
@@ -104,12 +135,57 @@ def graph_note(self, model: Model):
 
     plt.show()
 
-goal = Point(3,1)
+def getexit(vector: Vector):
+  return Point(vector.getX(), vector.getY())
+
+
+def getangle(model: Model, pm: ProjectileMotion, rpm: float, goal: Point, robot:Point):
+  vel = model.get_vel(rpm)
+  closestangle = -1
+  lineslist = []
+  for angle in range(1,85):
+    exitpoint = Point.add(robot, getexit(Vector(angle,0.1)))
+    points = prunepoints(pm.getpoints(vel, angle, exitpoint))
+    lineslist.append(points)
+  local_min = 10000
+  dist = 10000
+  closestdist = []
+  angles = []
+  for angle in range(len(lineslist)):
+    for line in range(len(lineslist[angle][line])):
+      dist = lineslist[angle][line].dist(goal)
+      local_min = min(local_min, dist)
+    closestdist.append(local_min)
+    angles.append(angle)
+  finalmin = min(closestdist)
+  for i in range(len(angles)):
+    if closestdist[i] == finalmin:
+      closestangle = i
+      break
+  return closestangle
+
+def plot(points: [], start: Point, end: Point, dt: float):
+  for i in range(len(points)):
+      points[i] = (i * dt,points[i])
+  figure, axis = plt.subplots()
+  axis.plot(np.array([pos[1].x for pos in points]),
+              np.array([pos[1].y for pos in points]))
+  plt.plot([start.x,end.x],[start.y,end.y],'o')
+
+  axis.set(xlabel="X Postion",
+             ylabel="Y Position",
+             title="Title")
+  axis.grid()
+
+  plt.show()
 
-robotPosition = Point(1,0)
-rpm = 3500
 
+goal = Point(1,2)
+setangle = -30
+robotPosition = Point(0,0)
+setrpm = 3500
+info = Model(robotPosition,goal,setrpm)
 
-robot_shooter = ShooterModel(0.02)
+robot_shooter = ProjectileMotion(0.02)
 
-robot_shooter.graph_note(Model(robotPosition,goal,rpm))
+plot(robot_shooter.getpoints(Vector(setangle,info.get_vel(setrpm)),info.rpos) , info.rpos,info.gpos,robot_shooter.dt)