Skip to content

Commit

Permalink
quadrotor control
Browse files Browse the repository at this point in the history
  • Loading branch information
@yrlu
yrlu committed May 7, 2017
1 parent 9fd4e5e commit f595b5c
Show file tree
Hide file tree
Showing 21 changed files with 1,010 additions and 0 deletions.
57 changes: 57 additions & 0 deletions control/controller.m
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,57 @@
function [F, M, trpy, drpy] = controller(qd, t, qn, params)
% CONTROLLER quadrotor controller
% The current states are:
% qd{qn}.pos, qd{qn}.vel, qd{qn}.euler = [roll;pitch;yaw], qd{qn}.omega
% The desired states are:
% qd{qn}.pos_des, qd{qn}.vel_des, qd{qn}.acc_des, qd{qn}.yaw_des, qd{qn}.yawdot_des
% Using these current and desired states, you have to compute the desired controls

% =================== Your code goes here ===================

% ordinary linear
% % position controller params
% Kp = ones(3,1)*30;
% Kd = ones(3,1)*10;
%
% % attitude controller params
% KpM = ones(3,1)*10000;
% KdM = ones(3,1)*500;


% position controller params
Kp = [15;15;30];
% Kd = [15;15;10];
Kd = [12;12;10];

% attitude controller params
KpM = ones(3,1)*3000;
KdM = ones(3,1)*300;

% t = qd{qn}.vel_des/norm(qd{qn}.vel_des+eps);
% n = qd{qn}.acc_des/norm(qd{qn}.acc_des+eps);
% b = cross(t,n);
% ep = ((qd{qn}.pos_des - qd{qn}.pos).*n).*n + ((qd{qn}.pos_des - qd{qn}.pos).*b).*b;
% ev = qd{qn}.vel_des - qd{qn}.vel;

acc_des = qd{qn}.acc_des + Kd.*(qd{qn}.vel_des - qd{qn}.vel) + Kp.*(qd{qn}.pos_des - qd{qn}.pos);
% acc_des = qd{qn}.acc_des + Kd.*ev + Kp.*ep

% Desired roll, pitch and yaw
phi_des = 1/params.grav * (acc_des(1)*sin(qd{qn}.yaw_des) - acc_des(2)*cos(qd{qn}.yaw_des));
theta_des = 1/params.grav * (acc_des(1)*cos(qd{qn}.yaw_des) + acc_des(2)*sin(qd{qn}.yaw_des));
psi_des = qd{qn}.yaw_des;

euler_des = [phi_des;theta_des;psi_des];
pqr_des = [0;0; qd{qn}.yawdot_des];
% Thurst
qd{qn}.acc_des(3);
F = params.mass*(params.grav + acc_des(3));
% Moment
M = params.I*(KdM.*(pqr_des - qd{qn}.omega) + KpM.*(euler_des - qd{qn}.euler));
% =================== Your code ends here ===================

% Output trpy and drpy as in hardware
trpy = [F, phi_des, theta_des, psi_des];
drpy = [0, 0, 0, 0];

end
43 changes: 43 additions & 0 deletions control/crazyflie.m
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,43 @@
function params = crazyflie()
% crazyflie: physical parameters for the Crazyflie 2.0
%
% 2016 Bernd Pfrommer
%
% This function creates a struct with the basic parameters for the
% Crazyflie 2.0 quad rotor (without camera, but with about 5 vicon
% markers)
%
% Model assumptions based on physical measurements:
%
% motor + mount + vicon marker = mass point of 3g
% arm length of mass point: 0.046m from center
% battery pack + main board are combined into cuboid (mass 18g) of
% dimensions:
%
% width = 0.03m
% depth = 0.03m
% height = 0.012m
%

m = 0.030; % weight (in kg) with 5 vicon markers (each is about 0.25g)
g = 9.81; % gravitational constant
I = [1.43e-5, 0, 0; % inertial tensor in m^2 kg
0, 1.43e-5, 0;
0, 0, 2.89e-5];
L = 0.046; % arm length in m

params.mass = m;
params.I = I;
params.invI = inv(I);
params.grav = g;
params.arm_length = L;

params.maxangle = 40*pi/180; % you can specify the maximum commanded angle here
params.maxF = 2.5*m*g; % left these untouched from the nano plus
params.minF = 0.05*m*g; % left these untouched from the nano plus

% You can add any fields you want in params
% for example you can add your controller gains by
% params.k = 0, and they will be passed into controller.m

end
40 changes: 40 additions & 0 deletions control/quadEOM.m
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,40 @@
function sdot = quadEOM(t, s, qn, controlhandle, trajhandle, params)
% QUADEOM Wrapper function for solving quadrotor equation of motion
% quadEOM takes in time, state vector, controller, trajectory generator
% and parameters and output the derivative of the state vector, the
% actual calcution is done in quadEOM_readonly.
%
% INPUTS:
% t - 1 x 1, time
% s - 13 x 1, state vector = [x, y, z, xd, yd, zd, qw, qx, qy, qz, p, q, r]
% qn - quad number (used for multi-robot simulations)
% controlhandle - function handle of your controller
% trajhandle - function handle of your trajectory generator
% params - struct, output from crazyflie() and whatever parameters you want to pass in
%
% OUTPUTS:
% sdot - 13 x 1, derivative of state vector s
%
% NOTE: You should not modify this function
% See Also: quadEOM_readonly, crazyflie

% convert state to quad stuct for control
qd{qn} = stateToQd(s);

% Get desired_state
desired_state = trajhandle(t, qn);

% The desired_state is set in the trajectory generator
qd{qn}.pos_des = desired_state.pos;
qd{qn}.vel_des = desired_state.vel;
qd{qn}.acc_des = desired_state.acc;
qd{qn}.yaw_des = desired_state.yaw;
qd{qn}.yawdot_des = desired_state.yawdot;

% get control outputs
[F, M, trpy, drpy] = controlhandle(qd, t, qn, params);

% compute derivative
sdot = quadEOM_readonly(t, s, F, M, params);

end
84 changes: 84 additions & 0 deletions control/quadEOM_readonly.m
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,84 @@
function sdot = quadEOM_readonly(t, s, F, M, params)
% QUADEOM_READONLY Solve quadrotor equation of motion
% quadEOM_readonly calculate the derivative of the state vector
%
% INPUTS:
% t - 1 x 1, time
% s - 13 x 1, state vector = [x, y, z, xd, yd, zd, qw, qx, qy, qz, p, q, r]
% F - 1 x 1, thrust output from controller (only used in simulation)
% M - 3 x 1, moments output from controller (only used in simulation)
% params - struct, output from crazyflie() and whatever parameters you want to pass in
%
% OUTPUTS:
% sdot - 13 x 1, derivative of state vector s
%
% NOTE: You should not modify this function
% See Also: quadEOM_readonly, crazyflie

%************ EQUATIONS OF MOTION ************************
% Limit the force and moments due to actuator limits
A = [0.25, 0, -0.5/params.arm_length;
0.25, 0.5/params.arm_length, 0;
0.25, 0, 0.5/params.arm_length;
0.25, -0.5/params.arm_length, 0];

prop_thrusts = A*[F;M(1:2)]; % Not using moment about Z-axis for limits
prop_thrusts_clamped = max(min(prop_thrusts, params.maxF/4), params.minF/4);

B = [ 1, 1, 1, 1;
0, params.arm_length, 0, -params.arm_length;
-params.arm_length, 0, params.arm_length, 0];
F = B(1,:)*prop_thrusts_clamped;
M = [B(2:3,:)*prop_thrusts_clamped; M(3)];

% Assign states
x = s(1);
y = s(2);
z = s(3);
xdot = s(4);
ydot = s(5);
zdot = s(6);
qW = s(7);
qX = s(8);
qY = s(9);
qZ = s(10);
p = s(11);
q = s(12);
r = s(13);

quat = [qW; qX; qY; qZ];
bRw = QuatToRot(quat);
wRb = bRw';

% Acceleration
accel = 1 / params.mass * (wRb * [0; 0; F] - [0; 0; params.mass * params.grav]);

% Angular velocity
K_quat = 2; %this enforces the magnitude 1 constraint for the quaternion
quaterror = 1 - (qW^2 + qX^2 + qY^2 + qZ^2);
qdot = -1/2*[0, -p, -q, -r;...
p, 0, -r, q;...
q, r, 0, -p;...
r, -q, p, 0] * quat + K_quat*quaterror * quat;

% Angular acceleration
omega = [p;q;r];
pqrdot = params.invI * (M - cross(omega, params.I*omega));

% Assemble sdot
sdot = zeros(13,1);
sdot(1) = xdot;
sdot(2) = ydot;
sdot(3) = zdot;
sdot(4) = accel(1);
sdot(5) = accel(2);
sdot(6) = accel(3);
sdot(7) = qdot(1);
sdot(8) = qdot(2);
sdot(9) = qdot(3);
sdot(10) = qdot(4);
sdot(11) = pqrdot(1);
sdot(12) = pqrdot(2);
sdot(13) = pqrdot(3);

end
159 changes: 159 additions & 0 deletions control/runsim.m
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,159 @@
% NOTE: This srcipt will not run as expected unless you fill in proper
% code in trajhandle and controlhandle
% You should not modify any part of this script except for the
% visualization part
%
% ***************** MEAM 620 QUADROTOR SIMULATION *****************
close all
clear all
addpath('utils')
addpath('trajectories')

% You can change trajectory here

% trajectory generator
% trajhandle = @step;
% trajhandle = @circle;
trajhandle = @diamond;

% controller
controlhandle = @controller;

% real-time
real_time = true;

% *********** YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW **********
% number of quadrotors
nquad = 1;

% max time
time_tol = 25;

% parameters for simulation
params = crazyflie();

%% **************************** FIGURES *****************************
fprintf('Initializing figures...\n')
h_fig = figure;
h_3d = gca;
axis equal
grid on
view(3);
xlabel('x [m]'); ylabel('y [m]'); zlabel('z [m]')
quadcolors = lines(nquad);

set(gcf,'Renderer','OpenGL')

%% *********************** INITIAL CONDITIONS ***********************
fprintf('Setting initial conditions...\n')
max_iter = 5000; % max iteration
starttime = 0; % start of simulation in seconds
tstep = 0.01; % this determines the time step at which the solution is given
cstep = 0.05; % image capture time interval
nstep = cstep/tstep;
time = starttime; % current time
err = []; % runtime errors
for qn = 1:nquad
% Get start and stop position
des_start = trajhandle(0, qn);
des_stop = trajhandle(inf, qn);
stop{qn} = des_stop.pos;
x0{qn} = init_state( des_start.pos, 0 );
xtraj{qn} = zeros(max_iter*nstep, length(x0{qn}));
ttraj{qn} = zeros(max_iter*nstep, 1);
end

x = x0; % state

pos_tol = 0.01;
vel_tol = 0.01;

%% ************************* RUN SIMULATION *************************
OUTPUT_TO_VIDEO = 1;
if OUTPUT_TO_VIDEO == 1
v = VideoWriter('diamond.avi');
open(v)
end

fprintf('Simulation Running....')
% Main loop
for iter = 1:max_iter
iter;
timeint = time:tstep:time+cstep;

tic;
% Iterate over each quad
for qn = 1:nquad
% Initialize quad plot
if iter == 1
QP{qn} = QuadPlot(qn, x0{qn}, 0.1, 0.04, quadcolors(qn,:), max_iter, h_3d);
desired_state = trajhandle(time, qn);
QP{qn}.UpdateQuadPlot(x{qn}, [desired_state.pos; desired_state.vel], time);
h_title = title(sprintf('iteration: %d, time: %4.2f', iter, time));
end

% Run simulation
[tsave, xsave] = ode45(@(t,s) quadEOM(t, s, qn, controlhandle, trajhandle, params), timeint, x{qn});
x{qn} = xsave(end, :)';

% Save to traj
xtraj{qn}((iter-1)*nstep+1:iter*nstep,:) = xsave(1:end-1,:);
ttraj{qn}((iter-1)*nstep+1:iter*nstep) = tsave(1:end-1);

% Update quad plot
desired_state = trajhandle(time + cstep, qn);
QP{qn}.UpdateQuadPlot(x{qn}, [desired_state.pos; desired_state.vel], time + cstep);
set(h_title, 'String', sprintf('iteration: %d, time: %4.2f', iter, time + cstep))
if OUTPUT_TO_VIDEO == 1
im = frame2im(getframe(gcf));
writeVideo(v,im);
end
end
time = time + cstep; % Update simulation time
t = toc;
% Check to make sure ode45 is not timing out
if(t> cstep*50)
err = 'Ode45 Unstable';
break;
end

% Pause to make real-time
if real_time && (t < cstep)
pause(cstep - t);
end

% Check termination criteria
if terminate_check(x, time, stop, pos_tol, vel_tol, time_tol)
break
end
end

if OUTPUT_TO_VIDEO == 1
close(v);
end

%% ************************* POST PROCESSING *************************
% Truncate xtraj and ttraj
for qn = 1:nquad
xtraj{qn} = xtraj{qn}(1:iter*nstep,:);
ttraj{qn} = ttraj{qn}(1:iter*nstep);
end

% Plot the saved position and velocity of each robot
for qn = 1:nquad
% Truncate saved variables
QP{qn}.TruncateHist();
% Plot position for each quad
h_pos{qn} = figure('Name', ['Quad ' num2str(qn) ' : position']);
plot_state(h_pos{qn}, QP{qn}.state_hist(1:3,:), QP{qn}.time_hist, 'pos', 'vic');
plot_state(h_pos{qn}, QP{qn}.state_des_hist(1:3,:), QP{qn}.time_hist, 'pos', 'des');
% Plot velocity for each quad
h_vel{qn} = figure('Name', ['Quad ' num2str(qn) ' : velocity']);
plot_state(h_vel{qn}, QP{qn}.state_hist(4:6,:), QP{qn}.time_hist, 'vel', 'vic');
plot_state(h_vel{qn}, QP{qn}.state_des_hist(4:6,:), QP{qn}.time_hist, 'vel', 'des');
end
if(~isempty(err))
error(err);
end

fprintf('finished.\n')
Loading

0 comments on commit f595b5c

Please sign in to comment.