Stephanie VicarteElizabeth VicarteIrak Mayer
Published © GPL3+

Robot Te. Dynamixel, Arduino, ESP32, and BLE Robot Arm

A robot arm that uses the Dynamixel Arduino servo shield by a BLE connection thru an ESP32 and an Adafruit joystick bonnet.

IntermediateFull instructions provided8 hours264

Things used in this project

Hardware components

Robotis - DYNAMIXEL Shield
×1
Adafruit HUZZAH32 – ESP32 Feather Board
Adafruit HUZZAH32 – ESP32 Feather Board
×1
Arduino 101 & Genuino 101
Arduino 101 & Genuino 101
×1
Adafruit Joy FeatherWing for all Feathers
×1
Adafruit FeatherWing OLED - 128x32 OLED Add-on For Feather
×1
Adafruit FeatherWing Tripler Mini Kit - Prototyping Add-on For Feathers
×1
Dynamixel servos
×1
Interbotix 6 Port AX/MX Power Hub
×1

Story

Read more

Schematics

Feather HUZZAH32, Featherwing OLED, and Featherwing Joy

The connections are manage by the triple featherwing shield

Side View

Arduino 101 and Robotis Dynamixel Shield

The shield snaps over the Arduino 101. From the shield a connection is done to the Dynamixel HUB.

Side view. Arduino 101 and Robotis Dynamixel Shield

Code

RobTeJoystick.ino

Arduino
This program creates an BLE UART service to transmit the joystick and button data to a BLE client.
// RobTeJoystick.ino
//4/20/2020
//Vistelilabs (www.vistelilabs.com)
//This program creates an BLE UART service to transmit the joystick and button data to a BLE client.
// Copyright <2020> <VisteliLabs>
//Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
//The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
//THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.


#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <SPI.h>
#include "Adafruit_seesaw.h"

Adafruit_seesaw ss;

#define BUTTON_RIGHT 6
#define BUTTON_DOWN  7
#define BUTTON_LEFT  9
#define BUTTON_UP    10
#define BUTTON_START   14
uint32_t button_mask = (1 << BUTTON_RIGHT) | (1 << BUTTON_DOWN) | 
                (1 << BUTTON_LEFT) | (1 << BUTTON_UP) | (1 << BUTTON_START);
#if defined(ESP8266)
  #define IRQ_PIN   2
#elif defined(ESP32)
  #define IRQ_PIN   14
#elif defined(ARDUINO_NRF52832_FEATHER)
  #define IRQ_PIN   27
#elif defined(TEENSYDUINO)
  #define IRQ_PIN   8
#elif defined(ARDUINO_ARCH_WICED)
  #define IRQ_PIN   PC5
#else
  #define IRQ_PIN   5
#endif

Adafruit_SSD1306 display = Adafruit_SSD1306(128, 32, &Wire);

BLEServer *pServer = NULL;
BLECharacteristic * pTxCharacteristic;
bool deviceConnected = false;
bool oldDeviceConnected = false;
uint8_t txValue = 0;

#define SERVICE_UUID           "8907022f-2a69-4dc4-9b98-85b1356ef422" // UART service UUID
#define CHARACTERISTIC_UUID_RX "42f1162a-4dea-4901-97ca-9cca75a1e8a3"
#define CHARACTERISTIC_UUID_TX "581108e2-6a9a-40d3-9d5f-db6f7529aa6a"


class MyServerCallbacks: public BLEServerCallbacks {
    void onConnect(BLEServer* pServer) {
      deviceConnected = true;
    };

    void onDisconnect(BLEServer* pServer) {
      deviceConnected = false;
    }
};

class MyCallbacks: public BLECharacteristicCallbacks {
    void onWrite(BLECharacteristic *pCharacteristic) {
      std::string rxValue = pCharacteristic->getValue();

      if (rxValue.length() > 0) {
        Serial.println("*********");
        Serial.print("Received Value: ");
        for (int i = 0; i < rxValue.length(); i++)
          Serial.print(rxValue[i]);

        Serial.println();
        Serial.println("*********");
      }
    }
};


void BLESetup()
{
  // Create the BLE Device
  BLEDevice::init("ROBOTTE Service");

  // Create the BLE Server
  pServer = BLEDevice::createServer();
  pServer->setCallbacks(new MyServerCallbacks());

  // Create the BLE Service
  BLEService *pService = pServer->createService(SERVICE_UUID);

  // Create a BLE Characteristic
  pTxCharacteristic = pService->createCharacteristic(
                    CHARACTERISTIC_UUID_TX,
                    BLECharacteristic::PROPERTY_NOTIFY
                  );
                      
  pTxCharacteristic->addDescriptor(new BLE2902());

  BLECharacteristic * pRxCharacteristic = pService->createCharacteristic(
                       CHARACTERISTIC_UUID_RX,
                      BLECharacteristic::PROPERTY_WRITE
                    );

  pRxCharacteristic->setCallbacks(new MyCallbacks());

  // Start the service
  pService->start();

  // Start advertising
  pServer->getAdvertising()->start();
  
}

void setup()   {
  //while (!Serial);
  Serial.begin(115200);

  if(!ss.begin(0x49)){
    Serial.println("ERROR!");
    while(1);
  }
  else{
    Serial.println("seesaw started");
    Serial.print("version: ");
    Serial.println(ss.getVersion(), HEX);
  }
  ss.pinModeBulk(button_mask, INPUT_PULLUP);
  ss.setGPIOInterrupts(button_mask, 1);
  pinMode(IRQ_PIN, INPUT);

  display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // Address 0x3C for 128x32

  Serial.println("Initialized");
  delay(500);

  // text display tests
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0,0);

  display.clearDisplay();
  display.display();

  BLESetup();
}

int last_x = 0, last_y = 0;
//msg contains the formatted commands.
char msg[20];

void loop() {
  int y = ss.analogRead(2);
  int x = ss.analogRead(3);
  
  display.setCursor(0,0);

  sprintf(msg,"");
  
  if ( (abs(x - last_x) > 3)  ||  (abs(y - last_y) > 3)) {
    sprintf(msg,"                    ");
    display.clearDisplay();
    display.display();
    display.setCursor(0,0);
    
    sprintf(msg,"(%03d,%03d)",x,y);
    display.println(msg);
  }
  else
  {
    display.println("");
  }

  if ( (abs(x - last_x) > 3)  ||  (abs(y - last_y) > 3)) {
    last_x = x;
    last_y = y;
  }
 
  if(!digitalRead(IRQ_PIN)){
    uint32_t buttons = ss.digitalReadBulk(button_mask);
    if (! (buttons & (1 << BUTTON_DOWN))) {
      display.println("B");
      sprintf(msg,"cmd:B          ");
    }
    
    if (! (buttons & (1 << BUTTON_RIGHT))) {
      display.println("A");
      sprintf(msg,"cmd:A          ");
    }
    
    if (! (buttons & (1 << BUTTON_LEFT))) {
      display.println("Y");
      sprintf(msg,"cmd:Y          ");
    }
    
    if (! (buttons & (1 << BUTTON_UP))) {
      display.println("X");
      sprintf(msg,"cmd:X          ");
    }
    
    if (! (buttons & (1 << BUTTON_START))) {
      display.println("START");
      sprintf(msg,"cmd:START          ");
    }
  }
  display.display();  
  delay(10);

    if (deviceConnected && strlen(msg) > 0) {
        pTxCharacteristic->setValue(msg); 
        pTxCharacteristic->notify();
        delay(10); // bluetooth stack will go into congestion, if too many packets are sent
  }  
}

RobotTe.ino

Arduino
This program connects to the BLE UART service and process the data received to control a robot arm.
// RobTeJoystick.ino
//4/20/2020
//Vistelilabs (www.vistelilabs.com)
//This program connects to the BLE UART service and process the data received to control a robot arm.
// Copyright <2020> <VisteliLabs>
//Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
//The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
//THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

#include <Dynamixel2Arduino.h>
#include <CurieBLE.h>

#define DXL_SERIAL   Serial1
#define DEBUG_SERIAL Serial
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
 

const uint8_t HAND_ID = 9; 
const uint8_t WRIST_ID = 11;
const uint8_t ELBOW_ID = 12;
const uint8_t SHOULDER_ID = 3;
const float DXL_PROTOCOL_VERSION = 1.0; 

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);

#define CMD_IDDLE       0
#define CMD_OPEN_HAND   1
#define CMD_CLOSE_HAND  2
#define CMD_WRIST_HAND  4
#define CMD_ELBOW_SHOULDER 8


bool StopFlag = false;
int cmdData = CMD_IDDLE;
int lastCmdData = CMD_IDDLE;
char *cmdStrCoordX;
char *cmdStrCoordY;
int cmdCoordX;
int cmdCoordY;
bool newData=false;
BLEDevice joystickPeripheral;

void UpdateValue(BLEDevice bledev, BLECharacteristic characteristic)
{
  char *data;
  data = (char *)characteristic.value();
  Serial.println(data);
  if (strstr(data,"START"))
  {
    StopFlag = true;
    characteristic.unsubscribe();
  }
  if (strstr(data,"cmd:B"))
  {
    if (lastCmdData != CMD_OPEN_HAND || lastCmdData != CMD_CLOSE_HAND)
    {
      lastCmdData = cmdData;
    }
    cmdData = CMD_CLOSE_HAND;
  }
  if (strstr(data,"cmd:X"))
  {
    if (lastCmdData != CMD_OPEN_HAND || lastCmdData != CMD_CLOSE_HAND)
    {
      lastCmdData = cmdData;
    }
    cmdData = CMD_OPEN_HAND;
  }
  if (strstr(data,"cmd:Y"))
  {
    cmdData = CMD_WRIST_HAND;
  }
  if (strstr(data,"cmd:A"))
  {
    cmdData = CMD_ELBOW_SHOULDER;
  }
  if (strstr(data,","))
  {
    cmdStrCoordX = strtok(data,",");
    cmdCoordX = atoi(cmdStrCoordX+1);
    cmdStrCoordY = strtok(NULL,")");
    cmdCoordY = atoi(cmdStrCoordY);
    newData = true;
  }
}

bool connect2Joystick()
{
  bool notFound = true;
  // initialize the BLE hardware
  BLE.begin();

  Serial.println("BLE Central - Peripheral Explorer");

  // start scanning for peripherals
  BLE.scan();
  
  while (notFound)
  {
    BLEDevice peripheral = BLE.available();
  
    if (peripheral) {
      // discovered a peripheral, print out address, local name, and advertised service
      Serial.print("Found ");
      Serial.print(peripheral.address());
      Serial.print(" '");
      Serial.print(peripheral.localName());
      Serial.print("' ");
      Serial.print(peripheral.advertisedServiceUuid());
      Serial.println();
  
      // see if peripheral is our UART service
      if (peripheral.localName() == "ROBOTTE Service") {
        // stop scanning
        BLE.stopScan();
  
        // connect to the peripheral
        if (!peripheral.connect()) {
          Serial.println("Failed to connect!");
          return false;
        }
      
        if (!peripheral.discoverAttributes()) {
          Serial.println("Attribute discovery failed!");
          peripheral.disconnect();
          return false;
        }
      
        // retrieve the LED characteristic
        BLECharacteristic notifyCharacteristic = peripheral.characteristic("581108e2-6a9a-40d3-9d5f-db6f7529aa6a");
      
        if (!notifyCharacteristic) {
          Serial.println("Peripheral does not have NOTIFY characteristic!");
          peripheral.disconnect();
          return false;
        } else 
        {
          if (notifyCharacteristic.canNotify())
          {
            notifyCharacteristic.subscribe();
            notifyCharacteristic.setEventHandler(BLEValueUpdated,UpdateValue);
            joystickPeripheral = peripheral;
          }
        }  
        
        notFound = false;
      }
    }
  }

  return(notFound);
}

void setup() {
  // Use UART port of DYNAMIXEL Shield to debug.
  DEBUG_SERIAL.begin(115200);
  //while (!DEBUG_SERIAL)
    //delay(10);

  connect2Joystick();
   
  // This has to match with DYNAMIXEL baudrate.
  dxl.begin(1000000);
  // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
  // Get DYNAMIXEL information
  dxl.ping(HAND_ID);
  dxl.ping(WRIST_ID);
  dxl.ping(ELBOW_ID);
  dxl.ping(SHOULDER_ID);

  dxl.torqueOff(HAND_ID);
  dxl.setOperatingMode(HAND_ID, OP_POSITION);
  dxl.torqueOn(HAND_ID);

  dxl.torqueOff(WRIST_ID);
  dxl.setOperatingMode(WRIST_ID, OP_POSITION);
  dxl.torqueOn(WRIST_ID);

  dxl.torqueOff(ELBOW_ID);
  dxl.setOperatingMode(ELBOW_ID, OP_POSITION);
  dxl.torqueOn(ELBOW_ID);

  dxl.torqueOff(SHOULDER_ID);
  dxl.setOperatingMode(SHOULDER_ID, OP_POSITION);
  dxl.torqueOn(SHOULDER_ID);

}

void loop() {
  bool contLoop = true;
  
  while (joystickPeripheral.connected() && contLoop) {
    if (StopFlag)
    {
      contLoop = false;
      continue;
    }

    switch(cmdData)
    {
      case CMD_OPEN_HAND:
      {
          Serial.println("OPEN HAND");
          // Set Goal Position in RAW value
          dxl.setGoalPosition(HAND_ID, 512);
          cmdData = lastCmdData;
      }
      break;
      case CMD_CLOSE_HAND:
      {
          Serial.print("CLOSE HAND");
          // Set Goal Position in DEGREE value
          dxl.setGoalPosition(HAND_ID, 5.7, UNIT_DEGREE);
          cmdData = lastCmdData;
      }
      break;
      case CMD_WRIST_HAND:
      {
          // Check if new data is available
          if (newData)
          {
            dxl.setGoalPosition(WRIST_ID, cmdCoordX);
            newData = false;
          }        
      }
      break;
      case CMD_ELBOW_SHOULDER:
      {
          // Check if new data is available
          if (newData)
          {
            dxl.setGoalPosition(SHOULDER_ID, cmdCoordX);
            newData = false;
            if (cmdCoordY > 128 && cmdCoordY < 512)
            {
              dxl.setGoalPosition(ELBOW_ID, cmdCoordY);
              delay(250);
              newData = false;
            }
          }        
      }
      break;
    }
  }

  joystickPeripheral.disconnect();
}

Credits

Stephanie Vicarte

Stephanie Vicarte

14 projects • 8 followers
Elizabeth Vicarte

Elizabeth Vicarte

11 projects • 4 followers
Irak Mayer

Irak Mayer

14 projects • 5 followers

Comments