Advitiy.setDisturbance_b(
torque_dist_total[i * int(Nmodel / Ncontrol) +
k, :].copy())
# Use rk4 solver to calculate the state for next step
for l in range(int(MODEL_STEP * PWM_FREQUENCY)):
integration_step_high = v_duty_cycle * (1 / PWM_FREQUENCY) / 25
for m in range(25): # for high voltage part of PWM
time_since_control_step = k * MODEL_STEP + m * integration_step_high
Advitiy.setTime(i * CONTROL_STEP + time_since_control_step)
rk4TimeArray = np.linspace(time_since_control_step,
time_since_control_step +
integration_step_high,
3,
endpoint=True)
torquerCurrentArray = act.getCurrentList(
v_duty_cycle, rk4TimeArray, 3, edgeCurrentArray)
torqueApplied = np.cross(
No_Turns * torquerCurrentArray * v_A_Torquer,
Advitiy.getMag_b_m_c())
sol.updateStateTimeRK4_act_1(Advitiy, x_dot_BO, h,
torqueApplied)
integration_step_low = (1 - v_duty_cycle) / PWM_FREQUENCY / 25
for m in range(25): # for low voltage part of PWM
time_since_control_step = k * MODEL_STEP + (
m + 25) * integration_step_high
Advitiy.setTime(i * CONTROL_STEP + time_since_control_step)
rk4TimeArray = np.linspace(time_since_control_step,
time_since_control_step +
integration_step_high,
3,
endpoint=True)
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]))
k3_q = quatDerBI(state_i[0:4] + k2_q / 2.0, state_i[4:7]) * h
k3_w = h * w_dot(state_i[4:7] + k2_w / 2,
appliedTorque(i_array[1], v_b_applied, state_i[0:4]))
k4_q = quatDerBI(state_i[0:4] + k3_q, state_i[4:7]) * h
k4_w = h * w_dot(state_i[4:7] + k3_w,
appliedTorque(i_array[2], v_b_applied, state_i[0:4]))
state_i[0:4] += (k1_q + 2.0 * k2_q + 2.0 * k3_q +
k4_q) / 6.0 # update step for q
state_i[0:4] /= np.linalg.norm(stateArray[i + 1, 1:4]) # normalising q
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=",")
end = timer.time()
print(start - end)
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
w_array[k + 1, 1:4] = w_array[k, 1:4]+(k1+2*k2+2*k3+k4)/6
I0 = edgeCurrentArray[len(edgeCurrentArray) - 1]
w_initial = np.array([w_array[len(w_array)-1]])
np.savetxt("actuator_test_tc_sinusoid_step_%d.csv"%(i+1), w_array[:, :], delimiter=",")
end = timer.time()
print(end-start)
num_cycles = int(CONTROL_STEP * PWM_FREQUENCY)
num_instants = int(num_cycles * n * 2)
timeArr = tc_step_sampling.getTimeArr(duty_cycle)
time1 = np.zeros(1)
for i in range(0, num_cycles):
time1 = np.concatenate((time1, timeArr + i / PWM_FREQUENCY))
timeArr = min_step_sampling.getTime(duty_cycle, 15)
time2 = np.zeros(1)
for i in range(0, num_cycles):
time2 = np.concatenate((time2, timeArr + i / PWM_FREQUENCY))
time3 = np.linspace(0, CONTROL_STEP, 1000001, endpoint=True)
current1 = aac.getCurrentList(duty_cycle, time1, len(time1), I0=np.zeros(3))
current2 = aac.getCurrentList(duty_cycle, time2, len(time2), I0=np.zeros(3))
current3 = aac.getCurrentList(duty_cycle, time3, len(time3), I0=np.zeros(3))
plt.plot(time1, current1[:, 0], label="tc method [0]")
#plt.plot(time2, current2[:, 0], label= "min step method [0]")
plt.plot(time3, current3[:, 0], label="true [0]")
plt.legend()
plt.xlabel = "time(s)"
plt.ylabel = "current(A)"
plt.show()
plt.plot(time1, current1[:, 1], label="tc method [1]")
#plt.plot(time2, current2[:, 1], label= "min step method [1]")
plt.plot(time3, current3[:, 1], label="true [1]")
plt.legend()
return v_w_dot
def getMag_b(time):
v_mag_b = np.array([1, 2, 3])*1e-3
return v_mag_b
for j in range(0, 2):
edgeCurrentArray = aac.getEdgeCurrent(v_duty_cycle, I0)
for k in range(j * int(num_cycles), (j + 1) * int(num_cycles)):
currentArray = np.zeros((3, 3))
print("cycle ", k + 1, "control step", j + 1)
for i in range(k * num_instants_per_cycle, (k + 1) * num_instants_per_cycle):
intTimeArr = np.linspace(time[i] % CONTROL_STEP, time[i+1] % CONTROL_STEP, 3, endpoint=True)
currentArray = aac.getCurrentList(v_duty_cycle, intTimeArr, 3, edgeCurrentArray)
h = time[i+1] - time[i]
w_bIb = w_array[i, 1:4]
# RK-4 routine
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
w_array[i+1, 1:4] = w_bIb + (k1 + 2*k2 + 2*k3 + k4)/6
I0 = edgeCurrentArray[len(edgeCurrentArray) - 1]
np.savetxt("simplified_actuator_tc_data_3.csv", w_array[:, :], delimiter=",")
end = timer.time()
print(end-start)
import numpy as np
import matplotlib.pyplot as plt
import analytical_act_current as aac
from constants_1U import PWM_FREQUENCY, CONTROL_STEP
duty_cycle = np.array([0.1, 0.01, 0.001])
I0 = np.array([0., 0., 0.])
edge_current = aac.getEdgeCurrent(duty_cycle, I0)
N = 100001
current_list_true = np.zeros((N, 3))
time_true = np.linspace(0, 2, N, endpoint=True)
current_list_true = aac.getCurrentList(duty_cycle, time_true, N, I0)
n_1 = 5
time_1 = np.linspace(0, 2, (n_1 * 2000) + 1,
endpoint=True) #10 uniform steps in one PWM cycle
current_list_uniform_steps = aac.getCurrentList(duty_cycle, time_1,
(n_1 * 2000) + 1, I0)
m = 10 * 2000
current_list_vaiable_spacing = np.zeros((m, 3))
n = 15
min_duty = np.abs(np.amin(duty_cycle))
max_duty = np.abs(np.amax(duty_cycle))
mid_duty = np.abs(duty_cycle[0] + duty_cycle[1] + duty_cycle[2] - min_duty -
max_duty)
time_a = min_duty / PWM_FREQUENCY
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()
print(end-start)
