机器人作业2

作业

练习 2_1

calculateWheelSpeeds.m 的 TODO 部分代码：

% ex_2_1
% In exercise2-assignment.pdf
% v is vu here
% w is omega here
% r is wheelRadius here
% l is halfWheelbase here
% /Phi(l) and /Phi(r) are LeftWheelVelocity and RightWheelVelocity here
LeftWheelVelocity = (vu - halfWheelbase * omega) / wheelRadius;
RightWheelVelocity = (vu + halfWheelbase * omega) / wheelRadius;

练习 2_2 和 2_3

calculateControlOutput.m 的 后半部分

% exercise2-assignment.pdf 中的 beta
beta = - (lambda + thetag);
beta = normalizeAngle(beta);
% ...
% 这里是原来程序自带的一堆注释
% ...
% ex_2_2
% exercise2-assignment.pdf 中的公式 (3) (4)
vu = parameters.Krho * rho;% [m/s] 
omega = parameters.Kalpha * alpha + parameters.Kbeta * beta;% [rad/s]
% ex_2_3
ex_2_3 = parameters.backwardAllowed && parameters.useConstantSpeed;
direction = 1; % 代表方向，1 正向，-1 反向
if ex_2_3
    % 如果是反向
    if abs(alpha) > pi() / 2
       % 重新计算 alpha, beta
       alpha = alpha - pi();
       beta = beta - pi();
       % 反向，速度数值取负
       vu = -vu;
       % 标记，后面 constantSpeed 用
       direction = -1; 
       % 重新计算 omega (pdf 中的 w)
       omega = parameters.Kalpha * alpha + parameters.Kbeta * beta;% [rad/s]
    end
    % 当速度不是 0 的时候
    if abs(vu) > 0.000001
        % 根据 vu 的缩放调整 omega
        omega = omega * (parameters.constantSpeed / abs(vu));
        % 常数速度 （有方向）
        vu = direction * parameters.constantSpeed;
    end    
end