Programs/code

clear; clc; close all;

%% Motion Profile generator.

%Takes in a .csv file of the acceleration profile

% then converts it to motion profile that the Talon can read.

% The printout of this code will be copy pasted into the

% MotionProfile.h file

% The exported file will be .csv since .xlsx keeps hitting the limit

% Open the file with a text document

%Note that this works if the dt is greater than 0.01s. This has not been

%tested with dt less than 0.01s or at 0.01s.

% Below is the goal format

%% Format

% const int kMotionProfileSz =185;

%

% const double kMotionProfile[][3] = {

% {0, 0 ,10},

% {4.76190476190476E-05, 0.571428571 ,10},

% {0.000214285714285714, 1.428571429 ,10},

% {0.000547619047619048, 2.571428571 ,10},

% ...

% {4.99997619047619, 0.285714286 ,10},

% {5, 0 ,10}};

%% Initialization/Constants

%Importing Files

acceleration_data_fname = 'target_acceleration.csv';

raw_acc_profile=readtable(acceleration_data_fname);

acc_profile=table2array(raw_acc_profile);

% acceleration profile is split for ease of use

acc = acc_profile(:,1);

time = acc_profile(:,2);

% plot(time,acc) %Checkpoint

% time difference between nodes

dt = 0.01; %seconds

a = 1.5; %(m) boom length

b = 0.14478; %(m) bucket arm length

g = 9.81; %(m/s^2) gravity

ang_vel = zeros(length(acc) , 1); % (rev/s) Initialize angular velocity of

%% Create a gravity floor

% Since the centrifuge's absolute minimum gravity simulation is 9.81, the

% gravitational acceleration, change the acceleration profile such that it

% is physically achieveable

%Teporary plotting

figure(1)

subplot(3,1,3)

plot(time,acc);

title('original accel profile')

xlabel('time (s)')

ylabel('acc (m/s^2)')

%

for i = 1:1:length(time)

if acc(i)<g

acc(i)=g;

end

end

% plot(time,acc) %Checkpoint

%% Conversion

%Converts acceleration to angular velocity

% Visit BLAST Week 7 Update slide for more details....or the Jamboard for

% derivation

% ang_vel = sqrt ( (sqrt (acc^2-g^2)/(a+b*sin(atan(Acc/g)))));

%Split up for readability

num = sqrt (acc.^2-g^2);

den = (a+b*sin(atan(acc./g)));

ang_vel = sqrt ( num ./ den);

% plot(time,ang_vel) %Checkpoint

%% Interpolation for velocity

% Fills out the missing times

% Algorithm:

% a for loop, and interpolation is taking place. To do this, a target time

% must between 2 times that are given, which can be referred to using

% indeces. To shorten the algorithm time (so it doesnt run n times), the

% last indeces used for reference time will be saved, and the search

% algorithm will start from the saved indeces.

% This way it will keep computing from (n+n-1+n-2...+1) to (n).

last_index = 1; %Last index used for reference

ntime = 0:dt:time(end); %new time frame

nang_vel = zeros(length(ntime),1); %angular velocity with new time frame

nang_vel(1) = ang_vel(1); %Assumption: They all start from time = 0

for t=2:1:length(ntime)

%Find reference times

for i = last_index:1:length(time)

if ntime(t) <= time(i) %For case when dt <= raw dt

last_index = i-1;

break

end

end

nang_vel(t) = ang_vel(last_index) + (ntime(t) - time(last_index) )* (ang_vel(last_index+1) - ang_vel(last_index)) / (time(last_index+1) - time(last_index));

end

nang_vel = nang_vel/(2*pi); %Convert rad/s to rotation per second

% plot(ntime, nang_vel) %Check point

%% Position

%Angular position

%Euler Formula will be used because I do not know any other better

%methods

%This section can be combined with above section to decrease computation by

%n times...but lazy

nang_pos = zeros(length(nang_vel) , 1); %Initialization

for i = 2:1:length(ntime)

nang_pos(i)=nang_pos(i-1)+nang_vel(i-1)*dt;

end

%% Output

MPfile = fopen('motionprofile_interpolated.txt' , 'w');

fprintf(MPfile , 'const int kMotionProfileSz =185;\n\nconst double kMotionProfile[][3] = { \n');

for i = 1:1:length(ntime)-1

fprintf(MPfile, '{%f, %f, %i},\n' , nang_pos(i) , nang_vel(i) , dt*1000);

end

fprintf(MPfile, '{%f, %f, %i}\n' , nang_pos(end) , nang_vel(end) , dt*1000);

fprintf(MPfile, '};');

fclose(MPfile);

figure(1)

subplot(3,1,1)

plot(ntime,nang_pos);

title('angular position (rev)')

xlabel('time (s)')

ylabel('pos (rev)')

subplot(3,1,2)

plot(ntime,nang_vel);

title('angular speed (rev/s)')

xlabel('time (s)')

ylabel('pos (rev/s)')

Last Updated: 6/2/21