# getting control torque by omega controller # k = 0.001 torque_control[i * int(Nmodel / Ncontrol):(i + 1) * (int(Nmodel / Ncontrol)), :] \ = -0.001 * Advitiy.getW_BI_b() # print(Advitiy.getW_BI_b()) Advitiy.setControl_b(torque_control[i * int(Nmodel / Ncontrol), :]) # torque applied # getting applied torque by actuator modelling (magnetic torque limitation is being considered) torqueRequired = Advitiy.getControl_b() v_mag_B = Advitiy.getMag_b_m_c() v_magMoment = np.cross(v_mag_B, torqueRequired) / (np.linalg.norm(v_mag_B)**2) currentRequired = v_magMoment / v_A_Torquer / No_Turns v_duty_cycle = currentRequired / RESISTANCE / PWM_AMPLITUDE edgeCurrentArray = act.getEdgeCurrent(v_duty_cycle, I0) I0 = edgeCurrentArray[len(edgeCurrentArray) - 1, :] for k in range(0, int(Nmodel / Ncontrol)): # loop for environment-cycle # Set satellite parameters # state is set inside solver Advitiy.setPos(m_sgp_output_i[i * int(Nmodel / Ncontrol) + k, 1:4]) Advitiy.setVel(m_sgp_output_i[i * int(Nmodel / Ncontrol) + k, 4:7]) Advitiy.setLight(m_light_output[i * int(Nmodel / Ncontrol) + k, 1]) Advitiy.setSun_i(m_si_output_b[i * int(Nmodel / Ncontrol) + k, 1:4]) Advitiy.setMag_i( m_magnetic_field_i[(i + 1) * int(Nmodel / Ncontrol) + k, 1:4]) # disturbance torque
T_P = 1 / PWM_FREQUENCY v_w_BI_b = np.zeros((1, 3)) NUM_STEPS = 2 I0 = np.zeros(3) NUM_CYCLES_PER_STEP = int(CONTROL_STEP / T_P) def integral_current_step(v_duty_cycle, v_edgeCurrent): v_duty_cycle = np.absolute(v_duty_cycle) res = V_max * T_P * v_duty_cycle / R res = res - V_max * L / R / R * (np.exp(-R * T_P / L * (1 - v_duty_cycle)) - np.exp(-R * T_P / L)) res = res + v_edgeCurrent * L / R * (1 - np.exp(-R * T_P / L)) res = np.cross(res, np.array([1, 2, 3]) * 1e-3) return res for i in range(0, NUM_STEPS): v_duty = np.array([i + 1, (-1**i) * (i + 2), i + 3]) * 1e-3 edgeCurrent = aac.getEdgeCurrent(v_duty, I0) for j in range(0, NUM_CYCLES_PER_STEP): angular_velocity = v_w_BI_b[i * NUM_CYCLES_PER_STEP + j] + v_A_Torquer[ 0] * No_Turns * integral_current_step(v_duty, edgeCurrent[2 * j]) v_w_BI_b = np.vstack((v_w_BI_b, angular_velocity)) I0 = edgeCurrent[len(edgeCurrent) - 1] v_w_BI_b = v_w_BI_b * v_A_Torquer[0] * No_Turns np.savetxt("simplified_actuator_true_long_data_2.csv", v_w_BI_b[:, :], delimiter=",")
sat.setRequiredTorque(0) Mag_i = np.array([1, 1, 1]) * 1e-4 sat.setMag_i(Mag_i) sat.setPos(np.array([1, 1, 1])) sat.setVel(np.array([1, 1, 2])) voltageRequired = TorqueApplied.ctrlTorqueToVoltage(sat) duty_cycle = voltageRequired / 3.3 n = 15 min_duty = np.abs(np.amin(duty_cycle)) max_duty = np.abs(np.amax(duty_cycle)) mid_duty = duty_cycle[0] + duty_cycle[1] + duty_cycle[3] - min_duty - max_duty denom_t = 1.0 / min_duty + 1.0 / max_duty + 1.0 / mid_duty + 10 time_arr_a = np.linspace(min_duty / PWM_FREQUENCY) current_applied = np.zeros((3, 3)) edgeCurrentList = aac.getEdgeCurrent(duty_cycle, np.zeros(3)) state_array = np.zeros((100001, 7)) start_1 = timer.time() for i in range(0, CONTROL_STEP * PWM_FREQUENCY * 15): current_applied = aac.getCurrentList( duty_cycle, np.linspace(time[i], time[i + 1], 3, endpoint=True), 3, np.zeros(3)) torque_applied = TorqueApplied.currentToTorque(current_applied, sat) solver.rk4Quaternion(sat, dynamics_actuator.x_dot_BO, time[i + 1] - time[i], torque_applied) state_array[i, :] = sat.getState() if (i % 100 == 0): print(i / 100, "%") end_1 = timer.time() print(end_1 - start_1) np.savetxt("aac_test.csv", state_array[:, :], delimiter=",")
return torque n = int(2e4) n_factor = 100000 v_duty_cycle = np.array([1.0, 1.0, 1.0 ]) * 1e-4 # duty cycle for the corresponding PWM time_array = np.linspace(0, CONTROL_STEP, CONTROL_STEP * PWM_FREQUENCY * n + 1, endpoint=True) stateArray = np.zeros((len((time_array - 1) / n_factor) + 1, 7)) stateArray[0] = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) v_b_applied = np.array([1.0, 2.0, 3.0]) * 1e-4 # b in the inertial frame v_v_applied = v_duty_cycle * PWM_AMPLITUDE # v = v_max * duty cycle edgeCurrentArray = aac.getEdgeCurrent(v_duty_cycle, np.zeros(3)) for i in range(len(stateArray) - 1): state_i = stateArray[i, :] # for indexing convenience for j in range(n_factor): time_rk4 = np.linspace( time_array[i], time_array[i + 1], 3, endpoint=True) # for convenience in accessing values h = float(time_array[i + 1] - time_array[i]) i_array = aac.getCurrentList(v_duty_cycle, time_rk4, 3, edgeCurrentArray) k1_q = quatDerBI(state_i[0:4], state_i[4:7]) * h # rk4 algorithm k1_w = h * w_dot(state_i[4:7], appliedTorque(i_array[0], v_b_applied, state_i[0:4])) k2_q = quatDerBI(state_i[0:4] + k1_q / 2.0, state_i[4:7]) * h k2_w = h * w_dot(state_i[4:7] + k1_w / 2, appliedTorque(i_array[1], v_b_applied, state_i[0:4]))
time_period = 1/PWM_FREQUENCY num_steps = 5 w_initial = np.zeros((1, 4)) num_cycles_per_step = int(CONTROL_STEP/time_period) for i in range(0, num_steps): time = np.array([i*CONTROL_STEP]) # initialising the time array for the control step duty = np.array([i+1, i+2, i+3]) * 1e-3*((-1)**i) # initialising the duty cycle timeArr = tc.getTimeArr(duty) # getting the time array for one PWM cycle num_instants_per_cycle = len(timeArr) for j in range(0, num_cycles_per_step): time = np.concatenate((time, timeArr + j*time_period + i*CONTROL_STEP)) # setting the time array for the whole step edgeCurrentArray = aac.getEdgeCurrent(duty, I0) # getting the current at the edges w_array = np.zeros((len(time), 4)) w_array[0] = w_initial w_array[:, 0] = time print("Step ", i + 1) for j in range(0, num_cycles_per_step): for k in range(j*num_instants_per_cycle, (j+1)*num_instants_per_cycle): intTimeArr1 = np.linspace(time[k]%2, time[k+1]%2, 3, endpoint=True) # setting the time array for the integration step intTimeArr = np.linspace(time[k], time[k+1], 3, endpoint=True) currentArray = aac.getCurrentList(duty, intTimeArr1, 3, edgeCurrentArray) # getting the current for the integration cycles h = time[k + 1] - time[k] k1 = w_dot_BI(currentArray[0], getMag_b(intTimeArr[0])) * h k2 = w_dot_BI(currentArray[1], getMag_b(intTimeArr[1])) * h k3 = w_dot_BI(currentArray[1], getMag_b(intTimeArr[1])) * h k4 = w_dot_BI(currentArray[2], getMag_b(intTimeArr[2])) * h
import analytical_act_current as aac from constants_1U import PWM_FREQUENCY, PWM_AMPLITUDE as V_max, v_A_Torquer, RESISTANCE as R, INDUCTANCE as L, No_Turns import numpy as np import matplotlib.pyplot as plt duty = np.array([3, 4, 5]) * 1e-3 t_p = 1 / PWM_FREQUENCY edgeCurrent_1 = aac.getEdgeCurrent(duty, np.zeros(3)) edgeCurrent_2 = aac.getEdgeCurrent(duty, edgeCurrent_1[len(edgeCurrent_1) - 1, :]) w_array = np.zeros((len(edgeCurrent_1), 1)) for i in range(0, 2000): w_array[i + 1] = w_array[i] + V_max / R * t_p * (1 * duty[1] - 2 * duty[0]) w_array[i + 1] = w_array[i + 1] + (V_max * L / R / R) * np.exp( -t_p * R / L) * (1 - 2) w_array[i + 1] = w_array[i + 1] - V_max / R * L / R * ( 1 * np.exp(-t_p * (1 - duty[1]) * R / L) - 2 * np.exp(-t_p * (1 - duty[0]) * R / L)) w_array[i + 1] = w_array[i + 1] + L / R * ( 1 * edgeCurrent_1[2 * (i % 2000), 1] - 2 * edgeCurrent_1[2 * (i % 2000), 0]) * (1 - np.exp(-R * t_p / L)) for i in range(2000, 4000): w_array[i + 1] = w_array[i] + V_max / R * t_p * (1 * duty[1] - 2 * duty[0]) w_array[i + 1] = w_array[i + 1] + (V_max * L / R / R) * np.exp( -t_p * R / L) * (1 - 2) w_array[i + 1] = w_array[i + 1] - V_max / R * L / R * ( 1 * np.exp(-t_p * (1 - duty[1]) * R / L) - 2 * np.exp(-t_p * (1 - duty[0]) * R / L)) w_array[i + 1] = w_array[i + 1] + L / R * (
I0 = np.zeros(3) time_period = 1/PWM_FREQUENCY num_steps = 8 w_array = np.zeros(4) time = np.zeros(1) num_cycles_per_step = int(CONTROL_STEP/time_period) for i in range(0, num_steps): duty = np.array([i+1, (-1**i)*(i+2), i+3])*1e-3 timeArr = minstep.getTime(duty, 15) num_instants_per_cycle = len(timeArr) for j in range(0, num_cycles_per_step): time = np.concatenate((time, timeArr+j*time_period)) edgeCurrentArray = aac.getEdgeCurrent(duty, I0) for j in range(0, num_cycles_per_step): print("step ", i+1, " cycle ", j) for k in range(j*num_instants_per_cycle, (j+1)*num_instants_per_cycle): intTimeArr = np.linspace(time[k] % time_period, time[k+1] % time_period, 3, endpoint=True) currentArray = aac.getCurrentList(duty, intTimeArr, 3, edgeCurrentArray) h = time[k + 1] - time[k] k1 = w_dot_BI(currentArray[0], getMag_b(intTimeArr[0])) * h k2 = w_dot_BI(currentArray[1], getMag_b(intTimeArr[1])) * h k3 = w_dot_BI(currentArray[1], getMag_b(intTimeArr[1])) * h k4 = w_dot_BI(currentArray[2], getMag_b(intTimeArr[2])) * h angular_velocity_new = np.hstack((time[k+1], w_array[i*num_cycles_per_step+k, 1:4]+(k1+2*k2+2*k3+k4)/6)) w_array = np.vstack((w_array, angular_velocity_new)) I0 = edgeCurrentArray[len(edgeCurrentArray) - 1] np.savetxt("simplified_actuator_minstep_long_data_cycle%d.csv"%i , w_array[:, :], delimiter=",") end = timer.time()