Index: dialin/hd/valves.py =================================================================== diff -u -r2c016f17a5468be8add2a467c989c19f69d501df -r39797925579b6770fbb48e634d73f518ab417d37 --- dialin/hd/valves.py (.../valves.py) (revision 2c016f17a5468be8add2a467c989c19f69d501df) +++ dialin/hd/valves.py (.../valves.py) (revision 39797925579b6770fbb48e634d73f518ab417d37) @@ -4,8 +4,9 @@ from ..utils.conversions import integer_to_bytearray from .constants import NO_RESET from ..protocols.CAN import (DenaliMessage, DenaliChannels) +from ..utils.base import _AbstractSubSystem, _publish -class HDValves: +class HDValves(_AbstractSubSystem): """ \class HDValves @@ -14,31 +15,61 @@ # Valve state message IDs MSG_ID_HD_VALVES_DATA = 0x3A - MSG_ID_HD_VALVES_SEND_INTERVAL_OVERRIDE = 0xA022 - MSG_ID_HD_VALVES_HOME = 0xA023 + MSG_ID_HD_VALVES_HOME = 0x802E + MSG_ID_HD_VALVES_SET_BLOOD_TRAP_VALVE = 0x8030 + MSG_ID_HD_VALVES_POSITION_OVERRIDE = 0x802F + MSG_ID_HD_VALVES_SET_PWM_OVERRIDE = 0x8031 + MSG_ID_VALVES_STATES_PUBLISH_INTERVAL_OVERRIDE = 0xA00F + MSG_ID_TEMP_FAST_HD_VALVES_REMOVE_LATER = 0x6D # TODO Remove + # Valves states publish message field positions START_POS_VALVES_ID = DenaliMessage.PAYLOAD_START_INDEX END_POS_VALVES_ID = START_POS_VALVES_ID + 4 + START_VALVES_STATE = END_POS_VALVES_ID END_VALVES_STATE = START_VALVES_STATE + 4 + START_POS_VALVES_CURR_POS = END_VALVES_STATE END_POS_VALVES_CURR_POS = START_POS_VALVES_CURR_POS + 4 + START_POS_VALVES_CURR_POS_CNT = END_POS_VALVES_CURR_POS END_POS_VALVES_CURR_POS_CNT = START_POS_VALVES_CURR_POS_CNT + 2 + START_POS_VALVES_NEXT_POS_CNT = END_POS_VALVES_CURR_POS_CNT END_POS_VALVES_NEXT_POS_CNT = START_POS_VALVES_NEXT_POS_CNT + 2 + START_POS_VALVES_CURRENT = END_POS_VALVES_NEXT_POS_CNT END_POS_VALVES_CURRENT = START_POS_VALVES_CURRENT + 4 - START_VALVES_PREV_POS_CNT = END_POS_VALVES_CURRENT - END_VALVES_PREV_POS_CNT = START_VALVES_PREV_POS_CNT + 2 + START_VALVES_POS_C = END_POS_VALVES_CURRENT + END_VALVES_POS_C = START_VALVES_POS_C + 2 + + START_VALVES_POS_A = END_VALVES_POS_C + END_VALVES_POS_A = START_VALVES_POS_A + 2 + + START_VALVES_POS_B = END_VALVES_POS_A + END_VALVES_POS_B = START_VALVES_POS_B + 2 + + START_VALVES_PWM = END_VALVES_POS_B + END_VALVES_PWM = START_VALVES_PWM + 4 + + # TODO Fast publish REMOVE LATER + START_FAST_VALVES_POS = DenaliMessage.PAYLOAD_START_INDEX + END_FAST_VALVES_POS = START_FAST_VALVES_POS + 2 + START_FAST_VALVES_CURR = END_FAST_VALVES_POS + END_FAST_VALVES_CURR = START_FAST_VALVES_CURR + 4 + START_FAST_VALVES_CMD = END_FAST_VALVES_CURR + END_FAST_VALVES_CMD = START_FAST_VALVES_CMD + 2 + # TODO Fast publish REMOVE LATER + def __init__(self, can_interface=None): """ DGDrainPump constructor \param outer_instance: reference to the DG (outer) class. """ + super().__init__() self.can_interface = can_interface if self.can_interface is not None: @@ -47,10 +78,16 @@ self.can_interface.register_receiving_publication_function(channel_id, msg_id, self.handler_hd_valves_sync) - self.states = ["VALVE_STATE_WAIT_FOR_POST", "VALVE_STATE_IDLE", "VALVE_STATE_IN_TRANSITION", - "VALVE_STATE_HOMING_NOT_STARTED", "VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE", - "VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE"] + # TODO remove + msg_id2 = self.MSG_ID_TEMP_FAST_HD_VALVES_REMOVE_LATER + self.can_interface.register_receiving_publication_function(channel_id, msg_id2, + self.handler_hd_valves_fast_temp_sync) + # TODO remove + self.states = ["VALVE_STATE_WAIT_FOR_POST", "VALVE_STATE_HOMING_NOT_STARTED", + "VALVE_STATE_HOMING_FIND_ENERGIZED_EDGE", "VALVE_STATE_HOMING_FIND_DEENERGIZED_EDGE", + "VALVE_STATE_IDLE", "VALVE_STATE_IN_TRANSITION", "VALVE_STATE_IN_BYPASS_MODE"] + self.positions = ["VALVE_POSITION_NOT_IN_POSITION", "VALVE_POSITION_A_INSERT_EJECT", "VALVE_POSITION_B_OPEN", "VALVE_POSITION_C_CLOSE"] @@ -60,19 +97,125 @@ self.hd_valve_state = 0 self.hd_valve_curr_pos_ID = 0 self.hd_valve_curr_pos_cnt = 0 - self.hd_valve_prev_pos_cnt = 0 self.hd_valve_next_pos_cnt = 0 self.hd_valve_current = 0 + self.hd_valves_pos_c = 0 + self.hd_valves_pos_a = 0 + self.hd_valves_pos_b = 0 + self.hd_valves_pwm = 0 - def cmd_home_hd_valve(self, valve): + #TODO Remove these later + self.hd_valve_fast_pos = 0 + self.hd_valve_fast_current = 0 + self.hd_valves_fast_cmd = 0 + + def cmd_temperature_sensors_broadcast_interval_override(self, ms, reset=NO_RESET): """ + Constructs and sends broadcast time interval + + @param ms: Publish time interval in ms + @param reset: integer - 1 to reset a previous override, 0 to override + @returns 1 if successful, zero otherwise + """ + reset_value = integer_to_bytearray(reset) + interval_value = integer_to_bytearray(ms) + payload = reset_value + interval_value + + message = DenaliMessage.build_message(channel_id=DenaliChannels.dialin_to_dg_ch_id, + message_id=self.MSG_ID_VALVES_STATES_PUBLISH_INTERVAL_OVERRIDE, + payload=payload) + + print("Sending {} ms publish interval to the HD valves module".format(ms)) + # Send message + received_message = self.can_interface.send(message) + + # If there is content in message + if received_message is not None: + # Response payload is OK or not + return received_message['message'][DenaliMessage.PAYLOAD_START_INDEX] + else: + print("Timeout!!!!") + return False + + def cmd_set_hd_valve_position(self, valve, position, reset=NO_RESET): + """ + Constructs and sends the HD valves set position for a valve + + \param valve: integer - Valve number: + VDI = 0 + VDO = 1 + VBA = 2 + VBV = 3 + \param position: integer - Position number: + VALVE_POSITION_A_INSERT_EJECT = 1 + VALVE_POSITION_B_OPEN = 2 + VALVE_POSITION_C_CLOSE = 3 + \returns 1 if successful, zero otherwise + """ + reset_value = integer_to_bytearray(reset) + vlv = integer_to_bytearray(valve) + pos = integer_to_bytearray(position) + payload = reset_value + pos + vlv + + message = DenaliMessage.build_message(channel_id=DenaliChannels.dialin_to_hd_ch_id, + message_id=self.MSG_ID_HD_VALVES_POSITION_OVERRIDE, + payload=payload) + # Send message + received_message = self.can_interface.send(message) + + # If there is content... + if received_message is not None: + # response payload is OK or not OK + return received_message['message'][DenaliMessage.PAYLOAD_START_INDEX] + else: + print("HD cmd_valve_override Timeout!!!") + return False + + def cmd_set_hd_valve_pwm(self, valve, pwm, direction, reset=NO_RESET): + """ Constructs and sends the HD valves home command - \param vavle: integer - Valve number to be homed (from 0 to 3) + \param valve: integer - Valve number: + VDI = 0 + VDO = 1 + VBA = 2 + VBV = 3 + \param direction: integer - Direction number: + 0 = Clockwise + 1 = Counter clockwise \returns 1 if successful, zero otherwise """ - #index = self.valves.index(valve) + reset_value = integer_to_bytearray(reset) + vlv = integer_to_bytearray(valve) + pwm = integer_to_bytearray(pwm) + dir = integer_to_bytearray(direction) + payload = reset_value + vlv + pwm + dir + message = DenaliMessage.build_message(channel_id=DenaliChannels.dialin_to_hd_ch_id, + message_id=self.MSG_ID_HD_VALVES_SET_PWM_OVERRIDE, + payload=payload) + # Send message + received_message = self.can_interface.send(message) + + # If there is content... + if received_message is not None: + # response payload is OK or not OK + return received_message['message'][DenaliMessage.PAYLOAD_START_INDEX] + else: + print("HD cmd_valve_override Timeout!!!") + return False + + def cmd_home_hd_valve(self, valve): + """ + Constructs and sends the HD valves home command + + \param vavle: integer - Valve number: + VDI = 0 + VDO = 1 + VBA = 2 + VBV = 3 + \returns 1 if successful, zero otherwise + """ payload = integer_to_bytearray(valve) message = DenaliMessage.build_message(channel_id=DenaliChannels.dialin_to_hd_ch_id, @@ -86,9 +229,58 @@ # response payload is OK or not OK return received_message['message'][DenaliMessage.PAYLOAD_START_INDEX] else: - print("HD cmd_valve_override Timeout!!!") + print("HD Homing Valve Timeout!!!") return False + def cmd_open_hd_air_trap_valve(self): + """ + Constructs and sends an open command to the HD air trap valve + + \returns 1 if successful, zero otherwise + """ + payload = integer_to_bytearray(1) + + message = DenaliMessage.build_message(channel_id=DenaliChannels.dialin_to_hd_ch_id, + message_id=self.MSG_ID_HD_VALVES_SET_BLOOD_TRAP_VALVE, + payload=payload) + # Send message + received_message = self.can_interface.send(message) + + # If there is content... + if received_message is not None: + print("Opening air trap valve") + # response payload is OK or not OK + return received_message['message'][DenaliMessage.PAYLOAD_START_INDEX] + else: + print("Opening air trap valve timeout!!") + return False + + def cmd_close_hd_air_trap_valve(self): + """ + Constructs and sends a stop command to the HD air trap valve + + \returns 1 if successful, zero otherwise + """ + payload = integer_to_bytearray(0) + + message = DenaliMessage.build_message(channel_id=DenaliChannels.dialin_to_hd_ch_id, + message_id=self.MSG_ID_HD_VALVES_SET_BLOOD_TRAP_VALVE, + payload=payload) + # Send message + received_message = self.can_interface.send(message) + + # If there is content... + if received_message is not None: + print("Closing air trap valve") + # response payload is OK or not OK + return received_message['message'][DenaliMessage.PAYLOAD_START_INDEX] + else: + print("Closing air trap valve timeout!!") + return False + + @_publish(["hd_valve_ID", "hd_valve_state", "hd_valve_curr_pos_ID", "hd_valve_curr_pos_cnt", + "hd_valve_next_pos_cnt", "hd_valve_current", "hd_valves_pos_c", "hd_valves_pos_a", + "hd_valves_pos_b", "hd_valves_pwm"]) def handler_hd_valves_sync(self, message): """ Handles published HD valves data messages. HD valves data are captured @@ -109,13 +301,38 @@ message['message'][self.START_POS_VALVES_NEXT_POS_CNT:self.END_POS_VALVES_NEXT_POS_CNT])) current = struct.unpack('f', bytearray( message['message'][self.START_POS_VALVES_CURRENT:self.END_POS_VALVES_CURRENT])) - prev_pos = struct.unpack('h', bytearray( - message['message'][self.START_VALVES_PREV_POS_CNT:self.END_VALVES_PREV_POS_CNT])) + pos_c = struct.unpack('h', bytearray( + message['message'][self.START_VALVES_POS_C:self.END_VALVES_POS_C])) + pos_a = struct.unpack('h', bytearray( + message['message'][self.START_VALVES_POS_A:self.END_VALVES_POS_A])) + pos_b = struct.unpack('h', bytearray( + message['message'][self.START_VALVES_POS_B:self.END_VALVES_POS_B])) + pwm = struct.unpack('i', bytearray( + message['message'][self.START_VALVES_PWM:self.END_VALVES_PWM])) self.hd_valve_ID = self.valves[vlv_ID[0]] self.hd_valve_state = self.states[state_ID[0]] self.hd_valve_curr_pos_ID = self.positions[pos_ID[0]] self.hd_valve_curr_pos_cnt = pos_cnt[0] - self.hd_valve_prev_pos_cnt = prev_pos[0] self.hd_valve_next_pos_cnt = next_pos[0] - self.hd_valve_current = current[0] \ No newline at end of file + self.hd_valve_current = current[0] + self.hd_valves_pos_c = pos_c[0] + self.hd_valves_pos_a = pos_a[0] + self.hd_valves_pos_b = pos_b[0] + self.hd_valves_pwm = pwm[0] + + #TODO REMOVE THIS FUNCTION + def handler_hd_valves_fast_temp_sync(self, message): + + fast_pos = struct.unpack('h', bytearray( + message['message'][self.START_FAST_VALVES_POS:self.END_FAST_VALVES_POS])) + fast_current = struct.unpack('f', bytearray( + message['message'][self.START_FAST_VALVES_CURR:self.END_FAST_VALVES_CURR])) + + fast_pos_cmd = struct.unpack('h', bytearray( + message['message'][self.START_FAST_VALVES_CMD:self.END_FAST_VALVES_CMD])) + + self.hd_valve_fast_pos = fast_pos[0] + self.hd_valve_fast_current = fast_current[0] + self.hd_valves_fast_cmd = fast_pos_cmd[0] + #TODO REMOVE THIS FUNCTION Index: tests/hd_valves_test.py =================================================================== diff -u -r2c016f17a5468be8add2a467c989c19f69d501df -r39797925579b6770fbb48e634d73f518ab417d37 --- tests/hd_valves_test.py (.../hd_valves_test.py) (revision 2c016f17a5468be8add2a467c989c19f69d501df) +++ tests/hd_valves_test.py (.../hd_valves_test.py) (revision 39797925579b6770fbb48e634d73f518ab417d37) @@ -13,12 +13,107 @@ valves = hd.valves + #valves.cmd_open_hd_air_trap_valve() + #sleep(2) + #valves.cmd_close_hd_air_trap_valve() + #sleep(2) + + #valves.cmd_home_hd_valve(2) + #sleep(4) + counter = 0 + overall_counter = 0 + sleep_time = 0.01 + overall_time_seconds = 3 + """ + while overall_counter < (overall_time_seconds/sleep_time): + + if counter >= 10: + print("Valve, {}, State, {}, Curr_Pos_ID, {}, Curr_Pos_Cnt, {}, Next_Pos_Cnt, {}, " + "Current, {}, Pos_C, {}, Pos_A, {}, Pos_B, {}" + .format(valves.hd_valve_ID, valves.hd_valve_state, valves.hd_valve_curr_pos_ID, + valves.hd_valve_curr_pos_cnt, valves.hd_valve_next_pos_cnt, valves.hd_valve_current, + valves.hd_valves_pos_c, valves.hd_valves_pos_a, valves.hd_valves_pos_b)) + counter = 0 + + print("Pos, {}, Cmd, {}, Curr, {}".format(valves.hd_valve_fast_pos, + valves.hd_valves_fast_cmd, valves.hd_valve_fast_current)) + + counter = counter + 1 + overall_counter = overall_counter + 1 + + sleep(sleep_time) + + """ + print( "Transitioning to ") + #counter = 0 + #overall_counter = 0 + # 1 Insert/Eject + # 2 Open + # 3 Close + valves.cmd_home_hd_valve(0) + sleep(2) valves.cmd_home_hd_valve(1) + sleep(2) + valves.cmd_home_hd_valve(2) + sleep(2) + valves.cmd_home_hd_valve(3) + sleep(2) - while True: - print("Valve, {}, State, {}, Curr_Pos_ID, {}, Prev_Pos_Cnt, {}, Curr_Pos_Cnt {}, Next_Pos_Cnt {}, Current, {}" - .format(valves.hd_valve_ID, valves.hd_valve_state, valves.hd_valve_curr_pos_ID, - valves.hd_valve_prev_pos_cnt, valves.hd_valve_curr_pos_cnt, valves.hd_valve_next_pos_cnt, - valves.hd_valve_current)) + #valves.cmd_set_hd_valve_pwm(2, 100, 1) + valves.cmd_set_hd_valve_position(0, 2) - sleep(0.1) + while overall_counter < (overall_time_seconds/sleep_time): + + if counter >= 10: + #print("Valve, {}, State, {}, Curr_Pos_ID, {}, Curr_Pos_Cnt, {}, Next_Pos_Cnt, {}, " + # "Current, {}, Pos_C, {}, Pos_A, {}, Pos_B, {}" + # .format(valves.hd_valve_ID, valves.hd_valve_state, valves.hd_valve_curr_pos_ID, + # valves.hd_valve_curr_pos_cnt, valves.hd_valve_next_pos_cnt, valves.hd_valve_current, + # valves.hd_valves_pos_c, valves.hd_valves_pos_a, valves.hd_valves_pos_b)) + counter = 0 + + print("Valve, {}, Pos, {}, Cmd, {}, Curr, {}, PosA, {}, PosB, {}, PosC, {}, State, {}, PWM, {}".format( + valves.hd_valve_ID, valves.hd_valve_fast_pos, + valves.hd_valves_fast_cmd, valves.hd_valve_fast_current, + valves.hd_valves_pos_a, valves.hd_valves_pos_b, + valves.hd_valves_pos_c, valves.hd_valve_state, + valves.hd_valves_pwm)) + + counter = counter + 1 + overall_counter = overall_counter + 1 + + sleep(sleep_time) + + """" + valves.cmd_set_hd_valve_pwm(2, 0, 0, reset=RESET) + + print("Reset") + #valves.cmd_set_hd_valve_pwm(2, 50, 0) + sleep(2) + + valves.cmd_set_hd_valve_position(2, 3) + + counter = 0 + overall_counter = 0 + overall_time_seconds = 3 + while overall_counter < (overall_time_seconds/sleep_time): + + if counter >= 10: + #print("Valve, {}, State, {}, Curr_Pos_ID, {}, Curr_Pos_Cnt, {}, Next_Pos_Cnt, {}, " + # "Current, {}, Pos_C, {}, Pos_A, {}, Pos_B, {}" + # .format(valves.hd_valve_ID, valves.hd_valve_state, valves.hd_valve_curr_pos_ID, + # valves.hd_valve_curr_pos_cnt, valves.hd_valve_next_pos_cnt, valves.hd_valve_current, + # valves.hd_valves_pos_c, valves.hd_valves_pos_a, valves.hd_valves_pos_b)) + counter = 0 + + print("Pos, {}, Cmd, {}, Curr, {}, PosA, {}, PosB, {}, PosC, {}, State, {}, PWM, {}".format(valves.hd_valve_fast_pos, + valves.hd_valves_fast_cmd, valves.hd_valve_fast_current, + valves.hd_valves_pos_a, valves.hd_valves_pos_b, + valves.hd_valves_pos_c, valves.hd_valve_state, + valves.hd_valves_pwm)) + + counter = counter + 1 + overall_counter = overall_counter + 1 + + sleep(sleep_time) + """"" \ No newline at end of file