#robot.arm_bus.write("Acceleration", [20], ["shoulder_pitch"]) ####DEBUGGER#################### # joint = input("Enter joint name: ") # encoder = EncoderReader("/dev/ttyUSB1", 115200) # start_angle = arm_calibration['start_pos'][arm_calibration['motor_names'].index(joint)] # end_angle = arm_calibration['end_pos'][arm_calibration['motor_names'].index(joint)] # # start_angle = shoulder_calibration['start_pos'][shoulder_calibration['motor_names'].index(joint)] # # end_angle = shoulder_calibration['end_pos'][shoulder_calibration['motor_names'].index(joint)] # while True: # angle = int(start_angle+(end_angle-start_angle)*encoder.read()/1000) # # robot.shoulder_bus.set_bus_baudrate(115200) # # robot.shoulder_bus.write("Goal_Position",angle, [joint]) # robot.shoulder_bus.set_bus_baudrate(1000000) # robot.arm_bus.write("Goal_Position",angle, [joint]) # print(angle) # time.sleep(0.1) #####SAFETY CHECKS EXPLAINED##### #There are two safety checks built-in: one is based on load and the other is based on current. #Current: if Protection_Current > Present_Current we wait Over_Current_Protection_Time (expressed in ms) and set Torque_Enable to 0 #Load: if Max_Torque_Limit*Overload_Torque (expressed as a percentage) > Present_Load, we wait Protection_Time (expressed in ms #and set Max_Torque_Limit to Protective_Torque) #Though we can specify Min-Max_Angle_Limit, Max_Temperature_Limit, Min-Max_Voltage_Limit, no safety checks are implemented for these values #robot.arm_bus.set_calibration(arm_calibration) #method 1 # robot.arm_bus.write("Overload_Torque", 80) # robot.arm_bus.write("Protection_Time", 10) # robot.arm_bus.write("Protective_Torque", 1) # robot.arm_bus.write("Protection_Current", 200,["shoulder_pitch"]) # robot.arm_bus.write("Over_Current_Protection_Time", 10) #method 2 # robot.arm_bus.write("Protection_Current", 500,["shoulder_pitch"]) # robot.arm_bus.write("Over_Current_Protection_Time", 10) # robot.arm_bus.write("Max_Torque_Limit", 1000) # robot.arm_bus.write("Overload_Torque", 40) # robot.arm_bus.write("Protection_Time", 10) # robot.arm_bus.write("Protective_Torque", 1) # robot.shoulder_bus.set_bus_baudrate(115200) # robot.shoulder_bus.write("Goal_Position",2500) # exit() ######LOGGER#################### # from test_torque.log_and_plot_feetech import log_and_plot_params # params_to_log = [ # "Protection_Current", # "Present_Current", # "Max_Torque_Limit", # "Protection_Time", # "Overload_Torque", # "Present_Load", # "Present_Position", # ] # servo_names = ["shoulder_pitch"] # servo_data, timestamps = log_and_plot_params(robot.shoulder_bus, params_to_log, servo_names, test_id="shoulder_pitch") # exit() #robot.arm_bus.write("Goal_Position",2300, ["shoulder_pitch"]) # dt = 2 # steps = 4 # max_pos = 1500 # min_pos = 2300 # increment = (max_pos - min_pos) / steps # # Move from min_pos to max_pos in steps # for i in range(steps + 1): # Include the last step # current_pos = min_pos + int(i * increment) # robot.arm_bus.write("Goal_Position", [current_pos], ["shoulder_pitch"]) # time.sleep(dt) # # Move back from max_pos to min_pos in steps # for i in range(steps + 1): # Include the last step # current_pos = max_pos - int(i * increment) # robot.arm_bus.write("Goal_Position", [current_pos], ["shoulder_pitch"]) # time.sleep(dt)shoulder_pitch #demo to show how sending a lot of values makes the robt shake # # Step increment # # # Move from min_pos to max_pos in steps # for i in range(steps + 1): # Include the last step # current_pos = min_pos + int(i * increment) # robot.arm_bus.write("Goal_Position", [current_pos], ["elbow_flex"]) # time.sleep(dt) # # Move back from max_pos to min_pos in steps # for i in range(steps + 1): # Include the last step # current_pos = max_pos - int(i * increment) # robot.arm_bus.write("Goal_Position", [current_pos], ["elbow_flex"]) # time.sleep(dt) # exit() #robot.arm_bus.write("Goal_Position", a # shoulder_calibration = robot.get_shoulder_calibration() # print(shoulder_calibration)m_calibration["start_pos"]) # robot.arm_bus.write("Over_Current_Protection_Time", 50) # robot.arm_bus.write("Protection_Current", 310, ["shoulder_pitch"]) # robot.arm_bus.write("Overload_Torque", 80, ["shoulder_pitch"]) # robot.arm_bus.write("Protection_Time", 100, ["shoulder_pitch"]) # robot.arm_bus.write("Over_Current_Protection_Time", 50, ["shoulder_pitch"]) # robot.arm_bus.write("Protective_Torque", 20, ["shoulder_pitch"]) # robot.arm_bus.write("Goal_Position", [600],["shoulder_pitch"]) # from test_torque.log_and_plot_feetech import log_and_plot_params # params_to_log = [ # "Present_Current", # "Protection_Current", # "Overload_Torque", # "Protection_Time", # "Protective_Torque", # "Present_Load", # "Present_Position", # ] # servo_names = ["shoulder_pitch"] # #robot.arm_bus.write("Goal_Position", arm_calibration["start_pos"]) #robot.hand_bus.set_calibration(hand_calibration) #interp = 0.3 #robot.arm_bus.write("Goal_Position", [int((i*interp+j*(1-interp))) for i, j in zip(arm_calibration["start_pos"], arm_calibration["end_pos"])]) #exit() # glove = HomonculusGlove() # glove.run_calibration() ####GOOD FOR GRASPING # start_pos = [ # 500, # 900, # 500, # 1000, # 100, # 450,#250 # 950,#750 # 100, # 300,#400 # 50,#150 # 100, # 120, # 980, # 100, # 950, # 750, # ] # end_pos = [ # start_pos[0] - 400, # start_pos[1] - 300, # start_pos[2] + 500, # start_pos[3] - 50, # start_pos[4] + 900, # start_pos[5] + 500, # start_pos[6] - 500, # start_pos[7] + 900, # start_pos[8] + 700, # start_pos[9] + 700, # start_pos[10] + 900, # start_pos[11] + 700, # start_pos[12] - 700, # start_pos[13] + 900, # start_pos[14] - 700, # start_pos[15] - 700, # ] SCS_SERIES_CONTROL_TABLE = { # "Max_Torque_Limit": (16, 2), # "Phase": (18, 1), # "Unloading_Condition": (19, 1), "Protective_Torque": (37, 1), "Protection_Time": (38, 1), #Baud_Rate": (48, 1), } def read_and_print_scs_values(robot): for param_name in SCS_SERIES_CONTROL_TABLE: value = robot.hand_bus.read(param_name) print(f"{param_name}: {value}") motor_1_values = { "Lock" : 255, #"Protection_Time": 20#so if you write to these they turn to 0 for some fucking reason. protection time was 100, procetive to } # motor_1_values = { # "Lock": 1, # "Protection_Time": 100, # "Protective_Torque": 20, # "Phase": 1,#thisu is bullshit # "Unloading_Condition": 32, # } #bug in writing to specific values of the scs0009 # Write values to motor 2, there is overload torque there #ok so i can write, the jittering is because of the overload torque which is still being triggered #TODO: i have to write a functioining version for the sc009 (or i dont who cares)