algorithm.txt

= New algorith (v2.0) =

TIME_STYPE DT_BUZZ_US = 125; // Half-period for the buzzer sound, us; computed as 10^6/(2*freq_Hz)
  TIME_UTYPE t_buzz; // timer for the buzzer
  byte buzz_state; // HIGH or LOW for the buzzer state
  TIME_STYPE dt1_buzz_us = 1000; // Current half-period for the buzzer sound, us


Pause restart bug

* Normal start:
g.stacker_mode = 1;
g.start_stacking = 0;




* After a pause (g.paused=1)
g.paused = 0;
g.stacker_mode = 2; //  2-point focus stacking
g.start_stacking = 1;  // =1 if we just initiated focus stacking
g.t0_stacking = g.t;
g.ipos_to_shoot = g.ipos;  // Position to shoot the next shot during focus stacking
g.noncont_flag = 1

Camera
g.AF_on = 1
g.t_AF = g.t
g.start_stacking = 2  //=2 when AF is triggered initially
  
Camera
g.start_stacking = 3; //=3 after CONT_STACKING_DELAY delay in continuous mode


POtential issues:
 - micros() returns unsigned long (uint32), but my TIME_TYPE is a signed int (s32) - I'll have issues with negative times!

Final corrections:
 * All stops are at maximum acceleration
 * The final leg of FF/REWIND is at maximum acceleration
 * The final leg of FF/REWIND is marked uninterrupted in runtime (when predicting the next step)
 - limit1 is calibrated on "switch on"
 - When turning on the rail, and we are too close to a limiter, first move to a safe spot, then execute the backlash compensation loop.


initialize:
  g.moving = 0;
  g.model_init = 0;
  g.model_type = MODEL_NONE;
  g.dt_lost = 0;
  g.direction = 1;
  g.dir = 1;

keypad:
                  g.model_type = MODEL_REWIND;
                  g.model_init = 1;

motor:
speed0 = 0
  g.model_ipos0 = g.ipos;  // Initial absolute coordinate
  g.model_t0 = g.t;  // Initial absolute time
  g.model_ipos1 = g.limit1;
    COORD_TYPE dx = g.model_ipos1 - g.ipos;  <0
      dx_prime = (float)dx - OVERSHOOT;  overshooting g.limit1! <0
        g.direction = -1;
		g.dir = -1;
     accel = g.accel_v[1]; -small_accel
     accel_last = g.accel_v[4];  +large accel
      Vmax0 = SPEED_LIMIT;
      dx1 = 0.0;
    float Vmax2 = 2*accel * (x1 - dx1); >0
    Vmax = sqrt(Vmax2);
 		
  g.model_init = 0;
  g.moving = 1;
  
   g.Npoints = 5 (4)
   g.model_dir[g.Npoints]
    g.model_accel[g.Npoints]
    g.model_time[g.Npoints]
    g.model_speed[g.Npoints
    g.model_pos[g.Npoints]
    g.model_ptype[g.Npoints]

dt = 0
i0 = 0
d_pos = 0
d_pos_next = -1
dt0 = 0
t_step = 0
d_pos0_next = -1;
i_next = 0
  g.t_next_step
  g.ipos_next_step = g.ipos -1
  
----

g.ipos =   g.ipos_next_step
g.dt_lost updated
g.t updated





status_flag
t_status

To take care of:

 - backlashing - remove
 backlash_init???
 - limiters()
 - EEPROM, parking


New backlashing model:
 - In generate_model, all last leg negative moves have backlashing added
 - In backlash(), when not moving, initiate go_to backlash compensation move.

== Remaking motor_control() ==

The newest approach:

 - Replace current calls to "go_to", "change_speed" etc with simply updating one variable (plus a few optional ones) - g.next_model:
 -- NONE: 0
 -- GO_TO: 1 // Only starting at rest. Also set model_accel, and g.model_speed, and model_ipos vars
 -- ACCEL: 2 // also set model_accel
 -- STOP: 3 // also set model_accel
 - Inside motor_control, the earliest moment model and real times are synced (either at rest, or after making a step), call generate_model() if g.next_model != NONE
 - Inside generate_model(), generate the next model based on g.next_model value (and model_speed, model_accel, model_ipos).
 - Initialize the new model - synced time, speed etc
 - Set g.next_model to NONE.
 
This way we can still stick to simpler models - e.g. go_to only works from rest (so we avoid complicated scenarios reqairing direction change). We are also consistent now - all models are initiated the same way.




!!! No need to sync model/real time for dir change, as it's not time sensitive. The only events I should predict (and sync times) are making a step. If I predict a dir change within the next step, I do the current step, then change direction, and then predict the next step in the opposite direction (ipos-1).

Inside motor_control()

At each special event (step, dir), we do the action, sync the model/real times, and predict the next event (step, dir).

If an outside event jus happened (dire ket press/depress, limiter switch), it is handled in the next special event, by modifying the model accordingly, before making a predictoion.

Soft limits are handled in go_to. Probably should also be handled in motor_control, for accelerate() commands.


Model is precomputed as 3 vectors: 
 - model_accel (char): indexes for model acceleration at the points when acceleration changes (-2...+2)
 - model_time (float, or even double): model time relative to the initial point for the points when acceleration changes.
  - model_x (float, or double): coordinates relative to the initial point for the acceleration change moments
  
We also memorize the initial absolute model time (int) and initial coordinate (int).

Event prediction:
 - g.t_next_step // absolute time
 - g.d_ipos_next // relative to the model starting point


Model description:
 - g.ipos_goto
 - g.ipos0
 - g.Npoints
 - g.model_t0 (initialized inside motor_control)
 - g.model_time[]
 - g.model_ptype[]
 - g.model_speed[]  : a signed number
 - g.model_accel[]
 - g.model_pos[] (float)
 - g.model_dir[]
 
Other important vars:
 - g.dt_lost
 - g.moving
 - g.started_moving
 - g.speed
 - g.ipos
 - g.direction
 - g.BL_counter
 - g.model_change : 0: nothing, 1: switch to accelerate model, 2: switch to stop model
 
Positive numbers:
 - speed
 - Vmax
Signed numbers:
 - speed_signed
 - dx
 
When changing direction, current model position is not integer!

 - Make current action (step, dir), sync time
 - Now check if one or more of the future model points is within a single step from the current int coordinate x: 
    if (model_x[i]>x && model_x[i]<x+1)
 - If yes, we do complex (piecewise) model timing prediction
 - If no, we do a simple prediction, based on the current leg o fthe model: solving a linear eq if accel=0, or qudratic eq if accel!=0.
 
- It looks like the only sensible way is to only act upon one event at a time. If more than one event happened since last event, we execute the earliest one.

 - Model should include soft limits. Make sure the last leg (after hitting a soft limit) is uninterrupted.

 - Stop creteria
 -- My model is coordinate (step) centric. All models end at a specific step.
 -- GoTo: the step specified
 -- Accelerate: step when stopped after reaching the soft limit
 -- Stop: step when speed becomes zero
 -- With all these final step estimates, add an OVERSHOOT parameter (1-3 steps), to account for roundoff errors.
 -- When in final leg, stop_now when hitting the destination coordinate.
 -- In final leg, if fail to predict the next step, stop_now (as the last resort).
 
Models:
 - go_to : only starting from rest, go to specified coordinate with a given maximum speed, medium acceleration. Also used for the backlash compensation move at the end if needed. Soft limits are enforced internally.
 
 - accelerate: when pressing forward or rewind keys. Moves at the constant given acceleration, until hitting the maximum speed. Starts decelerating at given acceleration before the soft limit, stops at the soft limit. Can be initiated when moving.
 
 - stop. Start breaking with the highest acceleration at the next step. After hitting a limiter, or emergency breaking keys.


=== Preliminary remarks ===
* No need to store floating pointg positions (g.pos, g.pos0), as when we stop, we can only be at an integer position relative to the initial point. 
* Make sure g.dt_lost=0 when not moving (???).

=== Initial motion ===

* g.started_moving = 1 elsewhere
* Introducing g.ipos (current integer position), and g.ipos_old (???; previous integer position), g.ipos0 (integer position at the last special event). The floating point position is not needed anymore.
* Gettibg rid of g.pos, g.pos_old, g.pos_int_old, g.pos_goto




= Old algorithm (pre-2.0)=
== Calibration ==

=== User initated (#C command) ===
g.accident = 0

* #C pressed, keypad.ino
** ignored if g.moving == 1 || g.paused || g.limit_on
** g.calibrate_flag = 1
** g.error = 4
** display_all() -> "Calibrate?"

* Any key pressed, keypad.ino
** g.error = 0

* calibration.ino
** change_speed(g.speed_limit, 0, 2) -> start moving in the positive direction at max speed/acceleration
** g.accel = 2
** g.t0 = g.t
** g.speed0 = 0
** g.speed1 = g.speed_limit
** g.pos0 = g.pos
** g.moving = 0
** started_moving = 1
** g.direction = 1 (??)

* motor.ino
** started_moving = 0
** g.moving = 1
** g.t0 = g.t // Time when acceleration changed

After triggering the limit2 switch:
* limiters.ino
** ignored if g.moving == 0 || g.uninterrupted == 1 || g.error > 0
** Read_limiters()
** g.limit_on = 1
** g.calibrate_flag = 2
** g.limit2 = g.pos_short_old // Temporary value for limit2 (in old coords)
** change_speed(0.0, 0, 2)
** g.uninterrupted = 1 // Not reading limiter until stopped

* stop_now()
**  g.moving = 0;
**  g.t_old = g.t;
**  g.pos_old = g.pos;
**  g.pos_short_old = (COORD_TYPE)floor(g.pos);
  g.uninterrupted = 0;
  g.backlashing = 0;
  g.speed = 0.0;


* calibration
// Will move back to limit1 at full speed:
    change_speed(-g.speed_limit, 0, 2);
    g.calibrate_flag = 3;
    g.accel = -2;
    g.t0 = g.t;
    g.speed0 = 0;
    g.pos0 = g.pos;
    g.started_moving = 1;
    g.direction = -1;
    g.speed1 = -g.speed_limit;

* motor.ino
** started_moving = 0
** g.moving = 1
** g.t0 = g.t // Time when acceleration changed

After triggering the limit1 switch:
* limiters.ino
  change_speed(0.0, 0, 2);
  g.uninterrupted = 1; // Not reading limiter until stopped

* stop_now()
**  g.moving = 0;
**  g.t_old = g.t;
**  g.pos_old = g.pos;
**  g.pos_short_old = (COORD_TYPE)floor(g.pos);
  g.uninterrupted = 0;
  g.backlashing = 0;
  g.speed = 0.0;


* calibration
// Triggered limit1 and stopped, will now move forward to calibrate limit1 on the first switch-off position
   go_to(g.pos + 4 * BREAKING_DISTANCE, g.speed_limit);
   g.calibrate_flag = 4;
   change_speed(g.speed_limit, 1, 2);
** g.accel = 2
** g.t0 = g.t
** g.speed0 = 0
** g.speed1 = g.speed_limit
** g.pos0 = g.pos
** g.moving = 0
** started_moving = 1
** g.direction = 1 (??)

* motor.ino
** started_moving = 0
** g.moving = 1
** g.t0 = g.t // Time when acceleration changed

//When limit1 goes off
limiters.ino
      g.calibrate_flag = 5;
      // Making sure only the very first turn of of limit one is registered (and used for calibration); ignoring the subsequent switch noise
      g.uninterrupted = 1;
      // At the end of calibration new coordinates will be derived from old by adding this parameter to the old ones:
      g.coords_change = -g.pos_short_old;

* stop_now()
**  g.moving = 0;
**  g.t_old = g.t;
**  g.pos_old = g.pos;
**  g.pos_short_old = (COORD_TYPE)floor(g.pos);
  g.uninterrupted = 0;
  g.backlashing = 0;
  g.speed = 0.0;

* calibration.ino
      g.pos = g.pos + (float)g.coords_change;
      g.pos_short_old = g.pos_short_old + g.coords_change;
      g.t0 = g.t;
      g.pos0 = g.pos;
      g.pos_old = g.pos;
      g.limit2 = g.limit2 + g.coords_change;
      EEPROM.put( ADDR_LIMIT2, g.limit2);
      // Saving the current position to EEPROM:
      EEPROM.put( ADDR_POS, g.pos );
      g.calibrate_flag = 0;
      g.accident = 0;