diff --git a/Massage/MassageControl/hardware/dobot_nova5.py b/Massage/MassageControl/hardware/dobot_nova5.py index b13cbd9..2b4c542 100644 --- a/Massage/MassageControl/hardware/dobot_nova5.py +++ b/Massage/MassageControl/hardware/dobot_nova5.py @@ -152,7 +152,8 @@ class dobot_nova5: atexit.register(self.exit_task) self.chooseRightFrame() self.init_pos = [-45.0009079*np.pi/180,55.5785141*np.pi/180,-120.68821716*np.pi/180,5.11103201*np.pi/180,90.00195312*np.pi/180,-90.00085449*np.pi/180] - self.standby_pos = [-35.0009079*np.pi/180,45.5785141*np.pi/180,-110.68821716*np.pi/180,2.11103201*np.pi/180,80.00195312*np.pi/180,-80.00085449*np.pi/180] + self.standby_pos = [-44.9244*np.pi/180, 29.5429*np.pi/180, -88.4464*np.pi/180, -1.7062*np.pi/180, 87.7421*np.pi/180, -90.0*np.pi/180] + # self.standby_pos = [-35.0009079*np.pi/180,45.5785141*np.pi/180,-110.68821716*np.pi/180,2.11103201*np.pi/180,80.00195312*np.pi/180,-80.00085449*np.pi/180] self.off_pos = [-44.996*np.pi/180,87.0092*np.pi/180,-147.0087*np.pi/180,-0.0048*np.pi/180,90.0010*np.pi/180,-90.0021*np.pi/180] self.cam_pose = [-45.0050*np.pi/180,30.0682*np.pi/180,-66.1887*np.pi/180,-21.8836*np.pi/180,-90.3064*np.pi/180,-89.9977*np.pi/180] self.cam_length = 1.75 @@ -940,7 +941,7 @@ if __name__ == "__main__": time.sleep(1) print('successs!') # joint = [-45.0050*np.pi/180,30.0682*np.pi/180,-66.1887*np.pi/180,-21.8836*np.pi/180,-90.3064*np.pi/180,-89.9977*np.pi/180] - # # dobot.move_joint(q = joint) + dobot.move_joint(q = dobot.cam_pose) # time.sleep(1) # joint1 = [-44.8416*np.pi/180, 39.5626*np.pi/180, -80.5683*np.pi/180, -18.0396*np.pi/180, -89.4168*np.pi/180, -90.0026*np.pi/180] @@ -956,10 +957,12 @@ if __name__ == "__main__": while True: time.sleep(1) except KeyboardInterrupt: + print("joint:",dobot.getAngel()) # robot_thread.join() dobot.stop_drag() print("用户中断操作。") except Exception as e: + print("joint:",dobot.getAngel()) # robot_thread.join() dobot.stop_drag() print("Exception occurred at line:", e.__traceback__.tb_lineno)