Page 59 - MDP2020-1

················································································ 명장양성프로젝트 【MDP】 과제발표회 자료집 Ⅰ | 53

servo_3.writeMicroseconds(ist[1]);
}

void calculate()

{
dif[0] = abs(ist[0]-sol[0]);
dif[1] = abs(ist[1]-sol[1]);
dif[2] = abs(ist[2]-sol[2]);

dif[3] = abs(ist[3]-sol[3]);

stepsMax = max(dif[0],dif[1]);
stepsMax = max(stepsMax,dif[2]);
stepsMax = max(stepsMax,dif[3]);

if (stepsMax < 500)
del = 1200;
else
del = 600;

if (sol[0] < ist[0]) dir[0] = 0-dif[0]/stepsMax; else dir[0] = dif[0]/stepsMax;
if (sol[1] < ist[1]) dir[1] = 0-dif[1]/stepsMax; else dir[1] = dif[1]/stepsMax;
if (sol[2] < ist[2]) dir[2] = 0-dif[2]/stepsMax; else dir[2] = dif[2]/stepsMax;
if (sol[3] < ist[3]) dir[3] = 0-dif[3]/stepsMax; else dir[3] = dif[3]/stepsMax;
}

void play_servo()
{
steps += 1;
if (steps < stepsMax)
{

if(steps == 20) time = del*4;
else if(steps == 40) time = del*3;
else if(steps == 80) time = del*2;
else if(steps == 100) time = del-1;

if(steps == stepsMax-200) time = del*2;
else if(steps == stepsMax-80) time = del*3;
else if(steps == stepsMax-40) time = del*4;
else if(steps == stepsMax-20) time = del*5;

ist[0] += dir[0];
ist[1] += dir[1];