def update_positions(self, pos_dict):
''' This method will update the positions of the pitch objects
that it gets passed by the vision system '''
robots = {'our_attacker':self.our_attacker,'their_attacker':self.their_attacker , 'our_defender':self.our_defender ,'their_defender':self.their_defender}
# Using these it should be possible to predict the position and angle of our robot
# Currently is set to predict 20 frames ahead
last_x = self.our_attacker.vector.x
last_y = self.our_attacker.vector.y
last_angle = self.our_attacker.vector.angle
x = pos_dict['our_attacker'].x
y = pos_dict['our_attacker'].y
a = pos_dict['our_attacker'].angle
dx = x - last_x
dy = y - last_y
dangle = a - last_angle
'''
x = World.low_pass(x, self.x)
y = World.low_pass(y, self.x)
a = World.low_pass(a, self.x)
dx = World.low_pass(dx, self.x)
dy = World.low_pass(dy, self.x)
dangle = World.low_pass(dangle, self.x)
predicted_pos = self.Kp.n_frames(20, [x,y,dx,dy])
predicted_angle = self.Ka.n_frames(20, [a,0,dangle,0])
self.our_attacker.predicted_vector = Vector(predicted_pos[0],predicted_pos[1],predicted_angle[0],0)
# Currently doesnt use any predicted velocity. If any reason to use this were to be found feel free to add
'''
# Update robot positions
for key in robots.keys():
robots[key].vector = pos_dict[key]
# Update ball
self.ball.vector = pos_dict['ball']
FrameEst.update()
self.get_future()
def get_future(self):
oa = self.our_attacker
cd, ca = ([oa._vector.x, oa._vector.y], oa._vector.angle)
return FrameEst.get_rot_future(cd, ca)
