Select Git revision
planner.ino
planner.ino 7.10 KiB
void planner()
{
for (int a=0;a<max_ax;a++)
{
if (speed[a]==null_speed) dir[a]=d_stop;
if (speed[a]>null_speed) dir[a]=d_up;
if (speed[a]<null_speed) dir[a]=d_down;
}
for (int a=0;a<max_ax;a++)
{
testpos(a);
}
running=false;
for (int a=0;a<max_ax;a++)
{
if (!sak[a]) running=true;
}
if (running!=lastrunning)
{
if (running) Serial.println("planner: running");
else
{
Serial.println("planner: stop");
for (int a=0;a<max_ax;a++)
{
if (sak[a] and usebrake[a]) setbrake(a,true);
}
if (issequence)
{
seq++;
if (seq==targetcount) {seq=0;}
delay(1000);
target[0]=targets[seq][0];
goposax(0);
}
/*else
{
if (repeat)
{
seq=0;
if (targets[0][seq]!=maxpos[0]+1)
{
delay(1000);
target[0]=targets[0][seq];
//goposax(0);
}
}
else issequence=false;
}*/
}
}
lastrunning=running;
}
//set start values for certain ax
void goposax(int ax)
{
#ifdef info
Serial.println("________________________________________________");
Serial.print("calculating vals for ax ");
#endif
Serial.println(ax);
#ifdef info
Serial.print("target: ");
#endif
Serial.println(target[ax]);
#ifdef info
Serial.print("pos: ");
#endif
Serial.println(pos[ax]);
//return;
step[ax]=0;
originpos[ax]=pos[ax];
maxdownspeed[ax]=0;
maxupspeed[ax]=0;
if (pos[ax]>target[ax])
{
dist[ax]=pos[ax]-target[ax];
trackspeed[ax]=2048-round((dist[ax]/100)*speedmultiplier[ax]);
if (trackspeed[ax]>2018) trackspeed[ax]=2018;
maxdownspeed[ax]=trackspeed[ax];
if (maxdownspeed[ax]<absmaxdownspeed[ax]) maxdownspeed[ax]=absmaxdownspeed[ax];
speed[ax]=mindownspeed[ax];
ramp[ax]=r_up;
#ifdef info
Serial.println("accel");
#endif
}
else
{
dist[ax]=target[ax]-pos[ax];
trackspeed[ax]=2048+round((dist[ax]/100)*speedmultiplier[ax]);
if (trackspeed[ax]<2078) trackspeed[ax]=2078;
maxupspeed[ax]=trackspeed[ax];
if (maxupspeed[ax]>absmaxupspeed[ax]) maxupspeed[ax]=absmaxupspeed[ax];
speed[ax]=minupspeed[ax];
ramp[ax]=r_up;
#ifdef info
Serial.println("accel");
#endif
}
#ifdef info
Serial.print("distance: ");
#endif
Serial.println(dist[ax]);
#ifdef info
Serial.print("maxdownspeed: ");
#endif
Serial.println(maxdownspeed[ax]);
#ifdef info
Serial.print("maxupspeed: ");
#endif
Serial.println(maxupspeed[ax]);
#ifdef info
Serial.print("initial speed: ");
#endif
Serial.println(speed[ax]);
if (dist[ax]<10) //too small distance, ignore
{
gopos[ax]=false;
speed[ax]=null_speed;
ramp[ax]=r_stop;
sak[ax]=true;
}
else
{ if (usebrake[ax]) setbrake(ax,false);
mot_on(ax);
sak[ax]=false;
running=true;
gopos[ax]=true;
}
}
//testing ax pos for needed action:
void testpos(int ax)
{
int rampunit=50;
if (gopos[ax]&&(dir[ax]==d_up))
{
if ((pos[ax]>originpos[ax]+step[ax]*rampunit)
&& (speed[ax]<maxupspeed[ax])
&& (ramp[ax]==r_up))
{
//(*BESCHLEUNIGEN*)
speed[ax]=speed[ax]+m[ax];
step[ax]++;
// Serial.print("speed: ");
// Serial.println(speed[ax]);
if (speed[ax]>=maxupspeed[ax])
{
ramp[ax]=r_max;
Serial.println("max");
speed[ax]=maxupspeed[ax];
}
}
if ((pos[ax]>target[ax]-step[ax]*rampunit)
&& (ramp[ax]==r_max))
{
ramp[ax]=r_down;
#ifdef info
Serial.println("down");
#endif
step[ax]--;
}
if (ramp[ax]==r_down)
{
if ((pos[ax]>target[ax]-step[ax]*rampunit)
&& (speed[ax]>minupspeed[ax]))
{
//(*BREMSEN*)
if (step[ax]>0) step[ax]--;
speed[ax]=speed[ax]-m[ax]*1;
if (speed[ax]<=minupspeed[ax])
{
speed[ax]=minupspeed[ax];
ramp[ax]=r_slow;
#ifdef info
Serial.println("slow");
#endif
}
}
}
if ((ramp[ax]==r_slow)
&&(pos[ax]>=target[ax]))
{
// (*STOP*)
speed[ax]=2048;
gopos[ax]=false;
mot_off(ax);
pos_to_eeprom();
ramp[ax]=r_stop;
#ifdef info
Serial.println("stop");
#endif
sak[ax]=true;
}
}
if (gopos[ax] && (dir[ax]==d_down))
{
if ((ramp[ax]==r_up)
&& (pos[ax]<(originpos[ax]-step[ax]*rampunit))
&& (speed[ax]>maxdownspeed[ax]))
{// (*BESCHLEUNIGEN*)
speed[ax]=speed[ax]-m[ax];
step[ax]++;
// Serial.print("speed: ");
// Serial.println(speed[ax]);
if (speed[ax]<=maxdownspeed[ax])
{
speed[ax]=maxdownspeed[ax];
ramp[ax]=r_max;
#ifdef info
Serial.println("max");
#endif
}
}
if ((ramp[ax]==r_max)
&& (pos[ax]<target[ax]+step[ax]*rampunit))
{
ramp[ax]=r_down;
#ifdef info
Serial.println("down");
#endif
step[ax]--;
}
if (ramp[ax]==r_down)
{
if ((pos[ax]<target[ax]+step[ax]*rampunit)
&& (speed[ax]<mindownspeed[ax]))
{// (*BREMSEN*)
if (step[ax]>0) step[ax]--;
speed[ax]=speed[ax]+m[ax]*1;
if (speed[ax]>=mindownspeed[ax])
{
speed[ax]=mindownspeed[ax];
ramp[ax]=r_slow;
#ifdef info
Serial.println("slow");
#endif
}
}
}
if ((ramp[ax]==r_slow)
&& (pos[ax]<=target[ax]))
{// (*STOP*)
mot_off(ax);
speed[ax]=2048;
gopos[ax]=false;
ramp[ax]=r_stop;
#ifdef info
Serial.println("stop");
#endif
sak[ax]=true;
}
}
}