def update(self,
end_position=None,
end_normal=None,
end_direction=None,
arm_size=None):
if end_position is None:
end_position = self.end_position
if end_normal is None:
end_normal = self.end_normal
if end_direction is None:
end_direction = self.end_direction
if arm_size is None:
arm_size = self.arm_size
arm_calculus = bib.arm(end_position, end_normal, end_direction,
arm_size)
if not arm_calculus.solve_min():
print("Position, normal and direction unreachable !")
return False
for i in range(len(self.bras.arm)):
if not ((arm_calculus.thetas[i] >= self.angle_limit[i][0]) and
(arm_calculus.thetas[i] <= self.angle_limit[i][1])):
print("Angle limit is reached !")
return False
self.end_position = bib.vec(end_position)
self.end_normal = bib.vec(end_normal)
self.end_direction = bib.vec(end_direction)
self.arm_size = arm_size
for i in range(len(self.bras.arm)):
self.set_angle(i, arm_calculus.thetas[i])
return True
def increase(self, end_position=None, end_normal=None, end_direction=None):
if not end_position is None :
end_position = bib.vec(self.end_position) + end_position
if not end_normal is None :
end_normal = bib.vec(self.end_normal) + end_normal
if not end_direction is None :
end_direction = bib.vec(self.end_direction) + end_direction
return self.update(end_position=end_position, end_normal=end_normal,end_direction=end_direction)
def walkto_point(Robot, M0, M5, Position):
"""
Position should be 3D coordinate in the form of a list [x,y,z]
"""
orientation = checkmagnets(M0, M5)
if orientation == 1:
Robot.update(Position, [0, 0, 1], [0, 1, 0], Robot.arm_size)
time.sleep(8)
M5.Turn_ON()
M0.Turn_OFF()
if orientation == 2: #NOT FINISHED, NEED TO FIX THE DIRECTION TODO
Direction = bib.normalize(bib.vec(Position))
#Robot.update(Position,[0,0,1],[0,1,0],Robot.arm_size)
Robot.update(Position, [0, 0, 1], Direction,
Robot.arm_size) #NEED TO TEST
time.sleep(8)
M5.Turn_ON()
M0.Turn_OFF()
Robot.update([0, 0, sum(Robot.arm_size)], [0, 0, -1], [0, 1, 0],
Robot.arm_size) #ARM PUTS ITSELF UPRIGHT
time.sleep(8)
"""