planner.ino

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;
                  }




         }


}