def setPose(self, x, y):
MotorDriver.resetWristHeight(self)
qBase, qElbow, qWrist = self.sLineTraj(x, y)
for i in range(len(qBase)):
MotorDriver.setBaseAngle(self, qBase[i])
MotorDriver.setElbowAngle(self, qElbow[i])
MotorDriver.setWristAngle(self, qWrist[i])
if (x < -0.01):
MotorDriver.setWristHeight(self, WRIST_TO_BOARD_HEIGHT + 15)
else:
MotorDriver.setWristHeight(self, WRIST_TO_BOARD_HEIGHT)
# input()
MotorDriver.eeDropDisk(self)
MotorDriver.resetWristHeight(self)
if ((self.curPos[0] != self.HOME_POS[0])
and (self.curPos[1] != self.HOME_POS[1])):
qBase, qElbow, qWrist = self.sLineTraj(self.HOME_POS[0],
self.HOME_POS[1])
for i in range(len(qBase)):
MotorDriver.setBaseAngle(self, qBase[i])
MotorDriver.setElbowAngle(self, qElbow[i])
MotorDriver.setWristAngle(self, qWrist[i])
time.sleep(0.2)
return qBase, qElbow, qWrist
def setPose(self, x, y):
MotorDriver.resetWristHeight(
self) # set linear actuator to home position
qBase, qElbow, qWrist = self.sLineTraj(
x, y) # get straight-line trajectory
#___drive motors with trajectory arrays_______#
for i in range(len(qBase)): #
MotorDriver.setBaseAngle(self, qBase[i]) #
MotorDriver.setElbowAngle(self, qElbow[i]) #
MotorDriver.setWristAngle(self, qWrist[i]) #
#---------------------------------------------#
#___move linear actuator closer to the connect 4 game board___#
if (x < -0.14): #
MotorDriver.setWristHeight(self, WRIST_TO_BOARD_HEIGHT + 15) #
else: #
MotorDriver.setWristHeight(self, WRIST_TO_BOARD_HEIGHT) #
#-------------------------------------------------------------#
# input() # uncomment to stop the robot arm when the EE reaches the game board
MotorDriver.resetWristHeight(
self) # reset linear actuator to home position
#___if current position is not home position, move the robot back to home position___#
if ((self.curPos[0] != self.HOME_POS[0])
and (self.curPos[1] != self.HOME_POS[1])): #
qBase, qElbow, qWrist = self.sLineTraj(self.HOME_POS[0],
self.HOME_POS[1]) #
#
for i in range(len(qBase)): #
MotorDriver.setBaseAngle(self, qBase[i]) #
MotorDriver.setElbowAngle(self, qElbow[i]) #
MotorDriver.setWristAngle(self, qWrist[i]) #
time.sleep(0.2) #
#------------------------------------------------------------------------------------#
return qBase, qElbow, qWrist # return last trajectory calculated, not really required at the moment
