gettorqu.m

来自「多智能体工具包」· M 代码 · 共 27 行

function [ThisTorque,TheSign]=gettorqu(DeltaAngle)
% GETTORQU	Calculate the torque by the force of a single robot.

%     Copyright (c) 1998-2000 Jiming Liu and Jianbing Wu

global ObjectSize;

ToqueRadius=ObjectSize;

PositiveToqueAngle=-DeltaAngle;
if PositiveToqueAngle<0
  PositiveToqueAngle=PositiveToqueAngle+2*pi;
end;
if (PositiveToqueAngle>=0) & (PositiveToqueAngle<pi/2)
  ThisTorque=ToqueRadius*sin(PositiveToqueAngle);
  TheSign=1;
end;

NegativeToqueAngle=DeltaAngle;
if NegativeToqueAngle<0
  NegativeToqueAngle=NegativeToqueAngle+2*pi;
end;
if (NegativeToqueAngle>0) & (NegativeToqueAngle<pi/2)
  ThisTorque=ToqueRadius*sin(NegativeToqueAngle);
  TheSign=-1;
end;