def read_all(self):
# write test
if self.trace.variables["square_current"].get() == 1:
value = self.trace.test_square_current()
if value != 0:
print("write RAM...")
self.protocol.write_word_command(self.id.current_id,
0x47, [value],
verbose=1)
# write test
elif self.trace.variables["square_position"].get() == 1:
value = self.trace.test_square_position()
if value != 0:
print("write RAM...")
self.protocol.write_word_command(self.id.current_id,
0x43, [value],
verbose=1)
elif self.trace.variables["triangle_position"].get() == 1:
value = self.trace.test_triangle_position()
if value != 0:
print("write RAM...")
self.protocol.write_word_command(self.id.current_id,
0x43, [value],
verbose=1)
elif self.trace.variables["round_position"].get() == 1:
update, servo_angles = self.trace.test_round_position()
if update:
#print("write RAM...")
self.protocol.write_word_command(
2,
0x43, [int((servo_angles[0, 0] + 45.0) * 10.0)],
verbose=0)
self.protocol.write_word_command(
1,
0x43, [int((135.0 - servo_angles[1, 0]) * 10.0)],
verbose=0)
# send read command
if (self.counter % 100) == 0:
verb = 1
end_time = time.time() * 1000.0
print("delay for 100 iterations:" +
str(end_time - self.start_time) + "ms")
self.start_time = end_time
else:
verb = 0
error, result = self.protocol.read_byte_command(
self.id.current_id, # ID
0x40, # from EEPROM
54, # byte number to read
verbose=verb) # TODO change ID through GUI
if error != 0:
print("error:" + str(error))
elif len(result) == 54:
goal_position = float((result[3] + (result[4] << 8)) / 10.0)
setpoint_position = float((result[22] + (result[23] << 8)) / 10.0)
present_position = float((result[13] + (result[14] << 8)) / 10.0)
goal_velocity = float(sign(result[5] + (result[6] << 8)))
setpoint_velocity = float(sign(result[24] + (result[25] << 8)))
present_velocity = float(sign(result[15] + (result[16] << 8)))
goal_current = float(sign(result[7] + (result[8] << 8)))
setpoint_current = float(sign(result[26] + (result[27] << 8)))
present_current = float(sign(result[17] + (result[18] << 8)))
goal_pwm = float(sign(result[9] + (result[10] << 8)))
setpoint_pwm = float(sign(result[28] + (result[29] << 8)))
self.trace.update(goal_position, setpoint_position,
present_position, goal_velocity,
setpoint_velocity, present_velocity,
goal_current, setpoint_current, present_current,
goal_pwm, setpoint_pwm)
if self.counter == 0:
self.variables['led_local'].set(str(result[1]))
self.variables['control_mode_local'].set(str(result[2]))
self.variables['goal_position_local'].set(str(goal_position))
self.variables['goal_velocity_local'].set(str(goal_velocity))
self.variables['goal_current_local'].set(str(goal_current))
self.variables['goal_pwm_local'].set(str(goal_pwm))
self.variables['torque_enable_servo'].set(str(result[0]))
self.variables['led_servo'].set(str(result[1]))
self.variables['control_mode_servo'].set(str(result[2]))
self.variables['goal_position_servo'].set(str(goal_position))
self.variables['goal_velocity_servo'].set(str(goal_velocity))
self.variables['goal_current_servo'].set(str(goal_current))
self.variables['goal_pwm_servo'].set(str(goal_pwm))
self.variables['present_position_servo'].set(str(present_position))
self.variables['present_velocity_servo'].set(str(present_velocity))
self.variables['present_current_servo'].set(str(present_current))
self.variables['present_voltage_servo'].set(str(result[19]))
self.variables['present_temperature_servo'].set(str(result[20]))
self.variables['moving_servo'].set(str(result[21]))
self.variables['setpoint_position_servo'].set(
str(setpoint_position))
self.variables['setpoint_velocity_servo'].set(
str(setpoint_velocity))
self.variables['setpoint_current_servo'].set(str(setpoint_current))
self.variables['setpoint_pwm_servo'].set(str(setpoint_pwm))
self.variables['motor_current_input_adc_servo'].set(
str(result[30] + (result[31] << 8)))
self.variables['motor_current_input_adc_offset_servo'].set(
str(sign(result[32] + (result[33] << 8))))
self.variables['position_input_adc_servo'].set(
str(result[34] + (result[35] << 8)))
self.variables['voltage_input_adc_servo'].set(
str(result[36] + (result[37] << 8)))
self.variables['protocol_crc_fail_servo'].set(str(result[52]))
self.variables['hardware_error_status_servo'].set(str(result[53]))
self.data_ready = 1
self.counter += 1
self.after(1, self.read_all)
def read_all(self):
if self.protocol:
print("real all EEPROM...")
# send read command
error, result = self.protocol.read_byte_command(
self.id.current_id, # ID
0x00, # from EEPROM
85, # byte number to read
verbose=1) # TODO change ID through GUI
if error != 0:
print("error:" + str(error))
elif len(result) == 85:
self.variables['model_number_servo'].set(
str(result[0] + (result[1] << 8)))
self.variables['version_servo'].set(str(result[2]))
self.variables['id_local'].set(str(result[3]))
self.variables['id_servo'].set(str(result[3]))
self.variables['baud_rate_local'].set(str(result[4]))
self.variables['baud_rate_servo'].set(str(result[4]))
#self.variables['return_delay_local'].set(str(result[5]))
#self.variables['return_delay_servo'].set(str(result[5]))
self.variables['min_position_local'].set(
str(sign(result[16] + (result[17] << 8))))
self.variables['min_position_servo'].set(
str(sign(result[16] + (result[17] << 8))))
self.variables['max_position_local'].set(
str(sign(result[18] + (result[19] << 8))))
self.variables['max_position_servo'].set(
str(sign(result[18] + (result[19] << 8))))
self.variables['max_velocity_local'].set(
str(result[20] + (result[21] << 8)))
self.variables['max_velocity_servo'].set(
str(result[20] + (result[21] << 8)))
#self.variables['max_acceleration_local'].set(str(result[22] + (result[23]<<8)))
#self.variables['max_acceleration_servo'].set(str(result[22] + (result[23]<<8)))
self.variables['max_current_local'].set(
str(result[24] + (result[25] << 8)))
self.variables['max_current_servo'].set(
str(result[24] + (result[25] << 8)))
self.variables['max_temperature_local'].set(str(result[28]))
self.variables['max_temperature_servo'].set(str(result[28]))
self.variables['min_voltage_local'].set(str(result[29]))
self.variables['min_voltage_servo'].set(str(result[29]))
self.variables['max_voltage_local'].set(str(result[30]))
self.variables['max_voltage_servo'].set(str(result[30]))
#self.variables['moving_threshold_local'].set(str(result[31]))
#self.variables['moving_threshold_servo'].set(str(result[31]))
#self.variables['status_return_level_local'].set(str(result[32]))
#self.variables['status_return_level_servo'].set(str(result[32]))
#self.variables['alarm_led_local'].set(str(result[33]))
#self.variables['alarm_led_servo'].set(str(result[33]))
#self.variables['alarm_shutdown_local'].set(str(result[34]))
#self.variables['alarm_shutdown_servo'].set(str(result[34]))
#self.variables['encoder_bits_local'].set(str(result[35]))
#self.variables['encoder_bits_servo'].set(str(result[35]))
self.variables['motor_pole_pairs_local'].set(str(result[36]))
self.variables['motor_pole_pairs_servo'].set(str(result[36]))
self.variables['motor_synchro_local'].set(
str(result[37] + (result[38] << 8)))
self.variables['motor_synchro_servo'].set(
str(result[37] + (result[38] << 8)))
self.variables['inv_phase_motor_local'].set(str(result[40]))
self.variables['inv_phase_motor_servo'].set(str(result[40]))
#self.variables['field_weaknening_k_local'].set(str(result[41]))
#self.variables['field_weaknening_k_servo'].set(str(result[41]))
#self.variables['pid_position_kp_local'].set(str(result[42] + (result[43]<<8)))
#self.variables['pid_position_kp_servo'].set(str(result[42] + (result[43]<<8)))
#self.variables['pid_position_ki_local'].set(str(result[44] + (result[45]<<8)))
#self.variables['pid_position_ki_servo'].set(str(result[44] + (result[45]<<8)))
#self.variables['pid_position_kd_local'].set(str(result[46] + (result[47]<<8)))
#self.variables['pid_position_kd_servo'].set(str(result[46] + (result[47]<<8)))
#self.variables['pid_velocity_kp_local'].set(str(result[48] + (result[49]<<8)))
#self.variables['pid_velocity_kp_servo'].set(str(result[48] + (result[49]<<8)))
#self.variables['pid_velocity_ki_local'].set(str(result[50] + (result[51]<<8)))
#self.variables['pid_velocity_ki_servo'].set(str(result[50] + (result[51]<<8)))
#self.variables['pid_velocity_kd_local'].set(str(result[52] + (result[53]<<8)))
#self.variables['pid_velocity_kd_servo'].set(str(result[52] + (result[53]<<8)))
#self.variables['pid_velocity_kff_local'].set(str(result[54] + (result[55]<<8)))
#self.variables['pid_velocity_kff_servo'].set(str(result[54] + (result[55]<<8)))
#self.variables['pid_acceleration_kff_local'].set(str(result[56] + (result[57]<<8)))
#self.variables['pid_acceleration_kff_servo'].set(str(result[56] + (result[57]<<8)))
self.variables['pid_flux_current_kp_local'].set(
str(result[58] + (result[59] << 8)))
self.variables['pid_flux_current_kp_servo'].set(
str(result[58] + (result[59] << 8)))
self.variables['pid_flux_current_ki_local'].set(
str(result[60] + (result[61] << 8)))
self.variables['pid_flux_current_ki_servo'].set(
str(result[60] + (result[61] << 8)))
#self.variables['pid_flux_current_kff_local'].set(str(result[62] + (result[63]<<8)))
#self.variables['pid_flux_current_kff_servo'].set(str(result[62] + (result[63]<<8)))
self.variables['pid_torque_current_kp_local'].set(
str(result[64] + (result[65] << 8)))
self.variables['pid_torque_current_kp_servo'].set(
str(result[64] + (result[65] << 8)))
self.variables['pid_torque_current_ki_local'].set(
str(result[66] + (result[67] << 8)))
self.variables['pid_torque_current_ki_servo'].set(
str(result[66] + (result[67] << 8)))
def read_all(self):
if self.protocol:
# write test
if self.trace.variables["square_position"].get() == 1:
value = self.trace.test_square_position()
if value != 0:
print("write RAM...")
self.protocol.write_word_command(self.id.current_id,0x83,[value],verbose=1)
elif self.trace.variables["triangle_position"].get() == 1:
value = self.trace.test_triangle_position()
if value != 0:
print("write RAM...")
self.protocol.write_word_command(self.id.current_id,0x83,[value],verbose=1)
elif self.trace.variables["sinus_position"].get() == 1:
value = self.trace.test_sinus_position()
if value != 0:
print("write RAM...")
self.protocol.write_word_command(self.id.current_id,0x83,[value],verbose=1)
# send read command
if (self.counter%100)==0:
verb = 1
end_time = time.time()*1000.0
print("delay for 100 iterations:" + str(end_time-self.start_time) + "ms")
self.start_time = end_time
else:
verb = 0
error, result = self.protocol.read_byte_command(
self.id.current_id, # ID
0x80, # from EEPROM
50, # byte number to read
verbose=verb
) # TODO change ID through GUI
if error != 0 :
print("error:"+str(error))
elif len(result)==50:
goal_position = float(sign(result[3] + (result[4]<<8))/10.0)
setpoint_position = float(sign(result[32] + (result[33]<<8))/10.0)
present_position = float(sign(result[16] + (result[17]<<8))/10.0)
goal_velocity = float(sign(result[5] + (result[6]<<8)))
setpoint_velocity = float(sign(result[34] + (result[35]<<8)))
present_velocity = float(sign(result[18] + (result[19]<<8)))
goal_torque_current = float(sign(result[7] + (result[8]<<8)))
self.setpoint_torque_current = (1.0-self.alpha)*self.setpoint_torque_current+self.alpha*float(sign(result[36] + (result[37]<<8)))
self.present_torque_current = (1.0-self.alpha)*self.present_torque_current+self.alpha*float(sign(result[20] + (result[21]<<8)))
goal_flux_current = float(sign( result[9] + (result[10]<<8)))
self.setpoint_flux_current = (1.0-self.alpha)*self.setpoint_flux_current+self.alpha*float(sign( result[38] + (result[39]<<8)))
self.present_flux_current = (1.0-self.alpha)*self.present_flux_current+self.alpha*float(sign( result[22] + (result[23]<<8)))
pos_kp = result[11]
pos_kd = result[12]
vel_kp = result[13]
goal_synchro_offset = float(sign( result[14] + (result[15]<<8)))
self.trace.update(
goal_position,
setpoint_position,
present_position,
goal_velocity,
setpoint_velocity,
present_velocity,
goal_torque_current,
self.setpoint_torque_current,
self.present_torque_current,
goal_flux_current,
self.setpoint_flux_current,
self.present_flux_current
)
if self.counter == 0:
self.variables['torque_enable_local'].set(str(result[0]))
self.variables['led_local'].set(str(result[1]))
#self.variables['control_mode_local'].set(str(result[2]))
self.variables['goal_position_local'].set(str( goal_position ))
self.variables['goal_velocity_local'].set(str( goal_velocity ))
self.variables['goal_torque_current_local'].set(str( goal_torque_current ))
self.variables['goal_flux_current_local'].set(str( goal_flux_current ))
self.variables['goal_pos_kp_local'].set(str( pos_kp ))
self.variables['goal_pos_kd_local'].set(str( pos_kd ))
self.variables['goal_vel_kp_local'].set(str( vel_kp ))
self.variables['goal_synchro_offset_local'].set(str( goal_synchro_offset ))
#self.variables['goal_open_loop_local'].set(str( result[15] ))
self.variables['torque_enable_servo'].set(str(result[0]))
self.variables['led_servo'].set(str(result[1]))
self.variables['control_mode_servo'].set(str(result[2]))
self.variables['goal_position_servo'].set(str( goal_position ))
self.variables['goal_velocity_servo'].set(str( goal_velocity ))
self.variables['goal_torque_current_servo'].set(str( goal_torque_current ))
self.variables['goal_flux_current_servo'].set(str( goal_flux_current ))
self.variables['goal_pos_kp_servo'].set(str( pos_kp ))
self.variables['goal_pos_kd_servo'].set(str( pos_kd ))
self.variables['goal_vel_kp_servo'].set(str( vel_kp ))
self.variables['goal_synchro_offset_servo'].set(str( goal_synchro_offset ))
#self.variables['goal_open_loop_servo'].set(str( result[15] ))
self.variables['present_position_servo'].set(str( present_position ))
self.variables['present_velocity_servo'].set(str( present_velocity ))
self.variables['present_torque_current_servo'].set(str( int(self.present_torque_current) ))
self.variables['present_flux_current_servo'].set(str( int(self.present_flux_current) ))
self.variables['present_voltage_servo'].set(str(result[24]))
self.variables['present_temperature_servo'].set(str(result[25]))
self.variables['moving_servo'].set(str(result[26]))
self.variables['setpoint_position_servo'].set(str( setpoint_position ))
self.variables['setpoint_velocity_servo'].set(str( setpoint_velocity ))
self.variables['setpoint_torque_current_servo'].set(str( int(self.setpoint_torque_current) ))
self.variables['setpoint_flux_current_servo'].set(str( int(self.setpoint_flux_current) ))
self.variables['processing_time_servo'].set(str(result[42]))
self.variables['foc_frequency_servo'].set(str(result[43]))
self.variables['pid_frequency_servo'].set(str(result[44]))
self.variables['mlp_frequency_servo'].set(str(result[45]))
self.variables['protocol_crc_fail_servo'].set(str(result[48]))
#self.variables['hardware_error_status_servo'].set(str(result[49]))
# process ERRORS
HW_ERROR_BIT_VOLTAGE = 0
HW_ERROR_BIT_POSITION_SENSOR_STATUS_ERROR = 1
HW_ERROR_BIT_POSITION_SENSOR_NOT_RESPONDING = 2
HW_ERROR_BIT_POSITION_SENSOR_TIMESTAMP = 3
HW_ERROR_BIT_FOC_TIMEOUT = 4
HW_ERROR_BIT_OVERLOAD = 5
HW_ERROR_BIT_OVERHEATING = 6
error_code = result[49]
error_str = ""
if error_code & (1<<HW_ERROR_BIT_VOLTAGE):
error_str += "VOLTAGE "
if error_code & (1<<HW_ERROR_BIT_POSITION_SENSOR_STATUS_ERROR):
error_str += "POS_SYS_ERR "
if error_code & (1<<HW_ERROR_BIT_POSITION_SENSOR_NOT_RESPONDING):
error_str += "POS_TIMEOUT "
if error_code & (1<<HW_ERROR_BIT_POSITION_SENSOR_TIMESTAMP):
error_str += "POS_INTERUPT "
if error_code & (1<<HW_ERROR_BIT_FOC_TIMEOUT):
error_str += "FOC_TIMEOUT "
if error_code & (1<<HW_ERROR_BIT_OVERLOAD):
error_str += "OVERLOAD "
if error_code & (1<<HW_ERROR_BIT_OVERHEATING):
error_str += "OVERHEAT "
self.variables['hardware_error_status_servo'].set(error_str)
self.data_ready = 1
self.counter += 1
else:
print("None")
self.after(1,self.read_all)