Surgical Gripper Purpose and Design Summary:
With the rise of
surgical robotic applications, we explored this technology by creating a
gripper which could replicate human touch in some sense with the ability to
know how hard to grip when lifting various sized objects without causing
damage. Water beads were used to experiment with to simulate soft-tissue.
To accomplish this, the gripper receives a command from the user to close on an
object. The gripper stops closing around
the object at the appropriate time, without any external feedback. To make this
device useful in many different applications, it was designed to lift objects
without scooping it from underneath. Additionally, the device responds to the
user’s biological signal input so that the mechanism can be characterized as
biomechatronic in nature. The gripper was attached to a two degree-of-freedom
arm to enable the pick-and-place of the object. Demonstrations of device
effectiveness was done using both large and small water beads, reference the
links at the bottom of this page.
The final gripper design
employs a stepper motor coupled to a threaded rod which closes the angled grip
on the ball against a stationary, flat, rubber grip in a controlled manner
(Figure 1). To control the grip strength, the gripper combines the stepper
motor’s ability to track linear distance precisely, an ultrasound detector for
distance calibration, and a force pad for touch sensing. The gripper uses
ultrasonic distance sensor feedback for proportional control of the stepper
motor to automatically set and calibrate three inches of separation between the
grips, accommodating the largest object size. The grip strength is controlled
based on a 20% deformation of the ball, which provides adequate force without
breaking it. Grip separation is tracked through controlled stepper motor
rotation angles and known thread count, and ball diameter was calculated when
the force sensor reached a small threshold from consistent ball compression.
The grip closing and opening is controlled biologically by taking a deep breath
and twitching both the index and middle finger, respectively. These are
detected by a galvanic skin response (GSR) sensor. A joystick, combined with a
bluetooth module, remotely adjusts the arm position to move the gripper to the
ball position swiftly and precisely. The system successfully picked up both the
small and large ball (see video demonstrations below). The full system employed
three Arduinos for maximum control and function separation (Figure 2). The
joystick (1) and GSR (3) used individual Arduinos for isolated signal
monitoring for clean communication to the gripper control Arduino (2) through
serial and bluetooth connections, respectively. Finally, the gripper had a
final manufacturing price of $47.14, which is a minimal cost for successful
control of a surgical gripper (an itemized list is provided in the Appendix).
Video Demonstrations:
- Locating and Picking up Small Ball: https://www.youtube.com/watch?v=MrqzxAcpoPk&t=3s
- Locating Ball: https://youtu.be/Q-IDm-9BubY
Figure 1. The CAD
assembly of the final design (top left) and final prototype (top right) include
an angled grip and a stationary grip that holds the force sensor pad and
ultrasound distance detector for calibration. A full range of motion is
possible while the Arduino remains stationary (bottom left). The wireless
remote controller allows arm movement from safe distances (bottom right).
Figure 2. The
complete schematic for the electrical components (top) and corresponding
detailed circuit diagram (bottom) are provided for clarity of design.
Design and Reasoning:
I. Arm Control
A joystick is used to
control of the arm and ensures accurate and controlled movements. It allows the
user to easily move the robotic arm up, down, right, left, and in all four
diagonal directions. Two HC-05 bluetooth modules were integrated into the circuit
to enable remote control of the system, as would be desired in surgical robotic
applications. The ‘master’ Arduino transmits joystick feedback to the ‘slave’
Arduino, which receives movement instructions and actuates servo motors to
facilitate appropriate robotic arm movement.
II.
Mechanical Gripper
Design
Figure 3. Robotic
arm assembly consists of the following: 1) Arduino; 2) stepper motor; 3)
shaft-coupling; 4) threaded-rod drive shaft; 5) bushing; 6) guide rails; 7)
hex-nut; 8) linear bearings; 9) ultrasonic distance sensor; 10) force sensor;
11) angled grips; 12) acrylic base.
The final mechanical
grip design employs two components: 1) a stationary acrylic piece which
attaches to the robotic arm and houses a force sensor; and 2) an angled grip
that, driven by the threaded-rod drive shaft, moves linearly toward/away the
stationary component using a stepper motor and threaded rod linear actuation
mechanism (Figure 3). The stationary side provides attachment to the mobile arm
and guide rails for the linear actuation mechanism. Testing led us to wrap the
stationary, acrylic piece in rubber. This helped create a high-friction
surface and allowed us to pick up low-friction objects with less compressive
force (Figure 4). The angled grips
provide three points of contact for the ball and have the ability to center the
ball for maximum accuracy (Figure 3). This component was constructed of PLA
because it is a light, rigid surface that can be easily molded into the complex
shape provided. The grip was designed to be able to successfully pick up balls
ranging from 0.3 to 2.5 inches in diameter. At the beginning stages of the
prototyping, the grip was made of two angled pieces joined together to create
the current gripper shape. Upon testing, the gap between the two angled pieces
created a pressure point on the ball occasionally causing it to break before
sufficient force was applied to pick up the ball, so the current gripper design
was created to prevent that potential failure (Figure 3). The total grip design
consisted of a single PLA piece (50g, $2.67), two pieces of acrylic ($3.00),
rubber bands (<$0.10), and a force sensor ($8.95, free from lab) for a total
cost of around $10. The gripper design was economical, easily manufacturable,
and effective.
Figure 4. A rubber
surface was added to increase friction while gripping objects (far left). The
next three pictures show the progression of the design to eliminate the
pressure point created by two separate angled grips. In testing, the balls
tended to form cracks along those deformation points at lower forces.
III.
Control Feedback
Ultrasonic Distance Calibration
An ultrasonic distance
detector was used to calibrate the separation distance between the two grips to
three inches to accommodate the largest ball size and provided an initial point
to track stepper motor motion. An ultrasonic distance uses the speed of sound
through air to measure distance based on how long a sound echo takes to return
back to the sensor (Figure 5). We used proportional control feedback to
continuously measure the distance between the grips and move the motor
appropriately to set them three inches apart. The ultrasound sensor was
embedded into the top of the stationary grip and directed towards the full
angled grip plate (Figure 1, two small circles above the force pad and arm
connection). Originally, we wanted to directly measure ball diameter using the
ultrasound detector. This would be accomplished by measuring the distance to
the table top vertically down then shift the grip over the ball. Unfortunately,
the sensor has limited resolution and takes the fastest and largest signal
return. For example, the ultrasound detector placed flat on a table can detect
the 0.6 inch water bead horizontally within 12 inches of the sensor. However,
if a larger plate is placed behind the ball, the sensor instead detects the
distance from the place due to the larger signal. Therefore, the ultrasound
detector was used to measure distance between grips because it was more
reliable and still allows calibration of distance.
The ultrasound sensor
has difficulty measuring short distances because of the separation between the
transmitter and receiver and overall sensor resolution. Therefore, the
ultrasound sensor could not be used as the sole detector of distance between
grips when picking up the smallest (0.3 inch) ball. Instead, the precise angle
movements of the stepper motor were employed to track grip separation distance
(see next section). In addition to the ultrasound sensor, an infrared light
sensor and photoresistor were tested to measure ball distance. The original
design consisted of an LED light source on one side of the ball and an array of
IR or photoresistor sensors to measure where light gets through. Many issues
arose because the water beads are transparent or translucent. First, the light
is not completely occluded, so it is difficult to determine a resolution of
ball diameter. Further, light is concentrated because the ball acts as a convex
lense, so the distance of the ball from the sensors led to a different level of
light penetrating through the ball. The combination of these issues created an
inconsistent and unreliable measure of ball diameter.
Figure 5. Ultrasound echo sensor theory for
object distance calculation. Adapted from Dejan 2015
Stepper Motor and Threaded Rod
This gripper is designed
to pick up fragile objects. We use a threaded-rod drive shaft to open and
close the gripper, this allows us to close in smaller increments (reference
Figure 3). The drive shaft has 32-threads-per-inch; one rotation of the
drive shaft will open/close it 0.03”. A
stepper motor is used because it allows more control and lower RPM than a DC motor.
It also allows us to move in 1.8 degree steps, meaning that it takes 200
steps rotate the drive shaft one rotation.
The motor must take six steps just to close the gripper 0.001”. This design allows for high-resolution
control over how much the gripper opens/closes. Additionally, steps can be counted and be
used to keep track of distance between grippers.
The drive shaft and
stepper motors are paired using a coupling. This shaft-coupling was
manufactured by one of our team members.
Similar to other shaft-couplings, it makes the motor and drive shaft
concentric and then uses set-screws to keep the shafts in place.
A hex-nut, press-fitted
into a piece of acrylic, is used to convert motor-rotation into linear
movement. This is what allows the gripper to open and close. When the driveshaft rotates, it moves the
hex-nut forward/backward. Two aluminum guide rails prevent the hex-nut,
and acrylic,from rotating around the drive shaft. The gripper is outfitted with linear bearings
which allow low-friction movement along the guide rails. A bushing is
also used to allow the drive shaft to rotate on a frictionless surface.
Communication Between Arduinos
The Main Control Arduino
controls the stepper motor and both servo motors (Figure 2). This Arduino
repetitively check for incoming messages from the GSR Arduino and the Joystick
Arduino.
The GSR Arduino is
outfitted with a unique sampling rate, for optimal GSR processing. When
peaks are detected, indicating deep breath or finger twitch, it tells the Main
Control Arduino to close or open the gripper, respectively. These two
Arduinos communicate using serial communication, via RX pin 0 and TX pin
1. These two pins are pre-programmed for
serial communication between Arduinos.
The Joystick Arduino is
equipped with a joystick, for controlling the robotic arm. This Arduino
(master) analyzes joystick output and converts it to instructions for robotic
arm movement. A HC-05 bluetooth module is used to send these motor-actuation
instructions to the Main Control Arduino (slave), via a second serial
communication channel. This second serial communication channel was
established by using the SoftwareSerial.h
Arduino library to convert digital pins 11 and 12 to TX and RX pins.
Force Pad and Feedback Control
The force pad was used
as a touch sensor to detect when the ball was nestled in the grip but not yet
secure enough to be successfully lifted. Only triggers that lasted a set amount
of time were accounted for so as to rule out false positives. Once the force
sensor was triggered, the diameter of the ball was calculated based on the
original distance between the two sections of the gripper calculated by the
ultrasound and the number of motor rotations from the start until the force
sensor was triggered. The ball was then deformed by 20% of its diameter based
on preliminary testing of the water beads to secure the ball in the gripper
without it slipping out or getting damaged. This was a safe percentage of
deformation because experimentation showed that the ball would not break until
uniaxial deformation was greater than 40%.
We had originally planned to use the force sensor to
detect more exact forces and to have the gripper stop closing when the force
hit a certain threshold. This turned out to be an impractical method for this
application. Different areas on the force pad have inconsistent readings and
calibration curves, so the ball applying a force at the center would register a
different force (resistance change) than if the ball contacted the pad slightly
down or toward one side. Although our gripper design assured that the ball
would be centered left-to-right, ball diameter would affect the contact
location. Therefore, calibration of predicted force from water beads was not
possible. An additional issue was the fact that the force readings were not
reliable. Applying constant force (exact mass) to a set location two times
would have a significant level of difference, so creating a threshold would not
be feasible. It further would have been difficult to accurately choose the
proper threshold force for each of the different ball sizes accounting for
multiple inconsistencies. The heavier, larger balls would requires a higher
force grip than the smallest size balls, and the goal was to create a gripper
that would account for all sizes. A smaller force pad was attempted but was
ultimately not an option since the surface area was not sufficient to provide a
contact surface for all sized balls sitting on the table top.
IV.
Biological Feedback:
Galvanic Skin Response
The galvanic skin
response, or electrodermal response, is the change in electrical skin
conductance related to arousal, emotional stress, and autonomic nervous
response. External and internal stimuli that physiologically arouse us cause an
increase in conductance of the skin (MIT Media Lab). Empatica, a company
started by Rosalind Picard, ScD, from MIT research, employs the electrodermal
sensor to detect the onset of a grand mal seizure in epileptic patients for
quicker response and prevention of Sudden Unexplained Death in Epilepsy (SUDEP)
(Poh et al. 2012; Poh et al. 2012). It is measured through two nickel
electrodes on the index and middle finger using the skin as a variable resistor
in a voltage divider (Figure 6).
Figure 6. Circuit
diagram of skin conductance measurement with two electrodes.
The galvanic skin
response sensor was used for signaling the closing and opening of the gripper.
Closing was triggered by taking a deep breath which led to a slow steady
decrease of skin conductance with a sustained period before slowly returning to
normal levels. The deep breath relaxes the body which creates an autonomic nervous
response and decreases the skin conductance over time. Interestingly, laughing
had a similar effect, which highlights the relaxing and beneficial effects of
laughing. The gripper was opened by a finger twitch (spreading of the index and
middle fingers), which created a quick spike in electrical conductance that was
immediately reduced when the fingers were relaxed. The finger twitch created a
spike for unknown reasons; it is most likely due to the local tightening and
arousal of the skin. The galvanic skin response is difficult to consistently
measure because it has a wandering baseline at multiple frequencies and it is
not centered around a zero point (a few seconds due to breathing, attention,
etc. and a few minutes due to hormones, activity, etc.). Since the desired
signal is less than 1 Hz, a DC offset filter could not be applied. Instead, to
measure both a finger twitch and a deep breath, a running baseline average was
calculated over 10 seconds (see Appendix for Arduino code). This accounted for
any slow wandering baseline and created a more current measure to compare for
quick positive/negative changes in conductance. The twitch was calculated by a
simple threshold that was clearly greater than any rapid baseline wandering
from breathing, etc. The deep breath was detected by requiring a steady
decreasing skin conductance for 2 seconds followed by a sustained measurement
below a -75 mV threshold relative to the running baseline. Since the -75 mV
drop occurs occasionally during normal recording, the additional requirements
were needed to distinguish distinct deep breaths. An example detection paradigm
is included in Figure 7, below.
Figure 7. The raw galvanic skin response
sensor data (top,green) shows the difficult to discern events such as taking a
breath or twitching fingers since simple thresholding cannot be applied. The
red indicates the presence of a breath and the blue indicates a finger twitch.
The relative signal to running baseline allows for a clearer visualization and
consistent detection of a breath and finger twitch (bottom).
Due to
specific sensor requirements and sampling rate (200 Hz with averaging to 20Hz),
the system required an independent Arduino because the delays prevented smooth
and effective implementation of stepper and servo motor control. This required
a Serial communication connection between Arduinos to rapidly send signals.
Unfortunately, delays in the motor control motor led to missed signal sent by
the GSR in which the Arduino was not actively looking for signals. This created
some inconsistency in the control of grip opening and closing. Moving forward,
the speed of servo arm movement could be controlled with smaller or no delays
and smaller degree movements (using writeMicroseconds instead) that would allow
briefer signal communication with the GSR sensor.
Limitations and Future
Work:
While we were satisfied
with most aspects of this project, there are several ways to make it better for
a future version. Regarding the robot arm controller, we would work on more
accurately updating the code with proper delays to ensure smooth integration
with the Arduino controlling the gripper movement. This would be necessary to
prevent rapid uncontrolled movement of the arm. Another factor we would
consider introducing would be a WiFi module instead of bluetooth since there is
a chance that the former would be more reliable than the latter.
In using the GSR, there were times when the main Arduino
would miss the signal from the GSR that a deep breath occurred due to delays in
place in the code. This would prevent the gripper from closing, and it would
take a short period of time for the GSR to reset before another deep breath
could be registered. This caused the system to slow down considerably. In the
future we would ensure that the delays were set up in such a way that the
incoming signals would not be missed as a result (perhaps using microseconds.)
Since the stepper motor used to close the gripper was
quite slow, we would like to refine the motor speed control so that it would
move faster when it was far from the ball and slow down once it got closer.
Relatedly, we would ideally obtain a faster stepper motor in a future iteration
of this project so that significantly faster motion would be possible.
A design flaw that we would also aim to adjust in the
future was the positioning of the threaded rod coupled to the stepper motor.
Due to slight imperfections in the prototype, the threaded rod was not
completely parallel to the motor shaft which introduced lateral loads,
vibration and noise to the system.
References:
Media
Lab Massachusetts Institute of Technology. Galvactactivator Frequently Asked
Questions. https://www.media.mit.edu/galvactivator/faq.html.
Poh, M.
Z., Loddenkemper, T., Reinsberger, C., Swenson, N. C., Goyal, S., Madsen, J.
R., & Picard, R. W. (2012). Autonomic changes with seizures correlate with
postictal EEG suppression. Neurology, 78(23), 1868-76.
Poh, M.
Z., Loddenkemper, T., Reinsberger, C., Swenson, N. C., Goyal, S., Sabtala,
M.C., Madsen, J. R., & Picard, R. W. (2012). Convulsive seizure detection
using a wrist‐worn electrodermal activity and accelerometry biosensor. Epilepsia,
53(5), e93-7.
Appendix - Itemized Cost Analysis
·
$ 9.90 - GSR sensor
·
$ 2.67 - 3D printed PLA
plows (50g at $48/0.9 kg)
·
$28.00 - Linear Bearing
(x2)
·
$0.49 - 12” threaded-rod
drive shaft
·
$6.08 - 93.2 in^2
acrylic - Single sheet for angled and stationary grip attachment, motor
attachment, and additional base for support.
o $18.80 for 288 in^2 acrylic = $0.07 per square inch of acrylic
o Interfacing with plow = 15.8 in^2
o Interfacing with force sensor = 21.4 in^2
o Interfacing with / mounting the motor = 3.6 in^2
o Interfacing with / mounting the Arduino = 7.4 in^2
o Base = 45.0 in^2
o Total = 93.2 in^2
·
$0.00 - Freely
accessible components via non-BE resources
o Rubber Bands
o Stepper Motor (17HS08)
o Arduino Motor Shield
o Bushing
o Extruded Aluminum
§ 0.25” diameter x 5” long - for guide rails
§ 0.75” diameter x1.25” long - for coupling
o Screws & nuts
§ M3x0.5” SHCS (x4) for fastening into stepper motor
§ 6-32 FHCS (x4) for mounting base
§ 6-32 hex-nut (x4) for mounting base
·
$ 0.00 - Freely accessible
components via BE-lab resources
o $ 45.00 - Arduino Uno (x3)
o $ 20.00 - Bluetooth Module (x2)
o $ 3.95 - Ultrasonic Sensor
o $ 8.95 - Force Sensor (38mm square)
o $ 4.00 - 9 V battery and attachment
o $ 0.10 - Resistors
o $ 5.00 - Breadboard
o $6.49 - Joy Stick
o Power Supply
o ABS material (3.2 in^3) for angled gripper
o Robotic Arm/Base Kit
Appendix - Pseudo Code
Pseudo Code for the Joystick Arduino
- Determine the current location of the joystick
- Send the joystick location from Master to Slave, via
bluetooth serial communication
- Joystick location is expressed by eight different
characters (up, down, left, right, up & right, down & right, down
& left, up & left)
Pseudo Code for the Main Control Arduino
- During setup, calibrate the gripper… this involves
opening the gripper until the ultrasonic distance sensor indicates that
the gripper is open to 3 inches
- Continuously check the bluetooth serial monitor for
incoming messages
- Move the servos / robotic arm
according to joystick locations, provided by the Master arduino
- Continuously check the serial monitor used to
communicate to the GSR Arduino
- If receive instructions to
close gripper then run closeGripper() funcion
- then mover stepper motor till
force sensor indicates that the ball is being pressed
- Calculate diameter, then
compress ball further using deformation calculation based off of % of
ball diameter
- Run moveBall() function -
this allows us to move the ball and does not detect deep breaths, so the
gripper will not close further. This function allows us to
compartmentalize operation. In
this function, we only listen for joystick instructions & finger
twitches. This allows the user to control the robotic arm, and
open it, without the risk of accidentally closing the gripper
further.
- If receive instruction to
open gripper (finger twitch identified by GSR Arduino) then open the
gripper 0.5 inches
- If receive instruction to open
gripper (finger twitch identified by GSR Arduino)
- Open the gripper 0.5 inches
- Return to main loop once tasks
are completed
Pseudo Code for the GSR Arduino
- Calculate the current galvanic skin measure (average 10
points to avoid noise)
- Reset the value to 0 to calculate a new measure every
10 steps
- Average the 10 measurements to remove the glitch/noise
- Read current galvanic sensor value
- Add for 10 loops
- Delay between samples to prevent noise capturing. Want
a 200Hz sampling rate and average to 20 samples per average
- Calculate running average
- Subtract the last reading:
- Put current sensor value in an array holding values
from the last 10 seconds
- Divided by five minimizes data storage
- Add the current value to the total:
- Advance to the next position in the array:
- If at the end of the array...
- Wrap around to the beginning:
- Calculate the baseline over the last 10 seconds:
- Calculate the skin conductance value relative to the
running baseline
- Calculate finger twitch movement above a 0.25 V
increase in output signal
- Calculate deep breath
- Need to have steady decrease in skin conductance over
time
- If current value is less than previous value count the
number of times that occurs
- 2 second continuous decrease triggers breath slope
signal
- Reset breath slope count if is positive
- Create a breath slope signal for 3.75 seconds while the
breath threshold can be triggered
- Deep breath threshold
- 75 mV decrease compared to baseline
- Breath threshold must be maintained for 1.5 seconds
- Reset after trigger
- Reset unless threshold is maintained
- Reset breath trigger after breath is done
- Signal communication to Arduino controlling the gripper
- Output for visualization
Appendix - Code for the
Joystick Arduino
int lr0;
int ud0;
int lr;
int ud;
int current_base;
int current_elbow;
int max_thresh = 150;
int min_thresh = 70;
#include <Servo.h>
SoftwareSerial BTSerial(11, 12);
void setup() {
BTSerial.begin(9600); // Start the
SoftwareSerial connection at specified baud rate.
pinMode(A3, INPUT);
pinMode(A2, INPUT);
base.attach(9);
elbow.attach(10);
Serial.begin(9600);
lr0
= analogRead(A3);
ud0
= analogRead(A2);
}
void loop() {
lr
= analogRead(A3);
ud
= analogRead(A2);
//
Measure output from the joystick and interpret it into motion. Send
motion-instructions over bluetooth by using a simple character to indicate
direction.
if
(lr > lr0 + 100 && ud > ud0 + 100) { //move base right and elbow
up
Serial.println('B');
BTSerial.print('B');
delay(10);
}
else if (lr > lr0 + 100 && ud < ud0 - 100) { //move base right
and elbow down
Serial.println('D');
BTSerial.print('D');
delay(10);
}
else if (lr < lr0 - 100 && ud > ud0 + 100) { //move base left and
elbow up
Serial.println('H');
BTSerial.print('H');
delay(10);
}
else if (lr < lr0 - 100 && ud < ud0 - 100) { //move base left and
elbow down
Serial.println('F');
BTSerial.print('F');
delay(10);
}
else if (lr > lr0 + 100 && ud > ud0 - 100 && ud < ud0
+ 100) { //move base right
Serial.println('Q');
BTSerial.print('Q');
delay(10);
}
else if (lr < lr0 - 100 && ud > ud0 - 100 && ud < ud0
+ 100) { //move base left
Serial.println('G');
BTSerial.print('G');
delay(10);
}
else if (lr > lr0 - 100 && lr < lr0 + 100 && ud > ud0
+ 100) { //move elbow up
Serial.println('A');
BTSerial.print('A');
delay(10);
}
else if (lr > lr0 - 100 && lr < lr0 + 100 && ud < ud0
- 100) { //move elbow down
Serial.println('E');
BTSerial.print('E');
delay(10);
}
else {
Serial.println('N');
delay(10);
}
delay(100);
BTSerial.flush(); // Waits for the
transmission of outgoing serial data to complete
}
Appendix - Code For the
Main Control Arduino
// Intialize variables and libraries for all
code/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// MOTOR SHIELD
#include <Adafruit_MotorShield.h>
Adafruit_MotorShield AFMS =
Adafruit_MotorShield(); // Create the motor shield object with the
default I2C address
// STEPPER MOTOR
// Connect a stepper motor with 200 steps per
revolution (1.8 degree) to motor port #1 (M1 and M2)
const int stepsPerRotation = 200;
const int stepsPerRevolution = 200;
double currentDistance = 3.36 * 25.4;
const int rotationsPerInch = 32; //threaded
drive shaft has 32 threads per inch
const int stepsPerInch = stepsPerRevolution *
rotationsPerInch;
Adafruit_StepperMotor *stepperMotor =
AFMS.getStepper(stepsPerRotation, 1);
int dirStepper = 0; // set direction to for
gripper movement. Negative (-) is close, Positive (+) is open, 0 is no movement
// SERVO MOTORS
#include <Servo.h> // Include Servo Motor
Library
Servo base; // create servo object to
control the base servo
int basePos = 0; // variable
to store the servo position
int dirBaseServo = 0; // direction of base
rotation. Negative (-) is left, Positive (+) is right
// Initialize variables for elbow servo motor
Servo elbow; // create servo object to
control the elbow servo
int elbowPos = 70; // start
elbow position at Vertical (100)
int dirElbowServo = 0; // direction of elbow
motion. Negative (-) is up, Positive (+) is down
//FORCE SENSOR
// Initialize variables for force sensing
involved with grip feedback
int force = 0; // force measured (voltage
divider through resistor)
int count = 0; // Count for consistent strong
force to prevent random spikes
//GSR MONITOR
// Initialize variables to read breath and
finger twitch triggers from GSR
int incomingByte;
int breathTrig = 0;
int twitchTrig = 0;
int twitchCount = 0;
//ULTRASOUND SENSOR
// Initialize ultrasound distance
#define trigPin 7
#define echoPin 6
float duration, distance;
// SWITCHING MODES
int ballGrabbed = 0;
float steps = 1; // track distance moved to
calculate ball diameter
double deformation = 0;
double diameter;
// JOYSTICK
int lr0;
int ud0;
int lr;
int ud;
int current_base;
int current_elbow;
int max_thresh = 150;
int min_thresh = 70;
//Bluetooth
#include <SoftwareSerial.h>
//Insert this at the head of your program to
access the SoftwareSerial library.
// It is part of the Arduino distribution so you
do not need to download anything.
SoftwareSerial BTSerial(11, 12); // Declare a
SoftwareSerial object called "BTSerial"
// RX and TX pin numbers, pins 11 and 12
respectively. Make declaration before setup
char c;
void setup() {
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
initialize the serial port:
Serial.begin(9600);
//
MOTOR SHIELD
AFMS.begin(); // create with the
default frequency 1.6KHz
stepperMotor->setSpeed(600); //
600 rpm
//
FORCE SENSOR
//
Initialize force (pressure sensor) pin
pinMode(A1, INPUT);
//GSR MONITOR
pinMode(A0, INPUT);
pinMode(A3,
INPUT);
//
Initialize ultrasound distance
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
int
stepsPerInch = rotationsPerInch * stepsPerRotation;
//SERVOMOTORS
//
Initialize object assignments of pins
pinMode(A3, INPUT);
pinMode(A2,
INPUT);
base.attach(9);// attaches the servo on
pin 9 to the base servo
elbow.attach(10);// attaches the servo
on pin 10 to the elbow servo
elbow.write(elbowPos);
base.write(basePos);
lr0
= analogRead(A3);
ud0
= analogRead(A2);
//Bluetooth
BTSerial.begin(9600); // Start the
SoftwareSerial connection at specified baud rate.
//
Start by calibrating the gripper, open it to 3 inches. Once triggered to
close, we will count the steps from 3 inches, in order to determine the ball diameter.
calibrateGripper();
}
void loop() {
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
current_base = base.read();
current_elbow = elbow.read();
if
(BTSerial.available()) { // Determine whether there's a BTSerial.available
c = BTSerial.read();
delay(10);
if (current_elbow >
max_thresh) {
current_elbow =
max_thresh;
elbow.write(current_elbow);
base.write(current_base);
} else if (current_elbow
< min_thresh) {
current_elbow =
min_thresh;
elbow.write(current_elbow);
base.write(current_base);
}
if (c == 'B') { //move base
right and elbow up
Serial.println("
- base right, elbow up");
base.write(current_base
+ 1);
elbow.write(current_elbow
- 1);
delay(10);
} else if (c == 'D') {
//move base right and elbow down
Serial.println("
- base right elbow down");
base.write(current_base
+ 1);
elbow.write(current_elbow
+ 1);
delay(10);
} else if (c == 'H') {
//move base left and elbow up
Serial.println("
- base left elbow up");
base.write(current_base
- 1);
elbow.write(current_elbow
- 1);
delay(10);
} else if (c == 'F') {
//move base left and elbow down
Serial.println("
- base left elbow dwon");
base.write(current_base
- 1);
elbow.write(current_elbow
+ 1);
delay(10);
} else if (c == 'Q') {
//move base right
Serial.println("
- base right");
base.write(current_base
+ 1);
elbow.write(current_elbow);
delay(20);
} else if (c == 'G') {
//move base left
Serial.println("
- base left");
base.write(current_base
- 1);
elbow.write(current_elbow);
delay(10);
} else if (c == 'A') {
//move elbow up
Serial.println("
- elbow up");
elbow.write(current_elbow
- 1);
base.write(current_base);
delay(10);
} else if (c == 'E') {
//move elbow down
Serial.println("
- Elbow Down");
base.write(current_base);
elbow.write(current_elbow
+ 1);
delay(10);
} else {
Serial.println("
- stay current");
base.write(current_base);
elbow.write(current_elbow);
delay(10);
}
delay(15);
BTSerial.flush();
}
// monitor for GSR trigger sensing
if
(Serial.available() > 0) {
//****************************************************************************************************************************************
// read the incoming byte:
incomingByte =
Serial.read();
if (incomingByte == 66) {
//****************************************************************************************************************************************
Serial.println("Breath");
dirStepper = -1;
closeGripper();
}
if (incomingByte == 84) {
//this format allows us to
Serial.println("Twitch");
//dirStepper=1;
stepperMotor->
step(50 * stepsPerRevolution, BACKWARD, DOUBLE);
steps = steps +
1 * stepsPerRevolution;
}
}
}
void calibrateGripper() {
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
current_base = base.read();
current_elbow = elbow.read();
elbow.write(current_elbow);
base.write(current_base);
int
counter = 0;
int
i;
float distanceSum, distanceAvg;
while (counter == 0) {
i = 0;
distanceSum = 0;
// First few readings of
ultrasonic distance sensor are incorrect. Takes time to warm up. So, take the average of the first 200
samples.
for (int k = 0; k < 50;
k++) {
// Read the
distance from the ultrasound detector
digitalWrite(trigPin,
LOW);
delayMicroseconds(2);
digitalWrite(trigPin,
HIGH);
delayMicroseconds(10);
digitalWrite(trigPin,
LOW);
duration =
pulseIn(echoPin, HIGH);
distance =
(duration / 2) / 29.1;
distance =
distance * 10; // convert cm measurement to mm
distance =
distance - 0.25 * 25.4;
if (distance
>= 10) {
Serial.print("distance:
"); // Sensor measurements accounts for size of 3D printed plow. Distance
is in milimeters
Serial.println(distance);
distanceSum
= distanceSum + distance;
i++;
}
}
distanceAvg = distanceSum /
i;
Serial.print("distanceAvg:
");
Serial.println(distanceAvg);
// Open Gripper to 3 inches
- FAST MODE - 65 & 87 mm = more than 0.6 inches away
if (distanceAvg < 65) {
stepperMotor->step(stepsPerRevolution
* 5, BACKWARD, DOUBLE);
} else if (distanceAvg >
87) {
stepperMotor->step(stepsPerRevolution
* 5, FORWARD, DOUBLE);
}
// Open Gripper to 3 inches
- Slow down when close to 76mm
// Move stepper motor so
that the gripper is open to ~76mm
if (distanceAvg < 75) {
stepperMotor->step(stepsPerRevolution
/ 3, BACKWARD, DOUBLE);
} else if (distanceAvg >
77) {
stepperMotor->step(stepsPerRevolution
/ 3, FORWARD, DOUBLE);
} else {
counter = 1;
}
}
}
void closeGripper()
{/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
while (dirStepper == -1) {
stepperMotor->step(1,
FORWARD, DOUBLE);
steps = steps + 1;
force = analogRead(A1); //
monitor for when ball touches
// Gripper Control Via
Pressure Sensor - Stop the closing of the gripper if the force threshold is
reached
if (force > 50) {
if (ballGrabbed
!= 1) {
//ballGrabbed
= 1;
dirStepper
= 0;
//
Calculate ball diameter in inches
diameter
= currentDistance / 25.4 - steps / stepsPerRevolution / 32;
delay(1000);
deformation
= 0;
//
Calculate deformation based off of diameter (inches) of the ball
if
(diameter < 0.5) {
deformation
= 0.15 * diameter;
deformation
= round(deformation * 32 * stepsPerRevolution);
}
else if (diameter < 1) {
deformation
= 0.2 * diameter;
deformation
= round(deformation * 32 * stepsPerRevolution);
}
else if (diameter < 2) {
deformation
= 0.2 * diameter;
deformation
= round(deformation * 32 * stepsPerRevolution);
}
else if (diameter > 2) {
deformation
= 0.2 * diameter;
deformation
= round(deformation * 32 * stepsPerRevolution);
}
Serial.print("diameter=");
Serial.println(diameter);
Serial.print("deformation=");
Serial.println(deformation
/ stepsPerRevolution);
stepperMotor->step(deformation,
FORWARD, DOUBLE);
}
ballGrabbed = 1;
dirStepper = 0;
}
}
moveBall();
}
void moveBall() {
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int
ii = 0;
while (ii < 1) {
current_base = base.read();
current_elbow =
elbow.read();
//Check #1 of 2 - Bluetooth
Instructions - Check bluetooth to know what the joystick is doing.
if (BTSerial.available()) {
// Determine whether there's a BTSerial.available
c =
BTSerial.read();
delay(10);
// Makes sure
that the joystick does not push the servos beyond their range of motion.
if
(current_elbow > max_thresh) {
elbow.write(max_thresh);
current_elbow
= max_thresh;
} else if (current_elbow
< min_thresh) {
elbow.write(min_thresh);
current_elbow
= min_thresh;
}
if (c == 'B') {
//move
base right and elbow up
Serial.println("
- base right, elbow up");
base.write(current_base
+ 1);
elbow.write(current_elbow
- 1);
delay(10);
} else if (c ==
'D') { //move base right and elbow down
Serial.println("
- base right elbow down");
base.write(current_base
+ 1);
elbow.write(current_elbow
+ 1);
delay(10);
} else if (c ==
'H') { //move base left and elbow up
Serial.println("
- base left elbow up");
base.write(current_base
- 1);
elbow.write(current_elbow
- 1);
delay(10);
} else if (c ==
'F') { //move base left and elbow down
Serial.println("
- base left elbow dwon");
base.write(current_base
- 1);
elbow.write(current_elbow
+ 1);
delay(10);
} else if (c ==
'Q') { //move base right
Serial.println("
- base right");
base.write(current_base
+ 1);
delay(20);
} else if (c ==
'G') { //move base left
Serial.println("
- base left");
base.write(current_base
- 1);
delay(10);
} else if (c ==
'A') { //move elbow up
Serial.println("
- elbow up");
elbow.write(current_elbow
- 1);
delay(10);
} else if (c ==
'E') { //move elbow down
Serial.println("
- Elbow Down");
elbow.write(current_elbow
+ 1);
delay(10);
} else {
Serial.println("
- stay current");
base.write(current_base);
elbow.write(current_elbow);
delay(10);
}
delay(15);
BTSerial.flush();
delay(15);
}
// Check #2 of 2 - Check the
Serial Monitor for GSR trigger sensing
if (Serial.available() >
0) {
// read the
incoming byte:
incomingByte =
Serial.read();
if (incomingByte
== 66) { // Byte 66 translates to character ‘B’
Serial.println("Breath");
ii =
1;
closeGripper();
}
if (incomingByte
== 84) { //// Byte 66 translates to character ‘T’.
Serial.println("Twitch");
stepperMotor->
step(5 * stepsPerRevolution, BACKWARD, DOUBLE);
steps
= steps + 1 * stepsPerRevolution;
ii =
1;
}
}
}
}
Appendix - Code For the
GSR Arduino
// Variable initialization for raw sensor votage
readings
const int GSRPin = A1; //yellow pin for
recording skin conductance
int sensorValue = 0; //current galvanic skin
sensor value
int gsr_average = 0; //averaging galvanic sensor
value over 50 ms (10 data points)
//Variable initialization for calculating GSR
baseline wandering
const int runningAverageLength = 200; // running
averaging for peak detection over 10 seconds
double runningAvgArray[runningAverageLength]; //
array for storing averaging peak detection
int readIndex = 0;
//
the index of the current reading
double runningAvgSum = 0;
// the running total
int baseline = 0;
//
calculated running average
int gsr_relative_baseline = 0; // current
galvanic skin sensor value relative to the baseline
double gsr_relative_baseline_Array[runningAverageLength];
// Detecting breaths and finger twitchs
int twitch = 0;
//
Variable to trigger twitch communication to control Arduino
int twitch_detected = 0;
// A marker for plot visualization that a twitch
has been detected
int breath = 0;
//
Variable to trigger breath communication to control Arduino
// variables for breath slope calculation
int breath_slope;
int breath_slope_Trig_time;
int breath_slope_Mark;
// variables for breath threshold detection
int breathThresh_trigger = 0;
int breathThresh;
// variable for whether a breath was taken or
not and communication to control Arduino
int breath_taken = 0;
void setup() {
Serial.begin(9600); // For visualizing
live data input
pinMode(GSRPin, INPUT);
}
void loop() {
//Calculate
the current galvanic skin measure (average 10 points to avoid noise)
long sumNoise = 0; //reset the value to
0 to calculate a new measure every 10 steps
for
(int i = 0; i < 10; i++) { //Average the 10 measurements to remove the
glitch/noise
sensorValue =
analogRead(GSRPin); // read current galavanic sensor value
sumNoise += sensorValue; //
add for 10 loops
delay(5); //delay between
samples to prevent noise capturing. Want a 200Hz sampling rate and average to
20 samples per second
}
gsr_average = sumNoise / 10; // average
//
Calculate running average
//subtract the last reading:
runningAvgSum = runningAvgSum -
runningAvgArray[readIndex];
//
Put current sensor value in an array holding values from the last 10 seconds
runningAvgArray[readIndex]
= gsr_average / 5; // divided by five minimizes data storage
//
add the current value to the total:
runningAvgSum = runningAvgSum +
gsr_average / 5;
//
advance to the next position in the array:
readIndex = readIndex + 1;
//
if at the end of the array...
if
(readIndex >= runningAverageLength) {
// ...wrap around to the
beginning:
readIndex = 0;
}
//
calculate the baseline over the last 10 seconds:
baseline = (runningAvgSum /
runningAverageLength) * 5;
//
Calculate the skin conductance value relative to the running baseline
gsr_relative_baseline = gsr_average -
baseline;
gsr_relative_baseline_Array[readIndex] =
gsr_average - baseline;
//Calculate
finger twitch movement above a 0.25 V increase in output signal
if
(gsr_relative_baseline > 50) {
twitch = 10;
}
else if (gsr_relative_baseline <= 50) {
twitch = 0;
}
//
Calculate deep breath
//
Need to have steady decrease in skin conductance over time
//
if current value is less than previous value count the number of times that
occurs
if
(gsr_relative_baseline_Array[readIndex] <=
gsr_relative_baseline_Array[readIndex - 1]) {
breath_slope = breath_slope
+ 1;
if (breath_slope > 40) {
// 2 second continuous decrease triggers breath slope signal
breath_slope_Mark
= 1;
breath_slope =
0;
}
}
else { // Reset breath slope count if is positive
breath_slope = 0;
}
//
create a breath slope signal for 3.75 seconds while the breath threshold can be
triggered
if
(breath_slope_Mark > 0) {
breath_slope_Trig_time =
breath_slope_Trig_time + 1;
if (breath_slope_Trig_time
> 75) {
breath_slope_Trig_time
= 0;
breath_slope =
0;
breath_slope_Mark
= 0;
}
}
//
deep breath threshold
if
(gsr_relative_baseline < -15) { // 75 mV decrease compared to baseline
breathThresh = breathThresh
+ 1;
if (breathThresh > 30) {
// breath threshold must be maintained for 1.5 seconds
breathThresh_trigger
= 10;
breathThresh =
0; // reset after trigger
}
}
else {
breathThresh = 0; // reset
unless threshold is maintained
breathThresh_trigger = 0;
//reset breath trigger after breath is done
}
//Signal communication to Arduino
conrolloing the gripper
breath_taken = 0;
if
(breathThresh_trigger == 10) {
if (breath_slope_Mark >
0) {
Serial.write('B');
Serial.write(',');
breath_taken =
20;
}
}
twitch_detected = 0;
if
(twitch == 10) {
Serial.write('T');
Serial.write(',');
twitch_detected = 20;
}
//
Output for visualization
Serial.print(twitch_detected);
Serial.print(',');
Serial.print(breath_taken);
Serial.print(',');
Serial.println(gsr_relative_baseline);
}
Appendix - Code For the Main Control Arduino
// Intialize variables and libraries for all code/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// MOTOR SHIELD
#include <Adafruit_MotorShield.h>
Adafruit_MotorShield AFMS = Adafruit_MotorShield(); // Create the motor shield object with the default I2C address
// STEPPER MOTOR
// Connect a stepper motor with 200 steps per revolution (1.8 degree) to motor port #1 (M1 and M2)
const int stepsPerRotation = 200;
const int stepsPerRevolution = 200;
double currentDistance = 3.36 * 25.4;
const int rotationsPerInch = 32; //threaded drive shaft has 32 threads per inch
const int stepsPerInch = stepsPerRevolution * rotationsPerInch;
Adafruit_StepperMotor *stepperMotor = AFMS.getStepper(stepsPerRotation, 1);
int dirStepper = 0; // set direction to for gripper movement. Negative (-) is close, Positive (+) is open, 0 is no movement
// SERVO MOTORS
#include <Servo.h> // Include Servo Motor Library
Servo base; // create servo object to control the base servo
int basePos = 0; // variable to store the servo position
int dirBaseServo = 0; // direction of base rotation. Negative (-) is left, Positive (+) is right
// Initialize variables for elbow servo motor
Servo elbow; // create servo object to control the elbow servo
int elbowPos = 70; // start elbow position at Vertical (100)
int dirElbowServo = 0; // direction of elbow motion. Negative (-) is up, Positive (+) is down
//FORCE SENSOR
// Initialize variables for force sensing involved with grip feedback
int force = 0; // force measured (voltage divider through resistor)
int count = 0; // Count for consistent strong force to prevent random spikes
//GSR MONITOR
// Initialize variables to read breath and finger twitch triggers from GSR
int incomingByte;
int breathTrig = 0;
int twitchTrig = 0;
int twitchCount = 0;
//ULTRASOUND SENSOR
// Initialize ultrasound distance
#define trigPin 7
#define echoPin 6
float duration, distance;
// SWITCHING MODES
int ballGrabbed = 0;
float steps = 1; // track distance moved to calculate ball diameter
double deformation = 0;
double diameter;
// JOYSTICK
int lr0;
int ud0;
int lr;
int ud;
int current_base;
int current_elbow;
int max_thresh = 150;
int min_thresh = 70;
//Bluetooth
#include <SoftwareSerial.h>
//Insert this at the head of your program to access the SoftwareSerial library.
// It is part of the Arduino distribution so you do not need to download anything.
SoftwareSerial BTSerial(11, 12); // Declare a SoftwareSerial object called "BTSerial"
// RX and TX pin numbers, pins 11 and 12 respectively. Make declaration before setup
char c;
void setup() { ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// initialize the serial port:
Serial.begin(9600);
// MOTOR SHIELD
AFMS.begin(); // create with the default frequency 1.6KHz
stepperMotor->setSpeed(600); // 600 rpm
// FORCE SENSOR
// Initialize force (pressure sensor) pin
pinMode(A1, INPUT);
//GSR MONITOR
pinMode(A0, INPUT);
pinMode(A3, INPUT);
// Initialize ultrasound distance
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
int stepsPerInch = rotationsPerInch * stepsPerRotation;
//SERVOMOTORS
// Initialize object assignments of pins
pinMode(A3, INPUT);
pinMode(A2, INPUT);
base.attach(9);// attaches the servo on pin 9 to the base servo
elbow.attach(10);// attaches the servo on pin 10 to the elbow servo
elbow.write(elbowPos);
base.write(basePos);
lr0 = analogRead(A3);
ud0 = analogRead(A2);
//Bluetooth
BTSerial.begin(9600); // Start the SoftwareSerial connection at specified baud rate.
// Start by calibrating the gripper, open it to 3 inches. Once triggered to close, we will count the steps from 3 inches, in order to determine the ball diameter.
calibrateGripper();
}
void loop() { ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
current_base = base.read();
current_elbow = elbow.read();
if (BTSerial.available()) { // Determine whether there's a BTSerial.available
c = BTSerial.read();
delay(10);
if (current_elbow > max_thresh) {
current_elbow = max_thresh;
elbow.write(current_elbow);
base.write(current_base);
} else if (current_elbow < min_thresh) {
current_elbow = min_thresh;
elbow.write(current_elbow);
base.write(current_base);
}
if (c == 'B') { //move base right and elbow up
Serial.println(" - base right, elbow up");
base.write(current_base + 1);
elbow.write(current_elbow - 1);
delay(10);
} else if (c == 'D') { //move base right and elbow down
Serial.println(" - base right elbow down");
base.write(current_base + 1);
elbow.write(current_elbow + 1);
delay(10);
} else if (c == 'H') { //move base left and elbow up
Serial.println(" - base left elbow up");
base.write(current_base - 1);
elbow.write(current_elbow - 1);
delay(10);
} else if (c == 'F') { //move base left and elbow down
Serial.println(" - base left elbow dwon");
base.write(current_base - 1);
elbow.write(current_elbow + 1);
delay(10);
} else if (c == 'Q') { //move base right
Serial.println(" - base right");
base.write(current_base + 1);
elbow.write(current_elbow);
delay(20);
} else if (c == 'G') { //move base left
Serial.println(" - base left");
base.write(current_base - 1);
elbow.write(current_elbow);
delay(10);
} else if (c == 'A') { //move elbow up
Serial.println(" - elbow up");
elbow.write(current_elbow - 1);
base.write(current_base);
delay(10);
} else if (c == 'E') { //move elbow down
Serial.println(" - Elbow Down");
base.write(current_base);
elbow.write(current_elbow + 1);
delay(10);
} else {
Serial.println(" - stay current");
base.write(current_base);
elbow.write(current_elbow);
delay(10);
}
delay(15);
BTSerial.flush();
}
// monitor for GSR trigger sensing
if (Serial.available() > 0) { //****************************************************************************************************************************************
// read the incoming byte:
incomingByte = Serial.read();
if (incomingByte == 66) { //****************************************************************************************************************************************
Serial.println("Breath");
dirStepper = -1;
closeGripper();
}
if (incomingByte == 84) { //this format allows us to
Serial.println("Twitch");
//dirStepper=1;
stepperMotor-> step(50 * stepsPerRevolution, BACKWARD, DOUBLE);
steps = steps + 1 * stepsPerRevolution;
}
}
}
void calibrateGripper() { /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
current_base = base.read();
current_elbow = elbow.read();
elbow.write(current_elbow);
base.write(current_base);
int counter = 0;
int i;
float distanceSum, distanceAvg;
while (counter == 0) {
i = 0;
distanceSum = 0;
// First few readings of ultrasonic distance sensor are incorrect. Takes time to warm up. So, take the average of the first 200 samples.
for (int k = 0; k < 50; k++) {
// Read the distance from the ultrasound detector
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = (duration / 2) / 29.1;
distance = distance * 10; // convert cm measurement to mm
distance = distance - 0.25 * 25.4;
if (distance >= 10) {
Serial.print("distance: "); // Sensor measurements accounts for size of 3D printed plow. Distance is in milimeters
Serial.println(distance);
distanceSum = distanceSum + distance;
i++;
}
}
distanceAvg = distanceSum / i;
Serial.print("distanceAvg: ");
Serial.println(distanceAvg);
// Open Gripper to 3 inches - FAST MODE - 65 & 87 mm = more than 0.6 inches away
if (distanceAvg < 65) {
stepperMotor->step(stepsPerRevolution * 5, BACKWARD, DOUBLE);
} else if (distanceAvg > 87) {
stepperMotor->step(stepsPerRevolution * 5, FORWARD, DOUBLE);
}
// Open Gripper to 3 inches - Slow down when close to 76mm
// Move stepper motor so that the gripper is open to ~76mm
if (distanceAvg < 75) {
stepperMotor->step(stepsPerRevolution / 3, BACKWARD, DOUBLE);
} else if (distanceAvg > 77) {
stepperMotor->step(stepsPerRevolution / 3, FORWARD, DOUBLE);
} else {
counter = 1;
}
}
}
void closeGripper() {/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
while (dirStepper == -1) {
stepperMotor->step(1, FORWARD, DOUBLE);
steps = steps + 1;
force = analogRead(A1); // monitor for when ball touches
// Gripper Control Via Pressure Sensor - Stop the closing of the gripper if the force threshold is reached
if (force > 50) {
if (ballGrabbed != 1) {
//ballGrabbed = 1;
dirStepper = 0;
// Calculate ball diameter in inches
diameter = currentDistance / 25.4 - steps / stepsPerRevolution / 32;
delay(1000);
deformation = 0;
// Calculate deformation based off of diameter (inches) of the ball
if (diameter < 0.5) {
deformation = 0.15 * diameter;
deformation = round(deformation * 32 * stepsPerRevolution);
} else if (diameter < 1) {
deformation = 0.2 * diameter;
deformation = round(deformation * 32 * stepsPerRevolution);
} else if (diameter < 2) {
deformation = 0.2 * diameter;
deformation = round(deformation * 32 * stepsPerRevolution);
} else if (diameter > 2) {
deformation = 0.2 * diameter;
deformation = round(deformation * 32 * stepsPerRevolution);
}
Serial.print("diameter=");
Serial.println(diameter);
Serial.print("deformation=");
Serial.println(deformation / stepsPerRevolution);
stepperMotor->step(deformation, FORWARD, DOUBLE);
}
ballGrabbed = 1;
dirStepper = 0;
}
}
moveBall();
}
void moveBall() { ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int ii = 0;
while (ii < 1) {
current_base = base.read();
current_elbow = elbow.read();
//Check #1 of 2 - Bluetooth Instructions - Check bluetooth to know what the joystick is doing.
if (BTSerial.available()) { // Determine whether there's a BTSerial.available
c = BTSerial.read();
delay(10);
// Makes sure that the joystick does not push the servos beyond their range of motion.
if (current_elbow > max_thresh) {
elbow.write(max_thresh);
current_elbow = max_thresh;
} else if (current_elbow < min_thresh) {
elbow.write(min_thresh);
current_elbow = min_thresh;
}
if (c == 'B') {
//move base right and elbow up
Serial.println(" - base right, elbow up");
base.write(current_base + 1);
elbow.write(current_elbow - 1);
delay(10);
} else if (c == 'D') { //move base right and elbow down
Serial.println(" - base right elbow down");
base.write(current_base + 1);
elbow.write(current_elbow + 1);
delay(10);
} else if (c == 'H') { //move base left and elbow up
Serial.println(" - base left elbow up");
base.write(current_base - 1);
elbow.write(current_elbow - 1);
delay(10);
} else if (c == 'F') { //move base left and elbow down
Serial.println(" - base left elbow dwon");
base.write(current_base - 1);
elbow.write(current_elbow + 1);
delay(10);
} else if (c == 'Q') { //move base right
Serial.println(" - base right");
base.write(current_base + 1);
delay(20);
} else if (c == 'G') { //move base left
Serial.println(" - base left");
base.write(current_base - 1);
delay(10);
} else if (c == 'A') { //move elbow up
Serial.println(" - elbow up");
elbow.write(current_elbow - 1);
delay(10);
} else if (c == 'E') { //move elbow down
Serial.println(" - Elbow Down");
elbow.write(current_elbow + 1);
delay(10);
} else {
Serial.println(" - stay current");
base.write(current_base);
elbow.write(current_elbow);
delay(10);
}
delay(15);
BTSerial.flush();
delay(15);
}
// Check #2 of 2 - Check the Serial Monitor for GSR trigger sensing
if (Serial.available() > 0) {
// read the incoming byte:
incomingByte = Serial.read();
if (incomingByte == 66) { // Byte 66 translates to character ‘B’
Serial.println("Breath");
ii = 1;
closeGripper();
}
if (incomingByte == 84) { //// Byte 66 translates to character ‘T’.
Serial.println("Twitch");
stepperMotor-> step(5 * stepsPerRevolution, BACKWARD, DOUBLE);
steps = steps + 1 * stepsPerRevolution;
ii = 1;
}
}
}
}
Appendix - Code For the GSR Arduino
// Variable initialization for raw sensor votage readings
const int GSRPin = A1; //yellow pin for recording skin conductance
int sensorValue = 0; //current galvanic skin sensor value
int gsr_average = 0; //averaging galvanic sensor value over 50 ms (10 data points)
//Variable initialization for calculating GSR baseline wandering
const int runningAverageLength = 200; // running averaging for peak detection over 10 seconds
double runningAvgArray[runningAverageLength]; // array for storing averaging peak detection
int readIndex = 0; // the index of the current reading
double runningAvgSum = 0; // the running total
int baseline = 0; // calculated running average
int gsr_relative_baseline = 0; // current galvanic skin sensor value relative to the baseline
double gsr_relative_baseline_Array[runningAverageLength];
// Detecting breaths and finger twitchs
int twitch = 0; // Variable to trigger twitch communication to control Arduino
int twitch_detected = 0; // A marker for plot visualization that a twitch has been detected
int breath = 0; // Variable to trigger breath communication to control Arduino
// variables for breath slope calculation
int breath_slope;
int breath_slope_Trig_time;
int breath_slope_Mark;
// variables for breath threshold detection
int breathThresh_trigger = 0;
int breathThresh;
// variable for whether a breath was taken or not and communication to control Arduino
int breath_taken = 0;
void setup() {
Serial.begin(9600); // For visualizing live data input
pinMode(GSRPin, INPUT);
}
void loop() {
//Calculate the current galvanic skin measure (average 10 points to avoid noise)
long sumNoise = 0; //reset the value to 0 to calculate a new measure every 10 steps
for (int i = 0; i < 10; i++) { //Average the 10 measurements to remove the glitch/noise
sensorValue = analogRead(GSRPin); // read current galavanic sensor value
sumNoise += sensorValue; // add for 10 loops
delay(5); //delay between samples to prevent noise capturing. Want a 200Hz sampling rate and average to 20 samples per second
}
gsr_average = sumNoise / 10; // average
// Calculate running average
//subtract the last reading:
runningAvgSum = runningAvgSum - runningAvgArray[readIndex];
// Put current sensor value in an array holding values from the last 10 seconds
runningAvgArray[readIndex] = gsr_average / 5; // divided by five minimizes data storage
// add the current value to the total:
runningAvgSum = runningAvgSum + gsr_average / 5;
// advance to the next position in the array:
readIndex = readIndex + 1;
// if at the end of the array...
if (readIndex >= runningAverageLength) {
// ...wrap around to the beginning:
readIndex = 0;
}
// calculate the baseline over the last 10 seconds:
baseline = (runningAvgSum / runningAverageLength) * 5;
// Calculate the skin conductance value relative to the running baseline
gsr_relative_baseline = gsr_average - baseline;
gsr_relative_baseline_Array[readIndex] = gsr_average - baseline;
//Calculate finger twitch movement above a 0.25 V increase in output signal
if (gsr_relative_baseline > 50) {
twitch = 10;
} else if (gsr_relative_baseline <= 50) {
twitch = 0;
}
// Calculate deep breath
// Need to have steady decrease in skin conductance over time
// if current value is less than previous value count the number of times that occurs
if (gsr_relative_baseline_Array[readIndex] <= gsr_relative_baseline_Array[readIndex - 1]) {
breath_slope = breath_slope + 1;
if (breath_slope > 40) { // 2 second continuous decrease triggers breath slope signal
breath_slope_Mark = 1;
breath_slope = 0;
}
} else { // Reset breath slope count if is positive
breath_slope = 0;
}
// create a breath slope signal for 3.75 seconds while the breath threshold can be triggered
if (breath_slope_Mark > 0) {
breath_slope_Trig_time = breath_slope_Trig_time + 1;
if (breath_slope_Trig_time > 75) {
breath_slope_Trig_time = 0;
breath_slope = 0;
breath_slope_Mark = 0;
}
}
// deep breath threshold
if (gsr_relative_baseline < -15) { // 75 mV decrease compared to baseline
breathThresh = breathThresh + 1;
if (breathThresh > 30) { // breath threshold must be maintained for 1.5 seconds
breathThresh_trigger = 10;
breathThresh = 0; // reset after trigger
}
} else {
breathThresh = 0; // reset unless threshold is maintained
breathThresh_trigger = 0; //reset breath trigger after breath is done
}
//Signal communication to Arduino conrolloing the gripper
breath_taken = 0;
if (breathThresh_trigger == 10) {
if (breath_slope_Mark > 0) {
Serial.write('B');
Serial.write(',');
breath_taken = 20;
}
}
twitch_detected = 0;
if (twitch == 10) {
Serial.write('T');
Serial.write(',');
twitch_detected = 20;
}
// Output for visualization
Serial.print(twitch_detected);
Serial.print(',');
Serial.print(breath_taken);
Serial.print(',');
Serial.println(gsr_relative_baseline);
}
