Hello everyone,
I have an Arduino Mega with a CAN bus shield and an incremental encoder (3600 ppr). I'm getting the data from the encoder and then it's sent through CAN bus protocol to the computer. The code seems to work but when I speed up the encoder (above 700 rpm) the data reading fluctuates a lot. The code just has to return the degree between the encoder zero and a point where the arduino receives an external signal that triggers an interruption, then the difference is calculated and sent via CAN.
Could be that the arduino is not fast enough to do this task? I would like use the encoder at 3000 RPM.
#include <mcp_can.h>
#include <SPI.h>
// LIBRERIA --------> https://github.com/coryjfowler/MCP_CAN_lib
//--------------- PIN de entrada de CS ---------------------------
MCP_CAN CAN0(53); // Set CS to pin 53
//---------------------------------------------------------------------
//----------------- Canales entrada ENCODER ---------------------------
// Canales entrada ENCODER
const int channelPinA = 18; // Ch.A (Black) a pin 18 (con interrupcion INT5)
const int channelPinB = 19; // Ch.B (White) a pin 19 (con interrupcion INT4)
const int channelPinZ = 3; // Ch.Z (Orange) a pin 20 (con interrupcion INT0)
//---------------------------------------------------------------------
//----------------- Canales entrada bomba -----------------------------
#define pinBomba 20 // Pin de interrupción (INT.3) pulso cuadrado de bomba (4 o 6 pulsos cuadrados)
//---------------------------------------------------------------------
//--------------- Contadores pulsos encoder ---------------------------
volatile signed int posBomba = 0;
volatile signed int ISRCounter = 0; // Contador de interrupciones
volatile signed int angulo = 0; // angulo de la bomba
volatile signed int counter = 0; // Detectar movimiento en encoder
volatile uint16_t myPosition; // Posicion enviada por CAN BUS
const int maxSteps = 3600; // Pulsos por revolución
//---------------------------------------------------------------------
//------------------------- Flags -------------------------------------
bool IsCW; // true ----> giro horario (CW)
// false ---> giro antihorario (CCW)
bool ZERO; // true ----> cero detectado
// false ---> no hay cero
bool aux; // true ----> se ha pasado por cero
// false ---> se pone a cero cuando se devuelve el valor del angulo de la bomba
//---------------------------------------------------------------------
//------------------------- Configuración -----------------------------
#define CAN_ID_TX 0x050 // ID de mensaje que se transmite
#define CAN_ID_RX 0x051 // ID de mensaje que se busca recibir
#define OUT_DIVIDER 4 // PIN salida OUT_DIVIDER
#define OUT_TRIGGER 6 // PIN salida OUT_TRIGGER
#define posAbs 0 // Nº byte donde se recibe posAbs (HIGH o LOW/ 1 o 0)
// es el 1º byte del mensaje recibido ----> rxBuf[0]
#define posDivider 1 // 2º byte recibido en rxBuf
#define posBloqueo 2 // 3º byte recibido en rxBuf
// posición en la que se guarda la posición del encoder mensaje transmitido
#define posEncoderH 0 // se guarda mensaje en 1º byte
#define posEncoderL 1 // se guarda mensaje en 2º byte
#define tTXMsg 100 // tiempo de transmisión entre mensajes
#define tSignTask 10 //
#define CAN0_INT 2 // Set INT to pin 2 (interrupción de CAN SHIELD)
//---------------------------------------------------------------------
// --------------------------- Variables ------------------------------
// Mensaje TRX
byte data[8]; // se envian arrays de datos de 8 bytes
// Mensaje RX
long unsigned int rxID; // ID recibida
unsigned char len; // longitud de mensaje (en bytes)
unsigned char rxBuf[8]; // array char ----> buffer mensajes recibidos (8 bytes)
// Señal de activación para transmisión continua
byte bContinuousTx = 0;
// Valores de tiempo (en ms) auxuliares
unsigned long tant; //
unsigned long tantSign; //
//---------------------------------------------------------------------
void setup(){
Serial.begin(115200);
if(CAN0.begin(MCP_STDEXT, CAN_500KBPS, MCP_16MHZ) == CAN_OK)
Serial.println("MCP2515 Initialized Successfully!");
else
Serial.println("Error Initializing MCP2515...");
CAN0.setMode(MCP_NORMAL);
DDRE = 0xDF; // pin pin 5 como input
DDRD = 0x00; // pines
PORTE = 0x00;
PORTD = 0x00;
//pinMode(channelPinA, INPUT);
//pinMode(channelPinZ, INPUT);
//pinMode(pinBomba, INPUT); // poner INPUT_PULLUP
//digitalWrite(pinBomba, HIGH); //Pull-up
attachInterrupt(digitalPinToInterrupt(pinBomba), INT_Bomba, RISING);
attachInterrupt(digitalPinToInterrupt(channelPinB), doEncode, RISING);
attachInterrupt(digitalPinToInterrupt(channelPinZ), doZero, RISING);
Serial.println(myPosition);
bContinuousTx = 0;
}
void loop(){
if ((millis() - tantSign) >= tSignTask){
tantSign = millis();
if (counter != ISRCounter){ // Se hace algoritmo si hay cambio en encoder
if(ISRCounter >= 0){
myPosition = ISRCounter;
if (ISRCounter > maxSteps-1){
ISRCounter = 0;
myPosition = 0;
}
}
if(ISRCounter < 0){
myPosition = 3600 - abs(ISRCounter);
if(ISRCounter < -maxSteps + 1){
ISRCounter = 0;
myPosition = 0;
}
}
//Serial.print(angulo);
if(bContinuousTx){
Serial.print(myPosition);
Serial.print("\t");
if(IsCW && ZERO) Serial.println("\tCW \tZERO");
if(IsCW && !ZERO) Serial.println("\tCW");
if(!IsCW && ZERO) Serial.println("\tCCW \tZERO");
if(!IsCW && !ZERO) Serial.println("\tCCW");
}else{
//Serial.print("Angulo bomba: ");
//Serial.println(posBomba);
}
}
ZERO = false;
counter = ISRCounter;
/*--------------- TX ------------------------------------*/
if (bContinuousTx){
if ((millis() - tant) >= tTXMsg){
tant = millis();
//Serial.println("SSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSS");
data[posEncoderL] = (myPosition & 0x000000FF); // posEncoderL = 0
data[posEncoderH] = (myPosition & 0x0000FF00) >> 8; // posEncoderH = 1
CAN0.sendMsgBuf(CAN_ID_TX, 0, 8, data);
}
}
if (!digitalRead(CAN0_INT)){
CAN0.readMsgBuf(&rxID, &len, rxBuf);
if(rxID == CAN_ID_RX){
digitalWrite(OUT_DIVIDER, rxBuf[posDivider]); // posDivider = 1
digitalWrite(OUT_TRIGGER, rxBuf[posAbs]); // posAbs = 0
if(rxBuf[posBloqueo] == 1){ //posBloqueo = 2
detachInterrupt(digitalPinToInterrupt(pinBomba));
//detachInterrupt(digitalPinToInterrupt(channelPinZ));
bContinuousTx = 1;
//Serial.println("posBloqueo = 1");
}else{
attachInterrupt(digitalPinToInterrupt(pinBomba), INT_Bomba, RISING);
attachInterrupt(digitalPinToInterrupt(channelPinZ), doZero, RISING);
bContinuousTx = 0;
}
}
}
}
}
void doEncode(){
if (digitalRead(channelPinB) == digitalRead(channelPinA)){
IsCW = true;
ISRCounter++;
angulo++;
}else{
IsCW = false;
ISRCounter--;
angulo--;
}
}
void doZero(){ // cada paso por zero se reinicia el contador
ZERO = true;
aux = true;
ISRCounter = 0;
myPosition = 0;
angulo = 0;
}
void INT_Bomba(){
if (aux == true){
if (ISRCounter < 0){
posBomba = abs(angulo);
}else{
posBomba = abs(angulo);
}
Serial.print("Angulo bomba: ");
Serial.println(posBomba);
data[posEncoderL] = (posBomba & 0x000000FF); // posEncoderL = 0
data[posEncoderH] = (posBomba & 0x0000FF00) >> 8; // posEncoderH = 1
CAN0.sendMsgBuf(CAN_ID_TX, 0, 8, data);
aux = false;
}
}