Duh-Vinci Surgical Robot

From LVL1
Jump to navigation Jump to search

Below is the Multi-Million Dollar Ultra-High Technology Da Vinci Surgical Robot.

JAC Da Vinci Surgical Robot.jpg

And this is the Cheap Low Technology Duh Vinci Surgical Robot.

JAC DV ARMS 1.jpg

There comes a time when medical advancements fall into the consumer market of things. The Duh-Vinci is that moment when medical technology meets the dollar store.

The Saga Begins

There is no good reason for this to exist. There is however a festering reason. A couple of years ago, LVL1 Hackerspace hosted a Hack-a-thon for useless and/or impractical devices. With only 48 hours, this was outside the scope of completion. The idea remained dormant until this year.

Creation Requirements

Parts List:

4 x Me Arm Open Source Robot

1 x Dissection Kit

1 x SparkFun Blynk Board

1 x 4 Line Voltage Level Shifter

1 x ESP32-Cam w/ OV2640 camera

1 x PCA9685 12-bit 16-PWM Servo Controller Board

4 x Cat6 RJ45 Surface Mount Box - 1 Port

2 x Cat6 RJ45 Surface Mount Box - 2 Port

4 x Cat5e Cables (3ft.)

1 x 5v/3.3V DC Power Supply

1 x 6v DC Power Supply (Servos)

Support Device:

1 x Android Tablet

Build Item List:

ESP32-CAM programming rig/FTDI

JAC DV CAMERA PROGRAMMER.jpg

Software Tools:

Arduino IDE

Blynk App for Android tablet

Overall System Layout

JAC DV SYSTEM OVERVIEW.jpg

JAC DV Staged Layout.jpg

Sub Systems

meArm

meArm Website

The robotic arms are meArm V0.3 possible V0.4 types. They were made several years ago and have been awaiting this opportunity to be of use to mankind.

This is one of four identical meArms. They differ in color to assist in identifying which arm is being used.

JAC DV meArm.jpg

Blue elements at base of arm are used to smooth rotation. This design had a gap between the stationary and rotating elements of the base.

The arms in this project use Tower Pro MG-90S Micro Servos. The servos are wired into the RJ-45 housing at the base of each arm.

JAC DV ARM SERVO WIRING.jpg

Socket Wiring
Servo Wire Color Servo Position RJ-45 Socket Layout
Base Brown 1 Right 2
Right Grey 2 Right 1
Left Purple 3 Right 4
Claw White 4 Left 3
Power + Red all Left 2
Power - Black all Left 1

Local Control System

The Control System can be visualized as below.

JAC-DV-Scheme-1.JPG

The controller displayed as a Fritzing Example.

JAC DV CIRCUIT-2.jpg

Note: Camera power supply is now separate because of current draw requirement.


JAC DV CONTROLLER-1.jpg

JAC DV CONTROLLER-2.jpg

meArm Cabling

meArms are connected to the controller via RJ-45 Cat5e cables. Cables are color matched to their associated arms.

Remote Control System

The remote control system consists of an android tablet running the Blynk App.

Blynk

JAC DV TABLET INTERFACE.jpg

Video System

Video for the system is provided by an ESP32-CAM with OV2640 camera. A generic ESP32-Cam from amazon was purchased. The device was found to be compatible with the AI Thinker variety of ESP32-Cams.

JAC DV CAMERA.jpg

Software

meArm Control Software

Basic software for controlling the meArms is from the GitHub Repository:

York Hackspace meArm GitHub Repository

The main program is a merger of the JoystickIK from the repository and the Blynk software skeleton. JoystickIK was modified to allow control of multiple arms. The modification is primarily assignment of arrays for variables to select which arm is being manipulated. My modification of the software is crude with unused fragments, but so is the Duh-Vinci. The NeoPixel on the SparkFun Blynk board changes color to reflect which arm is active.



/*************************************************************
  Download latest Blynk library here:
    https://github.com/blynkkk/blynk-library/releases/latest

  Blynk is a platform with iOS and Android apps to control
  Arduino, Raspberry Pi and the likes over the Internet.
  You can easily build graphic interfaces for all your
  projects by simply dragging and dropping widgets.

    Downloads, docs, tutorials: http://www.blynk.cc
    Sketch generator:           http://examples.blynk.cc
    Blynk community:            http://community.blynk.cc
    Follow us:                  http://www.fb.com/blynkapp
                                http://twitter.com/blynk_app

  Blynk library is licensed under MIT license
  This example code is in public domain.

 *************************************************************
  This example runs on Sparkfun Blynk Board.

  Note: This requires ESP8266 support package:
    https://github.com/esp8266/Arduino

  You can select NodeMCU 1.0 (compatible board)
  in the Tools -> Board menu

  Change WiFi ssid, pass, and Blynk auth token to run :)
  Feel free to apply it to any other example. It's simple!
 *************************************************************/

/* Comment this out to disable prints and save space */
#define BLYNK_PRINT Serial

#include <ESP8266WiFi.h>
#include <BlynkSimpleEsp8266.h>

#include "meArm_Mod.h"
#include <Adafruit_PWMServoDriver.h>
#include <Wire.h>
#include "wifikeys.h"

////////////////

#include <Adafruit_NeoPixel.h>

#define WS2812_PIN 4 // Pin connected to WS2812 LED
#define NUMRGB 1 // Number of WS2812's in the string
Adafruit_NeoPixel rgb = Adafruit_NeoPixel(NUMRGB, WS2812_PIN, NEO_GRB + NEO_KHZ800);
/*
#include <FastLED.h>
#define DATA_PIN 4
#define NUM_LEDS 1
CRGB leds[NUM_LEDS];
*/

// You should get Auth Token in the Blynk App.
// Go to the Project Settings (nut icon).
char auth[] = "xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx";

// Your WiFi credentials.
// Set password to "" for open networks.
//char ssid[] = "";
//char pass[] = "";

int armSelection;

struct genericArm
{
  int armSelection=0;
//  float armX[4];
//  float armY[4];
//  float armZ[4];
  int clawStatus[4];
};

genericArm ARMS;

meArm arm;

void ArmSelectionIndicator(int indicator,int value)
{
      switch(indicator)
      {
        case 0:
          Blynk.virtualWrite(V2, value);    
          break;
         case 1:
          Blynk.virtualWrite(V3, value);        
          break;
        case 2:
          Blynk.virtualWrite(V4, value);        
          break;
        case 3:
          Blynk.virtualWrite(V5, value);        
          break;      
      }
}


BLYNK_WRITE(V0) 
{
  int x = param[0].asInt();
  int y = param[1].asInt();

  float dx = map(x, 0, 1023, -5.0, 5.0);
  float dy = map(y, 0, 1023, 5.0, -5.0);
  float dz = 0;

  if (abs(dx) < 1.5) dx = 0.0;
  if (abs(dy) < 1.5) dy = 0.0;

  if (!(dx == 0.0 && dy == 0.0))
  {
    Serial.println("J1");      
    arm.goDirectlyTo(ARMS.armSelection, arm.getX(ARMS.armSelection) + dx, arm.getY(ARMS.armSelection) + dy, arm.getZ(ARMS.armSelection) + dz);    
  }
}

BLYNK_WRITE(V1) 
{
  int z = param[0].asInt();
  int c = param[1].asInt();

  float dx =0;
  float dy =0;
  float dc = map(c, 0, 1023, -5.0, 5.0);
  float dz = map(z, 0, 1023, 5.0, -5.0);

  if (abs(dc) < 1.5) dc = 0.0;
  if (abs(dz) < 1.5) dz = 0.0;

  if (!(dz == 0.0 && dc == 0.0))
  {
    Serial.println("J2");      
    arm.goDirectlyTo(ARMS.armSelection, arm.getX(ARMS.armSelection) + dx, arm.getY(ARMS.armSelection) + dy, arm.getZ(ARMS.armSelection) + dz);

    if (dc < -3.0)
      {
        arm.closeGripper(ARMS.armSelection);
      }
    else if (dc > 3.0)
      {
        arm.openGripper(ARMS.armSelection);
      }        
  }  
}

BLYNK_WRITE(V6)
{   
  int value = param.asInt(); // Get value as integer

  if (value == 1)
    {
      ArmSelectionIndicator(ARMS.armSelection,0);
      
      ++ARMS.armSelection;
      if(ARMS.armSelection > 3)
        ARMS.armSelection = 0;
    
      //Turn On Arm Selection LED
      ArmSelectionIndicator(ARMS.armSelection,255);

      neoPixelColor();

      Serial.println("AS");    
    }
}

void neoPixelColor()
{
      switch(ARMS.armSelection)
        {
        case 0:
          rgb.setPixelColor(0, rgb.Color(128, 64, 0));        
          break;
         case 1:
          rgb.setPixelColor(0, rgb.Color(128, 0, 0));
          break;
        case 2:
          rgb.setPixelColor(0, rgb.Color(0, 128, 0));
          break;
        case 3:
          rgb.setPixelColor(0, rgb.Color(0, 0, 128));
          break;                
        }

      rgb.show();
  
}
void setup()
{
  // Debug console
  Serial.begin(9600);

  Blynk.begin(auth, ssid, password);
  // You can also specify server:
  //Blynk.begin(auth, ssid, pass, "blynk-cloud.com", 80);
  //Blynk.begin(auth, ssid, pass, IPAddress(192,168,1,100), 8080);

/*
  FastLED.addLeds<NEOPIXEL, DATA_PIN>(leds, NUM_LEDS);
  leds[0] = CRGB::Red;
  FastLED.show();
*/

  rgb.begin();

  arm.begin();
  
  for(int w = 0; w < 3; ++w)
    {
//      ARMS.armX[w] = 0.0;      
//      ARMS.armY[w] = 0.0;      
//      ARMS.armZ[w] = 0.0;         
//      ARMS.clawStatus[w] = 0; 
      ArmSelectionIndicator(w,0);
    }
  ARMS.armSelection = 0;

  neoPixelColor();
    
  ArmSelectionIndicator(ARMS.armSelection,255);
}

void loop()
{
  Blynk.run();
}
 

ESP32-Cam Software

A video stream compatible with Blynk's video-stream widget was necessary. A RTSP stream would work with Blynk. An ESP32-CAM compatible GitHub library was found.

Geeksville Micro-RTSP GitHub Repository

The application used is under Examples: ESP32-devcam.ino

Modifications for the selection of correct camera type (esp32cam aithinker) were needed in the setup() function.

 
void setup()
{
  #ifdef ENABLE_OLED
    hasDisplay = display.init();
    if(hasDisplay) {
        display.flipScreenVertically();
        display.setFont(ArialMT_Plain_16);
        display.setTextAlignment(TEXT_ALIGN_CENTER);
    }
  #endif
    lcdMessage("booting");

    Serial.begin(115200);
    while (!Serial)
    {
        ;
    }


//////////////////////////////////////////////////////////////////////////////
//
// DUH-VINCI MODIFICATION
// SELECTION OF AITINKER CAMERA MODULE TYPE "esp32cam_aithinker_config"


    cam.init(esp32cam_aithinker_config);
//
//////////////////////////////////////////////////////////////////////////////


    IPAddress ip;


#ifdef SOFTAP_MODE
    const char *hostname = "devcam";
    // WiFi.hostname(hostname); // FIXME - find out why undefined
    lcdMessage("starting softAP");
    WiFi.mode(WIFI_AP);
    WiFi.softAPConfig(apIP, apIP, IPAddress(255, 255, 255, 0));
    bool result = WiFi.softAP(hostname, "12345678", 1, 0);
    if (!result)
    {
        Serial.println("AP Config failed.");
        return;
    }
    else
    {
        Serial.println("AP Config Success.");
        Serial.print("AP MAC: ");
        Serial.println(WiFi.softAPmacAddress());

        ip = WiFi.softAPIP();
    }
#else
    lcdMessage(String("join ") + ssid);
    WiFi.mode(WIFI_STA);
    WiFi.begin(ssid, password);
    while (WiFi.status() != WL_CONNECTED)
    {
        delay(500);
        Serial.print(F("."));
    }
    ip = WiFi.localIP();
    Serial.println(F("WiFi connected"));
    Serial.println("");
    Serial.println(ip);
#endif

    lcdMessage(ip.toString());

#ifdef ENABLE_WEBSERVER
    server.on("/", HTTP_GET, handle_jpg_stream);
    server.on("/jpg", HTTP_GET, handle_jpg);
    server.onNotFound(handleNotFound);
    server.begin();
#endif

#ifdef ENABLE_RTSPSERVER
    rtspServer.begin();
#endif
}


Another modification was made in OV2640.cpp. The modification reduces the video stream to QVGA size (320x240) in the camera_config_t esp32cam_aithinker_config structure variable values.


camera_config_t esp32cam_aithinker_config{

    .pin_pwdn = 32,
    .pin_reset = -1,

    .pin_xclk = 0,

    .pin_sscb_sda = 26,
    .pin_sscb_scl = 27,

    // Note: LED GPIO is apparently 4 not sure where that goes
    // per https://github.com/donny681/ESP32_CAMERA_QR/blob/e4ef44549876457cd841f33a0892c82a71f35358/main/led.c
    .pin_d7 = 35,
    .pin_d6 = 34,
    .pin_d5 = 39,
    .pin_d4 = 36,
    .pin_d3 = 21,
    .pin_d2 = 19,
    .pin_d1 = 18,
    .pin_d0 = 5,
    .pin_vsync = 25,
    .pin_href = 23,
    .pin_pclk = 22,
    .xclk_freq_hz = 20000000,
    .ledc_timer = LEDC_TIMER_1,
    .ledc_channel = LEDC_CHANNEL_1,
    .pixel_format = PIXFORMAT_JPEG,
    // .frame_size = FRAMESIZE_UXGA, // needs 234K of framebuffer space
    // .frame_size = FRAMESIZE_SXGA, // needs 160K for framebuffer
    // .frame_size = FRAMESIZE_XGA, // needs 96K or even smaller FRAMESIZE_SVGA - can work if using only 1 fb

//////////////////////////////////////////////////////////////////////////
//
// DUH-VINCI MODIFICATION RIGHT HERE 
// MODIFIED OUTPUT FRAME SIZE TO QVGA

    .frame_size = FRAMESIZE_QVGA,
//
//
/////////////////////////////////////////////////////////////////////////


    .jpeg_quality = 12, //0-63 lower numbers are higher quality
    .fb_count = 2       // if more than one i2s runs in continous mode.  Use only with jpeg
    
};




Blynk Application Widgets

Video Stream

The video stream is supplied by the ESP32-Cam. It is configured as a RTSP stream.

The video-stream widget is configured as :

URL Address: rtsp://192.168.1.19:8554/mjpeg/1

Output : V7

You will replace "192.168.1.19" with the IP address generated from the ESP32-devcam application. The IP address is displayed via the serial monitor in the Arduino IDE. This IP address will change as you change the WiFi network you connect.

Arm Selection

A Styled_Button widget is used to traverse the list of meArms (total of four).

Four LED widgets are used to display which meArm is active. A LED widget for each arm is "turned on" representing that arms color. Only one arm is active at anytime. The arms are labeled: SCAPEL, PROBE, TWEEZERS and CAMERA.

Arm Control

Two Joystick widgets are used to control an arm. The left Joystick widget controls the X-Y position of the claw. The right Joystick widget controls the Z position and the Open/Close claw function.

CAD Render

Made a preliminary CAD rendering.

MeArm - 3D

meArm CAD model from GrabCAD with following attributes:

meArm

Nemanja Petkov

December 21st, 2014

MeArm - Pocket Sized Industrial Robotics for Everybody

by Benjamin Gray

Model was modified for this rendering to match the V0.3 variety I have.

PCA9685 Module - 3D

Module from GrabCAD with following attributes:

16 channel PCA9685

Ali ZAHI

September 22nd, 2016

16-Channel 12-bit PWM/Servo Driver - I2C interface - PCA9685

Model was modified for this rendering to match the variety of PCA9685 Module I have.

ESP32-Cam - 3D

Module from GrabCAD with following attributes:

ESP32-CAM

Jan Jezek

May 6th, 2019

ESP32-S module with camera Geekcreit diymore.cc. iges, step, stp

Model was modified for this rendering to match the variety of ESP32-Cam I have.

Sparkfun Blynk Board

Module from SparkFun website. Converted from Blender file for this rendering.

JAC DV CAD RENDER.jpg

JAC DV Staged Render.jpg

Testing

Duh-Vinci Test One

Duh-Vinci Test Two

Duh-Vinci Demo

JAC DV Duh-Vinci-from-above.jpg

JAC DV TEST LAYOUT.jpg



The JAC Project Index