KinoWheels M5Stack porting WIP...

cepics

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

tested with:
R0nin 1

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

cepics

RESERVED

fonzie

@cepics

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

gunnar lindner

This post is deleted!

StarLog

Has anyone gotten this setup to work. Having issues with the Mac address setup, need help.
Thanks