KinoWheels M5Stack porting WIP...



  • Hi All, this is my porting of KinoWheels to m5stack using espnow to play wireless with R0nin

    tested with:
    R0nin 1

    KinoWheels
    R0nin Forum

    TX: M5Stack + PROTO MODULE
    RX: M5Atom Lite + GROVE cable

    SOFTWARE:

    first we upload rx code to know the mac address on serial monitor

    in the arduino IDE select M5StickC board for ATOM

    #include "M5Atom.h"
    #include <esp_now.h>
    #include <WiFi.h>
    
    #define WIFI_CHANNEL 1
    uint8_t localCustomMac[] = {0x36, 0x33, 0x33, 0x33, 0x33, 0x33};
    const byte maxDataFrameSize = 200;
    
    // must match the controller struct
    struct __attribute__((packed)) DataStruct {
      int X;
      int Y;
      int Z;
    };
    DataStruct myData;
    
    #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
      //               (baud-rate/protocol/RXpin/TXpin/INVERT)
        SERIALPORT.begin(BAUDRATE, SERIAL_8E2, 32, 26, true); // ATOM GROVE PORT 
    
      //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;
    
    
    int sentX;
    int sentY;
    int sentZ;
    
    
    /* ********************************************************
      /*                      Void setup                           *
      /* ****************************************************** */
    void setup() {
    
      //  M5.begin();
      M5.begin(true, false, true);
      Serial.begin(115200);
      delay(200);
      Serial.print("\r\n\r\n");
      WiFi.mode(WIFI_AP);
      Serial.println( WiFi.softAPmacAddress() );
      WiFi.disconnect();
      if (esp_now_init() == ESP_OK)
      {
        Serial.println("ESPNow Init Success!");
      }
      else
      {
        Serial.println("ESPNow Init Failed....");
      }
      esp_now_register_recv_cb(OnDataRecv);
    
      sentX = 1023;
      sentY = 1023;
      sentZ = 1023;
    
      // Start BMC_SBUS object
      mySBUS.begin();
    
    }
    
    /* ********************************************************
      /*                      Void Loop                           *
      /* ****************************************************** */
    void loop() { 
    
      sentX = (myData.X);
      sentY = (myData.Y);
      sentZ = (myData.Z);
    
    //  Serial.print("sentX ");
    //  Serial.println(sentX);
    //  Serial.print("sentY ");
    //  Serial.println(sentY);
    //  Serial.print("sentZ ");
    //  Serial.println(sentZ);
    
      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(panChannel, sentX);
        mySBUS.Servo(tiltChannel, sentY);
        mySBUS.Servo(rollChannel, sentZ);
    
        // Update SBUS object and send data
        mySBUS.Update();
        mySBUS.Send();
    
        delay(sbusWAIT);
    
      }
    
      if (M5.Btn.wasPressed())
      {}
    
      M5.update();
    }
    
    
    void OnDataRecv(const uint8_t *mac_addr, const uint8_t *incomingData, int data_len)
    {
      memcpy(&myData, incomingData, sizeof(myData));
      Serial.print ("MAC ADDRES = ");
      for (byte n = 0; n < 6; n++) {
        Serial.print (mac_addr[n], HEX);
      }
      stato ++;
      Serial.println ();
    }
    

    open arduino serial monitor and pick the receiver MAC ADDRESS.

    put the receiver MAC ADDRESS, in the TX code instead of "XX"

    
    #include <M5Stack.h>
    
    #include <ESP32Encoder.h>
    
    ESP32Encoder encoderX;
    ESP32Encoder encoderY;
    
    ////////////////////ESPNOW/////////////////////////
    #include <esp_now.h>
    #include <WiFi.h>
    
    #define WIFI_CHANNEL 1
    
    esp_now_peer_info_t slave;
    
    // RX MAC ADDRESS HERE
    uint8_t remoteMac[] = {0xXX, 0xXX, 0xXX, 0xXX, 0xXX, 0xXX};
    
    const uint8_t maxDataFrameSize = 200;
    
    // must match the controller struct
    struct __attribute__((packed)) DataStruct {
    
      int X;
      int Y;
      int Z;
    
    };
    DataStruct myData;
    const esp_now_peer_info_t *peer = &slave;
    uint8_t dataToSend[maxDataFrameSize];
    
    unsigned long lastSentMillis;
    unsigned long sendIntervalMillis = 10;
    unsigned long sentMicros;
    unsigned long ackMicros;
    
    ///////////////////////////FINE ESPNOW///////////////
    
    int xStampEnd = 0, yStampEnd = 0, timeStampEnd = 0;
    int xPassed, yPassed, timePassed;
    int X = 1023, Y = 1023, Z = 1023;
    int pulsesX, pulsesY;
    int  potiX = 100,  potiY = 100;
    
    byte gearX = 2;
    byte gearY = 2;
    
    bool toggleZ;
    bool toggleUSD;
    
    //bool StatoRX;
    
    
    void setup() {
      M5.begin();
      Serial.begin(115200);
    
      M5.Lcd.clear(BLACK);
    
      M5.Lcd.fillRoundRect(5, 10, 310, 150, 10, TFT_BLACK);
      M5.Lcd.fillRoundRect(15, 20, 290, 130, 5, TFT_YELLOW);
      M5.Lcd.fillRoundRect(25, 30, 270, 110, 55, TFT_BLACK);
    
      M5.Lcd.fillRoundRect(5, 150, 310, 90, 10, TFT_BLACK);
      M5.Lcd.fillRoundRect(15, 160, 290, 70, 5, TFT_YELLOW);
      M5.Lcd.fillRoundRect(25, 170, 270, 50, 25, TFT_BLACK);
    
      M5.Lcd.setTextColor(RED);
      M5.Lcd.setTextSize(5);
      M5.Lcd.setCursor(80, 60);
      M5.Lcd.println("RONIN");
      M5.Lcd.setTextColor(YELLOW);
      M5.Lcd.setCursor(55, 180);
      M5.Lcd.println("P");
      M5.Lcd.setCursor(245, 180);
      M5.Lcd.println("T");
    
      M5.update();
    
      Serial.print("\r\n\r\n");
      WiFi.mode(WIFI_STA);
      Serial.println( WiFi.softAPmacAddress() );
      WiFi.disconnect();
      if (esp_now_init() == ESP_OK)
      {
        Serial.println("ESP NOW INIT!");
      }
      else
      {
        Serial.println("ESP NOW INIT FAILED....");
      }
      memcpy( &slave.peer_addr, &remoteMac, 6 );
      slave.channel = WIFI_CHANNEL;
      slave.encrypt = 0;
      if ( esp_now_add_peer(peer) == ESP_OK)
      {
        Serial.println("Added Peer!");
        Serial.println();
      }
      esp_now_register_send_cb(OnDataSent);
    
      ///////////////////////////////////////////////////////////////////////////////
    
      // 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(34, 35);
    
      //////////////////////////////////////////////////////////
      Serial.println("<ready>");
    }
    
    void loop() {
      M5.update();
      ariloop();
      M5.update();
      sendData();
      Serial.print("x");
      Serial.print(pulsesX);
      Serial.print("y");
      Serial.print(pulsesY);
      Serial.println("end");
      delay(20); // PROVARE A TOGLIERLA
    }
    
    void ariloop() {
      pulsesX = encoderX.getCount();
      pulsesY = encoderY.getCount();
    
      timePassed = millis() - timeStampEnd;
      xPassed = xStampEnd - pulsesX;
      yPassed = pulsesY - yStampEnd;
    
      if (M5.BtnC.wasReleased()) {
        gearY ++;
        if (gearY == 4) {
          gearY = 1;
        }
        M5.Lcd.fillRoundRect(25, 170, 270, 50, 25, TFT_BLACK);
        if (toggleUSD) {
          M5.Lcd.setCursor(140, 180);
          M5.Lcd.println("UP");
        }
        M5.Lcd.setCursor(55, 180);
        M5.Lcd.println("P");
        M5.Lcd.setCursor(105, 180);
        M5.Lcd.println(gearX);
        M5.Lcd.setCursor(205, 180);
        M5.Lcd.println("T");
        M5.Lcd.setCursor(255, 180);
        M5.Lcd.println(gearY);
        delay(200);
      } else if (M5.BtnB.wasReleased()) {
        toggleZ = !toggleZ;
        if (toggleZ) {
          M5.Lcd.fillRoundRect(25, 170, 270, 50, 25, TFT_BLACK);
          M5.Lcd.setCursor(55, 180);
          M5.Lcd.println("(");
          M5.Lcd.setCursor(120, 180);
          M5.Lcd.println("ROLL");
          M5.Lcd.setCursor(255, 180);
          M5.Lcd.println(")");
        } else {
          M5.Lcd.fillRoundRect(25, 170, 270, 50, 25, TFT_BLACK);
          if (toggleUSD) {
            M5.Lcd.setCursor(140, 180);
            M5.Lcd.println("UP");
          }
          M5.Lcd.setCursor(55, 180);
          M5.Lcd.println("P");
          M5.Lcd.setCursor(105, 180);
          M5.Lcd.println(gearX);
          M5.Lcd.setCursor(205, 180);
          M5.Lcd.println("T");
          M5.Lcd.setCursor(255, 180);
          M5.Lcd.println(gearY);
          delay(200);
        }
        delay(200);
    
      } else if (M5.BtnA.wasReleased()) {
        gearX ++;
        if (gearX == 4) {
          gearX = 1;
        }
        M5.Lcd.fillRoundRect(25, 170, 270, 50, 25, TFT_BLACK);
        if (toggleUSD) {
          M5.Lcd.setCursor(140, 180);
          M5.Lcd.println("UP");
        }
        M5.Lcd.setCursor(55, 180);
        M5.Lcd.println("P");
        M5.Lcd.setCursor(105, 180);
        M5.Lcd.println(gearX);
        M5.Lcd.setCursor(205, 180);
        M5.Lcd.println("T");
        M5.Lcd.setCursor(255, 180);
        M5.Lcd.println(gearY);
        delay(200);
    
      } else if (M5.BtnB.wasReleasefor(700)) {
        toggleUSD = !toggleUSD;
        if (toggleUSD) {
          M5.Lcd.fillRoundRect(25, 170, 270, 50, 25, TFT_BLACK);
          M5.Lcd.setCursor(140, 180);
          M5.Lcd.println("UP");
          M5.Lcd.setCursor(55, 180);
          M5.Lcd.println("P");
          M5.Lcd.setCursor(105, 180);
          M5.Lcd.println(gearX);
          M5.Lcd.setCursor(205, 180);
          M5.Lcd.println("T");
          M5.Lcd.setCursor(255, 180);
          M5.Lcd.println(gearY);
          delay(200);
        } else {
          M5.Lcd.fillRoundRect(25, 170, 270, 50, 25, TFT_BLACK);
          M5.Lcd.setCursor(55, 180);
          M5.Lcd.println("P");
          M5.Lcd.setCursor(105, 180);
          M5.Lcd.println(gearX);
          M5.Lcd.setCursor(205, 180);
          M5.Lcd.println("T");
          M5.Lcd.setCursor(255, 180);
          M5.Lcd.println(gearY);
          delay(200);
        }
    
      }
    
      if (M5.BtnA.isPressed() && toggleZ) {
        Z = 723;   //823, 1023, 1223
        delay(100);
    
      }   else if (M5.BtnC.isPressed() && toggleZ) {
        Z = 1323;//823, 1023, 1223
        delay(100);
    
      } else {
        Z = 1023;  //823, 1023, 1223
      }
    
      if (gearX == 1) {
        potiX = 100;
      }
      if (gearX == 2) {
        potiX = 200;
      }
      if (gearX == 3) {
        potiX = 300;
      }
    
      if (gearY == 1) {
        potiY = 100;
      }
      if (gearY == 2) {
        potiY = 200;
      }
      if (gearY == 3) {
        potiY = 300;
      }
    
      if (toggleUSD) {
        // GRAFICA UPSIDEDOWN
        potiX = potiX * - 1;
        potiY = potiY * - 1;
      } else {
        // GRAFICA DRITTA
      }
    
      X = 1023 + potiX * xPassed / timePassed;
      Y = 1023 + potiY * yPassed / timePassed;
    
      pulsesX = encoderX.getCount();
      pulsesY = encoderY.getCount();
    
      xStampEnd = pulsesX;
      yStampEnd = pulsesY;
      timeStampEnd = millis();
    
    }
    
    void sendData() {
      if (millis() - lastSentMillis >= sendIntervalMillis) {
        lastSentMillis += sendIntervalMillis;
    
        myData.X = X;
        myData.Y = Y;
        myData.Z = Z;
    
        uint8_t bs[sizeof(myData)];
        memcpy(bs, &myData, sizeof(myData));
        sentMicros = micros();
        esp_now_send(NULL, bs, sizeof(myData)); // NULL means send to all peers
        Serial.println("  sent data");
        Serial.println();
      }
    }
    
    void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status)
    {
      if (status == ESP_NOW_SEND_SUCCESS) {
        Serial.println("RX ACCESO");
        M5.Lcd.fillCircle(60, 60, 10, TFT_GREEN);
        //    StatoRX = 1;
      } else {
        Serial.println("RX SPENTO");
        M5.Lcd.fillCircle(60, 60, 10, TFT_RED);
        //    StatoRX = 0;
      }
      Serial.println();
    }
    

    HARDWARE
    encoders wiring on M5Stack is a bit different of original kinowheels (arduino mega) one.

    0_1590398679133_Screen Shot 2020-05-25 at 11.21.09.png

    with esp32 based board you can invert ttl signal in serial.begin declaration... so no need to add a serial inverter IC for D-Bus, so ATOM GROVE cable go straight to R0nin D-Bus

    GROVE ------ D-BUS
    GRD ---------- Black
    5V ------------ Red
    TX ------------ White
    RX ------------- Yellow (not connected on R0nin1) to test on R0nin2

    this is the code to use M5Stack kinowheels in the simulator:

    #include <M5Stack.h>
    #include <ESP32Encoder.h>
    
    ESP32Encoder encoderX;
    ESP32Encoder encoderY;
    
    float pulsesX, pulsesY;
    
    void setup() {
      M5.begin();
    
      M5.Lcd.clear(BLACK);
    
      M5.Lcd.fillRoundRect(5, 10, 310, 150, 10, TFT_BLACK);
      M5.Lcd.fillRoundRect(15, 20, 290, 130, 5, TFT_YELLOW);
      M5.Lcd.fillRoundRect(25, 30, 270, 110, 55, TFT_BLACK);
    
      M5.Lcd.fillRoundRect(5, 150, 310, 90, 10, TFT_BLACK);
      M5.Lcd.fillRoundRect(15, 160, 290, 70, 5, TFT_YELLOW);
      M5.Lcd.fillRoundRect(25, 170, 270, 50, 25, TFT_BLACK);
    
      M5.Lcd.setTextColor(RED);
      M5.Lcd.setTextSize(5);
      M5.Lcd.setCursor(130, 40);
      M5.Lcd.println("PC");
      M5.Lcd.setTextColor(YELLOW);
      M5.Lcd.setCursor(55, 180);
      M5.Lcd.println("T");
      M5.Lcd.setCursor(245, 180);
      M5.Lcd.println("P");
    
      M5.update();
    
      Serial.begin(115200);
    
      // 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(34, 35);
    
    }
    
    void loop() {
    
      pulsesX = encoderX.getCount();
      pulsesY = encoderY.getCount();
    
      Serial.print("x");
      Serial.print(pulsesX);
      Serial.print("y");
      Serial.print(pulsesY);
      Serial.println("end");
      delay(20);
    }
    

    I want to use this case in the future...
    NaKino



  • RESERVED



  • @cepics

    Do you have a photo of the wiring? I don't understand the wiring diagram well. Thank you.