Code:
/************************************************************
* Programm zur Auswertung eines händisch 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 // maximum number of encoder motors supported by Arduino Uno == 2 // 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 8 // pwm enable0 << %timer
// 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 << %timer
// motor 2
#define pinenc2A 30 // enc2A yellow
#define pinenc2B 31 // enc2B blue
#define pinmot2d1 32 // dir2-1 <<
#define pinmot2d2 33 // dir2-2
#define pinmot2pwm 10 // pwm enable2 << %timer
// motor 3
#define pinenc3A 34 // enc3A yellow
#define pinenc3B 35 // enc3B blue
#define pinmot3d1 36 // dir3-1 <<
#define pinmot3d2 37 // dir3-2
#define pinmot3pwm 11 // pwm enable3 << %timer
// motor 4
#define pinenc4A 38 // enc4A yellow
#define pinenc4B 39 // enc4B blue
#define pinmot4d1 40 // dir4-1 <<
#define pinmot4d2 41 // dir4-2
#define pinmot4pwm 12 // pwm enable4 << %timer
// motor 5
#define pinenc5A 42 // enc5A yellow
#define pinenc5B 43 // enc5B blue
#define pinmot5d1 44 // dir5-1 <<
#define pinmot5d2 45 // dir5-2
#define pinmot5pwm 13 // pwm enable5 << %timer
volatile long motenc[MAXMOTORS] = {0, 0, 0, 0, 0, 0},
oldenc[MAXMOTORS] = {0, 0, 0, 0, 0, 0};
byte pinmotdir[MAXMOTORS][ 2] = {
{pinmot0d1, pinmot0d2}, // motor direction pin array
{pinmot1d1, pinmot1d2},
{pinmot2d1, pinmot2d2},
{pinmot3d1, pinmot3d2},
{pinmot4d1, pinmot4d2},
{pinmot5d1, pinmot5d2},
};
int pinmotpwm[MAXMOTORS] = {pinmot0pwm, pinmot1pwm, pinmot2pwm, // motor pwm pin array
pinmot3pwm, pinmot4pwm, pinmot5pwm,
};
volatile int8_t ISRab[MAXMOTORS] = {0, 0, 0, 0, 0, 0};
// 1/2 Auflösung
int8_t schrittTab[16] = {0, 0,0,0,1,0,0,-1, 0,0,0,1,0,0,-1,0};
/*************************************************************
* Interrupt Handler Routine
*************************************************************/
void myHandler() {
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(myHandler);
Timer1.start(100); // Calls every ...µs
Serial.begin(9600);
}
void loop() {
while(true) {
sprintf(sbuf, " 0=%5d, 1=%5d, 2=%5d, 3=%5d, 4=%5d, 5=%5d",
motenc[ 0],motenc[ 1], motenc[ 2], motenc[ 3], motenc[ 4], motenc[ 5]);
Serial.println(sbuf);
delay(100);
}
}
Berechtigungen
Zitieren
Lesezeichen