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