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

    step motor module for encoder..

    Arduino
    1
    1
    2.8k
    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 cepics

      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

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