# 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()
