DRO DIY

Kod: Zaznacz cały

/******************************************************************/
/*                                                                */
/*                            MillDRO - Z                         */
/*                                                                */
/*                            Apr 4, 2015                         */
/*                                                                */
/******************************************************************/

// Quad Encoder 600 ppr LPA3806
// Red    5 Vcc
// Black  Gnd
// Green  Chanel A Pin 2 (Int 1)
// White  Chanel B Pin 3 (Int 2)

// Display 4 digit SAA1064
// 3/8    None                                            -a-
// 5/8    None                                           f   b
// VCC    5 Vcc                                           -g-
// GND    Gnd                                            e   c
// ADR    Gnd (Read Addr 0x70 / Write Addr 0x71)          -d-   p
// SDA    Pin A4 SDA
// SCL    Pin A5 SCL                                    pgfe dcba

// Zero switch
// COM    Gnd
// NO     Pin 4   

/******************************************************************/

#include "Wire.h"     // Include I2C bus

#define DSP   0x3B    // Display Addr (0x70>>1)

#define DSD   100     // Refresh period (100 ms)

byte DspDig[10]={0x3F,0x0A,0x6D,0xEB,0x5A,0x73,0x77,0x2A,0x7F,0x7B};

/******************************************************************/

#define QFT   0.4     // Encoder Factor (pulse to inch * 1000)

#define QCA   2       // Encoder Chanel A (Pin 2)
#define QCB   3       // Encoder Chanel B (Pin 3)

#define QIA   0       // Encoder Chanel A (Int 1)
#define QIB   1       // Encoder Chanel B (Int 2)

long  QenCnt;         // Counter (inch * 1000) 

/******************************************************************/

#define SWZ   4       // EZero switch (Pin 4)

/******************************************************************/


void setup(void)
{
 Wire.begin();                              // Start up I2C bus
 delay(500);
 QenCnt=0;                                  // Counter init = 0
 IniDsp();
 ClrDsp();
 IniSwz();
 IniQen();
}


void loop(void)
{
 ThrSwz();
 ThrQen();
 delay(DSD);
}


/******************************************************************/


void IniSwz(void)                           // Init switch
{
 pinMode(SWZ,INPUT_PULLUP);
}


void ThrSwz(void)
{
 if(!digitalRead(SWZ))
  QenCnt=0;
}


/******************************************************************/


void IniQen(void)                           // Init encoder
{
 pinMode(QCA,INPUT_PULLUP);
 pinMode(QCB,INPUT_PULLUP);
 attachInterrupt(QIA,QenChAInt,CHANGE);
 attachInterrupt(QIB,QenChBInt,CHANGE);
}


void ThrQen(void)
{
 long v;

 v=(long)((float)QenCnt*QFT);
 VluDsp(v);
}


void QenChAInt(void)                        // Interruption chanel A
{
 if(PIND&4)
  {                 // Chanel A High
   if(PIND&8)
    QenCnt--;       // Chanel B High (decrease)
   else
    QenCnt++;       // Chanel B Low  (increase)
  }
 else
  {                 // Chanel A Low
   if(PIND&8)
    QenCnt++;       // Chanel B High (increase)
   else
    QenCnt--;       // Chanel B Low  (decrease)
  }
}


void QenChBInt(void)                        // Interruption chanel B
{
 if(PIND&8)
  {                 // Chanel B High
   if(PIND&4)
    QenCnt++;       // Chanel A High (increase)
   else
    QenCnt--;       // Chanel A Low  (decrease)
  }
 else
  {                 // Chanel B Low
   if(PIND&4)
    QenCnt--;       // Chanel A High (decrease)
   else
    QenCnt++;       // Chanel A Low  (increase)
  }
}


/******************************************************************/


void IniDsp(void)                           // Init display
{
 Wire.beginTransmission(DSP);
 Wire.write(B00000000);                     // Instruction byte. Zero: the next byte is the control byte
 Wire.write(B00100111);                     // Control byte (dynamic mode, digits 1+3 on, digits 2+4 on, 6 mA current
 Wire.endTransmission();
}


void ClrDsp(void)                           // Clears display
{
 Wire.beginTransmission(DSP);
 Wire.write(B00000001);                     // Instruction byte. One: first digit is 1 (right side)
 Wire.write(B00000000);                     // Digit 1 (Right)
 Wire.write(B00000000);                     // Digit 2
 Wire.write(B00000000);                     // Digit 3
 Wire.write(B00000000);                     // Digit 4 (Left)
 Wire.endTransmission();
}


void VluDsp(long v)                           // Display value
{
 byte i;
 byte d[4];
 
 if(v<0)
  v*=-1;                                         // Negative
 for(i=0;i<4;d[i]=v%10,v/=10,i++);
 Wire.beginTransmission(DSP);
 Wire.write(B00000001);                    // Instruction byte. One: first digit is 1 (right side)
 Wire.write(DspDig[d[3]]|0x80);          // Digit 4 (left) + decimal point
 Wire.write(DspDig[d[2]]);                  // Digit 3
 Wire.write(DspDig[d[1]]);                  // Digit 2
 Wire.write(DspDig[d[0]]);                  // Digit 1 (right)
 Wire.endTransmission();
} 
 

Kod: Zaznacz cały

Jeśli oś zmienia kierunek to pomiń [i]błąd[/i] impulsów i dopiero zacznij przeliczać przesunięcie.