Project tutorial

Robot Te. Dynamixel, Arduino, ESP32, and BLE Robot Arm © GPL3+

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

  • 1,744 views
  • 0 comments
  • 7 respects

Components and supplies

About this project

Introduction

Robotics have being a huge field of interested for our team, and this is the first time we have the chance to use a set with an independent micro-controller thanks to the Dynamixel shield.

The project although nothing new or original is intended as a user guide to new robotics enthusiasts with access to the Bioloid kit, and as a proof that things can be achieve even with basic robotics sets.

The design can be extended to work as a social distancing for clerks on banks or federal offices to receive documents without direct physical contact and preserving the required distance.

Materials

We use the Robotis Dynamixel Shield and libraries to control the AX series servos from Dynamixel, connected to an Arduino 101 board. The control interface is done using an Adafruit Feather HUZZA32 and an Adafruit Joy Featherwing. We use a total of 4 dynamixel servos, three AX-18A and one AX-12A, and the Bioloid frame parts to build the arm.

Other components are shown in the below snapshots.

Dynamixel Shield - Robotis

We use the startup guide from Robotis to install the shield, and you can access the latest API library here.

The library installation includes samples to manage the module. We found that using the module on an Arduino Genuino is tricky since the DEBUG serial port uses a software port and we couldn't make it work. Using an arduino 101 fixed the issue since it has two serial ports to handle the debug data and the communication with the shield.

We also found that some old Dynamixel servos requires of protocol 1.0 instead of 2.0, like the AX-12A and AX-18A we have. Make sure to you run the scan_dynamixel sample with the protocol changes and you will find servo id's and communication speed, these data is needed to establish a link with the connected servos. You could also find the software tools in the Robotis site useful, make sure you select the version that corresponds to your hardware.

The shield has a switch that allows the programming of the micro-controller by releasing the UART port during upload of the program, changing the switch to Dynamixel will send the commands to the servo connected. See image here.

Finally, the module have a power connector to provide power to the hardware devices, or you can use a power HUB to provide such power. Using the power from the Arduino board is not recommended and does not provide the 9 V minimum requirement that some servos need.

The programming of the module is initialize by instantiating an object of the Dynamixel library. The parameters indicates the UART serial port to use communicating with the shield and the DIR PIN.

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);

Once the object is instantiated, begin communication using the baudrate found on the scan process for you Dynamixel hardware. Indicate the protocol to be used, currently 1.0 and 2.0 are supported, then start the hardware by using the ping function with the servo ID. For each servo ID turn off the torque property since we need to set the direct positioning property of the servo, once done turn the torque on again. Repeat this for each servo connected.

// 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);

When data is received and one of the servos need to be position use

dxl.setGoalPosition(SHOULDER_ID, cmdCoordX);

BLE Service setup

It did take us some time to get the different definitions of BLE support on an Arduino 101 and the ESP32. The Arduino 101 treats the devices that consume BLE data as central device and the ones that send the data as peripherals. The Adafruit HUZZAH32 ESP32 device defines a central as a client and the peripherals as servers.

In our project we create an UART service thru the ESP32 with the following UUIDs

#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"

The first UUID will be use to create the service, and the other two define characteristics of the service.

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

// Create the BLE Server
pServer = BLEDevice::createServer();

Notice that the inti function defines the service name that will be published to the listening devices and the createServer defines the device as a server.

// 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
);

Then the service is created, together with the characteristics UUID and types. Notice that the TX characteristic is defined as a NOTIFY property which will send notification once new data is available to transmit.

The service and server is started by executing

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

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

We then submit new data thru

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

Notice the notify() function which sends a wake up message to the listening devices so they can extract the new information and processed it.

We use the nRF Connect app from Nordic Semiconductor to test the service and confirm that the data and the service are transmitting correctly. You can see the test on the video above.

On the client device we connect to a service by starting the BLE and scanning the available peripherals. Check for a matching peripheral with our needed device, in this case ROBOTTE Service.

BLE.begin();
// start scanning for peripherals
BLE.scan();

while (notFound)
{
BLEDevice peripheral = BLE.available();
if (peripheral) {
// see if peripheral is our UART service
if (peripheral.localName() == "ROBOTTE Service") {

Connect to the peripheral, and register to the service to receive messages.

// connect to the peripheral
if (!peripheral.connect()) {
Serial.println("Failed to connect!");
return false;
}


// retrieve the LED characteristic
BLECharacteristic notifyCharacteristic = peripheral.characteristic("581108e2-6a9a-40d3-9d5f-db6f7529aa6a");

Connect to the Notify characteristic using the UUID set on the server device.

Confirm that the connected service and characteristic has a notify property and subscribe to the events. Register a callback function to the characteristic which will be call on every new data sent.

if (notifyCharacteristic.canNotify())
{
notifyCharacteristic.subscribe();
notifyCharacteristic.setEventHandler(BLEValueUpdated,UpdateValue);
joystickPeripheral = peripheral;
}

The callback function on our project is defined as

void UpdateValue(BLEDevice bledev, BLECharacteristic characteristic)
{
char *data;
data = (char *)characteristic.value();
Serial.println(data);
if (strstr(data,"START"))
{
StopFlag = true;
characteristic.unsubscribe();
}

Note that the function receives objects to the BLE device and the subscribe characteristic. The function then retrieves the value sent in the characteristic and then it scans for commands and splits the data. Notice that the START command stops and unsubscribes to the service, allowing the BLE server to stop notifications to this client.

The defined commands are:

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

Each command is used as in the next sample:

if (strstr(data,"cmd:B"))
{
if (lastCmdData != CMD_OPEN_HAND || lastCmdData != CMD_CLOSE_HAND)
{
lastCmdData = cmdData;
}
cmdData = CMD_CLOSE_HAND;
}

Notice we check if the last receive command is the same as the currently received, this helps to avoid multiple commands received. The program expects one unique command per data cycle.

Then the loop executes the corresponding command with the received data, as we will see on the Robot Arm section.

Joystick Interface

The Adafruit Joy Featherwing setup guide from Adafruit is simple and straight forward, you can access it here.

In our program we instantiate an object to the class Adafruit_seesaw

Adafruit_seesaw ss;

A mask is defined in order to process the buttons in the shield

uint32_t button_mask = (1 << BUTTON_RIGHT) | (1 << BUTTON_DOWN) | 
(1 << BUTTON_LEFT) | (1 << BUTTON_UP) | (1 << BUTTON_START);

Then initialize the pins using the mask and activate the event on pull up. Turn on the GPIO interrupts and the pin to use as interrupt notify.

ss.pinModeBulk(button_mask, INPUT_PULLUP);
ss.setGPIOInterrupts(button_mask, 1);
pinMode(IRQ_PIN, INPUT);

Check if a button has being pressed

if(!digitalRead(IRQ_PIN)){
uint32_t buttons = ss.digitalReadBulk(button_mask);
if (! (buttons & (1 << BUTTON_DOWN))) {
display.println("B");
sprintf(msg,"cmd:B ");
}

Read all the buttons status with the digitalReadBulk function and start verifying which button was pressed by masking out the result.

The joystick data is read as an analog input

int y = ss.analogRead(2);
int x = ss.analogRead(3);

Format the data and send it thru the BLE service.

Robot Arm

The arm is build using the Bioloid framework and Dynamixel servos AX-18A and AX-12A.

The arm body design was taken from the robot guide for the Bioloid set. The head of the hand was a self design to allow the wrist turn around with the hand centered on the fingers. You can see a 3D view at the end of the video above.

Robot Te

We us the Adafruit FeatherWing Tripler to connect the ESP32, joystick and display to control the arm. This interface is easy to put together and to grab with your hand.

The above video shows a demo of the interface.

The arm servos are connected to a power HUB using a plug in power delivering 9V of current. The arduino and ESP32 are connected to LIPO batteries. The power consumption on these devices is minimum.

We notice that using this configuration in some extend created hiccup transmission issues.

Minimal Interface

We finally tried a minimal interface by attaching the joystick directly into the ESP32 board. This configuration is smaller and user much less power than the one with the display attached.

We also notice a more stable transmission than the one with display.

A demo of the interface is shown in the above video.

Future Work

Improve the latency by using binary commands instead of string. Improve the movement smoothness when positioning servos.

THANKS FOR READING. :)

Code

RobTeJoystick.inoArduino
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.inoArduino
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();
}

Schematics

Feather HUZZAH32, Featherwing OLED, and Featherwing Joy
The connections are manage by the triple featherwing shield
20200422 171117 gdb9qbqnsr
Side View
20200422 171142 c4y7dpd1ff
Arduino 101 and Robotis Dynamixel Shield
The shield snaps over the Arduino 101. From the shield a connection is done to the Dynamixel HUB.
20200422 171158 qctpocofrx
Side view. Arduino 101 and Robotis Dynamixel Shield
20200422 171204 ssayikdl15

Comments

Similar projects you might like

Robot Arm Controlled Through Ethernet

Project tutorial by Balázs Simon

  • 17,402 views
  • 5 comments
  • 66 respects

BLE Robot Using Blynk & Arduino

Project tutorial by Tom Moxon

  • 4,682 views
  • 0 comments
  • 15 respects

ARMin: Simple Robot Arm Controller Using Python

Project tutorial by HyperChiicken

  • 13,979 views
  • 1 comment
  • 13 respects

Tertiarm - 3d Printed Robot Arm

Project tutorial by Karagad

  • 11,474 views
  • 3 comments
  • 23 respects

ARMin v2: Simple Robot Car and Arm Controller Using Python

Project tutorial by HyperChiicken

  • 7,351 views
  • 0 comments
  • 14 respects

Smartphone-Controlled Robot Using BLE 4.0

Project tutorial by Harsh Dethe

  • 5,772 views
  • 0 comments
  • 6 respects
Add projectSign up / Login