Connect esp32 to X4 and X3 boards

Hello,

I managed to control the X4 board remotely with the help of the Adafruit ESP32 Feather. I als have the gripperbot with the X3 board. I can find both boards when using Totem.BLE.findRobot(onFoundRobot). When I connect to one the search stops. Is there a chance I can connect to both the X3 and X4 boards at the same time???

Thanks

Yes. When connected to the first board - call Totem.BLE.findRobot() again to connect another one. Search will always stop when trying to establish a connection.
This function returns TotemRobot instance, to represent individual connection.
Something like this:

TotemRobot board1 = Totem.BLE.findRobot();
Serial.print("Connected 1 to: ");
Serial.println(board1.getName());
TotemRobot board2 = Totem.BLE.findRobot();
Serial.print("Connected 2 to: ");
Serial.println(board2.getName());

There is also “non-blocking” function if you require more flexibility Totem.BLE.findRobotNoBlock() (but scan will autostop also).

Also there is an example where multiple Totem robots (boards) are connected: TotemArduino/PS4_Dual_MiniTrooper.ino at master · totemmaker/TotemArduino · GitHub

Hello,

When I use the following program I get the following result where onFoundRobot finds 2 robots: X3 and X4 but setup only sees one of them. And only one of them is responding to the statements in the loop. At one moment X3 board and on another X4 board.

What am I doing wrong??

Thanks Gerard

09:10:19.694 → onFoundRobot found: Totem X4
09:10:21.420 → onFoundRobot found: Totem X3
09:10:21.624 → setup: found: Totem X3
09:10:21.624 → setup: found: Totem X3
09:10:21.624 → connect myRobot1
09:10:22.286 → connect myRobot2
09:10:22.286 → connect done
09:10:22.286 → gripperbot
09:10:27.308 → truck
09:10:31.309 → gripperbot
09:10:34.625 → lld_pdu_get_tx_flush_nb HCI packet count mismatch (10, 11)
09:10:36.309 → truck

#include <Totem.h>

class Truck
{
    MotorDriver driver;
  public:
    Truck()
    {
      driver.addRearLeft("motorB", 58, 100);
      driver.addRearRight("motorA", 58, 100, true);
      driver.addServo(0, "servoA", -30, 0, 50, true);
    }
    void move(int drivePower, int brakePower) 
    {
      driver.move(drivePower);
      driver.brakeAll(brakePower);
      if (drivePower < 0)
          setLed(brakePower != 0 ? ReverseBrake : Reverse);
      else
          setLed(brakePower != 0 ? Brake : Idle);
    }
    void steer(int position) 
    {
      driver.moveServo(0, position);
    }
private:
    enum LedState 
    {
        Idle,
        Brake,
        Reverse,
        ReverseBrake
    };
    void setLed(LedState state) 
    {
      static LedState lastLedState = Idle;
      if (state == lastLedState) return;
      switch (state) 
      {
        case Idle:
            driver.getModule().write("rgbAll/reset");
            break;
        case Brake:
            driver.getModule().write("rgbAll", 255, 255, 0, 0); // Set to red
            break;
        case Reverse:
            driver.getModule().write("rgbAll", 255, 255, 255, 255); // Set to white
            break;
        case ReverseBrake:
            driver.getModule().write("rgbA", 255, 255, 255, 255); // Set led A to white
            driver.getModule().write("rgbB", 255, 255, 0, 0); // Set led B to red
            driver.getModule().write("rgbC", 255, 255, 0, 0); // Set led C to red
            driver.getModule().write("rgbD", 255, 255, 255, 255); // Set led D to white
            break;
      }
      lastLedState = state;
    }
} truck;

class GripperBot 
{
    MotorDriver driver;
  public:
    int gripperPos = 0;
    GripperBot() 
    {
      driver.addFrontLeft("motorA", 9, 100, true);
      driver.addFrontRight("motorB", 9, 100);
      driver.setTurnIntensity(70);
      driver.addServo(0, "servoA", 10, 55, 94); // Gripper
      driver.addServo(1, "servoB", -70, 50, 90, true); // Arm
    }
    void setArm(int position) 
    {
      driver.moveServo(1, position);
      position = constrain(position, -100, 100);
      position = map(position, -100, 100, 0, 11);
      setLightBar((1<<position));
    }
    void setGripper(int position) 
    {
      driver.moveServo(0, position);
      gripperPos = constrain(position, -100, 100);
    }
    void setLightBar(int ledBits) 
    {
      static int lastBitSet = 0;
      if (lastBitSet == ledBits) return;
      driver.getModule().write("leds", ledBits);
      lastBitSet = ledBits;
    }
    void move(int drive, int turn) 
    {
      driver.move(drive, turn);
    }
} gripperBot;

TotemRobot myRobots[10];
int myRobotsPointer;
bool X3Found = false;
bool X4Found = false;

TotemModule moduleX3(03);
TotemModule moduleX4(04);

void onFoundRobot(TotemRobot robot)
{
  if ((robot.getName() == "Totem X3") && !X3Found)
  {
    myRobots[0] = robot;
    X3Found = true;
    myRobotsPointer++;
    Serial.println("onFoundRobot found: " + myRobots[0].getName());
  }
  if  ((robot.getName() == "Totem X4") && !X4Found)
  {
    myRobots[1] = robot;
    X4Found = true;
    myRobotsPointer++;
    Serial.println("onFoundRobot found: " + myRobots[1].getName());
  }
}

void setup() 
{
    myRobotsPointer = 0;

    Serial.begin(115200);
    Serial.println("\n\n\n");

    Totem.BLE.begin(); 
    Totem.BLE.findRobotNoBlock(onFoundRobot);
    // Wait until I found 2 robots: X3 and X4
    while (myRobotsPointer < 2)
    {
      delay(1000);
    }
    // Result
    Serial.println("setup: found: " + myRobots[0].getName());
    Serial.println("setup: found: " + myRobots[1].getName());
    Serial.println("connect myRobot1");
    myRobots[0].connect();
    Serial.println("connect myRobot2");
    myRobots[1].connect();
    Serial.println("connect done");
}

void loop() 
{
  Serial.println("gripperbot");
  gripperBot.move(0, 0); // Stop
  delay(300);
  gripperBot.setArm(90); // Extend arm
  delay(500);
  gripperBot.setGripper(100); // Close gripper
  delay(500);
  gripperBot.setArm(-60); // Retract arm
  delay(500);
  gripperBot.move(0, 70); // Turn right
  delay(700);
  gripperBot.move(0, 0); // Stop
  delay(300);
  gripperBot.setArm(90); // Extend arm
  delay(500);
  gripperBot.setGripper(-60); // Open gripper
  delay(500);
  gripperBot.setArm(-60); // Retract arm
  delay(500);
  gripperBot.move(0, 70); // Turn right
  delay(700);

  Serial.println("truck");
  truck.steer(-40); // Steer left to max angle
  delay(500); // Wait 500ms
  truck.move(30, 0); // Drive forward at 30% power
  delay(1000);
  truck.move(0, 100); // Stop drive and brake
  delay(500);
  truck.steer(40); // Steer right to max angle
  delay(500);
  truck.move(-30, 0); // Drive backward at 30% power
  delay(1000);
  truck.move(0, 100); // Stop drive and brake
  delay(500);

}

*edit. Fixed formatting

Few things to point out:

  • I have released Totem Library v1.1.2 to solve some API issues. Make sure to update.
  • onFoundRobot function works as a filter to select which robot should be connected. It should only validate “robot” and call connect(). TotemRobot instance is returned by Totem.BLE.findRobot() function when connection is made.
  • Gripper bot motors (by default) are connected to C and D ports, but A and B is selected in the code. Make sure it matches your robot.
  • Also fixed truck control example. Wheels were not spinning.
  • When connecting multiple robots - you need to join TotemRobot instance with TotemModule (it’s inside MotorDriver class). This is done automatically if only single one is connected. TotemRobot represents connection while TotemModule provides control over selected connection.

Edited your example code:

#include <Totem.h>

class Truck
{
    MotorDriver driver;
  public:
    Truck()
    {
      driver.addRearLeft("motorB", 58, 100);
      driver.addRearRight("motorA", 58, 100, true);
      driver.addServo(0, "servoA", -30, 0, 50, true);
    }
    void drive(int drivePower) {
        driver.brakeAll(0);
        driver.move(drivePower);
        setLed(drivePower < 0 ? Reverse : Idle);
    }
    void brake(int brakePower = 100) {
        driver.brakeAll(brakePower);
        setLed(lastLedState == Reverse ? ReverseBrake : Brake);
    }
    void steer(int position) 
    {
      driver.moveServo(0, position);
    }
    void attachTo(TotemRobot &robot) {
        robot.attach(driver.getModule());
    }
private:
    enum LedState 
    {
        Idle,
        Brake,
        Reverse,
        ReverseBrake
    };
    LedState lastLedState = Idle;
    void setLed(LedState state) 
    {
      if (state == lastLedState) return;
      switch (state) 
      {
        case Idle:
            driver.getModule().write("rgbAll/reset");
            break;
        case Brake:
            driver.getModule().write("rgbAll", 255, 255, 0, 0); // Set to red
            break;
        case Reverse:
            driver.getModule().write("rgbAll", 255, 255, 255, 255); // Set to white
            break;
        case ReverseBrake:
            driver.getModule().write("rgbA", 255, 255, 255, 255); // Set led A to white
            driver.getModule().write("rgbB", 255, 255, 0, 0); // Set led B to red
            driver.getModule().write("rgbC", 255, 255, 0, 0); // Set led C to red
            driver.getModule().write("rgbD", 255, 255, 255, 255); // Set led D to white
            break;
      }
      lastLedState = state;
    }
} truck;

class GripperBot 
{
    MotorDriver driver;
  public:
    int gripperPos = 0;
    GripperBot() 
    {
      driver.addFrontLeft("motorA", 9, 100, true);
      driver.addFrontRight("motorB", 9, 100);
      driver.setTurnIntensity(70);
      driver.addServo(0, "servoA", 10, 55, 94); // Gripper
      driver.addServo(1, "servoB", -70, 50, 90, true); // Arm
    }
    void setArm(int position) 
    {
      driver.moveServo(1, position);
      position = constrain(position, -100, 100);
      position = map(position, -100, 100, 0, 11);
      setLightBar((1<<position));
    }
    void setGripper(int position) 
    {
      driver.moveServo(0, position);
      gripperPos = constrain(position, -100, 100);
    }
    void setLightBar(int ledBits) 
    {
      static int lastBitSet = 0;
      if (lastBitSet == ledBits) return;
      driver.getModule().write("leds", ledBits);
      lastBitSet = ledBits;
    }
    void move(int drive, int turn) 
    {
      driver.move(drive, turn);
    }
    void attachTo(TotemRobot &robot) {
        robot.attach(driver.getModule());
    }
} gripperBot;

TotemRobot myRobots[10];

void filterTotemX3(TotemRobot robot)
{
  Serial.println("filterTotemX3: found: " + robot.getName());
  if (robot.getName() == "Totem X3")
    robot.connect();
}

void filterTotemX4(TotemRobot robot)
{
  Serial.println("filterTotemX4: found: " + robot.getName());
  if (robot.getName() == "Totem X4")
    robot.connect();
}

void setup() 
{
    Serial.begin(115200);
    Serial.println("\n\n\n");

    Totem.BLE.begin(); 
    // Connect Totem X3
    Serial.println("setup: Scanning for Totem X3...");
    myRobots[0] = Totem.BLE.findRobot(filterTotemX3);
    if (myRobots[0].isConnected())
      Serial.println("setup: Connected");
    else
      Serial.println("setup: Failed!");
    // Connect Totem X4
    Serial.println("setup: Scanning for Totem X4...");
    myRobots[1] = Totem.BLE.findRobot(filterTotemX4);
    if (myRobots[1].isConnected())
      Serial.println("setup: Connected");
    else
      Serial.println("setup: Failed!");
    // Result
    Serial.println("setup: found: " + myRobots[0].getName());
    Serial.println("setup: found: " + myRobots[1].getName());
    Serial.println("connect done");
    // Attach robot connections to their control Modules
    gripperBot.attachTo(myRobots[0]);
    truck.attachTo(myRobots[1]);
}

void loop() 
{
  Serial.println("gripperbot");
  gripperBot.move(0, 0); // Stop
  delay(300);
  gripperBot.setArm(90); // Extend arm
  delay(500);
  gripperBot.setGripper(100); // Close gripper
  delay(500);
  gripperBot.setArm(-60); // Retract arm
  delay(500);
  gripperBot.move(0, 70); // Turn right
  delay(700);
  gripperBot.move(0, 0); // Stop
  delay(300);
  gripperBot.setArm(90); // Extend arm
  delay(500);
  gripperBot.setGripper(-60); // Open gripper
  delay(500);
  gripperBot.setArm(-60); // Retract arm
  delay(500);
  gripperBot.move(0, 70); // Turn right
  delay(700);

  Serial.println("truck");
  truck.steer(-40); // Steer left to max angle
  delay(500); // Wait 500ms
  truck.drive(30); // Drive forward at 30% power
  delay(1000);
  truck.brake(); // Stop drive and brake
  delay(500);
  truck.steer(40); // Steer right to max angle
  delay(500);
  truck.drive(-30); // Drive backward at 30% power
  delay(1000);
  truck.brake(); // Stop drive and brake
  delay(500);

}

Arnas,

Code is working fine. Thanks for your input.

Gerard