🤖Have you ever tried Chat.M5Stack.com before asking??😎
    M5Stack Community
    • Categories
    • Recent
    • Tags
    • Popular
    • Users
    • Groups
    • Search
    • Register
    • Login

    BMC_SBUS M5Stack porting

    General
    2
    2
    4.9k
    Loading More Posts
    • Oldest to Newest
    • Newest to Oldest
    • Most Votes
    Reply
    • Reply as topic
    Log in to reply
    This topic has been deleted. Only users with topic management privileges can see it.
    • C
      cepics
      last edited by

      Hi all
      I'm trying to port an Arduino Mega sketch, using BMC_SBUS, to M5Stack!!
      https://github.com/boldstelvis/BMC_SBUS ... /README.md
      to use this: https://www.kinowheels.com/
      with this: https://forum.dji.com/thread-167232-1-1.html
      this is the connections: https://forum11.djicdn.com/data/attachm ... htjjsl.png

      this is the Arduino Mega sketch connected to the ronin D-bus (it works!! I can move the ronin X and Y axis):

      #define BAUDRATE 100000  // oder  100000 115200
      #define SERIALPORT Serial  // - uncomment this line if using an arduino based board with more than one HW serial port
      
      class BMC_SBUS
      {
        public:
          uint8_t sbusData[25];
          int16_t servos[18];
          void begin(void);
          void Servo(uint8_t ch, int16_t position);
          void Send(void);
          void Update(void);
      
        private:
          uint8_t byte_in_sbus;
          uint8_t bit_in_sbus;
          uint8_t ch;
          uint8_t bit_in_servo;
      };
      
      
      void BMC_SBUS::begin()
      {
        //intialise private data arrays
        //sbus_data is formatted for correct serial output
        //note that the actual 11bit sbus data for each channel is embedded across multiple data bytes in a very stange order
        //byte 1 and bytes 24 and 25 should be left as is 
        //the first is a start byte, the last is a stop byte and the second last holds various flags
        //servos is the internal per channel position and is more straightforward - one int_16 per channel
      
        uint8_t loc_sbusData[25] = {0x0f,0x01,0x04,0x20,0x00,0xff,0x07,0x40,0x00,0x02,0x10,0x80,0x2c,0x64,0x21,0x0b,0x59,0x08,0x40,0x00,0x02,0x10,0x80,0x00,0x00};
        int16_t loc_servos[18]   = {1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,1023,0,0};
      
        //setup serial port to transmit at 100k baud and use 1 parity and 2 stop bits
      
        SERIALPORT.begin(BAUDRATE, SERIAL_8E2);
      
        //setup public data arrays
      
        memcpy(sbusData,loc_sbusData,25);
        memcpy(servos,loc_servos,18);
      }
      
      void BMC_SBUS::Servo(uint8_t ch, int16_t position) 
      {
        //set servo position on single channel
      
        if ((ch>0)&&(ch<=16)) 
        {
          constrain (position, 0, 2048); //keep within min/max values
          servos[ch-1] = position; //expects a non zero starting index to the channel
        }
      }
      
      void BMC_SBUS::Send(void)
      {
        //send data over serial port
        SERIALPORT.write(sbusData, 25); //according to docs for Serial we can send the array along as is without a loop
      }
      
      void BMC_SBUS::Update(void) 
      {
        //update positions for all servo channels within the SBUS data frame
        //ignores digital servos and any failsafe mode stuff that was originally written
      
        //clear out existing sbus data for all channel data bytes
        //ignores first and last bytes in the array (start and stop bytes)
        //mapping loop relies on initial 0 values - do not omit this step!
      
        uint8_t i;
        for (i=1; i<24; i++) 
        {
          sbusData[i] = 0;
        }
      
        //reset counters
      
        ch = 0;
        bit_in_servo = 0;
        byte_in_sbus = 1;
        bit_in_sbus = 0;
      
        //format sbus data - maps sevo data array to sbus data array 1bit at a time
        //correctly deals with the little endian byte order in the process
      
        for (i=0; i<176; i++) //16channels*11bits = 176bits
        {
          if (servos[ch] & (1<<bit_in_servo)) //bitwise AND to check if the correct servo databit is set to 1
          {
            sbusData[byte_in_sbus] |= (1<<bit_in_sbus); //bitwise OR sets the correct sbus databit if true
          }
      
          //increment bit counters
      
          bit_in_sbus++;
          bit_in_servo++;
      
          //if end of sbus byte reset sbus bit counter and increment sbus byte counter
      
          if (bit_in_sbus == 8) 
          {
            bit_in_sbus = 0;
            byte_in_sbus++;
          }
      
          // if we have reached bit 11 in the servo data increment channel index and reset servo bit counter
      
          if (bit_in_servo == 11) 
          {
            bit_in_servo = 0;
            ch++;
          }
        }
      }
      
      
      //Declare BMC_SBUS Object
      BMC_SBUS mySBUS;
      
      // Sbus delay value
      const int sbusWAIT = 7;      //frame timing delay in msecs
      
      // Declare sbus control channels
      int panChannel = 1;
      int tiltChannel = 2;
      int rollChannel = 4;
      
      
      // Declare Kinowheels Stuff
      int XA_SIG=0, XB_SIG=1, YA_SIG=0, YB_SIG=1, pulsesX, pulsesY;
      
      // Declare Stuff for calculating Speed
      int xStampEnd=0, yStampEnd=0, timeStampEnd=0, xPassed, yPassed, timePassed, sendX=1023, sendY=1023;
      
      
      
      
      void setup() {
        // Serial.begin(100000); überflüssig, weil in MC_SBUS enthalten
      
      
        // Start  KinoWheels Stuff
        attachInterrupt(0, XA_RISE, RISING); // Pin 2
        attachInterrupt(1, XB_RISE, RISING); // Pin 3
        attachInterrupt(4, YA_RISE, RISING); // Pin 19
        attachInterrupt(5, YB_RISE, RISING); // Pin 18
         
        
         // Start BMC_SBUS object
        mySBUS.begin();
        
      }
      
      void loop() {
      
        for (int i=0; i<1; i++){        //SBUS needs data every 7 Milliseconds. I repeat it three times for some time to pass for calculating speeds.
      
        mySBUS.Servo(tiltChannel,sendY);
        mySBUS.Servo(panChannel,sendX);
      
        // Update SBUS object and send data
        mySBUS.Update();
        mySBUS.Send();
       
        delay(sbusWAIT);
        
      }
      
        
       timePassed = millis() - timeStampEnd;
       xPassed = xStampEnd - pulsesX;
       yPassed = pulsesY - yStampEnd;
      
       sendX = 1023 + 100* xPassed / timePassed;
       sendY = 1023 + 100* yPassed / timePassed;
      
      for (int i=0; i<1; i++){          //Maybe this one is not needed. Will find it out later
      
        mySBUS.Servo(tiltChannel,sendY);
        mySBUS.Servo(panChannel,sendX);
      
        // Update SBUS object and send data
        mySBUS.Update();
        mySBUS.Send();
       
        delay(sbusWAIT);
        
      }
      
       xStampEnd = pulsesX;
       yStampEnd = pulsesY;
       timeStampEnd = millis(); 
      }
      
      
      
      //Rotary Encoder Stuff by KinoWheels
      
      void XA_RISE(){
       detachInterrupt(0);
       //delay(1);
       XA_SIG=1;
       
       if(XB_SIG==0)
       pulsesX++;//moving forward
       if(XB_SIG==1)
       pulsesX--;//moving reverse
      
       attachInterrupt(0, XA_FALL, FALLING);
      }
      
      void XA_FALL(){
        detachInterrupt(0);
        //delay(1);
       XA_SIG=0;
       
       if(XB_SIG==1)
       pulsesX++;//moving forward
       if(XB_SIG==0)
       pulsesX--;//moving reverse
      
       attachInterrupt(0, XA_RISE, RISING);  
      }
      
      void XB_RISE(){
       detachInterrupt(1);
       //delay(1);
       XB_SIG=1;
       
       if(XA_SIG==1)
       pulsesX++;//moving forward
       if(XA_SIG==0)
       pulsesX--;//moving reverse
      
       attachInterrupt(1, XB_FALL, FALLING);
      }
      
      void XB_FALL(){
       detachInterrupt(1);
       //delay(1);
       XB_SIG=0;
       
       if(XA_SIG==0)
       pulsesX++;//moving forward
       if(XA_SIG==1)
       pulsesX--;//moving reverse
      
       attachInterrupt(1, XB_RISE, RISING);
      }
      
      
      void YA_RISE(){
       detachInterrupt(4);
       //delay(1);
       YA_SIG=1;
       
       if(YB_SIG==0)
       pulsesY++;//moving forward
       if(YB_SIG==1)
       pulsesY--;//moving reverse
      
      
       attachInterrupt(4, YA_FALL, FALLING);
      }
      
      void YA_FALL(){
        detachInterrupt(4);
        //delay(1);
       YA_SIG=0;
       
       if(YB_SIG==1)
       pulsesY++;//moving forward
       if(YB_SIG==0)
       pulsesY--;//moving reverse
      
       attachInterrupt(4, YA_RISE, RISING);  
      }
      
      void YB_RISE(){
       detachInterrupt(5);
       //delay(1);
       YB_SIG=1;
       
       if(YA_SIG==1)
       pulsesY++;//moving forward
       if(YA_SIG==0)
       pulsesY--;//moving reverse
      
       attachInterrupt(5, YB_FALL, FALLING);
      }
      
      void YB_FALL(){
       detachInterrupt(5);
       //delay(1);
       YB_SIG=0;
       
       if(YA_SIG==0)
       pulsesY++;//moving forward
       if(YA_SIG==1)
       pulsesY--;//moving reverse
      
       attachInterrupt(5, YB_RISE, RISING);
      }
      

      How can I port to M5Stack?

      this in what I done ... but I have not Y axe control....
      M5Stack----------ronin D-bus
      grd-------------------grd
      5V--------------------5V
      TXD2(G17)------data

      #include <M5Stack.h>
      #include <ESP32Encoder.h>
      
      ESP32Encoder encoderX;
      ESP32Encoder encoderY;
      
      #define BAUDRATE 100000  // oder  100000 115200
      #define SERIALPORT Serial2  // - uncomment this line if using an arduino based board with more than one HW serial port
      
      
      class BMC_SBUS
      {
        public:
          uint8_t sbusData[25];
          int16_t servos[18];
          void begin(void);
          void Servo(uint8_t ch, int16_t position);
          void Send(void);
          void Update(void);
      
        private:
          uint8_t byte_in_sbus;
          uint8_t bit_in_sbus;
          uint8_t ch;
          uint8_t bit_in_servo;
      };
      
      
      void BMC_SBUS::begin()
      {
        //intialise private data arrays
        //sbus_data is formatted for correct serial output
        //note that the actual 11bit sbus data for each channel is embedded across multiple data bytes in a very stange order
        //byte 1 and bytes 24 and 25 should be left as is
        //the first is a start byte, the last is a stop byte and the second last holds various flags
        //servos is the internal per channel position and is more straightforward - one int_16 per channel
      
        uint8_t loc_sbusData[25] = {0x0f, 0x01, 0x04, 0x20, 0x00, 0xff, 0x07, 0x40, 0x00, 0x02, 0x10, 0x80, 0x2c, 0x64, 0x21, 0x0b, 0x59, 0x08, 0x40, 0x00, 0x02, 0x10, 0x80, 0x00, 0x00};
        int16_t loc_servos[18]   = {1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 0, 0};
      
        //setup serial port to transmit at 100k baud and use 1 parity and 2 stop bits
      
        SERIALPORT.begin(BAUDRATE, SERIAL_8E2);
      
        //setup public data arrays
      
        memcpy(sbusData, loc_sbusData, 25);
        memcpy(servos, loc_servos, 18);
      }
      
      void BMC_SBUS::Servo(uint8_t ch, int16_t position)
      {
        //set servo position on single channel
      
        if ((ch > 0) && (ch <= 16))
        {
          constrain (position, 0, 2048); //keep within min/max values
          servos[ch - 1] = position; //expects a non zero starting index to the channel
        }
      }
      
      void BMC_SBUS::Send(void)
      {
        //send data over serial port
        SERIALPORT.write(sbusData, 25); //according to docs for Serial we can send the array along as is without a loop
      }
      
      void BMC_SBUS::Update(void)
      {
        //update positions for all servo channels within the SBUS data frame
        //ignores digital servos and any failsafe mode stuff that was originally written
      
        //clear out existing sbus data for all channel data bytes
        //ignores first and last bytes in the array (start and stop bytes)
        //mapping loop relies on initial 0 values - do not omit this step!
      
        uint8_t i;
        for (i = 1; i < 24; i++)
        {
          sbusData[i] = 0;
        }
      
        //reset counters
      
        ch = 0;
        bit_in_servo = 0;
        byte_in_sbus = 1;
        bit_in_sbus = 0;
      
        //format sbus data - maps sevo data array to sbus data array 1bit at a time
        //correctly deals with the little endian byte order in the process
      
        for (i = 0; i < 176; i++) //16channels*11bits = 176bits
        {
          if (servos[ch] & (1 << bit_in_servo)) //bitwise AND to check if the correct servo databit is set to 1
          {
            sbusData[byte_in_sbus] |= (1 << bit_in_sbus); //bitwise OR sets the correct sbus databit if true
          }
      
          //increment bit counters
      
          bit_in_sbus++;
          bit_in_servo++;
      
          //if end of sbus byte reset sbus bit counter and increment sbus byte counter
      
          if (bit_in_sbus == 8)
          {
            bit_in_sbus = 0;
            byte_in_sbus++;
          }
      
          // if we have reached bit 11 in the servo data increment channel index and reset servo bit counter
      
          if (bit_in_servo == 11)
          {
            bit_in_servo = 0;
            ch++;
          }
        }
      }
      
      //Declare BMC_SBUS Object
      BMC_SBUS mySBUS;
      
      // Sbus delay value
      const int sbusWAIT = 7;      //frame timing delay in msecs
      
      // Declare sbus control channels
      int panChannel = 1;
      int tiltChannel = 2;
      int rollChannel = 4;
      
      float pulsesX, pulsesY;
      
      int xStampEnd = 0;
      int yStampEnd = 0;
      int timeStampEnd = 0;
      int xPassed;
      int yPassed;
      int timePassed;
      int sendX;// ??????
      int sendY;// ??????
      
      // Declare Poti Values
      int potiX = 100;
      int potiY = 100;
      
      bool PANTILTtoggle = 0;
      
      
      void setup() {
        M5.begin();
      
        // clear the encoder's raw count and set the tracked count to zero
        encoderX.clearCount();
        encoderY.clearCount();
      
        // Attache pins for use as encoder pins
        encoderX.attachHalfQuad(2, 3);
        encoderY.attachHalfQuad(36, 35);
        
        M5.update();
      
        sendX = 1023;
        sendY = 1023;
      
        // Start BMC_SBUS object
        mySBUS.begin();
      }
      
      void loop() {
      
        pulsesX = encoderX.getCount();
        pulsesY = encoderY.getCount();
      
        for (int i = 0; i < 1; i++) {   //SBUS needs data every 7 Milliseconds. I repeat it three times for some time to pass for calculating speeds.
      
          mySBUS.Servo(tiltChannel, sendY);
          mySBUS.Servo(panChannel, sendX);
      
          // Update SBUS object and send data
          mySBUS.Update();
          mySBUS.Send();
      
          delay(sbusWAIT);
      
        }
      
      
        timePassed = millis() - timeStampEnd;
        xPassed = xStampEnd - pulsesX;
        yPassed = pulsesY - yStampEnd;
      
        M5.update();
      
          sendX = 1023 + 100 * xPassed / timePassed;
          sendY = 1023 + 100 * yPassed / timePassed;
      
        if (sendX > 2047) {
          sendX = 2047;
        }
        if (sendX < 0) {
          sendX = 0;
        }
        if (sendY > 2047) {
          sendY = 2047;
        }
        if (sendY < 0) {
          sendY = 0;
        }
      
        for (int i = 0; i < 1; i++) {     //Maybe this one is not needed. Will find it out later
      
          mySBUS.Servo(tiltChannel, sendY);
          mySBUS.Servo(panChannel, sendX);
      
          // Update SBUS object and send data
          mySBUS.Update();
          mySBUS.Send();
      
          delay(sbusWAIT);
      
        }
      
        xStampEnd = pulsesX;
        yStampEnd = pulsesY;
        timeStampEnd = millis();
      
        M5.update();
      }
      

      the strange thing is that the information about X axe, go to the machine connected at G17 (TXD2), but not the Y one..
      I'm using an inverter on the TX-----RX line...for (d-bus/s-bus)

      tnks a lot

      1 Reply Last reply Reply Quote 0
      • L
        Lewis-H
        last edited by

        codice arduino mega stabilizzatore

        #define BAUDRATE 100000 // oder 100000 115200
        //#define BAUDRATE 115200 // oder 100000 115200
        #define SERIALPORT Serial // - uncomment this line if using an arduino based board with more than one HW serial port

        class BMC_SBUS
        {
        public:
        uint8_t sbusData[25];
        int16_t servos[18];
        void begin(void);
        void Servo(uint8_t ch, int16_t position);
        void Send(void);
        void Update(void);

        private:
        uint8_t byte_in_sbus;
        uint8_t bit_in_sbus;
        uint8_t ch;
        uint8_t bit_in_servo;
        };

        void BMC_SBUS::begin()
        {
        //intialise private data arrays
        //sbus_data is formatted for correct serial output
        //note that the actual 11bit sbus data for each channel is embedded across multiple data bytes in a very stange order
        //byte 1 and bytes 24 and 25 should be left as is
        //the first is a start byte, the last is a stop byte and the second last holds various flags
        //servos is the internal per channel position and is more straightforward - one int_16 per channel

        uint8_t loc_sbusData[25] = {0x0f, 0x01, 0x04, 0x20, 0x00, 0xff, 0x07, 0x40, 0x00, 0x02, 0x10, 0x80, 0x2c, 0x64, 0x21, 0x0b, 0x59, 0x08, 0x40, 0x00, 0x02, 0x10, 0x80, 0x00, 0x00};
        int16_t loc_servos[18] = {1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 1023, 0, 0};

        //setup serial port to transmit at 100k baud and use 1 parity and 2 stop bits

        SERIALPORT.begin(BAUDRATE, SERIAL_8E2);

        //setup public data arrays

        memcpy(sbusData, loc_sbusData, 25);
        memcpy(servos, loc_servos, 18);
        }

        void BMC_SBUS::Servo(uint8_t ch, int16_t position)
        {
        //set servo position on single channel

        if ((ch > 0) && (ch <= 16))
        {
        constrain (position, 0, 2048); //keep within min/max values
        servos[ch - 1] = position; //expects a non zero starting index to the channel
        }
        }

        void BMC_SBUS::Send(void)
        {
        //send data over serial port
        SERIALPORT.write(sbusData, 25); //according to docs for Serial we can send the array along as is without a loop
        }

        void BMC_SBUS::Update(void)
        {
        //update positions for all servo channels within the SBUS data frame
        //ignores digital servos and any failsafe mode stuff that was originally written

        //clear out existing sbus data for all channel data bytes
        //ignores first and last bytes in the array (start and stop bytes)
        //mapping loop relies on initial 0 values - do not omit this step!

        uint8_t i;
        for (i = 1; i < 24; i++)
        {
        sbusData[i] = 0;
        }

        //reset counters

        ch = 0;
        bit_in_servo = 0;
        byte_in_sbus = 1;
        bit_in_sbus = 0;

        //format sbus data - maps sevo data array to sbus data array 1bit at a time
        //correctly deals with the little endian byte order in the process

        for (i = 0; i < 176; i++) //16channels*11bits = 176bits
        {
        if (servos[ch] & (1 << bit_in_servo)) //bitwise AND to check if the correct servo databit is set to 1
        {
        sbusData[byte_in_sbus] |= (1 << bit_in_sbus); //bitwise OR sets the correct sbus databit if true
        }

        //increment bit counters
        
        bit_in_sbus++;
        bit_in_servo++;
        
        //if end of sbus byte reset sbus bit counter and increment sbus byte counter
        
        if (bit_in_sbus == 8)
        {
          bit_in_sbus = 0;
          byte_in_sbus++;
        }
        
        // if we have reached bit 11 in the servo data increment channel index and reset servo bit counter
        
        if (bit_in_servo == 11)
        {
          bit_in_servo = 0;
          ch++;
        }
        

        }
        }

        //Declare BMC_SBUS Object
        BMC_SBUS mySBUS;

        // Sbus delay value
        const int sbusWAIT = 7; //frame timing delay in msecs

        // Declare sbus control channels
        int panChannel = 1;
        int tiltChannel = 2;
        int rollChannel = 4;

        // Declare Kinowheels Stuff
        int XA_SIG = 0, XB_SIG = 1, YA_SIG = 0, YB_SIG = 1, pulsesX, pulsesY;

        // Declare Stuff for calculating Speed
        int xStampEnd = 0, yStampEnd = 0, timeStampEnd = 0, xPassed, yPassed, timePassed, sendX = 1023, sendY = 1023;

        void setup() {
        // Serial.begin(100000); überflüssig, weil in MC_SBUS enthalten

        // Start KinoWheels Stuff
        attachInterrupt(0, XA_RISE, RISING); // Pin 2
        attachInterrupt(1, XB_RISE, RISING); // Pin 3
        attachInterrupt(4, YA_RISE, RISING); // Pin 19
        attachInterrupt(5, YB_RISE, RISING); // Pin 18

        // Start BMC_SBUS object
        mySBUS.begin();

        }

        void loop() {

        for (int i = 0; i < 1; i++) { //SBUS needs data every 7 Milliseconds. I repeat it three times for some time to pass for calculating speeds.

        mySBUS.Servo(tiltChannel, sendY);
        mySBUS.Servo(panChannel, sendX);
        
        // Update SBUS object and send data
        mySBUS.Update();
        mySBUS.Send();
        
        delay(sbusWAIT);
        

        }

        timePassed = millis() - timeStampEnd;
        xPassed = xStampEnd - pulsesX;
        yPassed = pulsesY - yStampEnd;

        sendX = 1023 + 100 * xPassed / timePassed;
        sendY = 1023 + 100 * yPassed / timePassed;

        for (int i = 0; i < 1; i++) { //Maybe this one is not needed. Will find it out later

        mySBUS.Servo(tiltChannel, sendY);
        mySBUS.Servo(panChannel, sendX);
        
        // Update SBUS object and send data
        mySBUS.Update();
        mySBUS.Send();
        
        delay(sbusWAIT);
        

        }

        xStampEnd = pulsesX;
        yStampEnd = pulsesY;
        timeStampEnd = millis();
        }

        //Rotary Encoder Stuff by KinoWheels

        void XA_RISE() {
        detachInterrupt(0);
        //delay(1);
        XA_SIG = 1;

        if (XB_SIG == 0)
        pulsesX++;//moving forward
        if (XB_SIG == 1)
        pulsesX--;//moving reverse

        attachInterrupt(0, XA_FALL, FALLING);
        }

        void XA_FALL() {
        detachInterrupt(0);
        //delay(1);
        XA_SIG = 0;

        if (XB_SIG == 1)
        pulsesX++;//moving forward
        if (XB_SIG == 0)
        pulsesX--;//moving reverse

        attachInterrupt(0, XA_RISE, RISING);
        }

        void XB_RISE() {
        detachInterrupt(1);
        //delay(1);
        XB_SIG = 1;

        if (XA_SIG == 1)
        pulsesX++;//moving forward
        if (XA_SIG == 0)
        pulsesX--;//moving reverse

        attachInterrupt(1, XB_FALL, FALLING);
        }

        void XB_FALL() {
        detachInterrupt(1);
        //delay(1);
        XB_SIG = 0;

        if (XA_SIG == 0)
        pulsesX++;//moving forward
        if (XA_SIG == 1)
        pulsesX--;//moving reverse

        attachInterrupt(1, XB_RISE, RISING);
        }

        void YA_RISE() {
        detachInterrupt(4);
        //delay(1);
        YA_SIG = 1;

        if (YB_SIG == 0)
        pulsesY++;//moving forward
        if (YB_SIG == 1)
        pulsesY--;//moving reverse

        attachInterrupt(4, YA_FALL, FALLING);
        }

        void YA_FALL() {
        detachInterrupt(4);
        //delay(1);
        YA_SIG = 0;

        if (YB_SIG == 1)
        pulsesY++;//moving forward
        if (YB_SIG == 0)
        pulsesY--;//moving reverse

        attachInterrupt(4, YA_RISE, RISING);
        }

        void YB_RISE() {
        detachInterrupt(5);
        //delay(1);
        YB_SIG = 1;

        if (YA_SIG == 1)
        pulsesY++;//moving forward
        if (YA_SIG == 0)
        pulsesY--;//moving reverse

        attachInterrupt(5, YB_FALL, FALLING);
        }

        void YB_FALL() {
        detachInterrupt(5);
        //delay(1);
        YB_SIG = 0;

        if (YA_SIG == 0)
        pulsesY++;//moving forward
        if (YA_SIG == 1)
        pulsesY--;//moving reverse

        attachInterrupt(5, YB_RISE, RISING);
        }

        1 Reply Last reply Reply Quote 0
        • First post
          Last post