Code:
/************************************************************
* Programm zur Auswertung eines manuell betriebenen
* Drehencoders (Quadraturencoder) mit dem Arduino Due
* per Due-Timer mit einer Abfragefrequenz von rd. 4-10kHz
* Entlehnt an http ://www.meinDUINO.de
************************************************************/
#include <DueTimer.h>
char sbuf[100];
#define MAXMOTORS 6
// max number of encoder motors at Arduino Uno=2 // Due=6 // Mega=8
// motor 0
#define pinenc0A 22 // enc0A yellow
#define pinenc0B 23 // enc0B blue
#define pinmot0d1 24 // dir0-1 <<
#define pinmot0d2 25 // dir0-2
#define pinmot0pwm 10 // pwm enable0
// motor 1
#define pinenc1A 26 // enc1A yellow
#define pinenc1B 27 // enc1B blue
#define pinmot1d1 28 // dir1-1 <<
#define pinmot1d2 29 // dir1-2
#define pinmot1pwm 9 // pwm enable1
// motor 2
#define pinenc2A 30 // enc2A yellow
#define pinenc2B 31 // enc2B blue
#define pinmot2d1 32 // dir2-1 <<
#define pinmot2d2 33 // dir2-2
#define pinmot2pwm 8 // pwm enable2
// motor 3
#define pinenc3A 34 // enc3A yellow
#define pinenc3B 35 // enc3B blue
#define pinmot3d1 36 // dir3-1 <<
#define pinmot3d2 37 // dir3-2
#define pinmot3pwm 7 // pwm enable3
// motor 4
#define pinenc4A 38 // enc4A yellow
#define pinenc4B 39 // enc4B blue
#define pinmot4d1 40 // dir4-1 <<
#define pinmot4d2 41 // dir4-2
#define pinmot4pwm 6 // pwm enable4
// motor 5
#define pinenc5A 42 // enc5A yellow
#define pinenc5B 43 // enc5B blue
#define pinmot5d1 47 // dir5-1 <<
#define pinmot5d2 48 // dir5-2
#define pinmot5pwm 5 // pwm enable5
volatile long motenc[MAXMOTORS] = {0, 0, 0, 0, 0, 0},
oldenc[MAXMOTORS] = {0, 0, 0, 0, 0, 0};
byte pinmotdir[MAXMOTORS][ 2] = { // motor direction pin array
{pinmot0d1, pinmot0d2},
{pinmot1d1, pinmot1d2},
{pinmot2d1, pinmot2d2},
{pinmot3d1, pinmot3d2},
{pinmot4d1, pinmot4d2},
{pinmot5d1, pinmot5d2},
};
int pinmotpwm[MAXMOTORS] = { // motor pwm pin array
pinmot0pwm, pinmot1pwm, pinmot2pwm,
pinmot3pwm, pinmot4pwm, pinmot5pwm
};
volatile int8_t ISRab[MAXMOTORS] = {0, 0, 0, 0, 0, 0};
// 1/1 Auflösung
//int8_t schrittTab[16] = {0,-1,1,0,1,0,0,-1,-1,0,0,1,0,1,-1,0};
// 1/2 Auflösung ergibt bei Lego-Motoren 1 tick pro Grad (standard wie bei Lego)
int8_t schrittTab[16] = {0, 0,0,0,1,0,0,-1, 0,0,0,1,0,0,-1,0};
// 1/4 Auflösung
//int8_t schrittTab[16] = {0,0,0,0,0,0,0,-1,0,0,0,0,0,1,0,0};
/*************************************************************
* Interrupt Handler Routine
*************************************************************/
void encHandler() {
ISRab [ 0] <<= 2;
ISRab [ 0] &= B00001100;
ISRab [ 0] |= (digitalRead(pinenc0A) << 1) | digitalRead(pinenc0B);
motenc[ 0] += schrittTab[ISRab[0]]; //
ISRab [ 1] <<= 2;
ISRab [ 1] &= B00001100;
ISRab [ 1] |= (digitalRead(pinenc1A) << 1) | digitalRead(pinenc1B);
motenc[ 1] += schrittTab[ISRab[1]]; //
ISRab [ 2] <<= 2;
ISRab [ 2] &= B00001100;
ISRab [ 2] |= (digitalRead(pinenc2A) << 1) | digitalRead(pinenc2B);
motenc[ 2] += schrittTab[ISRab[2]]; //
ISRab [ 3] <<= 2;
ISRab [ 3] &= B00001100;
ISRab [ 3] |= (digitalRead(pinenc3A) << 1) | digitalRead(pinenc3B);
motenc[ 3] += schrittTab[ISRab[3]]; //
ISRab [ 4] <<= 2;
ISRab [ 4] &= B00001100;
ISRab [ 4] |= (digitalRead(pinenc4A) << 1) | digitalRead(pinenc4B);
motenc[ 4] += schrittTab[ISRab[4]]; //
ISRab [ 5] <<= 2;
ISRab [ 5] &= B00001100;
ISRab [ 5] |= (digitalRead(pinenc5A) << 1) | digitalRead(pinenc5B);
motenc[ 5] += schrittTab[ISRab[5]]; //
}
void setup() {
// motor pin settings
// setup for L293D motor driver
// motor 0
pinMode(pinenc0A, INPUT_PULLUP); // enc0A yellow
pinMode(pinenc0B, INPUT_PULLUP); // enc0B blue
pinMode(pinmot0d1, OUTPUT); // dir0-1
pinMode(pinmot0d2, OUTPUT); // dir0-2
pinMode(pinmot0pwm ,OUTPUT); // enable0
// motor 1
pinMode(pinenc1A, INPUT_PULLUP); // enc1A yellow
pinMode(pinenc1B, INPUT_PULLUP); // enc1B blue
pinMode(pinmot1d1, OUTPUT); // dir1-1
pinMode(pinmot1d2, OUTPUT); // dir1-2
pinMode(pinmot1pwm, OUTPUT); // enable1
// motor 2
pinMode(pinenc2A, INPUT_PULLUP); // enc2A yellow
pinMode(pinenc2B, INPUT_PULLUP); // enc2B blue
pinMode(pinmot2d1, OUTPUT); // dir2-1
pinMode(pinmot2d2, OUTPUT); // dir2-2
pinMode(pinmot2pwm, OUTPUT); // enable2
// motor 3
pinMode(pinenc3A, INPUT_PULLUP); // enc3A yellow
pinMode(pinenc3B, INPUT_PULLUP); // enc3B blue
pinMode(pinmot3d1, OUTPUT); // dir3-1
pinMode(pinmot3d2, OUTPUT); // dir3-2
pinMode(pinmot3pwm, OUTPUT); // enable3
// motor 4
pinMode(pinenc4A, INPUT_PULLUP); // enc4A yellow
pinMode(pinenc4B, INPUT_PULLUP); // enc4B blue
pinMode(pinmot4d1, OUTPUT); // dir4-1
pinMode(pinmot4d2, OUTPUT); // dir4-2
pinMode(pinmot4pwm, OUTPUT); // enable4
// motor 5
pinMode(pinenc5A, INPUT_PULLUP); // encA5 yellow
pinMode(pinenc5B, INPUT_PULLUP); // encB5 blue
pinMode(pinmot5d1, OUTPUT); // dir5-1
pinMode(pinmot5d2, OUTPUT); // dir5-2
pinMode(pinmot5pwm, OUTPUT); // enable5
Timer1.attachInterrupt(encHandler);
Timer1.start(100); // Calls every ...µs
Serial.begin(115200);
Serial.println( "safety delay before start");
delay(1000); // safety delay before start
Serial.println();
}
void loop() {
while(true) {
sprintf(sbuf, " 0=%6d, 1=%6d, 2=%6d, 3=%6d, 4=%6d, 5=%6d",
motenc[ 0], motenc[ 1], motenc[ 2], motenc[ 3], motenc[ 4], motenc[ 5]);
Serial.println(sbuf);
delay(100);
}
}
Code:
/************************************************************
*
* Demo-Programm zur Auswertung eines manuell betriebenen
* Drehencoders (Quadraturencoder) mit dem Arduino im
* Timer-Interrupt mit einer Abfragefrequenz von rd. 1kHz
*
* Kann von jederman frei verwendet werden, aber bitte den
* Hinweis: "Entnommen aus http://www.meinDUINO.de" einfügen
*
************************************************************/
// An die Pins 2 und 3 ist der Encoder angeschlossen
#define encoderA 2
#define encoderB 3
// Globale Variablen zur Auswertung in der
// Interrupt-Service-Routine (ISR)
volatile int8_t altAB = 0;
volatile int encoderWert = 0;
// Die beiden Schritt-Tabellen für volle oder 1/4-Auflösung
// 1/1 Auflösung
int8_t schrittTab[16] = {0,-1,1,0,1,0,0,-1,-1,0,0,1,0,1,-1,0};
// alternativ:
// 1/2 Auflösung ergibt bei Lego-Motoren 1 tick pro Grad (wie bei Lego)
// int8_t schrittTab[16] = {0, 0,0,0,1,0,0,-1, 0,0,0,1,0,0,-1,0};
// 1/4 Auflösung
//int8_t schrittTab[16] = {0,0,0,0,0,0,0,-1,0,0,0,0,0,1,0,0};
/*************************************************************
*
* Interrupt Service Routine
*
* Wird aufgerufen, wenn der entsprechende Interrupt
* ausgelöst wird
*
*************************************************************/
ISR(TIMER1_COMPA_vect) {
altAB <<= 2;
altAB &= B00001100;
altAB |= (digitalRead(encoderA) << 1) | digitalRead(encoderB);
encoderWert += schrittTab[altAB];
}
/*************************************************************
*
* void setup()
*
* Wird einmal beim Programmstart ausgeführt
*
*************************************************************/
void setup() {
pinMode(encoderA, INPUT);
pinMode(encoderB, INPUT);
noInterrupts(); // Jetzt keine Interrupts
TIMSK1 |= (1<<OCIE1A); // Timer 1 Output Compare A Match Interrupt Enable
TCCR1A = 0; // "Normaler" Modus
// WGM12: CTC-Modus einschalten (Clear Timer on Compare match)
// Stimmen OCR1A und Timer überein, wird der Interrupt
// ausgelöst
// Bit CS12 und CS10 setzen = Vorteiler: 1024
TCCR1B = (1<<WGM12) | (1<<CS12) | (1<<CS10);
// Frequenz = 16000000 / 1024 / 15 = rd. 1kHz Abtastfrequenz;
// Überlauf bei 14, weil die Zählung bei 0 beginnt
OCR1A = 14; // OCR1A = 7 für 2kHz = alle 500µs
interrupts(); // Interrupts wieder erlauben
Serial.begin(115200);
}
/*************************************************************
*
* void loop()
*
* Wird immer wieder durchlaufen
*
*************************************************************/
void loop() {
while(true) {
Serial.println(encoderWert);
delay(100);
}
}
und ich bin grad am Überlegen, wie ich mein Programm eigentlich aufsetze. Bin natürlich so gar nicht mehr drin im Programmieren und so wirklich aus dem Anfängerstadium raus war ich noch nie.
Zitieren
Lesezeichen