calcGoalBeltSpeedDiscrete.m

% This function is part of the NMSM Pipeline, see file for full license.
%
% This function calculates the difference between the belt speed and the
% error center for the specified foot. 
%
% (struct, struct, struct) -> (Array of number)
%


function cost = calcGoalBeltSpeedDiscrete(values, params, costTerm)

errorCenter = valueOrAlternate(costTerm, "errorCenter", 1);
counter = 1;
for i = 1:length(params.userDefinedVariables)
    if strcmp(params.userDefinedVariables{i}.type, 'belt_speed')
        if params.userDefinedVariables{i}.is_left_foot == ...
                costTerm.is_left_foot
            beltSpeed = values.belt_speed(counter);
        end
        counter = counter + 1;
    end
end
cost = calcTrackingCostArrayTerm(beltSpeed, errorCenter , 1);
end