% TOGAC43_AMBF_VelocityController.m

% This file will run basic control inputs on the galen robot for the ambf
% simulation. This contects matlab with ambf using the galen_interface
% class. Ensure ROS is running by running roscore in a terminal and then
% open the ambf model in a separate terminal
% Instructions on how to install the AMBF application and how to use the
% robot with ambf are found in:

% TOGAC25_AMBF_IN_AMBFforGalen.pdf

% Instructions for using matalb with ambf are found in:

% TOGAC29_AMBF_IN_ConnectMatlab2AMBF.pdf

% 3 main control types are avaliable. Chirp, Linear Trajectory, Sine, and Step inputs can be
% used. After the control loop sections, you can plot rv and cv, the output
% results.

% !!!!This script is best used when run in sections!!!!
% It is not reccommended to simply run this script as is. Initializatoin of
% the Galen_robot is a finicky process. Ensure you have the correct IP
% address or ROS initialization steps updated for your machine in the
% galen_interace.m file before running. HINT: running sections is easy.
% Instead of pressing the run section button in the editor, simply place
% and click your cursor in the section you want to run, then type cntrl +
% enter

% to run the system, run section 1 until you have successfully read the
% joint states and the force sensor data at the E35 joint
% if it fails to run or if you're waiting for the loop to continue because
% it's hung and subscribing or receiving then cancel the loop and try
% again. If this does not eventually work, then update your ROS_MASTER_URI
% or your ROS_IP in either the galen_interface file or on your linux
% machine. You may need to do a restart of your device.

% After running section 1
% control.
% After running section 1, you may run any of the section 2 commands to
% preemtively move the robot. If you don't want gravity on, don't forget to
% click in your AMBF window and press 2

% You can then run section 3 to control the robot

% Created by Brevin Banks
% Modified 4/29/2023


%% Section 1
galen_robot = galen_interface();  % initiate the robot interface

%%% Section 2
%% Section 2.1
galen_center_vel_based(galen_robot) % Home the robot
%% Section 2.2
uppose = single([.35,.35,.35,0,0,0]); % Home the galen robot partially above the midpoints
galen_center_vel_based(galen_robot,uppose)
%% Section 2.3
galen_robot.get_current_joints();  % center the galen robot rotational joints
current_position = galen_robot.extract_galen_pose();
uppose = single([current_position(1),current_position(2),current_position(3),0,0,0]);
galen_center_vel_based(galen_robot,uppose)
%% Section 3
% Assign Chirp Input
galen_robot.set_control_type('Velocity');  % setup velocity control

% Initialize the error for iteration
error = 1;

%Generate Chirp
des_sim_time = 60;
sim_time_window = (0:1/200:des_sim_time); % 50 second window with 200Hz sampling
f0 =   0.001; % Starting frequency 0.001 Hz
f1 = 5; % ending frequency 5 Hz
Fs = 1/mean(diff(sim_time_window));
x = chirp(sim_time_window,f0,sim_time_window(end),f1,'linear',-90); % generate chirp magnitude 0.05

% Initialize iteratior count, cv - output data of robot, rv - desired input
% for robot
i=1;
cv = [];
rv = [];
rqd = [];
cqd = [];
% Initialize a vector to capture the change in time
time_vec = [];

r = rosrate(200);% rate control object will hold the loop at 200Hz
current_sim_time =0; % initialize time at 0
reset(r) % clear any previously stored rate statistics

J = galen_robot.galen_jacob();
while(current_sim_time<des_sim_time+1) % for ~50 seconds run the loop
    %Grab the current sampling point in time relative to total time
    current_sim_time = r.TotalElapsedTime;

    % find the closest point to time in the chirp function
    [ d, ix ] = min( abs( sim_time_window-current_sim_time) );
    if ix <2
        ix =2; % can't interpolate starter position without more than 2 samples
    elseif ix>length(x)-1
        ix = length(x)-1; % can't interpolate end position without if more than 10000 samples
    end
    %interpolate to get the value of the chirp function
    Cinput = x(ix-1)+(current_sim_time-sim_time_window(ix-1))*(((x(ix+1) - x(ix-1))/(sim_time_window(ix+1) - sim_time_window(ix-1))));
    if current_sim_time>des_sim_time % can't interpolate at the end conditions. Just assign 0
        Cinput = 0;
    end

    % Assign the desired velocity for 3 carriage joints
    ref_velocity = single([0,0,Cinput*0.01,0,0,0]');

    qdot_ref = J\ref_velocity;
    qdot_ref = [qdot_ref(1);qdot_ref(2);qdot_ref(3);qdot_ref(4);0;qdot_ref(5)];
    


    % Move the galen robot
    galen_robot.move_galen(qdot_ref);
    
    % Retrieve the current robot joints
    qdot_out = galen_robot.get_current_joints();
    J = galen_robot.galen_jacob();

    qdot_out =[qdot_out(1);qdot_out(2);qdot_out(3);qdot_out(4);qdot_out(6)];
    current_velocity = J*qdot_out;
    % Find the error between desired and output
    error = norm(current_velocity-qdot_ref);

    %Store input and output history
    cv(:,i) = current_velocity;
    rv(:,i) = ref_velocity;
    rqd(:,i) = qdot_ref;
    cqd(:,i) = qdot_out;
    % Increase iterator and record current time
    i = i+1;
    time_vec(i) = current_sim_time;

    % Hold the loop if the sampling time is under 200Hz
    waitfor(r);
end
% Output the resulting total loop time and quality of the sampling rate
stats = statistics(r)
disp(r)
time_elapsed = r.TotalElapsedTime;

% Plot the shape of the Velocity input response
figure(2)
plot(sim_time_window,x)
title('Chirp Input 0.001Hz to 5Hz for 60s')
xlabel('Time (s)')
ylabel('Velocity input (m/s)')


%% Assign A Step Input

galen_robot.set_control_type('Velocity');  % setup velocity control

% Step Input
current_velocity = galen_robot.get_current_joints();
current_position = galen_robot.extract_galen_pose();

% Initialize iteratior count, cv - output data of robot, rv - desired input
% for robot
i=1;
cv = [];
rv = [];
rqd = [];
cqd = [];
% Initialize a vector to capture the change in time
time_vec = [];

ref_velocity = single([0.10,0.20,0.0,0.02,0.01,0.0]'); % Step input reference 0.05 magnitude
    
r = rosrate(200);% rate control object will hold the loop at 200Hz
current_sim_time = 0; % initialize time at 0
reset(r) % clear any previously stored rate statistics


J = galen_robot.galen_jacob();
while(current_sim_time<10)
    %Grab the current sampling point in time relative to total time
    current_sim_time = r.TotalElapsedTime;
    
    qdot_ref = J\ref_velocity;
    qdot_ref = [qdot_ref(1);qdot_ref(2);qdot_ref(3);qdot_ref(4);0;qdot_ref(5)];
    


    % Move the galen robot
    galen_robot.move_galen(qdot_ref);
    
    % Retrieve the current robot joints
    qdot_out = galen_robot.get_current_joints();
    J = galen_robot.galen_jacob()

    qdot_out =[qdot_out(1);qdot_out(2);qdot_out(3);qdot_out(4);qdot_out(6)];
    current_velocity = J*qdot_out;
    % Find the error between desired and output
    error = norm(current_velocity-qdot_ref);

    %Store input and output history
    cv(:,i) = current_velocity;
    rv(:,i) = ref_velocity;
    rqd(:,i) = qdot_ref;
    cqd(:,i) = qdot_out;
    % Increase iterator and record current time
    i = i+1;
    time_vec(i) = current_sim_time;

    % Hold the loop if the sampling time is under 200Hz
    waitfor(r);
end

% Output the resulting total loop time and quality of the sampling rate
stats = statistics(r)
disp(r)
time_elapsed = r.TotalElapsedTime;


%% Apply Sine input on the robot

galen_robot.set_control_type('Velocity');  % setup velocity control

t = 0:pi/36:2*pi;%discretized input to have a period over 2pi separated by 36 parts
timer = 1;% Initialize starter time variable

r = rosrate(200);
current_sim_time =0;
reset(r)

while(current_sim_time<51)
    if timer>length(t)-1
        timer = 1;
    end
    upsign = 0.01*sin(t(timer));
    timer = 1 + timer;
    ref_velocity = single([upsign,upsign,upsign,0,0,0]');
     % Move the galen robot
    galen_robot.move_galen(ref_velocity);

    % Retrieve the current robot joints
    current_velocity = galen_robot.get_current_joints();
    % Find the error between desired and output
    error = norm(current_velocity-ref_velocity);

    %Store input and output history
    cv(1:3,i) = current_velocity(1:3);
    rv(1:3,i) = ref_velocity(1:3);

    % Increase iterator and record current time
    i = i+1;
    time_vec(i) = current_sim_time;

    % Hold the loop if the sampling time is under 200Hz
    waitfor(r);
end

% Output the resulting total loop time and quality of the sampling rate
stats = statistics(r)
disp(r)
time_elapsed = r.TotalElapsedTime;



%% Section 4

% Plot the current values store in cv and rv. This should theoretically be
% the 3 different carriage joint values, and they should all be nearly
% identical if not for noise between sampling times and robot model
% innacuracies

t = 1:length(cv); % not true seconds, Just a count for every existing sample.
% Update t above to be the true total loop time for the collected sample to
% get an accurate figure for the repsonse time

% plot all the cartesian velocities
close all
figure(1)
subplot(5,1,1)
hold on
plot(t,rv(1,:))
plot(t,cv(1,:))
legend('X Vref','X Vout')
ylabel('mm/s')

subplot(5,1,2)
hold on
plot(t,rv(2,:))
plot(t,cv(2,:))
legend('Y Vref','Y Vout')
ylabel('mm/s')

subplot(5,1,3)
hold on
plot(t,rv(3,:))
plot(t,cv(3,:))
legend('Z Vref','Z Vout')
ylabel('mm/s')

subplot(5,1,4)
hold on
plot(t,rv(4,:))
plot(t,cv(4,:))
legend('wx Vref','wx Vout')
ylabel('rad/s')

subplot(5,1,5)
hold on
plot(t,rv(5,:))
plot(t,cv(5,:))
legend('wy Vref','wy Vout')
xlabel('Time (s)')
ylabel('rad/s')


% plot the joint velocities

figure(2)
subplot(5,1,1)
hold on
plot(t,rqd(1,:))
plot(t,cqd(1,:))
legend('Car1 Vref','Car1 Vout')
ylabel('mm/s')

subplot(5,1,2)
hold on
plot(t,rqd(2,:))
plot(t,cqd(2,:))
legend('Car2 Vref','Car2 Vout')
ylabel('mm/s')

subplot(5,1,3)
hold on
plot(t,rqd(3,:))
plot(t,cqd(3,:))
legend('Car3 Vref','Car3 Vout')
ylabel('mm/s')

subplot(5,1,4)
hold on
plot(t,rqd(4,:))
plot(t,cqd(4,:))
legend('Roll Vref','Roll Vout')
ylabel('rad/s')

subplot(5,1,5)
hold on
plot(t,rqd(6,:))
plot(t,cqd(5,:))
legend('Tilt Vref','Tilt Vout')
xlabel('Time (s)')
ylabel('rad/s')

%%


% Center the robot
% Will move the robot to the indicated pose in other_pose, or the home
% position if no input is given
% Input:
%   galen_robot - the robot object from galen_interface()
%   other_pose - desired centered pose
% Note: this function will not use any optimization of trajectories and
% will drive the galen hard towards its goal poses
function galen_center_vel_based(galen_robot,other_pose)

galen_robot.set_control_type('Velocity');  % setup velocity control
if nargin<2
    ref_pose = single([0.210,0.210,0.210,0.0,0.0,0.0]'); %Home pose
else
    ref_pose = other_pose'; %Home pose
end
galen_robot.get_current_joints();
current_position = galen_robot.extract_galen_pose();

% Initialize iteratior count, cv - output data of robot, rv - desired input
% for robot
i=1;
% Initialize a vector to capture the change in time



ref_velocity = single([0.0,0.0,0.0,0.0,0.0,0.0]');



r = rosrate(200);% rate control object will hold the loop at 200Hz

reset(r) % clear any previously stored rate statistics

sendout = false;

while(sendout==false || i<50)
    %Grab the current sampling point in time relative to total time


    for k = 1:6
        if k~=5
            if abs(current_position(k)-ref_pose(k))>0.002
                if current_position(k)>ref_pose(k)
                    ref_velocity(k) = ref_velocity(k)-single(0.002);
                elseif current_position(k)<ref_pose(k)
                    ref_velocity(k) = ref_velocity(k)+single(0.002);
                end
            else
                ref_velocity(k) = ref_velocity(k)/2;
            end
            if abs(ref_velocity(k))>0.05
                ref_velocity(k)= sign(ref_velocity(k))*0.05;
            end
        end
    end

    % Move the galen robot
    galen_robot.move_galen(ref_velocity);

    % Retrieve the current robot joints
    galen_robot.get_current_joints();
    current_position = galen_robot.extract_galen_pose();

    % Increase iterator and record current time
    i = i+1;

    norm(current_position-ref_pose);
    norm(ref_velocity);

    if norm(current_position-ref_pose)<0.003 && norm(ref_velocity)<0.003
        if sendout == false
            i = 0;
        end
        sendout = true;

    end

    % Hold the loop if the sampling time is under 200Hz
    waitfor(r);
    if i>7000
        break
    end

end

end