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