T3.x/T4 - Half Duplex support...

Status
Not open for further replies.

KurtE

Senior Member+
I know there have been a few different conversations about this in different threads, but as I testing out some stuff on T4 again, and ran into the issues, I thought I would mention it again, and see how much if any support we might want to build into core...

For me most of my experience on Teensy needing/wanting to do Half duplex Serial, revolves around support for Dynamixel Servos by Robotis.

Often times I support them, by using some form of level shifter/buffer to convert the RX/TX signals into half duplex output at 5V.

But other times I would prefer to use the built in support for Half duplex.

For T3.x with my library bioloidSerial (based off of original bioloid library used at Trossen), I had it setup to work either way. If you defined a TransmitEnable pin it used the built in support, which is great, else it configured the Uart to be in half duplex mode, and it has to manually know when you are going to do TX and change an internal register of the UART, and then to switch back to RX mode, it has do a flush and then muck with that register again. Would be great if we could simply have the HardwareSerial code handle this for us. This could remove the library or sketch developer from having to know too much about the internals of the UART code...

There are some new additional wrinkles with T4...
To sort of demonstrate it, I hacked up a version of the example code I did that sub-classed the Port handling code that is part of the Dynamixel2Arduino library, to work with T4 on Serial3 in half duplex mode. Warning, this it not pretty! I am also working with some of the Robotis people on updates to their new library Dynamixel2Arduino...

Code:
#include <Dynamixel2Arduino.h>

// Kurt's T4-T36 board
#define TRY_HALF_DUPLEX
#ifdef TRY_HALF_DUPLEX
// T4 test
#define DXL_SERIAL   Serial3
[COLOR="#FF0000"]IMXRT_LPUART_t *s_pkuart = &IMXRT_LPUART2;[/COLOR]
#define DXL_TX_PIN 14  // 
const int DXL_DIR_PIN = -1;
#else
const int DXL_DIR_PIN = 6; // OpenCR Board's DIR PIN.
#endif

class NewSerialPortHandler : public DYNAMIXEL::SerialPortHandler
{
  public:
    NewSerialPortHandler(HardwareSerial& port, const int dir_pin = -1)
      : SerialPortHandler(port, dir_pin), port_(port), dir_pin_(dir_pin)
    {}
    virtual size_t write(uint8_t c) override
    {
      size_t ret = 0;
#ifdef TRY_HALF_DUPLEX
      if (s_pkuart) s_pkuart->CTRL |= LPUART_CTRL_TXDIR;
      else digitalWrite(dir_pin_, HIGH);
#else
      digitalWrite(dir_pin_, HIGH);
#endif
      ret = port_.write(c);

      port_.flush();
#ifdef TRY_HALF_DUPLEX
      if (s_pkuart) s_pkuart->CTRL &= ~LPUART_CTRL_TXDIR;
      else digitalWrite(dir_pin_, LOW);
#else
      digitalWrite(dir_pin_, LOW);
#endif
      return ret;
    }

    virtual size_t write(uint8_t *buf, size_t len) override
    {
      size_t ret;
#ifdef TRY_HALF_DUPLEX
     [COLOR="#FF0000"] if (s_pkuart) s_pkuart->CTRL |= LPUART_CTRL_TXDIR;[/COLOR]
      else digitalWrite(dir_pin_, HIGH);
#else
      digitalWrite(dir_pin_, HIGH);
#endif
      ret = port_.write(buf, len);

      port_.flush();
#ifdef TRY_HALF_DUPLEX
      [COLOR="#FF0000"]if (s_pkuart) s_pkuart->CTRL &= ~LPUART_CTRL_TXDIR;[/COLOR]
      else digitalWrite(dir_pin_, LOW);
#else
      digitalWrite(dir_pin_, LOW);
#endif
      return ret;
    }

  private:
    HardwareSerial& port_;
    const int dir_pin_;
};

Dynamixel2Arduino dxl;
NewSerialPortHandler dxl_port(DXL_SERIAL, DXL_DIR_PIN);
uint8_t id = 0xff;
float     goal_pos = 512.0;
float     max_delta = 128.0;
float     incr = 1.0;
float     center_pos;


void setup() {
  // put your setup code here, to run once:

  // Use Serial to debug.
  while (!Serial && millis() < 5000) ;
  Serial.begin(115200);

  // Set Port instance
  dxl.setPort(dxl_port);
  // Set Port baudrate to 1000000. This has to match with DYNAMIXEL baudrate.
  // BUGBUG: probably this begin should be virtual as well
  dxl.begin(1000000);
#ifdef TRY_HALF_DUPLEX
  if (DXL_DIR_PIN == -1) {
    Serial.println("Try changing Serial to half duplex");
    // Lets setup that IO pin to be in half duplex
  [COLOR="#FF0000"]  s_pkuart->CTRL |= LPUART_CTRL_LOOPS | LPUART_CTRL_RSRC;

    // Lets try to enable PU resistor on that pin...
    *(portControlRegister(DXL_TX_PIN)) = IOMUXC_PAD_DSE(7) | IOMUXC_PAD_PKE | IOMUXC_PAD_PUE | IOMUXC_PAD_PUS(3) | IOMUXC_PAD_HYS;
    IOMUXC_LPUART2_TX_SELECT_INPUT = 1; // need to get to right one...[/COLOR]
  }
#endif

  // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
  Serial.println("Setup completed");
  Serial.flush();
}

uint8_t findServo() {
  Serial.println("Search for a Servo");
  Serial.println("First try Protocol 1");
  dxl.setPortProtocolVersion(1.0);
  for (uint8_t servo_id = 0; servo_id < 253; servo_id++) {
    if (dxl.ping(servo_id) == true) {
      Serial.printf("Found ID:%u Model Number:%u\n", servo_id, dxl.getModelNumber(servo_id));
      center_pos = goal_pos = 512.0;
      max_delta = 128.0;
      incr = 1.0;
      return servo_id;
    }
  }
  Serial.println("Now try Protocol 2");
  dxl.setPortProtocolVersion(2.0);
  for (uint8_t servo_id = 0; servo_id < 253; servo_id++) {
    if (dxl.ping(servo_id) == true) {
      Serial.printf("Found ID:%u Model Number:%u\n", servo_id, dxl.getModelNumber(servo_id));
      center_pos = goal_pos = 2048.0;
      max_delta = 256.0;
      incr = 2.0;
      return servo_id;
    }
  }
  Serial.println("No Servos found");
  return 0xff;
}

void loop() {
  if (id == 0xff) {
    id = findServo();
    if (id == 0xff) {
      delay(1000);
      return;
    }
    dxl.torqueOn(id);
  }
  goal_pos += incr;
  if (goal_pos >= (center_pos + max_delta)) incr = -incr;
  if (goal_pos <= (center_pos - max_delta)) incr = -incr;
  dxl.setGoalPosition(id, goal_pos);
  if (Serial.available()) {
    Serial.println("Paused");
    while (Serial.read() != -1);
    while (Serial.read() == -1);
    while (Serial.read() != -1);
  }
}

I marked some of the lines in RED, that show some of the hacking/knowledge that one needs to get some of these things working.. Things like:

a) You need to know that it is a T4 and that Serial3 uses LPUART2 (and pointer to registers associated)
b) You need to know which register to change to LOOPS and RSRC bits in...
c) How to probably turn on Pull UP resistor on the TX pin. First off how does library know what is the TX PIN?
d) New complication with T4 - You may need to know that the TX pin for the UART has an Input select register which may have to be set.
e) You can currently use the automatic switch to outputSelect mode, so you have to know to turn on output and be able to control when it goes back to input...

It would be great if we could some add a new member method and/or special case a current one to allow us to do most if not all of this internal to HardwareSerial.

Example Suppose I say something like: Serial3.transmitterEnable(0xff);
Which might signal go into half duplex mode... Or if the pin passed in is same as TX pin signals or ???

Obviously this HardwareSerial code has (or could have) all of the knowledge it needs
a) knows which UART it is using, b) can do here c) dito d) I have fields in tables that can be used. Some are not filled in as no uses yet, but could.

e) Would take a little work in write functions to detect this case and set the TXDIR bit and when queue is empty clear the TXDIR bit...

Thoughts? Should we do? What API?

Should I submit part or all of this as an API, or APIs?

Thanks
Kurt
 
Status
Not open for further replies.
Back
Top