An introduction to Robotic Arm Theory
-
Shoulder pitch
-
Shoulder roll
-
Arm yaw
-
Elbow pitch
-
Wrist pitch
-
Wrist yaw
-
Wrist roll
T = m × g
T = A × L1 + F × L2
T = A × L1 + F × (L1/2)
Using an Online Tool to Calculate Your Servos
robotshop.com
offers a very nice online tool that helps you calculate the servos that must be used in your arm. To use this tool, visit
http://www.robotshop.com/blog/en/robot-arm-torque-calculator-9712
and enter the arm and servo data. They use a different notation than presented on this chapter, where L is the arm’s length, M is the weight in kg of each part of your arm, and A is the weight of your servos. Don’t forget to include the load max in the gripper servo, as explained previously.Considerations About This Project
The Robotic Arm
-
Four U-shaped long brackets
-
Three U-shaped brackets used for the base
-
One mechanical gripper
-
Six metallic servo wheels
-
Four cup bearings
-
Several screws and nuts (more than you need)
-
Four servos MG996R that provide a stall torque of 9.4kg/cm @ 4.8V and 12kg/cm @ 6V
-
Two servos 5521MG with a stall torque of 17,25Kg/cm @ 4.8V and 20.32 Kg/cm @ 6V
-
Three servo extension wire cables
-
Servo accessories packages with rubber rings, horns, and fittings
Quantity
|
Description
|
---|---|
1 | Robotic arm kit with servos (only if you do not have the servos recommended; otherwise, order the armor kit). |
4 | Servo extension wire cables. |
1 | Piece of wood around 7 x 12 x 0.5 inches (at least). |
1 | C-clamp if you are using the arm body in a table. |
4 | M6-30mm hex-head cap screw with nuts and flat washers or equivalent. |
3 | Dupont jumper cables, male-to-male, or pieces of 1/4 watt wires. |
1 | Five-inch nose plier. |
1 | Screwdrivers for M3 and M4 screws. |
Assembling the Robotic Arm
-
The M3 10mm screws are used in the bearings to connect the servo that will manipulate the gripper and the wrist.
-
The M4 10mm screws connect the servos with servo brackets, but if you are using the 5521MG servo then you need to use the M3 10mm screws. If you are using any other servo and M4 10mm is being difficult, try the M3 10mm screws.
-
The M3 8mm flat screws connect the servo wheel to the servo only.
-
The M3 8mm rounded screws connect the rest of arm parts—like the short U-shapes—to each other and the servo to the long U-shapes.
Step 1: Preparing the Servos
prepare_servos.ino
#include <Servo.h>
#define PIN 9
Servo prepareServo;
void setup() {
// Attaching the servo to PIN
prepareServo.attach(PIN);
}
void loop() {
// moves the servo to 90 degrees
prepareServo.write(90);
delay(250);
}
prepareServo
as an instance of the Servo
class and attaches to the pin 9 defined by PIN
. In the loop, the servo then moves to 90 degrees. For more details regarding the Servo API, consult Chapter 4.Step 2: Assembling the Base
Step 3: Assembling the Shoulder
Step 4: Assembling the Elbow
Step 5: Assembling the Wrist
Step 6: Assembling the Mechanical Gripper
Step 7: Assembling a Base
Controlling the Robotic Arm
-
Power supply : During my tests I realized the servos can drain up to 2.2A for a load of 100 grams at 5V. That means Intel Galileo boards and Edison Arduino toolkit boards do not provide a current (source) that guarantees the functionality of all servos since the limit is 80mA.
-
Servos control : The servos must be controlled somehow. You should have at least five potentiometers and a simple button to open and close the gripper. If you use joysticks or thumbsticks, each axis (x and y) could control two servo channels, so three of them would be enough to control the whole arm.
-
Quantity of wires : Each servo requires three wires. including ground, power, and pulse. Considering there are six servos in the arm the quantity of wires goes to 18. Besides this, it is necessary to add wire expanders to guarantee the robotic arm will be far enough from Intel Galileo or Intel Edison and to avoid damaging your board. Considering you cannot use an Intel Galileo or Edison board to power the servos, as explained previously, you also need to consider more cables related to an external power supply. If you have a potentiometer or joysticks or thumbsticks to control the robotic arm, you need at least 15 wires. All these wires can lead to a considerable mess.
Building a Servo Control Board
The Hardware
Number | Description |
---|---|
3 | Thumbsticks SKU 121340 from dealxtream.com or equivalent |
3 | 1K Ohm 1/4W resistors |
1 | Single-sided prototyping board, 6x3 inches (universal board) |
1 | Three-pole terminal block |
1 | 470uF electrolytic capacitor |
1 | Diode 5404 (or three 1N4007 or 1N4004 diodes) |
2 | 40 header pins 2.54mm single row |
6 | Servo extension cables |
1 | Power supply 5 or 6 VDC with 3A (minimum) |
12 | M3 0.5 x 10mm screws with nuts |
37 | Dupont male-to-female cables |
5 | Hex or round M3 nylon spacers (standoff) with screws |
1 | Soldering iron with lead |
Some | AWG23 0.5mm2 or breadboard cables with at least two different colors (30 inches is enough) |
Some | 18 AWG 1.5mm2 cables with at least two different colors (11 inches is enough) |
The Thumbsticks
dealxtream
(see
http://www.dx.com/s/121340
).External Power Supply
Circuit Protection
Assembling the Board
-
The prototyping board is specified in the material list as 6x3 inches. If you use a small board (even if the thumbsticks fits), make sure the controllers will have space and the thumbsticks will not collide with each other when you move their towers. That’s why there should be a gap of at least 1.2 inches between each thumbstick.
-
Note in Figure 11-47 that five standoffs were used, four in the edges and one in the center. The reason for the central one is that, since you are using this board as a controller, you might rest your hands on the board. The central standoff will keep the board from warping given the weight of your hands.
digikey.com
sells different types of standoffs; the nylon ones are only around $0.23 each. -
Be wise when you arrange the terminals and avoid wires passing over the board. This can impact the thumbstick’s usability.
The Software
-
There is more than one simple thumbstick to be controlled.
-
The thumbsticks must be calibrated.
-
The code should control the servo limits of each component in the robotic arm.
-
Considering there are six servos to be controlled, the control must be easy for the user.
An API for Servos Board Control
-
Absolute position: The servos will follow the thumbstick movement. So if you release the thumbstick, the servo will move to its initial position because the thumbstick automatically moves to the central position. You need to keep holding the sticks to move the servos that will follow the same movements.
-
Step in touch: The servos do not follow the stick, but move in the direction you point the thumbsticks.
Thumbstick.h
/*
This file is part of Thumstick API.
Thumstick API is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
Thumstick API is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
by Manoel Carlos Ramon (manoel.ramon@gm ail.com)
Nov 2014
version 1.0
*/
#ifndef _THUMBSTICK_API
#define _THUMBSTICK_API
#include <Arduino.h>
#define NOT_CALIBRATED 22
#define MAX_FLOATING 10
#define STEP_X 5
#define STEP_Y 5
#define NUMBER_OF_SAMPLES 25
#define DEBUG 1
class Thumbstick {
private:
int stick_step = 100;
int lastButtonState = HIGH;
long lastDebounceTime = 0; // the last time the output pin was toggled
long debounceDelay = 400; // adjust this value if necessary to avoid flickering
int X_max = NOT_CALIBRATED; // max X retrieved during calibration
int Y_max = NOT_CALIBRATED; // max Y retrieved during calibration
int X_min = NOT_CALIBRATED; // min X retrieved during calibration
int Y_min = NOT_CALIBRATED; // min Y retrieved during calibration
int centerX = 0;
int centerY = 0;
int pinAnalogX;
int pinAnalogY;
int pinButton;
int pin_select;
bool centerCalibrated = false; // informs if the center was calibrated
// 0 and 180 are the default angles values
int servoX_MinAngle = 0; // the minimum angle supported by servo attached to axis X
int servoX_MaxAngle = 180; // the maximum angle supported by servo attached to axis X
int servoY_MinAngle = 0; // the minimum angle supported by servo attached to axis Y
int servoY_MaxAngle = 180; // the maximum angle supported by servo attached to axis Y
// number of samples to estabelish the center during the calibration
int samplesOfrCenterXCounter = NUMBER_OF_SAMPLES, samplesOfrCenterYCounter = NUMBER_OF_SAMPLES;
int lastAngleX = -1, lastAngleY = -1;
// checks the button state considering the debounce
int checkButtonState();
public:
enum {
ANALOG_RAW, // Only brings the raw values... for testing only
ABSOLUTE_POSITION, // The servos will move according joystick position
STEPS_IN_TOUCH, // The servos moves increases and decreases its angles
INVALID_MOVE // Just to mark the invalid boundaries
} typedef MOVE_T;
MOVE_T move_type = ANALOG_RAW; // initial mode
struct {
int X; // the current X angle
int Y; // the current Y angle
int buttonPressed; // True if thumbstick button is pressed
} typedef JOYSTICK_XY_T;
int Xvalue; // current X value read from analog port
int Yvalue; // current Y value read from analog port
int Zvalue; // current button state
// Contructor
// _pin_select : which digital pin that selects the thumbstick during the multiplexation
// pinX : which analog port reads the variation of X axis
// pinY : which analog port reads the variation of Y axis
// _pinButton : which digital pin reads the button state
// minX : the minimum servo angle on axis X (default is 0)
// maxX : the maximum servo angle on axis X (default is 180)
// minY : the minimum servo angle on axis Y (default is 0)
// maxY : the maximum servo angle on axis Y (default is 180)
Thumbstick(int _pin_select, int pinX, int pinY, int _pinButton, int minX = 0, int maxX = 180, int minY = 0, int maxY = 180);
// Destructor
∼Thumbstick();
// init the pin mode (must be in setup() function)
void initPins();
// Performs the calibration
void calibrate();
// checks if the thumbstick was calibrated
// Returns: TRUE if calibrated or FALSE if it is not.
bool isCalibrated();
// only for debug purposes
void dumpCalibration();
// informs if center was calibrated
bool isCenterCalibrated() { return centerCalibrated; };
// reads the current status of thumbstick
// Parameter:
// MOVE_T format - the mode of reading
// Returns:
// JOYSTICK_XY_T - the coordinates and button state
JOYSTICK_XY_T read(MOVE_T format);
};
#endif
Thumbstick.cpp
/*
This file is part of Thumstick API.
Thumstick API is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
Thumstick API is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
by Manoel Carlos Ramon (manoel.ramon@gmail.com)
Nov 2014
version 1.0
*/
#include "Thumbstick.h"
// Contructor
// _pin_select : which digital pin that selects the thumbstick during the multiplexation
// pinX : which analog port reads the variation of X axis
// pinY : which analog port reads the variation of Y axis
// _pinButton : which digital pin reads the button state
// minX : the minimum servo angle on axis X (default is 0)
// maxX : the maximum servo angle on axis X (default is 180)
// minY : the minimum servo angle on axis Y (default is 0)
// maxY : the maximum servo angle on axis Y (default is 180)
Thumbstick::Thumbstick(int _pin_select, int pinX, int pinY, int _pinButton, int minX, int maxX, int minY, int maxY)
{
pinAnalogX = pinX;
pinAnalogY = pinY;
pinButton = _pinButton;
pin_select = _pin_select;
servoX_MinAngle = minX;
servoX_MaxAngle = maxX;
servoY_MinAngle = minY;
servoY_MaxAngle = maxY;
}
// Destructor
Thumbstick::∼Thumbstick()
{
}
// init the pin mode (must be in setup() function)
void Thumbstick::initPins()
{
pinMode(pinButton, INPUT);
pinMode(pin_select, OUTPUT);
}
// Performs the calibration
void Thumbstick::calibrate()
{
JOYSTICK_XY_T res = this->read(ANALOG_RAW);
// the following logic tries to find the maximum and minimum
// values for all axis. Such values will be used to identify
// the limits of your thumbstick.
if (res.X < X_min)
{
X_min = res.X;
}
if (res.X > X_max)
{
X_max = res.X;
}
if (res.Y < Y_min)
{
Y_min = res.Y;
}
if (res.Y > Y_max)
{
Y_max = res.Y;
}
// the center of each thumbstick must be found
// considering the center is the first thing that calibration process search for
// then some samples must be aquire until ADC turn stable.
if ((centerX == 0) && (--samplesOfrCenterXCounter ==0))
{
centerX = res.X;
Serial.print("centerX:");
Serial.println(centerX);
}
if ((centerY == 0) && (--samplesOfrCenterYCounter == 0))
{
centerY = res.Y;
Serial.print("centerY:");
Serial.println(centerY);
}
// just to inform the centers were got
if ((centerX !=0) && (centerY !=0))
{
centerCalibrated = true;
}
if ((lastAngleX == -1) && (centerX != 0) && (X_min != NOT_CALIBRATED) && (X_max != NOT_CALIBRATED) && (Y_max != NOT_CALIBRATED) && (Y_min != NOT_CALIBRATED))
{
lastAngleX = constrain(map(centerX , X_min , X_max , servoX_MinAngle, servoX_MaxAngle), servoX_MinAngle, servoX_MaxAngle);
lastAngleY = constrain(map(centerY , Y_min , Y_max , servoY_MinAngle, servoY_MaxAngle), servoY_MinAngle, servoY_MaxAngle);
}
}
// checks if the thumbstick was calibrated
// Returns: TRUE if calibrated or FALSE if it is not.
bool Thumbstick::isCalibrated()
{
bool result = false;
if ((X_min != NOT_CALIBRATED) && (X_max != NOT_CALIBRATED) &&
(Y_min != NOT_CALIBRATED) && (Y_max != NOT_CALIBRATED)) {
result = true;
}
return result;
}
// only for debug purposes
void Thumbstick::dumpCalibration()
{
if (DEBUG)
{
Serial.print("Xmax:");
Serial.print(X_max);
Serial.print(" Xmin:");
Serial.print(X_min);
Serial.print(" Ymax:");
Serial.print(Y_max);
Serial.print(" Ymin:");
Serial.print(Y_min);
Serial.print(" centerX:");
Serial.print(centerX);
Serial.print(" Angle centerX:");
Serial.print(lastAngleX);
Serial.print(" centerY:");
Serial.print(centerY);
Serial.print(" Angle centerY:");
Serial.println(lastAngleY);
}
}
// checks the button state considering the debounce
int Thumbstick::checkButtonState()
{
// read the state of the switch into a local variable:
int reading = digitalRead(this->pinButton);
// check to see if you just pressed the button
// (i.e. the input went from LOW to HIGH), and you've waited
// long enough since the last press to ignore any noise:
// If the switch changed, due to noise or pressing:
if (reading != lastButtonState) {
// reset the debouncing timer
lastDebounceTime = millis();
}
if ((millis() - lastDebounceTime) > debounceDelay) {
// whatever the reading is at, it's been there for longer
// if the button state has changed:
if (reading != Zvalue) {
Zvalue = reading;
}
}
lastButtonState = reading;
return lastButtonState;
}
// reads the current status of thumbstick
// Parameter:
// MOVE_T format - the mode of reading
// Returns:
// JOYSTICK_XY_T - the coordinates and button state
Thumbstick::JOYSTICK_XY_T Thumbstick::read(MOVE_T format)
{
int stepX = 0, absoluteX, absoluteY, stepY = 0;
Thumbstick::JOYSTICK_XY_T result = {0,0, false};
// selecting the right thumbstick to provide 5V
digitalWrite(pin_select, HIGH);
delay(5);
result.buttonPressed = false;
if (checkButtonState() == LOW)
{
result.buttonPressed = true;
}
// reading the raw analog values
Xvalue = analogRead(this->pinAnalogX);
Yvalue = analogRead(this->pinAnalogY);
switch (format)
{
case ANALOG_RAW: // Only brings the raw values of the joystick potentiometers
result.X = Xvalue;
result.Y = Yvalue;
break;
case ABSOLUTE_POSITION: // The servo will move according joystick position
result.X = constrain(map(Xvalue , X_min , X_max , servoX_MinAngle, servoX_MaxAngle), servoX_MinAngle, servoX_MaxAngle);
result.Y = constrain(map(Yvalue , Y_min , Y_max , servoY_MinAngle, servoY_MaxAngle), servoY_MinAngle, servoY_MaxAngle);
break;
case STEPS_IN_TOUCH: // The joystick moves increases and decreases servos coordinates
int x;
int centerx;
x = map(Xvalue , X_min , X_max , 0, 255);
centerx = map(centerX , X_min , X_max , 0, 255);
// checking if the reading is floating
if (abs(x - centerx) > MAX_FLOATING)
{
if (x > centerx) stepX = STEP_X;
if (x < centerx) stepX = -STEP_X;
}
// reading the last angle and adding result.X
int y;
int centery;
y = map(Yvalue , Y_min , Y_max , 0, 255);
centery = map(centerY , Y_min , Y_max , 0, 255);
// checking if the reading is floating
if (abs(y - centery) > MAX_FLOATING)
{
if (y > centery) stepY = STEP_Y;
if (y < centery) stepY = -STEP_Y;
}
if (lastAngleX >= 0)
{
lastAngleX += stepX;
lastAngleX = constrain(lastAngleX, servoX_MinAngle, servoX_MaxAngle);
result.X = lastAngleX;
}
lastAngleY >=0)
{
lastAngleY += stepY;
lastAngleY = constrain(lastAngleY, servoY_MinAngle+1, servoY_MaxAngle);
result.Y = lastAngleY;
}
break;
default:
if (DEBUG) Serial.println("Wrong mode chosen!");
break;
}
// disable the thumbstick selection
digitalWrite(pin_select, LOW);
return result;
}
Reviewing the Thumbstick API
Thumbstick
that determines how the thumbstick is connected to Intel Galileo or Edison and the limits of each servo. In other words, how are the thumbstick’s VRx and VRy terminals connected to the analog headers, how the button is connected to the digital ports, which pin on Intel Galileo or Edison will select the thumbstick (TS), and what the limits of the servos controlled by the x- and y-axis are.MOVE_T
defines the mode the thumbstick must work in; you can choose between ABSOLUTE_POSITION
and STEPS_IN_TOUCH
.JOYSTICK_XY_T
determines the position related to the X and Y axes and whether the button was pressed (buttonPressed
). This type is specifically returned by the read()
method, which receives MOVE_T
as an input parameter and internally makes all the conversions to returns the angles in the correct mode.calibrate()
, which performs the calibration, and isCalibrate()
, which checks if the thumbstick was calibrated properly.initPins()
method must be added to the setup()
function of the sketches to guarantee the digital and analog pins will be configured correctly.digitalWrite()
functions. Then it checks the button state by calling a private method called checkButtonState()
, which checks the current button state using a debounce. It uses the same technique used in the project called “Moisture Sensor” described in Chapter 8. The rest of the code reads the analog values of x and y using analogRead()
and, according to the mode passed through the variable format, makes the proper conversion. Remember that analogRead()
converts the reading in a value between 0 to 23 (read Chapter 3 for more details), and the ABSOLUTE_POSITION
or STEP_IN_TOUCH
mode converts these values to the proper angles on each mode. Of course, the ADC is not perfect. Even with 12 bits and considering you built a decent prototyping board, a small amount of floating is expected; that tolerance is controlled by MAX_FLOATING
.Thumbstick::JOYSTICK_XY_T Thumbstick::read(MOVE_T format)
{
Thumbstick::JOYSTICK_XY_T
result
= {0,0, false};
...
...
...
digitalWrite(pin_select, HIGH);
...
if (checkButtonState() == LOW)
{
result.buttonPressed = true;
}
...
// reading the raw analog values
Xvalue = analogRead(this->pinAnalogX);
Yvalue = analogRead(this->pinAnalogY);
switch (format)
{
...
...
...
case ABSOLUTE_POSITION: // The servo will move according joystick position
result.X = constrain(map(Xvalue , X_min , X_max , servoX_MinAngle, servoX_MaxAngle), servoX_MinAngle, servoX_MaxAngle);
result.Y = constrain(map(Yvalue , Y_min , Y_max , servoY_MinAngle, servoY_MaxAngle), servoY_MinAngle, servoY_MaxAngle);
break;
case STEPS_IN_TOUCH: // The joystick moves increases and decreases servos coordinates
int x;
int centerx;
x = map(Xvalue , X_min , X_max , 0, 255);
centerx = map(centerX , X_min , X_max , 0, 255);
// checking if the reading is floating
if (abs(x - centerx) > MAX_FLOATING)
{
if (x > centerx) stepX = STEP_X;
if (x < centerx) stepX = -STEP_X;
}
// reading the last angle and adding result.X
int y;
int centery;
y = map(Yvalue , Y_min , Y_max , 0, 255);
centery = map(centerY , Y_min , Y_max , 0, 255);
// checking if the reading is floating
if (abs(y - centery) > MAX_FLOATING)
{
if (y > centery) stepY = STEP_Y;
if (y < centery) stepY = -STEP_Y;
}
...
...
...
lastAngleX += stepX;
lastAngleX = constrain(lastAngleX, servoX_MinAngle, servoX_MaxAngle);
result.X = lastAngleX;
...
...
...
lastAngleY += stepY;
lastAngleY = constrain(lastAngleY, servoY_MinAngle+1, servoY_MaxAngle);
result.Y = lastAngleY;
...
...
...
}
// disable the thumbstick selection
digitalWrite(pin_select, LOW);
return result;
}
calibrate()
method reads the analog values when each thumbstick is on the central position during some samples controlled by the samplesOfrCenterXCounter
and samplesOfrCenterYCounter
variables. The user usually spins the thumbstick to check the maximum x and y values of each thumbstick. Such calibration is critical for getting the right result when the read()
method is called. Note all values are initiated with the value defined by NOT_CALIBRATED
, which is 22. The thumbsticks usually start in the center position and the analogic conversion varies between 0 and 1023. 22 is then too far from the center (average point) of 512 and mathematically you can conclude that the thumbsticks were not calibrated.Installing the API
code/Thumbstick
folder and it must be installed in your IDE. The installation process is simple:
Thumbstick
folder to the arduino-1.5.3/libraries
directory of your IDE installation.Thumbstick
. If it is, open the RoboticArm.ino
sketch in the examples
folder of the library or select Files ➤ Examples ➤ Thumbstick ➤ RoboticArm.ino using the menu. If the library is not present, check if the library is properly installed. You can also learn more about libraries at
http://arduino.cc/en/Guide/Libraries
.
A Sketch to Control the Robotic Arm
RoboticArm.ino
#include <Thumbstick.h>
#include <Servo.h>
#define PIN_SERVO_BASE 3
#define PIN_SERVO_SHOULDER 5
#define PIN_SERVO_ELBOW 6
#define PIN_SERVO_WRISTX 9
#define PIN_SERVO_WRISTY 11
#define PIN_SERVO_GRIPPER 10
// Defining the gripper angle limitations
#define GRIPPER_OPENED 90
#define GRIPPER_CLOSED 125
// Declaring the servos in the arm
Servo baseServo;
Servo shoulderServo;
Servo elbowServo;
Servo wristYServo;
Servo wristXServo;
Servo gripper;
// Declaring the sticks with
Thumbstick joystick1(8, A0, A1, 0, 0, 180, 90, 180);
Thumbstick joystick2(12, A2, A3, 1, 0, 180, 0, 180);
Thumbstick joystick3(13, A4, A5, 2);
// Variables used to store the results
Thumbstick::JOYSTICK_XY_T res1;
Thumbstick::JOYSTICK_XY_T res2;
Thumbstick::JOYSTICK_XY_T res3;
void setup()
{
// for debug purposes let's use the serial terminal
Serial.begin(115200);
// init the joystick pins
joystick1.initPins();
joystick2.initPins();
joystick3.initPins();
baseServo.attach(PIN_SERVO_BASE); // joystick 1 - axis -> X
shoulderServo.attach(PIN_SERVO_SHOULDER); // joystick 1 - axis -> y
elbowServo.attach(PIN_SERVO_ELBOW); // jostick 2 - axis -> Y
wristYServo.attach(PIN_SERVO_WRISTX); // joystick 3 - axis -> X
wristXServo.attach(PIN_SERVO_WRISTY); // joystick 3 - axis -> Y
gripper.attach(PIN_SERVO_GRIPPER); // joystick 1, 2 OR 3 - button
// initial position - all servos in 90 degrees and gripper opened
baseServo.write(90);
shoulderServo.write(90);
elbowServo.write(90);
wristYServo.write(90);
wristXServo.write(90);
gripper.write(GRIPPER_OPENED);
// a small delay to allow the user to open the serial terminal
delay(3000);
Serial.println("*** Calibrating Joysticks **** ");
Serial.println();
Serial.println("Reading the CENTERS of each control stick");
// let's run the calibration process for 15 seconds (15000 ms)
boolean msgShown = false;
long t = millis();
while (millis()-t < 15000) {
joystick1.calibrate();
joystick2.calibrate();
joystick3.calibrate();
if (joystick1.isCenterCalibrated()
&&
joystick2.isCenterCalibrated()
&&
joystick3.isCenterCalibrated()
&&
msgShown == false)
{
msgShown = true; // only to show the message one single time
Serial.println("Please, start spinning the thumbsticks !!!!");
}
}
if (DEBUG)
{
joystick1.dumpCalibration();
joystick2.dumpCalibration();
joystick3.dumpCalibration();
}
Serial.println("*** Calibration process completed.");
}
void loop()
{
static boolean isGripperOpened = true;
// checking the joysticks calibration
if (joystick1.isCalibrated() == false)
{
Serial.println("ERROR: Joystick 1 not calibrated");
}
if (joystick2.isCalibrated() == false)
{
Serial.println("ERROR: Joystick 2 not calibrated");
}
if (joystick3.isCalibrated() == false)
{
Serial.println("ERROR: Joystick 3 not calibrated");
}
// In case some tumbstick is not calibrated the user must be informed
if ((joystick1.isCalibrated() == false) ||
(joystick2.isCalibrated() == false) ||
(joystick3.isCalibrated() == false))
{
// aborting
Serial.println("You need to calibrate the joysticks !!! RESTART the sketch pressing RESET button!! ");
delay(1000);
return;
}
Thumbstick::MOVE_T move_type = Thumbstick::STEPS_IN_TOUCH; //let's use this first
//Thumbstick::MOVE_T move_type = Thumbstick::ABSOLUTE_POSITION;
//Thumbstick::MOVE_T move_type = Thumbstick::ANALOG_RAW;
// making the reading of all 3 thumbsticks
res1 = joystick1.read(move_type);
res2 = joystick2.read(move_type);
res3 = joystick3.read(move_type);
if (DEBUG)
{
Serial.print("Joystick1 -> X:");
Serial.print(res1.X);
Serial.print(" Y:");
Serial.print(res1.Y);
Serial.print(" Z:");
Serial.print(res1.buttonPressed);
Serial.print(" Joystick2 -> X:");
Serial.print(res2.X);
Serial.print(" Y:");
Serial.print(res2.Y);
Serial.print(" Z:");
Serial.print(res2.buttonPressed);
Serial.print(" Joystick3 -> X:");
Serial.print(res3.X);
Serial.print(" Y:");
Serial.print(res3.Y);
Serial.print(" Z:");
Serial.print(res3.buttonPressed);
Serial.print(" gripper:");
Serial.println(gripper.read());
}
// moving the servos according the responses of each thumbstick
baseServo.write(res1.X);
shoulderServo.write(res1.Y);
elbowServo.write(res2.Y);
wristYServo.write(res3.Y);
wristXServo.write(res3.X);
// in our example the button functionality of all button might be used
// to manipulate the gripper
if ((res1.buttonPressed) || (res2.buttonPressed) || (res3.buttonPressed))
{
if (isGripperOpened)
{
gripper.write(GRIPPER_CLOSED);
isGripperOpened = false;
}
else
{
isGripperOpened = true;
gripper.write(GRIPPER_OPENED);
}
// this small delay is to allow the gripper complete
// it is task before a new movement
delay(600);
}
}
Reviewing RoboticArm.ino
#define PIN_SERVO_BASE 3
#define PIN_SERVO_SHOULDER 5
#define PIN_SERVO_ELBOW 6
#define PIN_SERVO_WRISTX 9
#define PIN_SERVO_WRISTY 11
#define PIN_SERVO_GRIPPER 10
// Defining the gripper angle limitations
#define GRIPPER_OPENED 90
#define GRIPPER_CLOSED 125
// Declaring the servos in the arm
Servo baseServo;
Servo shoulderServo;
Servo elbowServo;
Servo wristYServo;
Servo wristXServo;
Servo gripper;
// Declaring the sticks with
Thumbstick joystick1(8, A0, A1, 0, 0, 180, 90, 180);
Thumbstick joystick2(12, A2, A3, 1, 0, 180, 0, 180);
Thumbstick joystick3(13, A4, A5, 2);
joystick3
the default values 0 and 180 degrees are used.setup()
function, the thumbsticks pins are initiated, the servos are attached to their respective pins, and the arm is placed at 90 degrees with the gripper opened. No calibration takes place.// init the joystick pins
joystick1.initPins();
joystick2.initPins();
joystick3.initPins();
baseServo.attach(PIN_SERVO_BASE); // joystick 1 - axis -> X
shoulderServo.attach(PIN_SERVO_SHOULDER); // joystick 1 - axis -> y
elbowServo.attach(PIN_SERVO_ELBOW); // jostick 2 - axis -> Y
wristYServo.attach(PIN_SERVO_WRISTX); // joystick 3 - axis -> X
wristXServo.attach(PIN_SERVO_WRISTY); // joystick 3 - axis -> Y
gripper.attach(PIN_SERVO_GRIPPER); // joystick 1, 2 OR 3 - button
// initial position - all servos in 90 degrees and gripper opened
baseServo.write(90);
shoulderServo.write(90);
elbowServo.write(90);
wristYServo.write(90);
wristXServo.write(90);
gripper.write(GRIPPER_OPENED);
setup()
function, the calibration is invoked in 15 seconds. It means you have 15 seconds to spin your thumbstick to calibrate them after the center of each thumbstick is detected.while (millis()-t <
15000
) {
joystick1.calibrate();
joystick2.calibrate();
joystick3.calibrate();
if (joystick1.isCenterCalibrated()
&&
joystick2.isCenterCalibrated()
&&
joystick3.isCenterCalibrated()
&&
msgShown == false)
{
msgShown = true; // only to show the message one single time
Serial.println("Please, start spinning the thumbsticks !!!!");
}
}
loop()
function and it is very simple. If the calibration was not made for any of thumbsticks, the program aborts and asks you to reset the sketch. Otherwise, the thumbsticks are read using “step in touch” mode, as explained previously.Thumbstick::MOVE_T move_type = Thumbstick::STEPS_IN_TOUCH; //let's use this first
// making the reading of all 3 thumbsticks
res1 = joystick1.read(move_type);
res2 = joystick2.read(move_type);
res3 = joystick3.read(move_type);
// moving the servos according the responses of each thumbstick
baseServo.write(res1.X);
shoulderServo.write(res1.Y);
elbowServo.write(res2.Y);
wristYServo.write(res3.Y);
wristXServo.write(res3.X);
// in our example the button functionality of all button might be used
// to manipulate the gripper
if ((res1.buttonPressed) || (res2.buttonPressed) || (res3.buttonPressed))
{
if (isGripperOpened)
{
gripper.write(GRIPPER_CLOSED);
isGripperOpened = false;
}
else
{
isGripperOpened = true;
gripper.write(GRIPPER_OPENED);
}
// this small delay is to allow the gripper complete
// it is task before a new movement
delay(600);
}
Running RoboticArm.ino
*** Calibrating Joysticks ****
Reading the CENTERS of each control stick
517
509
495
491
478
489
Please, start spinning the thumbsticks !!!!
At this point, spin each of the thumbsticks in all directions. Try to reach the maximum possible. Recall that you have 15 seconds to spin all the thumbsticks.
*** Calibration process completed.
ERROR: Joystick 1 not calibrated
ERROR: Joystick 2 not calibrated
ERROR: Joystick 3 not calibrated
You need to calibrate the joysticks !!! RESTART the sketch pressing RESET button!!
Joystick1 -> X:90 Y:177 Z:0 Joystick2 -> X:88 Y:88 Z:0 Joystick3 -> X:85 Y:88 Z:0 gripper:90
Joystick1 -> X:90 Y:177 Z:0 Joystick2 -> X:88 Y:88 Z:0 Joystick3 -> X:85 Y:88 Z:0 gripper:90
Joystick1 -> X:90 Y:177 Z:0 Joystick2 -> X:88 Y:88 Z:0 Joystick3 -> X:85 Y:88 Z:0 gripper:90
Joystick1 -> X:90 Y:177 Z:0 Joystick2 -> X:88 Y:88 Z:0 Joystick3 -> X:85 Y:88 Z:0 gripper:90
Joystick1 -> X:90 Y:177 Z:0 Joystick2 -> X:88 Y:88 Z:0 Joystick3 -> X:85 Y:88 Z:0 gripper:90
Joystick1 -> X:90 Y:177 Z:0 Joystick2 -> X:88 Y:88 Z:0 Joystick3 -> X:85 Y:88 Z:0 gripper:90
Joystick1 -> X:90 Y:177 Z:0 Joystick2 -> X:88 Y:88 Z:0 Joystick3 -> X:85 Y:88 Z:0 gripper:90
Joystick1 -> X:90 Y:177 Z:0 Joystick2 -> X:88 Y:88 Z:0 Joystick3 -> X:85 Y:88 Z:0 gripper:90
calibrating_the_arm.mp4
in the video
folder of this chapter. It shows the calibration process and the working arm.A Gripper Based on Coffee and a Balloon
http://creativemachines.cornell.edu/sites/default/files/PNAS10_Amend.pdf
.
Number | Description |
---|---|
3 | Vacuum pump, 12V 0-16'' Hg |
3 | 12V power supply 2A with switching power supply adapter |
1 | Relay module shield (one relay is enough) |
1 | Party balloon at least 24 inches wide |
1 | Funnel with 10mm or 8mm orifice |
1 | 4 feet 3/8'' (10mm) OD and 1/4'' ID (8mm) clear vinyl house |
1 | Small piece (2x2 inch) of filter fabric socket (also called drain sleeve) |
Some | Around 10 inches of 1/4w wires |
Some | Around 10 inches of 1/2w wires of at least two different colors |
sparkfun.com
. Check out the product at
https://www.sparkfun.com/products/10398
.http://www.amazon.com/JACKYLED-Switching-Power-Supply-Adapter/dp/B006NTNGN0
. It comes with a switching power supply adapter that makes the connection to the air pump easier.OUTPUT ALTERNATIVE 5V
.Preparing the Coffee Gripper
A Sketch for the Coffee Gripper
RoboticArm.ino
but make a small change to it. You’ll use the first thumbstick declared as joystick1
to control the coffee gripper. The digital header pin 4 will command the relay. If you make these changes to Listing 11-3, you’ll get the following:void setup()
{
...
...
...
pinMode(4, OUTPUT);
}
void loop()
{
static boolean isGripperOpened = true,
isCoffeeGripperActive = false
;
...
...
...
// in our example the button functionality of all button might be used
// to manipulate the gripper
if ((res2.buttonPressed) || (res3.buttonPressed))
{
if (isGripperOpened)
{
gripper.write(GRIPPER_CLOSED);
isGripperOpened = false;
}
else
{
isGripperOpened = true;
gripper.write(GRIPPER_OPENED);
}
// this small delay is to allow the gripper complete
// it is task before a new movement
delay(600);
}
if (res1.buttonPressed)
{
if (isCoffeeGripperActive == true)
{
isCoffeeGripperActive = false;
digitalWrite(4, LOW);
}
else
{
isCoffeeGripperActive = true;
digitalWrite(4, HIGH);
}
}
}
setup()
function, the digital pin 4 was set to OUTPUT
and it is used to command the relay shield. In the loop()
function, a second static variable called isCoffeeGripperActive
determines whether the air pump must be turned on. The second and third thumbsticks can control the mechanical gripper. On other hand, if the first thumbstick’s button is pressed, the air pump is turned on due to the relay activation on pin 4. This means the coffee gripper is working.code
directory of this chapter, there is a sketch named RoboticArmWithCoffeeGripper.ino based on Listing 11-3 with these changes already made.