from faultmodel.fdiptypes import Model #from ezservo.MassServo import MassServo #from ezservo.ServoConfig import ServoConfig from faultmodel.mediator import MassMediator from daq.fp_cdaq import CDaq from microstrain import gx5_25 import datetime import time import h5py import numpy as np import re def ConstantAcquisitionAndModel(): """ Configured for use on the subsea DAQ implementation on LRAUV """ def process_data(data: np.ndarray): voltage_motorcurrent = np.mean(data[0, :]) voltage_motorvoltage = np.mean(data[1, :]) voltage_stringpot = np.mean(data[2, :]) return (voltage_motorcurrent, voltage_motorvoltage, voltage_stringpot) def process_data_dig(data: np.ndarray): angle_motor = np.mean(data) return (angle_motor) def process_ahrs_data(data: np.ndarray): time_ns = data[0] roll = data[1] pitch = data[2] yaw = data[3] #rolld = data[4] #pitchd = data[5] #yawd = data[6] return (time_ns, roll, pitch, yaw) # Initialize the Model model = MassMediator(Model.V1) cmd_position = 0; # Initialize the AHRS ahrs = gx5_25.GX5_25() ahrs.open() # Initialize the Data Acquisition with CDaq() as cdaq: # Setup Acquisition cdaq.add_channel('cDAQ1Mod1/ai0', 'voltage', 'MotorCurrent') cdaq.add_channel('cDAQ1Mod1/ai1', 'voltage', 'MotorVoltage') cdaq.add_channel('cDAQ1Mod1/ai2', 'voltage', 'StringPotVoltage') cdaq.add_channel('cDAQ1Mod3/ctr0', 'ang_encoder','MotorPosition') n_samples = 160 sample_rate = 1600.0 cdaq.setup_task(samplerate=sample_rate, nsamples=n_samples) # Enter in conversion constants (from mkemp email 3/26/18) conv_motor_current = lambda x: x*1000.0 conv_motor_voltage = lambda x: x*2.0 conv_mass_position = lambda x: x*13.7 # Create a data file time_str = datetime.datetime.now().strftime("%Y%m%d%H%M%S") hf = h5py.File("SingleExecution_{}.hdf5".format(time_str), 'w') # Analog Data Setup hf_group_analog = hf.create_group('analog') dset_rmv = hf_group_analog.create_dataset('raw_voltage_motor', (0, 1), maxshape=(None, 1)) dset_rmc = hf_group_analog.create_dataset('raw_current_motor', (0, 1), maxshape=(None, 1)) dset_rmp = hf_group_analog.create_dataset('raw_voltage_string_pot', (0, 1), maxshape=(None, 1)) dset_mmv = hf_group_analog.create_dataset('mean_voltage_motor', (0, 1), maxshape=(None, 1)) dset_mmc = hf_group_analog.create_dataset('mean_current_motor', (0, 1), maxshape=(None, 1)) dset_mms = hf_group_analog.create_dataset('mean_voltage_string_pot', (0, 1), maxshape=(None, 1)) dset_mmp = hf_group_analog.create_dataset('mean_position_string_pot', (0, 1), maxshape=(None, 1)) dset_rma = hf_group_analog.create_dataset('raw_angle_motor', (0, 1), maxshape=(None, 1)) dset_mma = hf_group_analog.create_dataset('mean_angle_motor', (0, 1), maxshape=(None, 1)) hf_group_analog.create_dataset('sample_rate_hertz', data=sample_rate) hf_group_analog.create_dataset('samples_per_loop', data=n_samples) # EZServo data setup #hf_group_servo = hf.create_group('ezservo') # AHRS data setup hf_group_ahrs = hf.create_group('ahrs') dset_ahrs_time = hf_group_ahrs.create_dataset('time', (0, 1), maxshape=(None, 1)) dset_ahrs_roll = hf_group_ahrs.create_dataset('roll_radians', (0, 1), maxshape=(None, 1)) dset_ahrs_pitch = hf_group_ahrs.create_dataset('pitch_radians', (0, 1), maxshape=(None, 1)) dset_ahrs_yaw = hf_group_ahrs.create_dataset('yaw_radians', (0, 1), maxshape=(None, 1)) dset_ahrs_rolld = hf_group_ahrs.create_dataset('roll_degrees', (0, 1), maxshape=(None, 1)) dset_ahrs_pitchd = hf_group_ahrs.create_dataset('pitch_degrees', (0, 1), maxshape=(None, 1)) dset_ahrs_yawd = hf_group_ahrs.create_dataset('yaw_degrees', (0, 1), maxshape=(None, 1)) #TODO need to add the LCM receiver to this datagroup. #dset_ezpos = hf_group_servo.create_dataset('position', (0, 1), maxshape=(None, 1)) #dset_ezvel = hf_group_servo.create_dataset('velocity', (0, 1), maxshape=(None, 1)) # Model Data setup hf_group_model = hf.create_group('model') str_datatype = h5py.special_dtype(vlen=str) dset_model_state = hf_group_model.create_dataset('state', (0, 1), dtype=str_datatype, maxshape=(None, 1)) dset_model_health = hf_group_model.create_dataset('health', (0, 1), dtype=str_datatype, maxshape=(None, 1)) dset_model_fault = hf_group_model.create_dataset('fault_type', (0, 1), dtype=str_datatype, maxshape=(None, 1)) dset_model_ttf = hf_group_model.create_dataset('time_to_failure', (0, 1), maxshape=(None, 1)) # Contextual Data setup and population hf_group_context = hf.create_group('info') hf_group_context.create_dataset('version',data='0.1') hf_group_context.create_dataset('start_date',data=datetime.datetime.now().strftime("%Y/%m%d")) hf_group_context.create_dataset('start_time',data=time.time()) # Set initial state 0 will rehome, 1 will start a movement state = 1 start_time = time.process_time() cdaq.start_acquisition() n_acqs = 0 ahrs.start() yaw = 0.0 n_acqs_ahrs = 0 while state < 4: # log the ahrs data while ahrs.data_available(): data = ahrs.data_buff.get() if data is not None: # Log AHRS Data (time_ns, roll, pitch, yaw) = process_ahrs_data(data) #print(f"\t\t\t\t\t\t\t\tR:{roll} P:{pitch} Y:{yaw}") dset_ahrs_time.resize(dset_ahrs_time.shape[0] + 1, axis=0) dset_ahrs_roll.resize(dset_ahrs_roll.shape[0] + 1, axis=0) dset_ahrs_pitch.resize(dset_ahrs_pitch.shape[0] + 1, axis=0) dset_ahrs_yaw.resize(dset_ahrs_yaw.shape[0] + 1, axis=0) dset_ahrs_time[n_acqs_ahrs, 0] = time_ns dset_ahrs_roll[n_acqs_ahrs, 0] = roll dset_ahrs_pitch[n_acqs_ahrs, 0] = pitch dset_ahrs_yaw[n_acqs_ahrs, 0] = yaw n_acqs_ahrs += 1 #check for data from the daq if cdaq.databuff.empty() or cdaq.databuff_dig.empty(): time.sleep(0.01) continue # log & Process DAQ Data data = cdaq.databuff.get() data_dig = cdaq.databuff_dig.get() # Increment counter loop_time = time.process_time() str_time = "Elapsed: {:0.2f} ".format(loop_time - start_time) # Generate Means (voltage_motorcurrent, voltage_motorvoltage, voltage_stringpot) = process_data(data) (angle_servo) = process_data_dig(data_dig) voltage_servo = conv_motor_voltage(voltage_motorvoltage) current_servo = conv_motor_current(voltage_motorcurrent) position_string = conv_mass_position(voltage_stringpot) #TODO conversions on means for angular position?? str_daq = "Vservo: {:5.2f} Vstring: {:5.2f} Iservo: {:5.2f} Lservo: {:5.2f} Pstring: {:5.2f}".format(voltage_servo, voltage_stringpot, current_servo, angle_servo, position_string) # Log Motor Current dset_rmc.resize(dset_rmc.shape[0] + n_samples, axis=0) dset_rmc[-n_samples:, :] = np.reshape(data[0, :], (n_samples, 1)) # Log Motor Voltage dset_rmv.resize(dset_rmv.shape[0] + n_samples, axis=0) dset_rmv[-n_samples:, :] = np.reshape(data[1, :], (n_samples, 1)) # Log Motor Position Voltage dset_rmp.resize(dset_rmp.shape[0] + n_samples, axis=0) dset_rmp[-n_samples:, :] = np.reshape(data[2, :], (n_samples, 1)) # Log Motor Angle dset_rma.resize(dset_rma.shape[0] + n_samples, axis=0) dset_rma[-n_samples:, :] = np.reshape(data_dig, (n_samples, 1)) # Log Mean values dset_mmv.resize(dset_mmv.shape[0] + 1, axis=0) dset_mms.resize(dset_mms.shape[0] + 1, axis=0) dset_mmp.resize(dset_mmp.shape[0] + 1, axis=0) dset_mmc.resize(dset_mmc.shape[0] + 1, axis=0) dset_mma.resize(dset_mma.shape[0] + 1, axis=0) dset_mmv[n_acqs, 0] = voltage_servo dset_mms[n_acqs, 0] = voltage_stringpot dset_mmp[n_acqs, 0] = position_string dset_mmc[n_acqs, 0] = current_servo dset_mma[n_acqs, 0] = angle_servo # Get Position & Velocity from EZServo #(timestamp, ez_position) = massServo.get_position_mm() #(timestamp, ez_velocity) = massServo.get_velocity_actual() # Log Position & Velocity from EZServo #dset_ezpos.resize(dset_ezpos.shape[0] + 1, axis=0) #dset_ezvel.resize(dset_ezvel.shape[0] + 1, axis=0) #dset_ezpos[n_acqs, 0] = ez_position #dset_ezvel[n_acqs, 0] = ez_velocity #str_ez = "EZPos: {:5.2f} EZVelocity: {:5.2f}".format(ez_position, ez_velocity) # Feed the Model # TODO need to add pitch eventually and position from LCM #model_state = model.run_state_machine(ez_position, cmd_position, current_servo, ez_velocity, 0.0) # Log the model input/output dset_model_state.resize(dset_model_state.shape[0] + 1, axis=0) dset_model_health.resize(dset_model_health.shape[0] + 1, axis=0) dset_model_fault.resize(dset_model_fault.shape[0] + 1, axis=0) dset_model_ttf.resize(dset_model_ttf.shape[0] + 1, axis=0) print(str_time, str_daq) #dset_model_state[n_acqs, 0] = model_state['state'] #dset_model_health[n_acqs, 0] = model_state['health'] #dset_model_fault[n_acqs, 0] = model_state['faulttype'] #dset_model_ttf[n_acqs, 0] = model_state['ttf'] # Run the movement script #servostate = massServo.query_status() #if servostate is 'READY': # if state is 0: # print('Homing') # massServo.run_servo('home') # cmd_position = 0 # state += 1 # elif state is 1: # print('Move To 10') # cmd_position = 10 # massServo.run_servo('run', cmd_position) # state += 1 # elif state is 2: # print('Move To 0') # cmd_position = 0 # massServo.run_servo('run', cmd_position) # end_state = 0 # state += 1 # elif state is 3: # if end_state < 20: # end_state += 1 # else: # state += 1 # time.sleep(thread_pacer(loop_time)) n_acqs += 1 hf.close() if __name__ == '__main__': try: ConstantAcquisitionAndModel() except KeyboardInterrupt: pass