Exemplo n.º 1
0
                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)
Exemplo n.º 5
0
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()
Exemplo n.º 6
0
    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)
Exemplo n.º 7
0
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)