Code:
// encoder test
// pigpio, pinchange IRQs
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <math.h>
#include <fcntl.h>
#include <string.h>
#include <sys/ioctl.h>
#include <stdint.h>
#include <errno.h>
#include <pthread.h>
#include <termios.h>
#include <iostream>
#include <pigpio.h>
#include "VG/openvg.h"
#include "VG/vgu.h"
#include "fontinfo.h"
#include "shapes.h"
#define byte uint8_t;
#define MAXMOTORS 2 // max number of encoder motors at Arduino Uno=2 // Due=6 // Mega=8
// motor 0
#define pigpio0A 23
#define pigpio0B 24
#define pigpio0d1 11
#define pigpio0d2 14
#define pigpio0pwm 15
// motor 1
#define pigpio1A 25
#define pigpio1B 4
#define pigpio1d1 28
#define pigpio1d2 29
#define pigpio1pwm 30
volatile long motenc[MAXMOTORS],
oldenc[MAXMOTORS] ;
/*************************************************************
* Interrupt Handler Routine
*************************************************************/
typedef void (*re_decoderCB_t)(int);
class re_decoder
{
int mygpioA, mygpioB, levA, levB, lastGpio;
re_decoderCB_t mycallback;
void _pulse(int gpio, int level, uint32_t tick);
/* Need a static callback to link with C. */
static void _pulseEx(int gpio, int level, uint32_t tick, void *user);
public:
re_decoder(int gpioA, int gpioB, re_decoderCB_t callback);
/*
This function establishes a rotary encoder on gpioA and gpioB.
When the encoder is turned the callback function is called.
*/
void re_cancel(void);
/*
This function releases the resources used by the decoder.
*/
};
void re_decoder::_pulse(int gpio, int level, uint32_t tick)
{
if (gpio == mygpioA) levA = level; else levB = level;
if (gpio != lastGpio) /* debounce */
{
lastGpio = gpio;
if ((gpio == mygpioA) && (level == 1))
{
if (levB) (mycallback)(1);
}
else if ((gpio == mygpioB) && (level == 1))
{
if (levA) (mycallback)(-1);
}
}
}
void re_decoder::_pulseEx(int gpio, int level, uint32_t tick, void *user)
{
/*
Need a static callback to link with C.
*/
re_decoder *mySelf = (re_decoder *) user;
mySelf->_pulse(gpio, level, tick); /* Call the instance callback. */
}
re_decoder::re_decoder(int gpioA, int gpioB, re_decoderCB_t callback)
{
mygpioA = gpioA;
mygpioB = gpioB;
mycallback = callback;
levA=0;
levB=0;
lastGpio = -1;
gpioSetMode(gpioA, PI_INPUT);
gpioSetMode(gpioB, PI_INPUT);
/* pull up is needed as encoder common is grounded */
gpioSetPullUpDown(gpioA, PI_PUD_UP);
gpioSetPullUpDown(gpioB, PI_PUD_UP);
/* monitor encoder level changes */
gpioSetAlertFuncEx(gpioA, _pulseEx, this);
gpioSetAlertFuncEx(gpioB, _pulseEx, this);
}
void re_decoder::re_cancel(void)
{
gpioSetAlertFuncEx(mygpioA, 0, this);
gpioSetAlertFuncEx(mygpioB, 0, this);
}
void setup() {
int i;
// motor pin settings
// setup for L293D motor driver
// pullup resistors:
// wiringPi: pullUpDnControl (butpin, PUD_UP);
// pigpio: gpioSetPullUpDown(butpin, PI_PUD_UP);
// motor 0
gpioSetMode(pigpio0A, PI_INPUT); // 4 enc0A yellow
gpioSetPullUpDown(pigpio0A, PI_PUD_UP);
gpioSetMode(pigpio0B, PI_INPUT); // 5 enc0B blue
gpioSetPullUpDown(pigpio0B, PI_PUD_UP);
gpioSetMode(pigpio0d1, PI_OUTPUT); // dir0-1
gpioSetMode(pigpio0d2, PI_OUTPUT); // dir0-2
gpioSetMode(pigpio0pwm, PI_OUTPUT); // enable0
// motor 1
gpioSetMode(pigpio1A, PI_INPUT); // 6 enc1A yellow
gpioSetPullUpDown(pigpio1A, PI_PUD_UP);
gpioSetMode(pigpio1B, PI_INPUT); // 7 enc1B blue
gpioSetPullUpDown(pigpio1B, PI_PUD_UP);
gpioSetMode(pigpio1d1, PI_OUTPUT); // dir1-1
gpioSetMode(pigpio1d2, PI_OUTPUT); // dir1-2
gpioSetMode(pigpio1pwm, PI_OUTPUT); // enable1
for( i=0; i< MAXMOTORS; ++i) motenc[i] = 0;
}
void callback0(int way){
static int pos = 0;
pos += way;
std::cout << "pos=" << pos << std::endl;
}
void callback1(int way){
static int pos = 0;
pos += way;
std::cout << "pos=" << pos << std::endl;
}
int main(int argc, char *argv[])
{
if (gpioInitialise() < 0) return 1;
setup();
re_decoder dec0(pigpio0A, pigpio0B, callback0);
re_decoder dec1(pigpio1A, pigpio1B, callback1);
sleep(3000);
dec0.re_cancel();
dec1.re_cancel();
gpioTerminate();
}
Zitieren
Lesezeichen