/***************************************************************************/
/*         --------------------------------------------------------        */
/*                         Andrea Cipriani, Dec 2000.                      */
/*                    E-Mail :  a71cip@tiscali.it                          */
/*           Libreria di funzioni per il pilotaggio di motori              */
/*                              passo passo.                               */
/*         --------------------------------------------------------        */
/*                                                                         */
/* 1. void show_bin (struct nibble_field *nibble_to_show);                 */
/*    Visualizza il campo bit di tipo nibble_field in formato di nibble    */
/*    binario. Riceve come argomento un puntatore a campo di bit di tipo   */
/*    nibble_field                                                         */
/*                                                                         */
/* 2. void reset_nibble (struct nibble_field *);                           */
/*    Resetta i campi del campo di bit di tipo nibble field passato alla   */
/*    funzione per indirizzo                                               */
/*                                                                         */
/* 3. void assign_nibble (struct nibble field *, char, char, char, char    */
/*    Assegna ai campi del campo di bit di tipo nibble_field passato       */
/*    come argomento per indirizzo i valori contenuti nei parametri        */
/*    specificati come bit3, bit2, bit1, bit0. Tali valori devono          */
/*    valere 0 o 1.                                                        */
/*                                                                         */
/* 4. struct nibble_field *shift_right (struct nibble_field *);            */
/*    Esegue uno shift unitario verso destra  dei  campi  contenuti  nel   */
/*    campo di bit di tipo nibble_field passato come argomento alla        */
/*    funzione con un puntatore a campo di bit di tipo nibble_field.       */
/*    Restituisce un puntatore a campo di bit di tipo nibble_field.        */
/*                                                                         */
/* 5. struct nibble_field *rotate_right (struct nibble_field *);           */
/*    Esegue una rotazione unitaria verso destra dei campi contenuti nel   */
/*    campo di bit di tipo nibble_field passato come argomento alla        */
/*    funzione con un puntatore a campo di bit di tipo nibble_field.       */
/*    Restituisce un puntatore a campo di bit di tipo nibble_field.        */
/*                                                                         */
/* 6. struct nibble_field *shift_left (struct nibble_field *);             */
/*    Esegue uno shift unitario verso  sinistra  dei  campi  contenuti nel */
/*    campo di bit di tipo nibble_field passato come argomento alla        */
/*    funzione con un puntatore a campo di bit di tipo nibble_field.       */
/*    Restituisce un puntatore a campo di bit di tipo nibble_field.        */
/*                                                                         */
/* 7. struct nibble_field *rotate_left (struct nibble_field *);            */
/*    Esegue una rotazione unitaria verso sinistra dei campi contenuti nel */
/*    campo di bit di tipo nibble_field passato come argomento alla        */
/*    funzione con un puntatore a campo di bit di tipo nibble_field.       */
/*    Restituisce un puntatore a campo di bit di tipo nibble_field.        */
/*									                     */
/* 8. unsigned char convert_nibble_to_value (struct nibble_field *);       */
/*    Converte i campi di un nibble (campo di bit di tipo nibble_field),   */
/*    tenendo conto dei pesi dei singoli bit (campi) in un unsigned char.  */
/*    Il campo di bit di tipo nibble_field viene passato tramite puntatore.*/
/*                                                                         */
/* 9. struct nibble_field *convert_value_to_nibble (unsigned char value);  */
/*    Converte un unsigned char che preventivamente deve essere stato      */
/*    controllato da programma essere <= a 15 in un campo bit di tipo      */
/*    nibble_field. Restituisce un puntatore a campo bit di tipo           */
/*    nibble_field.   						                     */
/*									                     */
/* *********************************************************************** */
/* Implementazione funzioni per la gestione di motori passo passo unipolari*/
/* a 4 fasi. Allo scopo è stata definita la struttura campo di bit di tipo */
/* nibble_field e le funzioni sopradescritte.                              */
/* *********************************************************************** */
/*                                                                         */
/* Tutte queste funzioni funzionano con motori passo passo unipolari a 4   */
/* fasi collegati alla porta parallela (indirizzo add delle funzioni) con  */
/* i driver delle fasi corrispondenti ai bit 0,1,2 e 3 della DATA PORT     */
/*								                           */
/*									                     */
/* 10.void unlock (unsigned int add, struct nibble_field *po);             */
/*    Libera il motore passo-passo unipolare a 4 fasi (pp4f) collegato     */
/*    sull'indirizzo add. pp4f passato come puntatore a struttura campo di */
/*    bit di tipo nibble_field					               */
/*								                           */
/* 11.void lock   (unsigned int add, struct nibble_field *pa);             */
/*    Blocca il pp4f.							               */
/*   									                     */
/* 12.void lock_init(unsigned int add, struct nibble_field *ps, char bit3, */
/*    char bit2, char bit1, char bit0); 				               */
/*    Blocca il pp4f sulle fasi passate a 1 sui bitx.			         */
/* 									                     */
/* 13.struct nibble_field *rot_uni_left  (unsigned int add,                */
/*    struct nibble_field *pd);						         */
/*    Effettua una rotazione di uno step in senso antiorario,restituisce un*/
/*    puntatore a campo di bit di tipo nibble_field che è il ff4p ruotato. */
/*									                     */
/* 14.struct nibble_field *rot_uni_right (unsigned int add,                */
/*    struct nibble_field *pd);						         */
/*    Effettua una rotazione di uno step in senso orario, restituisce un   */
/*    puntatore a campo di bit di tipo nibble_field che è il ff4p ruotato. */
/*									                     */
/* 15.struct nibble_field *turning_left (unsigned int addr, 		   */
/*    struct nibble_field *pg, float dps, unsigned int dt);		         */
/*    Effettua una giro in senso antiorario, resituisce il pp4f ruotato    */
/*    dps = angolo per step, dt = ritardo di eccitazione per fase.         */
/*									                     */
/* 16.struct nibble_field *turning_right (unsigned int addr, 		   */
/*    struct nibble_field *pg, float dps, unsigned int dt);		         */
/*    Effettua una giro in senso orario, resituisce il pp4f ruotato        */
/*    dps = angolo per step, dt = ritardo di eccitazione per fase.         */
/*									                     */
/* 17.struct nibble_field *turn_angle (unsigned int addr,		         */
/*    struct nibble_field *pj, float dps, float angle, unsigned char sense,*/ 
/*    unsigned int dt);						                     */
/*    Effettua una rotazione in gradi di una angolo pari a angle.          */
/*    Restituisce il pp4f ruotato.					               */
/*    dps = angolo per step, dt = ritardo fase, sense=0 : rotazione oraria */
/*    sense=1 : rotazione antioraria.					         */
/*								                           */
/* 18.struct nibble_field *open_and_back (unsigned int addr,	         */
/*    struct nibble_field *pk, float dps, float angle, unsigned char senso,*/
/*    unsigned int pause, unsigned int dt);				         */
/*    Apre in senso orario (sense=0) o antiorario (sense=1) e poi ritorna  */
/*    al luogo di partenza.					                     */
/*    dps = angolo per step, angle = angolo di apertura, pause = pausa fra */
/*    la fine dell'apertura e l'inizio del ritorno, dt = ritardo di fase   */
/*												   */
/* dal punto 19 al 24 sono state implementate le stesse funzioni descritte */
/* dal punto 13 al 18, questa volta per il pilotaggio a mezzo passo        */                     
/***************************************************************************/

# include <stdio.h>
# include <conio.h>
# include <math.h>
# include <dos.h>


// Campo bit costituito da un nibble
struct nibble_field
{
 unsigned nd0 :1;
 unsigned nd1 :1;
 unsigned nd2 :1;
 unsigned nd3 :1;
};


// -------------------------
// Variabili globali -------
// -------------------------

struct nibble_field *nibbley_field;


// -------------------------
// Prototipi  delle funzioni
// -------------------------

void show_bin (struct nibble_field *); //1
void reset_nibble (struct nibble_field *); //2
void assign_nibble (struct nibble_field *, char, char, char, char); //3
struct nibble_field *shift_right (struct nibble_field *); //4
struct nibble_field *rotate_right (struct nibble_field *); //5
struct nibble_field *shift_left (struct nibble_field *); //6
struct nibble_field *rotate_left (struct nibble_field *); //7
unsigned char convert_nibble_to_value (struct nibble_field *); //8
struct nibble_field *convert_value_to_nibble (unsigned char); //9
void unlock (unsigned int, struct nibble_field *); //10
void lock   (unsigned int, struct nibble_field *); //11
void lock_init(unsigned int, struct nibble_field *, char, char, char, char); //12
struct nibble_field *rot_uni_left  (unsigned int, struct nibble_field *); //13
struct nibble_field *rot_uni_right (unsigned int, struct nibble_field *); //14
struct nibble_field *turning_left  (unsigned int, struct nibble_field *, float, unsigned int); //15
struct nibble_field *turning_right (unsigned int, struct nibble_field *, float, unsigned int); //16
struct nibble_field *turn_angle    (unsigned int, struct nibble_field *, float, float, unsigned char, unsigned int); //17
struct nibble_field *open_and_back (unsigned int, struct nibble_field *, float, float, unsigned char, unsigned int, unsigned int); //18
struct nibble_field *half_rot_uni_left  (unsigned int, struct nibble_field*); //19
struct nibble_field *half_rot_uni_right (unsigned int, struct nibble_field*); //20
struct nibble_field *half_turning_left  (unsigned int, struct nibble_field*, float, unsigned int); //21
struct nibble_field *half_turning_left  (unsigned int, struct nibble_field*, float, unsigned int); //22
struct nibble_field *half_turn_angle    (unsigned int, struct nibble_field*, float, float, unsigned char, unsigned int); //23
struct nibble_field *half_open_and_back (unsigned int, struct nibble_field*, float, float, unsigned char, unsigned int, unsigned int); //24

// -----------------------------------------------------
// Rappresentazione di un campo bit di tipo nibble_field
// -----------------------------------------------------
// Campo bit di tipo nibble_field passato per indirizzo.
void show_bin (struct nibble_field *pn)
{
 printf("%u",pn->nd3);
 printf("%u",pn->nd2);
 printf("%u",pn->nd1);
 printf("%u",pn->nd0);
}


// ------------------------------------------------------------------------
// Resetta i campi del campo bit di tipo nibble_field passato per indirizzo
// ------------------------------------------------------------------------
void reset_nibble (struct nibble_field *pe)
{
 pe->nd0 = 0;
 pe->nd1 = 0;
 pe->nd2 = 0;
 pe->nd3 = 0;
}

// --------------------------------------------------------------
// Assegna ai campi del campo di bit di tipo nibble_field passato
// come argomento per indirizzo i valori contenuti nei parametri
// specificati come bit3, bit2, bit1, bit0. Tali valori devono
// valere 0 o 1.
// --------------------------------------------------------------
void assign_nibble (struct nibble_field *pt, char bit3, char bit2, char bit1, char bit0)
{
 pt->nd0 = bit0;
 pt->nd1 = bit1;
 pt->nd2 = bit2;
 pt->nd3 = bit3;
}

// -------------------------------------------------------------
// Shift a destra unitario per campo di bit di tipo nibble_field
// -------------------------------------------------------------
struct nibble_field *shift_right (struct nibble_field *pq)
{
 pq->nd0 = pq->nd1;
 pq->nd1 = pq->nd2;
 pq->nd2 = pq->nd3;
 pq->nd3 = 0;
 return pq;
}


// -----------------------------------------------------------------
// Rotazione a destra unitaria per campo di bit di tipo nibble_field
// -----------------------------------------------------------------
struct nibble_field *rotate_right(struct nibble_field *pz)
{
 unsigned char appoggio;
 //Banale
 appoggio = pz->nd0;
 pz->nd0  = pz->nd1;
 pz->nd1  = pz->nd2;
 pz->nd2  = pz->nd3;
 pz->nd3  = appoggio;
 return pz;
}

// ---------------------------------------------------------------
// Shift a sinistra unitario per campo di bit di tipo nibble_field
// ---------------------------------------------------------------
struct nibble_field *shift_left (struct nibble_field *pw)
{
 pw->nd3 = pw->nd2;
 pw->nd2 = pw->nd1;
 pw->nd1 = pw->nd0;
 pw->nd0 = 0;
 return pw;
}

// -------------------------------------------------------------------
// Rotazione a sinistra unitaria per campo di bit di tipo nibble_field
// -------------------------------------------------------------------
struct nibble_field *rotate_left(struct nibble_field *pz)
{
 unsigned char appoggio;
 //Banale
 appoggio=pz->nd3;
 pz->nd3=pz->nd2;
 pz->nd2=pz->nd1;
 pz->nd1=pz->nd0;
 pz->nd0=appoggio;
 return pz;
}


// -------------------------------------------------------------------
// Converte i campi di un nibble (campo di bit di tipo nibble_field),
// tenendo conto dei pesi dei singoli bit (campi) in un unsigned char.
// -------------------------------------------------------------------
unsigned char convert_nibble_to_value (struct nibble_field *pj)
{
 unsigned char valore;

 valore=0;
 //Aggiunge a valore il valore del peso dei bit a 1

 if (pj->nd0)
  {
   valore=valore+1;
  }
 if (pj->nd1)
  {
   valore=valore+2;
  }
 if (pj->nd2)
  {
   valore=valore+4;
  }
 if (pj->nd3)
  {
   valore=valore+8;
  }
 return valore;
}


// ---------------------------------------------------------------
// Converte un unsigned char che preventivamente deve essere stato
// controllato da programma essere < di 15 in un campo bit di tipo
// nibble_field. Restituisce un puntatore a campo bit di tipo
// nibble_field.
// ---------------------------------------------------------------
struct nibble_field *convert_value_to_nibble (unsigned char value)
{
 struct nibble_field *pu;
 //Reset del nibble
 reset_nibble(pu);
 //Algoritmo di conversione secondo la regola dei resti
 pu->nd0 = value%2;
 value = value/2;
 pu->nd1 = value%2;
 value = value/2;
 pu->nd2 = value%2;
 value = value/2;
 pu->nd3 = value%2;
 return pu;
}


// Libera l'albero del motore passo passo
void unlock (unsigned int add, struct nibble_field *po)
{
 unsigned char dat;

 assign_nibble(po,0,0,0,0);
 dat=convert_nibble_to_value(po);
 outportb(add,dat);
}

// Blocca l'albero del motore passo passo
void lock   (unsigned int add, struct nibble_field *pa)
{
 unsigned char dat;

 assign_nibble(pa,0,1,0,1);
 dat=convert_nibble_to_value(pa);
 outportb(add,dat);
}

// Blocca l'albero del motore passo passo alla posizione che viene passata dai bitx
void lock_init(unsigned int add, struct nibble_field *ps, char bit3, char bit2, char bit1, char bit0)
{
 unsigned char dat;

 assign_nibble(ps,bit3,bit2,bit1,bit0);
 dat=convert_nibble_to_value(ps);
 outportb(add,dat);
}

// Ruota l'albero di uno step a destra
struct nibble_field *rot_uni_left (unsigned int add, struct nibble_field *pd)
{
 unsigned char dat;

 pd=rotate_right(pd);
 dat=convert_nibble_to_value(pd);
 outportb(add,dat);
 return pd;
}

// Ruota l'albero di uno step a sinistra
struct nibble_field *rot_uni_right (unsigned int add, struct nibble_field *pf)
{
 unsigned char dat;

 pf=rotate_left(pf);
 dat=convert_nibble_to_value(pf);
 outportb(add,dat);
 return pf;
}

// Rotazione di 360ø antioraria
struct nibble_field *turning_left (unsigned int addr, struct nibble_field *pg, float dps, unsigned int dt)
{
 unsigned char j,ppt;

 ppt = 360/dps;
 for (j=0; j<ppt; j++)
  {
   pg = rot_uni_left(addr,pg);
   delay(dt);
  }
 return pg;
}

// Rotazione di 360ø oraria
struct nibble_field *turning_right (unsigned int addr, struct nibble_field *ph, float dps, unsigned int dt)
{
 unsigned char j,ppt;

 ppt = 360/dps;
 for (j=0; j<ppt; j++)
  {
   ph = rot_uni_right(addr,ph);
   delay(dt);
  }
 return ph;
}

// Rotazione di un angolo pari a angle, se sense = 0 a destra se = 1 a sinistra
struct nibble_field *turn_angle (unsigned int addr, struct nibble_field *pj, float dps, float angle, unsigned char sense, unsigned int dt)
{
 unsigned char n,ppn;

 ppn = angle/dps;

 if (sense)
  {
   for (n=0; n<ppn; n++)
    {
     pj = rot_uni_right(addr,pj);
     delay(dt);
    }
  }

 if (!sense)
  {
   for (n=0; n<ppn; n++)
    {
     pj = rot_uni_left(addr,pj);
     delay(dt);
    }
  }
 return(pj);
}

// Apre in senso orario (sense=0) o antiorario (sense=1) e poi torna al luogo
// di partenza. dps=degree per step, angle=angolo di apertura, dt=ritardo di
// fase, pause= pausa fra fine apertura e inizio chiusura
struct nibble_field *open_and_back (unsigned int addr, struct nibble_field *pk, float dps, float angle, unsigned char senso, unsigned int pause, unsigned int dt)
{
 unsigned char n,ppn;

 if (senso)
  {
   pk = turn_angle (addr,pk,dps,angle,1,dt);
   delay(pause);
   pk = turn_angle (addr,pk,dps,angle,0,dt);
  }

  if (!senso)
   {
    pk = turn_angle (addr,pk,dps,angle,0,dt);
    delay(pause);
    pk = turn_angle (addr,pk,dps,angle,1,dt);
   }
  return(pk);
}

// Rotazione unitaria in senso antiorario in modo half_step_drive
struct nibble_field *half_rot_uni_left (unsigned int add, struct nibble_field *pl)
{
 unsigned char value,new_value;

 value = convert_nibble_to_value(pl);
 switch (value)
  {
   case 0x01:
    new_value = 0x09;
    break;

   case 0x09:
    new_value = 0x08;
    break;

   case 0x08:
    new_value = 0x0c;
    break;

   case 0x0c:
    new_value = 0x04;
    break;

   case 0x04:
    new_value = 0x06;
    break;

   case 0x06:
    new_value = 0x02;
    break;

   case 0x02:
    new_value = 0x03;
    break;

   case 0x03:
    new_value = 0x01;
    break;
  }

 pl = convert_value_to_nibble(new_value);
 outportb(add,new_value);
 return(pl);
}

// Rotazione unitaria in senso orario in modo half_step_drive
struct nibble_field *half_rot_uni_right (unsigned int add, struct nibble_field *pz)
{
 unsigned char value,new_value;

 value = convert_nibble_to_value(pz);
 switch (value)
  {
   case 0x01:
    new_value = 0x03;
    break;

   case 0x03:
    new_value = 0x02;
    break;

   case 0x02:
    new_value = 0x06;
    break;

   case 0x06:
    new_value = 0x04;
    break;

   case 0x04:
    new_value = 0x0c;
    break;

   case 0x0c:
    new_value = 0x08;
    break;

   case 0x08:
    new_value = 0x09;
    break;

   case 0x09:
    new_value = 0x01;
    break;
  }

 pz = convert_value_to_nibble(new_value);
 outportb(add,new_value);
 return(pz);
}

// Rotazione di 360ø antioraria
struct nibble_field *half_turning_left  (unsigned int add, struct nibble_field *pm, float dps, unsigned int dt)
{
 unsigned int j,ppt;

 ppt = 360/(dps/2);
 for (j=0; j<ppt; j++)
  {
   pm = half_rot_uni_left(add,pm);
   delay(dt);
  }
 return pm;
}

// Rotazione di 360 ø oraria
struct nibble_field *half_turning_right  (unsigned int add, struct nibble_field *pc, float dps, unsigned int dt)
{
 unsigned int j,ppt;

 ppt = 360/(dps/2);
 for (j=0; j<ppt; j++)
  {
   pc = half_rot_uni_right(add,pc);
   delay(dt);
  }
 return pc;
}

// Rotazione di un angolo pari a angle, se sense = 0 a destra se = 1 a sinistra
struct nibble_field *half_turn_angle (unsigned int add, struct nibble_field *pb, float dps, float angle, unsigned char sense, unsigned int dt)

{
 unsigned char n,ppn;

 ppn = angle/(dps/2);

 if (sense)
  {
   for (n=0; n<ppn; n++)
    {
     pb = half_rot_uni_left(add,pb);
     delay(dt);
    }
  }

 if (!sense)
  {
   for (n=0; n<ppn; n++)
    {
     pb = half_rot_uni_right(add,pb);
     delay(dt);
    }
  }
 return(pb);
}


// Apre in senso orario (sense=0) o antiorario (sense=1) e poi torna al luogo
// di partenza. dps=degree per step, angle=angolo di apertura, dt=ritardo di
// fase, pause= pausa fra fine apertura e inizio chiusura
struct nibble_field *half_open_and_back (unsigned int add, struct nibble_field *pl, float dps, float angle, unsigned char sense, unsigned int pause, unsigned int dt)
{
 unsigned char n,ppn;

 if (sense)
  {
   pl = half_turn_angle (add,pl,dps,angle,1,dt);
   delay(pause);
   pl = half_turn_angle (add,pl,dps,angle,0,dt);
  }

  if (!sense)
   {
    pl = turn_angle (add,pl,dps,angle,0,dt);
    delay(pause);
    pl = turn_angle (add,pl,dps,angle,1,dt);
   }
  return(pl);
}