diff --git a/src/main/python/modeling/shooter_model.py b/src/main/python/modeling/shooter_model.py index a6295fc0..53f13a71 100644 --- a/src/main/python/modeling/shooter_model.py +++ b/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)