// https://forum.arduino.cc/t/rotary-encoder-using-interrupts/469066/10 // This only sends data if the button on the board is pushed. #include "Arduino.h" #include // The below just prepares a whole bunch of variables // for receving data from pins, leds, // interrupt handling, sending data, etc. // the below are pin assignments for sgn samd21 board const byte encoderPinA = 4; //outputA digital pin4 const byte encoderPinB = 5; //outoutB digital pin5 const byte encoderPinC = 8; //outputC digital pin8 const byte encoderPinD = 9; //outoutD digital pin9 const byte encoderPinE = 19; //outputE digital pin19 const byte encoderPinF = 22; //outoutF digital pin22 // the counts are collecting number of changes in rotary encoders // there are three encoders, I just made three counts volatile int count = 0; int protectedCount = 0; int previousCount = 0; volatile int count2 = 0; int protectedCount2 = 0; int previousCount2 = 0; volatile int count3 = 0; int protectedCount3 = 0; int previousCount3 = 0; volatile int encoderCounter; // Prepare the button int buttonState = 1; #define BUTTON_PIN 7 //prepare neopixels and led #define NEOPIXEL_PIN 27 // sgn SAMD21 Board int LED_PIN = 23; // normal led pin #define NUMPIXELS 1 // only 1 on board Adafruit_NeoPixel pixels(NUMPIXELS, NEOPIXEL_PIN, NEO_RGBW + NEO_KHZ800); // RGBW neopixel // This is looking for data on encoder pins #define readA digitalRead(encoderPinA) #define readB digitalRead(encoderPinB) #define readC digitalRead(encoderPinC) #define readD digitalRead(encoderPinD) #define readE digitalRead(encoderPinE) #define readF digitalRead(encoderPinF) //length of arms #define ARM1 175 // Arm E1-R2 length[mm.] #define ARM2 175 // Arm E2-tip length[mm.] //Offsets from mechanical set-up: #define Z_OFFSET 80.782 // E1 axis height above table [mm.] #define X_OFFSET -125.0 // Distance from E0 axis to preset position [mm.] #define Y_OFFSET 125.0 // Distance from E0 axis to preset position [mm.] //Angles from mechanical set-up: #define E0_PRESET -45.0 #define E1_PRESET 32.048 #define E2_PRESET -113.174 #define E3_PRESET 0.0 #define STATES_PER_REV 4000 void setup() { delay(10); SerialUSB.begin(0); //get USB serial ready delay(20); // prepare pins for encoders pinMode(encoderPinA, INPUT); pinMode(encoderPinB, INPUT); pinMode(encoderPinC, INPUT); pinMode(encoderPinD, INPUT); pinMode(encoderPinE, INPUT); pinMode(encoderPinF, INPUT); delay(20); // "attachs" the interrupts in case a pin sees a changing signal attachInterrupt(digitalPinToInterrupt(encoderPinA), isrA, CHANGE); attachInterrupt(digitalPinToInterrupt(encoderPinB), isrB, CHANGE); attachInterrupt(digitalPinToInterrupt(encoderPinC), isrC, CHANGE); attachInterrupt(digitalPinToInterrupt(encoderPinD), isrD, CHANGE); attachInterrupt(digitalPinToInterrupt(encoderPinE), isrE, CHANGE); attachInterrupt(digitalPinToInterrupt(encoderPinF), isrF, CHANGE); delay(20); pinMode(BUTTON_PIN, INPUT_PULLUP); pinMode(LED_PIN, OUTPUT); pixels.begin(); // INITIALIZE NeoPixel strip object (REQUIRED) pixels.setBrightness(35); // about 1/3 brightness pixels.show(); //turn off pixels } void loop() { noInterrupts(); protectedCount = count; protectedCount2 = count2; protectedCount3 = count3; interrupts(); buttonState = digitalRead(BUTTON_PIN); // read button pin pixels.clear(); //clear pixels, prepare for next color if (buttonState == 0) { // if button pressed, turn on normal LED/neopixel to white digitalWrite(LED_PIN, HIGH); pixels.clear(); pixels.setPixelColor(0, pixels.Color(0, 0, 0, 250)); pixels.show(); // Send the updated pixel colors to the hardware. } else { digitalWrite(LED_PIN, LOW); pixels.clear(); pixels.setPixelColor(0, pixels.Color(240, 0, 80, 0)); pixels.show(); // Send the updated pixel colors to the hardware. } //pixels.setPixelColor(0, pixels.Color(20, 150, 100, 0)); //pixels.show(); // Send the updated pixel colors to the hardware. //read encoders in radians // I don't understand why I have to do the encoderCounter = protectedCount 3 times. // but it works with it, doesn't without it. encoderCounter = protectedCount; double A = getRadians(encoderCounter); encoderCounter = protectedCount2; double B = getRadians(encoderCounter); encoderCounter = protectedCount3; double C = getRadians(encoderCounter); //calculate distance from E0 axis to tip 'r' and height above table 'z' double r = cos(A) * ARM1 + cos(A + B) * ARM2; double z = (sin(A) * ARM1) + (sin(A + B) * ARM2) + Z_OFFSET; //Calculate tip x,y double x = r * cos(C)+ X_OFFSET; double y = r * sin(C)+ Y_OFFSET; // This only sends data if something changes (if arms are moved or button press) //send data over USB serial if(protectedCount != previousCount || protectedCount2 != previousCount2 || protectedCount3 != previousCount3 || buttonState == 0) { // data sent is: SerialUSB.print(protectedCount); // encoder 1 SerialUSB.print(","); SerialUSB.print(protectedCount2); // encoder 2 SerialUSB.print(","); SerialUSB.print(protectedCount3); // encoder 3 SerialUSB.print(","); SerialUSB.print(buttonState); // button state SerialUSB.print(","); SerialUSB.print(r); // theta of base encoder SerialUSB.print(","); SerialUSB.print(x); // x position of tip SerialUSB.print(","); SerialUSB.print(y); //y position of tip SerialUSB.print(","); SerialUSB.println(z); //z position of tip } delay(20); //so Processing can keep up. // This keeps track of the encoders positions previousCount = protectedCount; previousCount2 = protectedCount2; previousCount3 = protectedCount3; /* // More Testing SerialUSB.print(A); SerialUSB.print(", "); SerialUSB.print(B); SerialUSB.print(", "); SerialUSB.print(C); SerialUSB.print(", "); SerialUSB.print(r); SerialUSB.print(", "); SerialUSB.print(x); SerialUSB.print(", "); SerialUSB.print(y); SerialUSB.print(", "); SerialUSB.println(z); */ } // The below are each Interrupt Handler // Each encoder has 2 pins, and as such, this has 6 handlers for 3 encoders void isrA() { if(readB != readA) { count ++; } else { count --; } } void isrB() { if (readA == readB) { count ++; } else { count --; } } void isrC() { if(readD != readC) { count2 ++; } else { count2 --; } } void isrD() { if (readC == readD) { count2 ++; } else { count2 --; } } void isrE() { if(readF != readE) { count3 ++; } else { count3 --; } } void isrF() { if (readE == readF) { count3 ++; } else { count3 --; } } //do trig for positions of arms float getRadians(int encoderCount) { return (double)encoderCounter * M_PI * 2 / (double)STATES_PER_REV; } float getDegrees(int encoderCount) { return (double)encoderCounter * 360.0 / (double)STATES_PER_REV; }