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