// ===========================================================================
// Programma  : SPINDLE.C
//              scheda x comando mandrino AC SIEI AVY3110
//              riferimento analogico e comando orientamento mandrino
//              con sensore FANUC x centro di lavoro MAZAK V 7.5(1)
//              Ditta MES di ROMA
// Versione   :
// Eseguito   : Ditta Crivellaro Mauro Engineer
// Data       : 21-01-2003
// Locazione  : FAE 429
//
// ===========================================================================
// Uscite analogiche
//
// DAC0:        uscita analogica x riferimento differenziale azionamento
//              funzionamento in M03 ed M04 ed M19
// DAC1:
//
//
// ingressi analogici
//
// ADC0:        riferimento analogico CN
// ADC1:        ingresso analogico MSA sensore FANUC
// ADC2:        ingresso analogico LSA sensore FANUC
// ADC3:
//
// Ingressi - Uscite
//
// INP0:        bit 0   Arresto rapido                      SET
//              bit 1   Rotazione oraria                    SRN
//              bit 2   Rotazione antioraria                SRI
//              bit 3   Orientamento                        ORCI
//              bit 4   Gamma bassa                         CTL
//              bit 5   NA Teleruttore potenza mandrino     POTON
//              bit 6   velocita raggiunta                  RVN
//              bit 7   velocita zero                       RV0
//
// OUT0:        bit 0   start inverter                      Pin 13 AVY
//              bit 1   Teleruttore potenza mandrino
//              bit 2   cambio rampa                        Pin 36 AVY
//              bit 3   Orientamento eseguito               ORA1-ORA2
//              bit 4   enable inverter                     Pin 12 AVY
//              bit 5
//              bit 6   in posizione
//              bit 7   Zona lineare
//
#include <io6811f1.h>                   /* definizione registri 68hc11f1 */
#include <int6811.h>                    /* definizione interrupt 68hc11f1 */
#include <stdlib.h>

#define ADC0 0x07               /* Abilitazione lettura adc0   */
#define ADC1 0x17               /* Abilitazione lettura adc0   */
#define ADC2 0x27               /* Abilitazione lettura adc0   */
#define ADC3 0x37               /* Abilitazione lettura adc0   */

#define DAC0 0xdf               /* Abilitazione scrittura dac0 */
#define DAC1 0xbf               /* Abilitazione scrittura dac1 */

#define STROBE (* (unsigned char *) (0x4003))
#define CKENC  (* (unsigned char *) (0x4000))
#define ENCL   (* (unsigned char *) (0x4001))
#define ENCH   (* (unsigned char *) (0x4002))

#define ENC1   0x06
#define ENC2   0x05

#define SGL_LSA   300
#define JOGL      40
#define JOGH      10
#define KVL       8
#define KVH       10


   void dac(char out,int data);
   int adc(char inp);
   void set_uscite(void);

   zpage unsigned char inp0;
   zpage unsigned char out0;
   zpage unsigned char set ;
   zpage unsigned char srn ;
   zpage unsigned char sri ;
   zpage unsigned char orci;
   zpage unsigned char ctl ;
   zpage unsigned char poton;
   zpage unsigned char pot;
   zpage unsigned char rv0;
   zpage unsigned char rvn;
   zpage unsigned char m34;
   zpage unsigned char enable;
   zpage unsigned char m19;
   zpage unsigned char ab_m19;
   zpage unsigned char fsm19;
   zpage unsigned char start_ori;
   zpage unsigned char jog_ori;
   zpage unsigned char jog_pos;
   zpage unsigned char zona_lineare;
   zpage unsigned char fs_lineare;
   zpage unsigned char rpm;
   zpage unsigned char ori_ok;
   zpage unsigned char flag1;
   zpage unsigned char flag2;
   zpage int riferimento;
   zpage int rifvel;
   zpage int rifpos;
   zpage int lsa;
   zpage int msa;
   zpage int inpos;

void main(void) {

   inp0 = 0;
   out0 = 0;
   set = 0;
   srn = 0;
   sri = 0;
   orci = 0;
   ctl = 0;
   poton = 0;
   pot = 0;
   rv0 = 0;
   rvn = 0;
   m34 = 0;
   enable = 0;
   m19 = 0;
   start_ori = 0;
   jog_ori = 0;
   jog_pos = 0;
   rpm = 0;

   adc(ADC0);

   while (1) {

      set_uscite();

      rifvel = adc(ADC1);            /*           ATTENZIONE !!!!!!!!         */
      msa = adc(ADC2);               /*  la lettura e' quella del ADC chiesta */
      lsa = adc(ADC0);               /*  la volta prima                       */

      if (srn || sri) {
         if (sri){
            rifvel = rifvel>>4;
         } else {
            rifvel = - (rifvel>>4) ;
         }
      } else {
         rifvel = 0;
      }
      if (fsm19){
         start_ori = 1;
         jog_pos = 0;
         jog_ori = 0;
         ori_ok = 0;
         rpm = 0;
      }
      if (!m19 || jog_ori) start_ori = 0;
      if (start_ori && rvn) jog_ori = 1;
      if (!m19 || jog_pos) jog_ori = 0;
      if (jog_ori) {
         if (fs_lineare) rpm++;
         if (rpm >= 2 ) jog_pos = 1;
      }
      if (!m19){
         jog_pos=0;
         ori_ok = 0;
      }
      if (m19) {

         if (start_ori || jog_ori || jog_pos && !zona_lineare){
            if (ctl){
               rifpos = JOGL;
            } else {
               rifpos = JOGH;
            }
         }
         if (jog_pos && zona_lineare) {
            if (ctl){
               rifpos = msa >> KVL;
            } else {
               rifpos = msa >> KVH;
            }
         }
      } else {
         rifpos = 0;
      }
      if (m19) {
         dac(DAC0,rifpos);
      } else {
         dac(DAC0,rifvel);
      }
   }
}

void set_uscite(void){

   inp0 = ~PORTE;

   set = inp0 & 1;
   sri = inp0 & 2;
   srn = inp0 & 4;
   orci = inp0 & 8;
   ctl = inp0 & 0x10;
   poton = inp0 & 0x20;
   rvn  = inp0 & 0x40;
   rv0  = inp0 & 0x80;

   zona_lineare = lsa > SGL_LSA;

   if (msa<100 && msa>-100){
      inpos=1;
   } else {
      inpos=0;
   }
   pot = ((srn || sri || orci) && (set || orci)) || rv0 && pot && poton;
   enable = pot && poton;
   m34 = (srn || sri) && set && pot && poton;
   m19   = orci ;

   fsm19 = !flag1 && m19;
   flag1 = m19;
   fs_lineare = zona_lineare && !flag2;
   flag2 = zona_lineare;

   if (m34 || m19) {
      out0 |= 0x01;
   } else {
      out0 &= 0xfe;
   }
   if (pot) {
      out0 |= 2;
   } else {
      out0 &= 0xfd;
   }
   if (m19 && (jog_ori || jog_pos)) {
      out0 |= 4;
   } else {
      out0 &= 0xfb;
   }
   if (inpos && jog_pos && zona_lineare && !rv0) {
      ori_ok=1;
      out0 |= 8;
   } else {
      out0 &= 0xf7;
   }
   if (enable) {
      out0 |= 0x10;
   } else {
      out0 &= 0xef;
   }
   if (0) {
      out0 |= 0x20;
   } else {
      out0 &= 0xdf;
   }
   if (inpos) {
      out0 |= 0x40;
   } else {
      out0 &= 0xbf;
   }
   if (zona_lineare ) {
      out0 |= 0x80;
   } else {
      out0 &= 0x7f;
   }

   PORTG = out0;

}


void dac (char out,int data) {

//12 bit     +- 3 volt

   if (data > 2047) data=2047;
   if (data < -2048) data=-2048;

   SPCR = 0x54;
   PORTA &= out;
   SPDR = (char)(data >> 8);
   while (!(SPSR & 0x80))
      {
      }
   SPDR = (char)data;
   while (!(SPSR & 0x80))
      {
      }
   PORTA |= ~out;
}

 int adc (char i) {

//16 bit        +- 5volt

   int data_conv;

   SPCR = 0x50;
   PORTA &= ~(0x10);
   SPDR = i;
   while (!(SPSR & 0x80))
      {
      }
   data_conv = (int)SPDR << 8;
   SPDR = 0x0f;
   while (!(SPSR & 0x80))
      {
      }
   data_conv |= (int)SPDR;
   PORTA |= 0x10;

   return data_conv;
}