Sin型加速度曲线在codesys下的实现

Sin型加速度,输入加减速时间和匀速时间,得到每个时间点的位置Factor

这个版本设计的加速度和减速度是一样的,所以只使用了maxAccel和maxSpeed

Codesys代码

FUNCTION_BLOCK SinAccelProfile
VAR_INPUT
	accelTime : LREAL;
	constTime : LREAL;
	decelTimePoint : LREAL;
	totalTime : LREAL;
	totalFactor : LREAL;
END_VAR
VAR_OUTPUT
END_VAR
VAR
END_VAR

//----------------------------------------------
METHOD getDistanceFactor : LREAL
VAR_INPUT
	t : LREAL;
END_VAR
VAR
	d : LREAL;
	tf : LREAL;
END_VAR
IF t < 0.0 THEN
	d := 0.0;
ELSIF t<=accelTime THEN
	tf := (t*Math.PI)/accelTime;
	d := tf - SIN(tf);
ELSIF t<=decelTimePoint THEN
	tf := (t-accelTime)/accelTime;
	d := Math.PI + 2*Math.PI*tf;
ELSIF t<=totalTime THEN
	tf := (Math.PI*(t-decelTimePoint))/accelTime;
	d := totalFactor - Math.PI + (tf - SIN(Math.PI+tf));
ELSE
	getDistanceFactor := 1.0;
	RETURN;
END_IF
getDistanceFactor := d/totalFactor;


//-----------------------------------------
METHOD initByDynamic
VAR_INPUT
	length : LREAL;
	maxAccel : LREAL;
	maxSpeed : LREAL;
	minAccelTime : LREAL;
END_VAR
VAR
	nominalAccel : LREAL;
	minTime : LREAL;
	minLen : LREAL;
	constLen : LREAL;
	accelDist : LREAL;
END_VAR
nominalAccel := maxAccel/(0.5*Math.PI);
minTime := 2.0*maxSpeed/nominalAccel;
minLen := minTime*0.5*maxSpeed;
IF length > minLen THEN
	constLen :=length - minLen;
ELSE
	constLen := 0;
END_IF
constTime := constLen/maxSpeed;
accelDist := 0.5*(length-constLen);
accelTime := SQRT(Math.PI*accelDist/maxAccel);
update(minAccelTime);


//----------------------------------------
METHOD initByTime
VAR_INPUT
	accelTime : LREAL;
	constTime : LREAL;
END_VAR
THIS^.accelTime := accelTime;
THIS^.constTime := constTime;
update(0.01);

//-----------------------------------------
METHOD update
VAR_INPUT
	minAccelTime : LREAL;
END_VAR
accelTime := MAX(accelTime,minAccelTime);
decelTimePoint := accelTime + constTime;
totalTime := accelTime +constTime + accelTime;
totalFactor := 2.0*Math.PI + 2.0*Math.PI*constTime/accelTime;

Matlab验证代码

clear all;

maxSpeed=1000;
maxAccel=5000;
minAccelTime=0.1;
length=500;

nominalAccel = maxAccel/(0.5*pi);
minTime = 2.0*maxSpeed/nominalAccel;
minLen = minTime*0.5*maxSpeed;
if length > minLen
  constLen = length - minLen;
  constTime = constLen/maxSpeed;
  accelDist = 0.5*(length - constLen);
else
  constLen=0;
  constTime=0;
  accelDist = 0.5*(length - constLen);
endif

accelTime = sqrt(pi*accelDist/maxAccel);
accelTime = max(accelTime,minAccelTime);

%accelTime = 0.1;
%constTime = 0.1;

decelTimePoint = accelTime + constTime;
totalTime = accelTime + constTime + accelTime;
totalFactor = 2.0*pi + 2.0*pi*constTime/accelTime;

accelTime
constTime

tArray = [];
distArray = [];
dt = 0.005;

t = 0;
while(true)
  if t<0.0
    d = 0;
  elseif t<=accelTime
    tf = t*pi/accelTime;
    d = tf - sin(tf);
  elseif t<=decelTimePoint
    tf = (t-accelTime)/accelTime;
    d = pi + 2*pi*tf;
  elseif t<=totalTime
    tf = pi*(t-decelTimePoint)/accelTime;
    d = totalFactor - pi + (tf-sin(pi+tf));
  else
    d = totalFactor;
  endif
  df = d/totalFactor;
  tArray = [tArray t];
  distArray = [distArray df];
  if(t > totalTime)
      break;
  endif
  t = t + dt;
endwhile

velArray = [0 diff(distArray)];
accArray = [0 diff(velArray)];

%close all
subplot(3,1,1);
plot(tArray,distArray*length,'.-');
title('dist');
grid on;
subplot(3,1,2);
plot(tArray,velArray*length/dt,'.-');
title('vel');
grid on;
subplot(3,1,3);
plot(tArray,accArray*length/(dt*dt),'.-');
title('acc');
grid on;

Matlab结果