Code:
//// pin assignments
// Teensy 3.5/4.0/4.1 - These assignments work across all versions
const uint8_t PWM_SPEED_PIN(2);
const uint8_t BRAKE_PIN(3);
const uint8_t MOTOR_DIR_PIN(4);
const uint8_t LED_BUILTIN_PIN(13);
const uint8_t U_ENCODER_SIGNAL_PIN(14); // yellow - Ha - U
const uint8_t V_ENCODER_SIGNAL_PIN(15); // blue - Hb - V
const uint8_t W_ENCODER_SIGNAL_PIN(16); // green - Hc - W
const uint8_t BUTTON_PIN(23);
volatile int32_t tickCount;
volatile int32_t uCount;
volatile int lastUVal;
volatile int32_t vCount;
volatile int lastVVal;
volatile int32_t wCount;
volatile int lastWVal;
volatile char lastEncoder;
...
pinMode(U_ENCODER_SIGNAL_PIN, INPUT);
lastUVal = digitalRead(U_ENCODER_SIGNAL_PIN);
attachInterrupt(U_ENCODER_SIGNAL_PIN, countUTick, CHANGE);
pinMode(V_ENCODER_SIGNAL_PIN, INPUT);
lastVVal = digitalRead(V_ENCODER_SIGNAL_PIN);
attachInterrupt(V_ENCODER_SIGNAL_PIN, countVTick, CHANGE);
pinMode(W_ENCODER_SIGNAL_PIN, INPUT);
lastWVal = digitalRead(W_ENCODER_SIGNAL_PIN);
attachInterrupt(W_ENCODER_SIGNAL_PIN, countWTick, CHANGE);
/**
Interrupt handler for the U encoder signal.
**/
void countUTick() {
// Read the current signal value
int val = digitalRead(U_ENCODER_SIGNAL_PIN);
// Check for an encoder fault
bool encoderFault = (lastEncoder == 'U') || // Last encoder was this one
(lastEncoder == 'W' && motorContext.motorDirection) || // Last encoder was W, but going forward
(lastEncoder == 'V' && !motorContext.motorDirection); // Last encoder was V, but going reverse
//if (encoderFault) {
DebugMsgs.notification().print("U ").print(lastUVal).print(' ').print(val).print(' ')
.print(lastEncoder).println(encoderFault ? " *" : "");
//}
// If there is a fault, don't record a tick.
if (encoderFault) { return; }
int increment = (lastEncoder == 'W') ? 1 : -1;
uCount += increment;
tickCount += increment;
lastEncoder = 'U';
lastUVal = val;
}
When you paste code can you put it between CODE tags using the # button above (when editing post).
It makes your code so much more easy to read, understand and therefore be able to help you.