stmbl

HAL_COMP(cauto);

HAL_PIN(i_q) = 0.0;
HAL_PIN(i_d) = 0.0;

HAL_PIN(pos) = 0.0; // commutation pos

HAL_PIN(start) = 0.0;
HAL_PIN(ready) = 0.0;

HAL_PIN(mag_offset) = 0.0; // conf0.fb_offset
HAL_PIN(start_offset) = 0.0; // start offset from fault
HAL_PIN(mo) = 0.0; // live

HAL_PIN(moffset) = 0.0; // out
HAL_PIN(coffset) = 0.0; // const phasing offset to index => save to conf0.fb_offset

HAL_PIN(fb_in) = 0.0;
HAL_PIN(fb_out) = 0.0;

HAL_PIN(isabs) = 0.0;

HAL_PIN(cur) = 1.0;
HAL_PIN(time) = 0.1;


MEM(float fbi) = 0.0;
MEM(float fb) = 0.0;
MEM(float pos) = 0.0;
MEM(float id) = 0.0;
MEM(float iq) = 0.0;
MEM(float moffset) = 0.0;
MEM(float encoffset) = 0.0;

RT(
   fbi = minus(PIN(fb_in), encoffset); // commutation pos
   fb = minus(PIN(fb_in), PIN(start_offset)); // pid pos
   float mo = PIN(mo);

   HT(
      moffset = PIN(mag_offset);
      GOTO(0);
      STATE(0){ // start
         iq = 0.0;
         id = 0.0;
         pos = minus(fbi, moffset + mo);
         if(PIN(start) > 0.0){
            GOTO(1);
            PIN(ready) = 0.0;
         }else{
         }
      }
      STATE(1){ // start phasing
         PIN(ready) = 0.0;
         id = PIN(cur);
         iq = 0.0;
         pos = 0.0;
         fb = 0.0;
         SLEEP(PIN(time));
         GOTO(2);
      }
      STATE(2){ // readout commutation offset
         PIN(ready) = 0.0;
         moffset = fbi;
         if(PIN(isabs) > 0.0){
            PIN(coffset) = fbi;
         }
         id = 0.0;
         iq = 0.0;
         SLEEP(PIN(time) / 50.0);
         GOTO(3);
      }
      STATE(3){ // after phasing
         PIN(ready) = 1.0;
         iq = 0.0;
         id = 0.0;
         pos = minus(fbi, moffset + mo);
         if(PIN(start) == 0.0){
            PIN(ready) = 0.0;
            GOTO(0);
         }
      }
   );
      
   if(RISING_EDGE(PIN(isabs))){
      encoffset = fbi;
      moffset = PIN(mag_offset);
      pos = -moffset;
   }
   if(FALLING_EDGE(PIN(isabs))){
      encoffset = 0.0;
   }

   PIN(moffset) = moffset;
   PIN(i_d) = id;
   PIN(i_q) = iq;
   PIN(pos) = pos;
   PIN(fb_out) = fb;
);


ENDCOMP;