Dimitris S.
Published © GPL3+

LEGO TECHNICS robot arm

LEGO robot arm built with LEGO technics only, controlled with MQTT commands over WiFi with Node MCU and Adafruit Motor Shields!

AdvancedFull instructions provided182
LEGO TECHNICS robot arm

Things used in this project

Hardware components

Adafruit DC Motor Shield
×2
Adafruit INA219 DC current sensor
×1
NodeMCU v2
×1
Jumper wires (generic)
Jumper wires (generic)
×1
Perma-Proto Breadboard Half Size
Perma-Proto Breadboard Half Size
×1

Software apps and online services

MQTT Dash
A simple app to send MQTT commands. Simply match the buttons to the characters that appear in the code for controlling each motor

Hand tools and fabrication machines

Soldering iron (generic)
Soldering iron (generic)

Story

Read more

Schematics

Schematic for the project

Assume that the shield on the right is stacked at the bottom of the shield on the left. The SDA SCL, 3V and GND pins of the top shield are shared with the bottom so no extra wiring is required.

Code

LEGO motors control

C/C++
#include <ESP8266WiFi.h>
#include <WiFiUdp.h>
#include <PubSubClient.h>
#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include <Adafruit_INA219.h>

// UDP instance to send UDP messages
WiFiUDP Udp;

// INA219 instance
Adafruit_INA219 ina219;

// Create the motor shield objects. Addresses depend on soldering pins.
Adafruit_MotorShield MSbot60 = Adafruit_MotorShield(0x60); 
Adafruit_MotorShield MStop61 = Adafruit_MotorShield(0x61); 

// Assign robot motors to the shield ports
Adafruit_DCMotor *MStopM1 = MStop61.getMotor(1);
Adafruit_DCMotor *MStopM3 = MStop61.getMotor(3);
Adafruit_DCMotor *MStopM4 = MStop61.getMotor(4);
Adafruit_DCMotor *MSbotM1 = MSbot60.getMotor(1);
Adafruit_DCMotor *MSbotM2 = MSbot60.getMotor(2);
Adafruit_DCMotor *MSbotM3 = MSbot60.getMotor(3);

// Network settings. Update accordingly
const char* ssid = "********";
const char* password = "********";
const char* mqtt_server = "192.168.1.10";

// MQTT instance  
WiFiClient espClient;
PubSubClient client(espClient);

void setup_wifi() {

  delay(10);
  // We start by connecting to a WiFi network
  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(ssid);

  WiFi.begin(ssid, password);

  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.print(".");
  }

  randomSeed(micros());

  Serial.println("");
  Serial.println("WiFi connected");
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());
}

void reconnect() {
  // Loop until we're reconnected
  while (!client.connected()) {
    Serial.print("Attempting MQTT connection...");
    // Create a random client ID
    String clientId = "ESP8266Client-";
    clientId += String(random(0xffff), HEX);
    // Attempt to connect
    if (client.connect(clientId.c_str())) {
      Serial.println("connected");
      // Once connected resubscribe
      client.subscribe("inTopic");
    } else {
      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" try again in 5 seconds");
      // Wait 5 seconds before retrying
      delay(5000);
    }
  }
}

void callback(char* topic, byte* payload, unsigned int length) {
//  Serial.print("Message arrived [");
//  Serial.print(topic);
//  Serial.print("] ");
//  for (int i = 0; i < length; i++) {
//    Serial.print((char)payload[i]);
//  }
//  Serial.println();

// Open-close grip
  if ((char)payload[0] == '1') {
    MSbotM3->setSpeed(200);
    MSbotM3->run(FORWARD); // close
  } else if ((char)payload[0] == '2'){
      MSbotM3->setSpeed(200);
      MSbotM3->run(BACKWARD); // open
  } else
      MSbotM3->run(RELEASE); 
    
// Rotate grip
  if ((char)payload[0] == '3'){
    MSbotM1->setSpeed(200);
    MSbotM1->run(BACKWARD); // counter clock-wise
  } else if ((char)payload[0] == '4'){
      MSbotM1->setSpeed(200);
      MSbotM1->run(FORWARD); // counter clock-wise
  } else
      MSbotM1->run(RELEASE);
      
// Rotate base
  if ((char)payload[0] == '5'){
    MSbotM2->setSpeed(255);
    MSbotM2->run(BACKWARD); // counter clock-wise
  } else if ((char)payload[0] == '6'){
      MSbotM2->setSpeed(255);
      MSbotM2->run(FORWARD); // counter clock-wise
  } else
      MSbotM2->run(RELEASE);

// Lift arm
  if ((char)payload[0] == '7'){
    MStopM1->setSpeed(175);
    MStopM1->run(BACKWARD); // up
  } else if ((char)payload[0] == '8'){
      MStopM1->setSpeed(175);
      MStopM1->run(FORWARD); // down
  } else
      MStopM1->run(RELEASE);

// Lift top-arm
  if ((char)payload[0] == 'a'){
    MStopM3->setSpeed(150);
    MStopM4->setSpeed(150);
    MStopM3->run(BACKWARD); // up
    MStopM4->run(BACKWARD); // up
  } else if ((char)payload[0] == 'b'){
      MStopM3->setSpeed(150);
      MStopM4->setSpeed(150);
      MStopM3->run(FORWARD); // down
      MStopM4->run(FORWARD); // down
  } else {
      MStopM3->run(RELEASE);
      MStopM4->run(RELEASE);
  }
}

void setup() {
  Serial.begin(115200);
  setup_wifi();
  ina219.begin();
  
  client.setServer(mqtt_server, 1883);
  client.setCallback(callback);

// create motor objects with frequency 1.2KHz
  MSbot60.begin(1200);
  MStop61.begin(1200);  

// cut-off motor power upon start-up
  MStopM1->run(RELEASE);
  MStopM3->run(RELEASE); 
  MStopM4->run(RELEASE); 
  MSbotM1->run(RELEASE);
  MSbotM2->run(RELEASE);
  MSbotM3->run(RELEASE);
}
  
  float current_mA = 0.0;
  char charCurrent[10];
  uint32_t currentFrequency;
  unsigned int remoteUdpPort = 1890;  // remote UDP port
  const char* remoteIPaddr = "192.168.1.10"; 

void loop() {
  current_mA = ina219.getCurrent_mA();

// convert float to array of characters to send as UDP packet
  dtostrf(current_mA, 8, 3, charCurrent);
  
//send current measurements as udp packets  
  Udp.beginPacket(remoteIPaddr, remoteUdpPort);
  Udp.write(charCurrent);
  Udp.endPacket();
  delay(250);
      
  if (!client.connected()) {
    reconnect();
  }
  client.loop();

}

Credits

Dimitris S.

Dimitris S.

0 projects • 0 followers

Comments