step motor module for encoder..



  • Hi all, is it possible read this encoder with this module?
    ENCODER TEARDOWN
    I would like to connect three encoder to the three motor ports.....

    is it possible to modify this sketch for M5Stack?

    
    #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);
    }
    
    

    tnks a lot