IMU Record
SUMMARY: For a closed loop feedback control system to operate, it was imperative that there was an inertial-measurement system (IMU) device on board to routinely gather the state the cubesat was in. This script pulled that information from the IMU device, fed it to the control law script (see other script page), which converted the output to where the actuators had to move to establish stability.
RAW CODE
%% NOTES:
% Prior to running this program for the first time, run the following
% functions:
% VNserial()
% VNreadregister()
% linearization_2_mass_actuator
% The current save file is configured for Flight 1. Once flight 1 has been
% completed, change "Flight1parabola" in line ?? to say "Flight2parabola"
%% Housekeeping
clear; clc; instrreset;
%% Serial Connection to VectroNav VN-100 and Actuator
% VectroNav VN-100 IMU
imu = VNserial('COM3');
% Newmark Actuators
act = serial('COM4');
%% Setting Actuator Serial Connection to Accept Position Commands
set(act,'OutputBufferSize',2048)
fopen(act)
set(act, 'Terminator', 'CR/LF')
%% Save File Initialization, User Input, and Actuator Reset Relocation
for parabola_number=1:30 % Estimation on # of parabolas per flight
% Command Window outpurs parabola number
fprintf('This is paraboda number %d\n', parabola_number)
% User Input
timeRate = input('Input how long you want to sample. Hit enter to run for 20 seconds. ');
if isempty(timeRate)
timeRate = 20;
end
% Actuators are told to move to its '0' position
initial_actuator_a = double('AMA 0');
fprintf(act, initial_actuator_a);
initial_actuator_b = double('BMA 0');
fprintf(act, initial_actuator_b);
% Intializes Save File to an Excel Spreadsheet
filename = ['Flight1parabola' num2str(parabola_number) '.xls'];
savefile = fopen(filename, 'w');
fprintf(savefile,'Timestep OmegaX OmegaY OmegaZ ActuatorA ActuatorB\n');
% Loads [2 x 2] feedback matrix
switch parabola_number
case 1
load('K1.mat');
K = K1;
case 2
load('K1.mat');
K = K1;
case 3
load('K1.mat');
K = K1;
case 4
load('K2.mat');
K = K2;
case 5
load('K2.mat');
K = K2;
case 6
load('K2.mat');
K = K2;
case 7
load('K3.mat');
K = K3;
case 8
load('K3.mat');
K = K3;
case 9
load('K3.mat');
K = K3;
case 10
load('K4.mat');
K = K4;
case 11
load('K4.mat');
K = K4;
case 12
load('K4.mat');
K = K4;
case 13
load('K5.mat');
K = K5;
case 14
load('K5.mat');
K = K5;
case 15
load('K5.mat');
K = K5;
case 16
load('K6.mat');
K = K6;
case 17
load('K6.mat');
K = K6;
case 18
load('K6.mat');
K = K6;
case 19
load('K7.mat');
K = K7;
case 20
load('K7.mat');
K = K7;
case 21
load('K7.mat');
K = K7;
case 22
load('K8.mat');
K = K8;
case 23
load('K8.mat');
K = K8;
case 24
load('K8.mat');
K = K8;
case 25
load('K9.mat');
K = K9;
case 26
load('K9.mat');
K = K9;
case 27
load('K9.mat');
K = K9;
case 28
load('K10.mat');
K = K10;
case 29
load('K10.mat');
K = K10;
case 30
load('K10.mat');
K = K10;
end
% Data Recorded Process is About to Begin
fprintf('\n Data recording started:\n');
fprintf(' Timestep OmegaX OmegaY OmegaZ Actuator A Actuator B\n');
from 1 to however long time specified in user input
for i = 1:30*timeRate
% Records angular velocities from IMU
GYR = VNreadregister(imu, 'GYR');
% Separating the [3 x 1] angular velocity matrix to individual
% [1 x 1] vectors
omega_x(i) = GYR(1);
omega_y(i) = GYR(2);
omega_z(i) = GYR(3);
% Accounting the [2 x 2] K gain matrix to output actuator positions
% in terms of steps
u = -5092500*K*[GYR(1); GYR(2)];
% Separating [2 x 1] u matrix to individual vectors
disp_actA(i) = u(1);
disp_actB(i) = u(2);
% EXTRA: CONVERTED u Matrix for Output Purposes to Inches
disp_actA_INCHES(i) = (u(1)/5092500)*39.370;
disp_actB_INCHES(i) = (u(2)/5092500)*39.370;
% Correction if step size output is outside of [-475000 475000]
if disp_actA(i) > 475000
disp_actA(i) = 475000;
else if disp_actA(i) < -475000
disp_actA(i) = -475000;
end
end
if disp_actB(i) > 475000
disp_actB(i) = 475000;
else if disp_actB(i) < -475000
disp_actB(i) = -475000;
end
end
% Sending stepsizes to the actuators
move_actuator_a=double(['AMA ',num2str(round(disp_actA(i)))]);
fprintf(act,move_actuator_a);
move_actuator_b=double(['BMA ',num2str(round(disp_actB(i)))]);
fprintf(act,move_actuator_b);
% print_position_a = double('APR P'); % M-Code command for actuator to output its position, converted to ASCII.
% fprintf(act, print_position_a); % Send the actuator the 'print it's position' command.
%
% print_position_b = double('BPR P'); % M-Code command for actuator to output its position, converted to ASCII.
% fprintf(act, print_position_b); % Send the actuator the 'print it's position' command.
%
% feedback_matrix = double(fgets(act));
% feedback = [feedback_matrix(2), feedback_matrix(3), feedback_matrix(4), feedback_matrix(5), feedback_matrix(6)];
% if feedback == double('APR P')
% actuator_a_position(i) = str2num(fgets(act));
% elseif feedback == double('BPR P')
% actuator_b_position(i) = str2num(fgets(act));
% end
% Displays iteration, angular velocities, and actuator step sizes
X = [i, omega_x(i), omega_y(i), omega_z(i), disp_actA(i), disp_actB(i);];
disp(X)
% Saves data to Excel document
Y = [i, omega_x(i), omega_y(i), omega_z(i), disp_actA_INCHES(i), disp_actB_INCHES(i);];
fprintf(savefile, '\n %d %9.3d %9.3d %9.3d %9.3d %9.3d', Y);
end
fprintf('Data recording ended. Units are in rad/s\n\n\n');
% %% Plot
%
% % Creates variables to output into plotted figures
% output = [omega_x; omega_y; omega_z];
% disp_act = [disp_actA; disp_actB];
%
% % Angular Rates Plot
% angular_rates_plot = figure(1);
% plot(output');
% title('Angular Rates Recorded by Vectronav IMU')
% xlabel('Timestep');
% ylabel('Angular Rates (rad/s)');
% legend('omega x', 'omega y', 'omega z');
% grid on;
%
% % Plot Actuator Positions
% actuator_positions_plot = figure(2);
% plot(disp_act');
% title('Actuator Positions')
% xlabel('Timestep');
% ylabel('Actuator Position (steps)');
% legend('Actuator A', 'Actuator B');
% grid on;
%
% File Save has Finished
fclose(savefile);
end