EmaMaker
/
thesis
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

extend unicycle to differential drive

master
EmaMaker 2024-07-31 09:47:09 +02:00
parent ce50ac13d4
commit c02aa51088
4 changed files with 27 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
control_act.m
View File

@ -1,5 +1,5 @@
function [u, ut, uc, U_corr_history] = control_act(t, q, sim_data)
dc = decouple_matrix(q, sim_data.b);
dc = decouple_matrix(q, sim_data);
ut = utrack(t, q, sim_data);
[uc, U_corr_history] = ucorr(t, q, sim_data);
@ -48,9 +48,13 @@ function [u_corr, U_corr_history] = ucorr(t, q, sim_data)
t_ = t + tc * (k-1);
u_track_ = utrack(t_, q_act, sim_data);
T_inv = decouple_matrix(q_act, sim_data.b);
T_inv = decouple_matrix(q_act, sim_data);
% compute inputs (wr, wl)
u_ = T_inv * (u_corr_ + u_track_);
% map (wr, wl) to (v, w) for unicicle
u_ = diffdrive_to_uni(u_, sim_data);
% integrate unicycle
theta_new = q_act(3) + tc*u_(2);
% compute the state integrating with euler
%x_new = q_act(1) + tc*u_(1) * cos(q_act(3));
@ -148,11 +152,21 @@ function q_track = feedback(q, b)
q_track = [q(1) + b*cos(q(3)); q(2) + b*sin(q(3)) ];
end
function T_inv = decouple_matrix(q, b)
function T_inv = decouple_matrix(q, sim_data)
theta = q(3);
st = sin(theta);
ct = cos(theta);
T_inv = [ct, st; -st/b, ct/b];
b = sim_data.b;
r = sim_data.r;
d = sim_data.d;
%a1 = sim_data.r*0.5;
%a2 = sim_data.b*sim_data.r/sim_data.d;
%det_inv = -sim_data.d/(sim_data.b*sim_data.r*sim_data.r);0
%T_inv = det_inv * [ a1*st - a2*ct, -a1*ct - a2*st;-a1*st-a2*ct , a1*ct - a2*st];
T_inv = [2*b*ct - d*st, d*ct + 2*b*st ; 2*b*ct + d*st, -d*ct+2*b*st] / (2*b*r);
end

3
diffdrive.m Normal file
View File

@ -0,0 +1,3 @@
function dq = diffdrive(t, q, u, sim_data)
dq = unicycle(t, q, diffdrive_to_uni(u, sim_data));
end

3
diffdrive_to_uni.m Normal file
View File

@ -0,0 +1,3 @@
function u_ = diffdrive_to_uni(u, sim_data)
u_ = [sim_data.r/2, sim_data.r/2 ; sim_data.r/sim_data.d -sim_data.r/sim_data.d] * u;
end

4
sistema_discr.m
View File

@ -1,3 +1,3 @@
function dq = sistema_discr(t, q, u_discr)
dq = unicycle(t, q, u_discr);
function dq = sistema_discr(t, q, u_discr, sim_data)
dq = diffdrive(t, q, u_discr, sim_data);
end