diff --git a/Arduino/test_r2_constantforce/test_r2_constantforce.ino b/Arduino/test_r2_constantforce/test_r2_constantforce.ino index 1d90acb905027a3070c11729fd7b4fc2cfcfd211..7e4db829677cccababf5277d546a3bbe93529db6 100644 --- a/Arduino/test_r2_constantforce/test_r2_constantforce.ino +++ b/Arduino/test_r2_constantforce/test_r2_constantforce.ino @@ -36,8 +36,8 @@ // NOTE: DIR=0 is positive encoder (on CH1) -const int INPUT_ENC1A = 2; -const int INPUT_ENC1B = 3; +#define INPUT_ENC1A 2 +#define INPUT_ENC1B 3 const int OUTPUT_VREF1 = 5; // D5 const int OUTPUT_VREF2 = 6; // D6 const int OUTPUT_MOT_ON = 9; @@ -46,7 +46,6 @@ const int OUTPUT_CLK1 = 13; const int INPUT_VREF1 = 15; // A1 const int INPUT_POT1 = 18; -volatile int last_1; volatile int val_1; int ctrl_phase_1; // Current "phase" of the controller (0-3) @@ -75,14 +74,13 @@ void step_once_1(int dir) { void setup() { Serial.begin(9600); - pinMode(OUTPUT_VREF1, OUTPUT); + pinMode(OUTPUT_VREF1, OUTPUT); pinMode(OUTPUT_MOT_ON, OUTPUT); pinMode(OUTPUT_DIR1, OUTPUT); pinMode(OUTPUT_CLK1, OUTPUT); // pinMode(PIN_LED, OUTPUT); - - attachInterrupt(0, isr_enc1, CHANGE); - attachInterrupt(1, isr_enc1, CHANGE); + + init_isr_enc1(); // Set output current to 50% analogWrite(OUTPUT_VREF1, 128); @@ -230,18 +228,38 @@ void loop() } } -const int E = -99; -const int GREY_MAPPING[16] = { - 0, -1, 1, E, - 1, 0, E, -1, - -1, E, 0, 1, - E, 1, -1, 0, +const char E = -99; +// Mapping of transitions of the two encoder state bits to relative encoder counts +// - low bits are current value +// - high bits are previous value +const char GREY_MAPPING[16] = { + 0, -1, 1, E, + 1, 0, E, -1, + -1, E, 0, 1, + E, 1, -1, 0, }; + +uint8_t pinmask_ENC1A; +uint8_t pinmask_ENC1B; +volatile uint8_t *pinport_ENC1A; +volatile uint8_t *pinport_ENC1B; +void init_isr_enc1() { + pinmask_ENC1A = digitalPinToBitMask(INPUT_ENC1A); + pinmask_ENC1B = digitalPinToBitMask(INPUT_ENC1B); + pinport_ENC1A = portInputRegister(digitalPinToPort(INPUT_ENC1A)); + pinport_ENC1B = portInputRegister(digitalPinToPort(INPUT_ENC1B)); + attachInterrupt(0, isr_enc1, CHANGE); + attachInterrupt(1, isr_enc1, CHANGE); +} +volatile char last_1; void isr_enc1() { - int v = digitalRead(INPUT_ENC1A) * 2 + digitalRead(INPUT_ENC1B); + uint8_t pinValue_ENC1A = (*pinport_ENC1A & pinmask_ENC1A) != 0; + uint8_t pinValue_ENC1B = (*pinport_ENC1B & pinmask_ENC1B) != 0; + + char v = (char)pinValue_ENC1A << 1 + (char)pinValue_ENC1B; if( v != last_1 ) { - int adj = GREY_MAPPING[last_1 * 4 + v]; + char adj = GREY_MAPPING[ (char)(last_1 << 2) + v ]; if(adj == E) { } else { @@ -250,4 +268,3 @@ void isr_enc1() { last_1 = v; } } -