Tendon Flex Controlled Robot Hand
Caeli MacLennan (cam476), Cassandra Scarpa (cls354), Parth Saraswat (ps978)
Introduction:
Our final project is a robotic hand controlled using a cuff on the wearer’s forearm. By measuring the
voltages of an array of flex sensors, we are able to determine changes in tendon flexure and muscle
movements on a user’s arm. Based on calibration data, we can then determine what hand or finger
movement occurred, if any. This project has real-world applications, primarily as a prosthetic.
Design:
Rationale:
An advanced prosthetic hand costs between $7500-$100,000. The high availability and
affordability of sensors, microcontrollers and 3D printing makes lower scale versions of such products
possible. Inspired by this, we decided to build a prosthetic hand capable of most of what is offered by
advanced prosthetic hands at ~1-10% of the cost.
Physiology:
The sensing method for this project was inspired by the visible deformation apparent below the
wrist when a finger is flexed. Each finger is connected by a tendon to a muscle in the lower
forearm. The flexor muscle can flex in unique shapes to pull specific tendons while applying
minimal force to the others in order to flex a finger. In past designs, using an electromyogram
(EMG) has been popular to determine muscle flexure; however, because the tendons controlling
the hand are attached to the same or very close together muscles, hand movement resolution is
poor. A prosthetic hand developed by Jason Maderer at Georgia Tech uses ultrasound of the
muscle and machine learning to determine the shape of the muscle in relation to the finger the
amputee intends to move. Other hand and arm prosthetics involve rearrangement of the nerves in
the remaining limb and placing electrodes to both give the user control over the robotic hand and
allow the user to feel what the robotic hand is touching. While incredibly impressive, both of the
aforementioned methods require bulky hardware that forces the device stationary. Using the
PIC32 and these small flex sensors, we wanted to explore the possibility of an inexpensive and
portable prosthetic that was capable of detailed hand movement.
Anatomy of muscles and tendons in anterior forearm
Credit:
https://teachmeanatomy.info/upper-limb/muscles/anterior-forearm/
Superficial flexor muscles of anterior (inner)
forearm
Deep Flexor Muscles of the anterior (inner) forearm
Hardware:
Components:
4 servo motors
PIC32
MUX
3D printed hand (credit: Ryan Gross)
Schottky Diodes
Resistors
Capacitor
TFT screen
Fishing line and paracord
2 Power supplies
Wooden Mounting board
Screws
Programmer
Coaxial cable
The Hand:
The robotic hand for our project was 3D printed from a model posted under a creative commons license,
designed by Ryan Gross. A length of paracord was run through the fingers and attached to the palm,
where the knuckles would usually be. This paracord forces the fingers to an open position. Through the
center of each finger, fishing line was strung. This line extended through channels running from the tip to
the base of the palm, where the wrist would be. Each line was tied to a servo motor with the middle and
ring finger being controlled by the same motor. The hand was taped to a wooden board, and the servos
were mounted using screws to the same board.
3D printed hand attached to board mount with servo motors
Flex Sensors:
For flex sensors, we used thin film piezos covered in a plastic laminate with two leads crimped at one end.
The piezo generates voltage in proportion to the speed at which is was flexed, a phenomenon known as
the piezo effects which is a result in a change of the electric polarization of the material. The reverse is
also true: applying a voltage across the leads causes the film to vibrate at a frequency that increases with
applied voltage. The properties of the sensors make them ideal for measuring acceleration and frequency,
or for use as a high frequency speaker or even a lighter. The extreme sensitivity of these sensors made
them attractive to us for use in measuring forearm deformation from muscle and tendon flexure.
A Piezo Film LDT0 Solid State Switch Vibration Sensor and the schematic, from the data sheet.
From probing the forearm first with our fingers and then with a piezo sensor, we
determined the ideal flex spots to be on the inner forearm, just below the wrist. However, the
majority of the differentiable, finger-by-finger tendon movement in this area occurs within a
surface area of approximately two by four centimeters, meaning we could not assign one flex
sensor to each tendon. Piezo films have the advantage of being trimmable to whatever size is
convenient, however the sensor was too stiff to give a reliable signal when trimmed short; the
torque applied from tendon flexure was not high enough. We decided to leave the sensors as one
cm by two cm tabs and use the combinations of sensor readings to determine which finger was
moving at what time. In addition to four the wrist sensors that primarily reacted to tendon
movement, we attached three other sensors to the lower forearm where muscle flexure was most
prominent for full hand open and closure, thumb open and closure, and combined middle and
ring closure. To eliminate 60Hz noise, the flex sensors were connected to a multiplexor using
coaxial cable.
Sensor placement on cuff
Schottky diodes:
Because the piezo sensors can generate up to 70V, we used schottky diodes to short the voltage to ground
or Vdd whenever the voltage dipped below 0V or exceeded 5V. To allow for the capture of the negative
portion of the signal, the piezos were attached between Vdd and Vdd/2 so that the signal centered around
Vdd/2.
Circuit diagram for one piezo sensor
Multiplexor (MUX):
To accommodate a large number of sensors we used a multiplexor connected to a single pin on the board.
The multiplexor is an analog device with three select pins, a power, a ground, a disable pin, a universal
input/output pin, and eight selectable input/output pins. The input/output pins can transmit voltages that
lie between the ground and power; anything outside of that range will destroy the MUX. We used the
selectable pins as inputs for the signals from the piezo films. The select pins were used to queue through
the selectable pins to quickly read all 8 sensors using just one pin on the PIC. The disable pin was shorted
to ground, as it prevented the MUX from transmitting data.
Servo Motors:
Four servo motors were used, each running on an external 5V power supply with a PWM signal from the
PIC. Most servo motors require the same peak to peak time of 20ms and an on time between one and two
milliseconds. The servo motors were capable of rotating up to 120 degrees.
Diagram of a single servo circuit
Software:
The software design of this project can be broken down into the following 6 units:
1. Data structures used
2. ISR
3. Calibration thread
4. Calculation thread
5. Motor thread
6. Main
The entire code for the project is written in one C file. Let’s look into further details of these units below:
Data structures used:
As mentioned in the Hardware Design section, we use piezo flex sensors as our primary sensors to
measure tendon flexes. In order to keep track of all the useful information from these sensors, we create a
flex sensor struct. The variables in the flex sensor structs are seen below:
Peak value read by each sensor
Minimum value read by each sensor
An array of peak values for particular movements
An array of min values for particular movements
An array of peak percentage values for particular movements
An array of min percentage values for particular movements
typedef struct flex_sensor {
float peak;
float min;
float peaks[11];
float mins[11];
float peak_percents[11];
float min_percents[11];
int valid; //0 or 1
float offset;
}flex_sensor;
flex_sensor sensors[8];
We create an array of these sensors on the last line, as we use 8 of them placed in strategic positions
across the forearm. Next, we focus on the functions of the ISR.
Interrupt Service Routine:
The interrupt service routine performs 3 main jobs:
1. Checks for calibration mode:
The calibration mode is used in order to ensure that the readings we take from the sensors are
calibrated such that the predictions we eventually make are personalised for the current user. We
will look into further detail about this mode in the calibration thread section.
2. Sets up and controls the counter:
As we are using a CD4051-BE multiplexer, we set up a counter in the ISR that counts through
0-7, and cycles through the multiple sensors.
3. Reads values from ADC:
The ISR finally reads the values from the mux’s output. This ensures that we are reading values at
a very quick rate and not missing any readings.
Calibration mode
What is calibration mode?
No two arms are the exact same, and calibration mode helps us ensure that the readings we take from the
sensors are calibrated such that the predictions we eventually make are personalized for the current user.
The mode takes the user through a series of hand movements and collects data that it will later use to
make predictions. Calibration mode starts off by defining the number of trials - which is the number of
runs of the predetermined movements that a user would be guided through during calibration. It then
checks if the user has initiated calibration mode by pressing a switch, and if so, zeros out all the peaks and
mins and peak percents and min percents in all the sensor structs. The calibration thread then collects
sensor peak, min, peak percent, min percent for each different movement, for each different sensor, for
each trial and maintains a running average of each of these values. The code snippet for this running
average can be seen below:
1. //Zero out arrays
2. for (i = 0; i < num_valid_sensors; i++)
3. {
4. for (j = 0; j < 11; j++)
5. {
6. sensors[i].peaks[j] = 0.0;
7. sensors[i].mins[j] = 0.0;
8. sensors[i].peak_percents[j] = 0.0;
9. sensors[i].min_percents[j] = 0.0;
10. }
11. }
12.
13. //print start message
14. tft_fillRoundRect(0, 0, 300, 380, 1, ILI9340_BLACK);
15. tft_setCursor(2, 180);
16. sprintf(buffer2, "%s", finger_msgs[0]);
17. tft_writeString(buffer2);
18. PT_YIELD_TIME_msec(1000);
19.
20. for (i = 0; i < num_trials; i++)
21. {
22. for (j = 0; j < 11; j++)
23. {
24. tft_fillRoundRect(0, 0, 300, 280, 1, ILI9340_BLACK);
25. tft_setCursor(2, 180);
26. sprintf(buffer2, "%s", finger_msgs[j + 1]);
27. tft_writeString(buffer2);
28. PT_YIELD_TIME_msec(2000);
29. for (k = 0; k < num_valid_sensors; k++) // adding all he peaks and peak percents to later average
30. {
31. peak_data[i][k][j] = float_abs(sensors[k].peak);
32. min_data[i][k][j] = float_abs(sensors[k].min);
33. peak_ratio_data[i][k][j] = float_abs(sensors[k].peak);
34. min_ratio_data[i][k][j] = float_abs(sensors[k].min);
35.
36. sensors[k].peaks[j] = sensors[k].peaks[j] + float_abs(sensors[k].peak);
37. sensors[k].mins[j] = sensors[k].mins[j] + float_abs(sensors[k].min);
38. sensors[k].peak_percents[j] = sensors[k].peak_percents[j] + float_abs(sensors[k].peak);
39. sensors[k].min_percents[j] = sensors[k].min_percents[j] + float_abs(sensors[k].min);
40. sensors[k].peak = -1024.0;
41. sensors[k].min = 1024.0;
42. }
43. }
44. }
It is important to note that all sensors’ readings may not be the best indicators of what the desired
movement was: so we assign different weights to each sensor for each particular movement. This
assignment is also done within the calibration thread:
1. //DETERMINE BEST WEIGHTS
2.
3. static float current_peaks[num_valid_sensors];
4. static float current_mins[num_valid_sensors];
5. static float errors[11][5];
6.
7. static float expected;
8. static float curr_peak_error;
9. static float curr_min_error;
10. static float current_peak_sum;
11. static float current_min_sum;
12. static float current_peak_percents[num_valid_sensors];
13. static float current_min_percents[num_valid_sensors];
14. static float scaler = 100;
15.
16. static int actions[10]; //CHECK
17.
18. static int max_accuracy = 0;
19. for (a = 0; a < num_trials; a++)
20. {
21. for (b = 0; b < 5; b++)
22. {
23. //zero out errors array
24. for (j = 0; j < 11; j++)
25. {
26. errors[j][b] = 0.0;
27. }
28.
29. for (i = 0; i < num_valid_sensors; i++)
30. {
31. for (j = 0; j < 11; j++)
32. {
33. expected = sensors[i].peaks[j];
34. curr_peak_error = float_abs(expected - peak_data[a][i][j]) * possible_weights[b];
35. errors[j][b] = errors[j][b] + curr_peak_error;
36. expected = sensors[i].mins[j];
37. curr_min_error = float_abs(expected - min_data[a][i][j]) * possible_weights[b];
38. errors[j][b] = errors[j][b] + curr_min_error;
39. expected = sensors[i].peak_percents[j];
40. curr_peak_error = float_abs(expected - peak_ratio_data[a][i][j]) * scaler * (1 - possible_weights[b]);
41. errors[j][b] = errors[j][b] + curr_peak_error;
42. expected = sensors[i].min_percents[j];
43. curr_min_error = float_abs(expected - min_ratio_data[a][i][j]) * scaler * (1 - possible_weights[b]);
44. errors[j][b] = errors[j][b] + curr_min_error;
45. }
46. }
47. }
48. }
49.
50. static float min;
51. static int min_weight_index;
52. for (j = 0; j < 11; j++)
53. {
54. min = errors[j][0];
55. min_weight_index = 0;
56. for (b = 1; b < 5; b++)
57. {
58. if (errors[j][b] <= min)
59. {
60. min = errors[j][b];
61. min_weight_index = b;
62. }
63. }
64. difference_weights[j] = possible_weights[min_weight_index];
65. ratio_weights[j] = 1 - difference_weights[j];
66. }
Calculation thread:
The calculation thread is in-charge of handling the data coming in from the 8 sensors attached to the
multiplexer and calculating what the predicted action is. It then sets the appropriate flag which the motor
thread can read and move the right motors to translate the action onto the 3D printed prosthetic hand.
The calculation thread starts off by declaring all the data structures and variables it will use, and initializes
the errors
array to all zeros. This errors array is the primary tool used in order to make our final
prediction. The code snippet for the variable initialization is:
1. static float current_peaks[num_valid_sensors];
2. static float current_mins[num_valid_sensors];
3. static float errors[11];
4.
5. static int i,j;
6.
7. static float expected;
8. static float curr_peak_error;
9. static float curr_min_error;
10. static float current_peak_sum;
11. static float current_min_sum;
12. static float current_peak_percents[num_valid_sensors];
13. static float current_min_percents[num_valid_sensors];
14. static float scaler = 100;
15.
16. static int actions[10];
17.
18. //zero out errors array
19. for (j=0; j < 11; j++) {
20. errors[j] = 0.0;
21. }
22. for (j=0; j < num_valid_sensors; j++) {
23. current_peak_percents[j] = 0.0;
24. current_min_percents[j] = 0.0;
25. }
26. current_peak_sum = 0.0;
27. current_min_sum = 0.0;
Then, the calculation thread iterates through the sensors, while keeping a running sum of the max and min
values that they are reading. It then sets the expected
variable equal to the values collected during
calibration. The key idea is to compare our readings at any given point of time to these expected values,
and perform whichever movement is closest to the values coming in from the sensors. The thread does
this by calculating the difference between the current values being read and the expected values, and then
keeps track of all these values by keeping a running sum of the differences in the errors array. Finally, the
calculation thread checks the errors array and finds which element of the errors array is the smallest. The
smallest index corresponds to the movement that the system has predicted. The code snippet
corresponding to this is shown here:
1. for (i = 0; i < num_valid_sensors; i++)
2. {
3. current_peaks[i] = float_abs(sensors[i].peak);
4. current_mins[i] = float_abs(sensors[i].min);
5. sensors[i].peak = sensors[i].peak - sensors[i].offset;
6. sensors[i].min = sensors[i].min - sensors[i].offset;
7. current_peak_sum += float_abs(sensors[i].peak);
8. current_min_sum += float_abs(sensors[i].min);
9. }
10. for (i = 0; i < num_valid_sensors; i++)
11. {
12. current_peak_percents[i] = float_abs(sensors[i].peak / current_peak_sum);
13. if (current_min_sum < 0.00001)
14. current_min_percents[i] = 0.0;
15. else
16. current_min_percents[i] = float_abs(sensors[i].min / current_min_sum);
17. }
18. for (i = 0; i < num_valid_sensors; i++)
19. {
20. for (j = 0; j < 11; j++)
21. {
22. expected = sensors[i].peaks[j];
23. curr_peak_error = float_abs(expected - current_peaks[i]) * difference_weights[j];
24. errors[j] = errors[j] + curr_peak_error;
25. expected = sensors[i].mins[j];
26. curr_min_error = float_abs(expected - current_mins[i]) * difference_weights[j];
27. errors[j] = errors[j] + curr_min_error;
28. expected = sensors[i].peak_percents[j];
29. curr_peak_error = float_abs(expected - current_peak_percents[i]) * scaler * ratio_weights[j];
30. errors[j] = errors[j] + curr_peak_error;
31. expected = sensors[i].min_percents[j];
32. curr_min_error = float_abs(expected - current_min_percents[i]) * scaler * ratio_weights[j];
33. errors[j] = errors[j] + curr_min_error;
34. }
35. }
36.
37. static float min;
38. min = errors[0];
39. min_index = 0;
40. for (j = 1; j < 11; j++)
41. {
42. if (errors[j] <= min)
43. {
44. min = errors[j];
45. min_index = j;
46. }
47. }
This minimum index can be used by the motor thread to perform the desired movements. Let’s look into
the functioning of the motor thread in the next subsection.
Motor thread:
The motor thread simply reads the value of the min_index of the errors array, and based on the value, tells
the appropriate motors to move clockwise or anticlockwise. It consists of a switch case statement that
turns the right servo motors based on the index of the smallest element in the errors array. The switch case
statement used by the motor thread can be seen in the following code snippet:
1. switch (min_index)
2. {
3. case 0: //close thumb
4. SetDCOC1PWM(pwm_close_time);
5. break;
6. case 1: //open thumb
7. SetDCOC1PWM(pwm_open_time);
8. break;
9. case 2: //close index
10. SetDCOC2PWM(pwm_close_time);
11. break;
12. case 3: //open index
13. SetDCOC2PWM(pwm_open_time);
14. break;
15. case 4: //close middle+ring
16. SetDCOC3PWM(pwm_close_time);
17. SetDCOC4PWM(pwm_close_time);
18. break;
19. case 5: //open middle+ring
20. SetDCOC3PWM(pwm_open_time);
21. SetDCOC4PWM(pwm_open_time);
22. break;
23. case 6: //close pinky
24. SetDCOC5PWM(pwm_close_time);
25. break;
26. case 7: //open pinky
27. SetDCOC5PWM(pwm_open_time);
28. break;
29. case 8: //close hand
30. SetDCOC1PWM(pwm_close_time);
31. SetDCOC2PWM(pwm_close_time);
32. SetDCOC3PWM(pwm_close_time);
33. SetDCOC4PWM(pwm_close_time);
34. SetDCOC5PWM(pwm_close_time);
35. break;
36. case 9: //open hand
37. SetDCOC1PWM(pwm_open_time);
38. SetDCOC2PWM(pwm_open_time);
39. SetDCOC3PWM(pwm_open_time);
40. SetDCOC4PWM(pwm_open_time);
41. SetDCOC5PWM(pwm_open_time);
42. break;
43. case 10:
44. default:
45. break;
46. }
Main:
The main function in this code sets up the timers and the output compares for the PWM used to control
the servo motors. It also initializes some of the arrays and values in the sensor structs so that we don’t
encounter any bugs because of values in memory as a result of previous runs. Finally, the main thread
schedules the aforementioned threads in a round robin fashion:
1. // round-robin scheduler for threads
2. while (1)
3. {
4. PT_SCHEDULE(protothread_timer(&pt_timer));
5. PT_SCHEDULE(protothread_motor(&pt_motor));
6. PT_SCHEDULE(protothread_calibration(&pt_calibration));
7. PT_SCHEDULE(protothread_calculation(&pt_calculation));
8. }
Results:
Speed of execution:
In order to react to human motion, which requires a rapid response to input, our software consisted of
multiple concurrent threads and an interrupt service routine, each responsible for different calculations
and events. The ISR, which collects flex sensor values from the ADC, runs at 200kHz and reads 1 of the 8
flex sensors each time. This allows us to sample each sensor at a fast enough rate that we can be sure that
we will not miss a significant peak or minimum value from any sensor. The calculation thread, which
compares current values to those collected during calibration in order to determine whether a movement
has occurred, runs every 500 milliseconds, giving enough time for an accurate peak or minimum to have
been gathered for each sensor. The motor thread, which generates the PWM signals that actually move the
hand, runs every 20 milliseconds, so that any action predicted by the calculation thread can be converted
to movement almost immediately. The calibration thread itself executes very slowly, allowing 2 seconds
for each movement. This is mainly done to minimize human error - in this timeframe, the user must read
the prompt from the TFT and move their hand accordingly. This slow runtime does not impact the rest of
our system, however, as calibration mode is completely blocking.
Calibration data:
Oscilloscope grabs are shown below for 3 different flex sensors on an open of the index finger. These
types of responses were a large motivation for us to consider both the maximum and minimum values of
each flex sensor reading, as well as their ratios to each other.
Additionally, we set up serial communication to perform automated data collection over any number of
actions and trials. Below is an example of data collected during a close of an index finger. Column 0
contains the sensor id, which corresponds to a placement on the cuff. Columns 1 and 2 are the peak and
minimum values, respectively, read from that sensor over the sampling interval.
Accuracy:
Due to the nature of materials used in our design, there were many factors impacting accuracy that were
variable from trial to trial. These mainly included the positioning of the sensors on the cuff, how tightly
they were able to be taped down, as well as the positioning and stretchiness of the cuff itself. Thus, the
only actions that we were able to obtain a consistent accuracy of almost 100% were opening and closing
the hand. Index and thumb actions could be predicted with reasonable accuracy around 50% of the time,
although the index would sometimes move the ring and pinky fingers with it as well. The middle, ring,
and pinky fingers generally had low accuracy, but we have been able to run a few successful trials of
differentiating pinky finger movements, as well as moving the middle and ring together as a unit. Treating
the middle, ring, and pinky fingers as a unit improved accuracy significantly - using this scheme, we were
able to predict this movement more than 75% of the time.
Safety:
Although our project involves measuring human physiology, safety was not a major concern, as the
hardware we used was very low risk. The only components making contact with human skin (through a
cuff) are the flex piezos, which do not pose any dangers. All lead solder joints are covered. The cuff itself
was made form a sock designed to be worn for compression, so it will not cut off circulation.
Additionally, we do not supply any voltages greater than 5V, and do not use any wireless communication.
Interference:
We were careful to eliminate noise during the making of this project, as our own circuits are sensitive. We
use shielded wire to prevent incoming noise. Interference is not an issue, as the technologies we use, such
as ADC and PWM, generate very little noise.
Usability:
Our project is designed to be used as a prosthetic for people either without a hand, or with limited use of
one(due to medical conditions such as a severe tremor). The calibration aspect is intended to increase
usability, as it allows for different arm sizes and muscle and tendon movements. The calibration process
is user-friendly, guiding the wearer through a series of available actions. While calibration does have a
learning curve, as it requires deliberate movements, this is not rare for prosthetics.
Video:
Check out our project on Youtube!
Video we took during lab!
Conclusion:
Our project did not fully meet our original expectations, mainly due to the accuracy limitations discussed
in the previous section. Otherwise, we believe it is a successful prototype. There are multiple future
improvements that we would like to make. These include: a better cuff design such as a 3D printed
exoskeleton, and integrating a Raspberry Pi or another device that would allow us to use machine learning
models while still maintaining a timely response to movement. Several aspects of the signals were
ignored, mainly information in the frequency domain. A faster processor and more inclusive data
collection would allow for the integration of the frequency information into action identification.
Currently, the hand is only capable of complete opens and closes; because the piezo sensors respond to
acceleration as opposed to displacement, determination of intermediate positions between open and closed
is not feasible. The inclusion of EMG and other sensors could allow for both better categorical accuracy
in regards to finger determination and collect data used to map partial finger flexes onto the robotic hand.
Using a different kind of piezo film may allow in higher spatial resolution, and integrating a greater
number sensors would allow for a more complete mapping of forearm topography.
Standards:
This project adheres to IEEE standards being a project that is intended to function for people of all ages,
races, gender, etc. Because it is not intended for market, there should be no conflict of interest involved in
the creation of this demo. If we were designing the hand as a marketable product, we would need the help
of a prosthetist and the individuals physician to comply with the sixth agreement; however, since this is a
concept project and not intended for use, this is not necessary.
Intellectual Property Considerations:
The 3D model of prosthetic hand for this project is licensed under creative commons. The attribution card
is linked in the Appendix.
Ethical Considerations:
As our project is meant to help people with physical disabilities, it is consistent with the IEEE code of
ethics in that we seek to improve health, reduce discrimination, and our project has no other design
aspects that would be considered harmful or unethical.
Safety Considerations:
There were no other significant safety considerations.
Legal Considerations:
Our device would fall under the FDA as a class 1 medical device and would only be regulated by general
controls. On the FDA website this device falls under “external limb prosthetic component” because it
does not include the elbow or shoulder. The requirements state, “The device is exempt from the premarket
notification procedures in subpart E of part 807 of this chapter, subject to the limitations in 890.9. The
device is also exempt from the current good manufacturing practice requirements of the quality system
regulation in part 820 of this chapter, with the exception of 820.180, regarding general requirements
concerning records and 820.198, regarding complaint files.”Under the IEEE ethical standards we would
likely still have to comply with good manufacturing practices, which requires maintaining records, reports
and replacement/repair of defective products.
Appendix:
Permissions:
The group approves this report for inclusion on the course website.
The group approves the video for inclusion on the course youtube channel.
Commented code:
/*
* File: Robot Hand Software
* Author: Caeli MacLennan, Cassandra Scarpa, Parth Saraswat
* For use with Sean Carroll's Big Board
* Adapted from:
* main.c by
* Author: Syed Tahmid Mahbub
* Target PIC: PIC32MX250F128B
*/
////////////////////////////////////
// clock AND protoThreads configure
#include "config_1_3_2.h"
// threading library
#include "pt_cornell_1_3_2.h"
#include "tft_master.h"
#include "tft_gfx.h"
#include <stdlib.h>
#define DAC_config_chan_A 0b0011000000000000
#define DAC_config_chan_B 0b1011000000000000
// pullup/down macros
// PORT B
#define EnablePullDownB(bits) CNPUBCLR=bits; CNPDBSET=bits;
#define DisablePullDownB(bits) CNPDBCLR=bits;
#define EnablePullUpB(bits) CNPDBCLR=bits; CNPUBSET=bits;
#define DisablePullUpB(bits) CNPUBCLR=bits;
//PORT A
#define EnablePullDownA(bits) CNPUACLR=bits; CNPDASET=bits;
#define DisablePullDownA(bits) CNPDACLR=bits;
#define EnablePullUpA(bits) CNPDACLR=bits; CNPUASET=bits;
#define DisablePullUpA(bits) CNPUACLR=bits;
// string buffer
char buffer[60];
char buffer2[60];
////////////////////////////////////
// DAC ISR
// A-channel, 1x, active
#define DAC_config_chan_A 0b0011000000000000
// B-channel, 1x, active
#define DAC_config_chan_B 0b1011000000000000
//== Timer 2 interrupt handler ===========================================
volatile unsigned int DAC_data ;// output value
volatile SpiChannel spiChn = SPI_CHANNEL2 ; // the SPI channel to use
volatile int spiClkDiv = 2 ; // 20 MHz max speed for this DAC
volatile float adc_9;
//PWM
volatile unsigned int pwm_open_time = 5250;//2500 PWM unit = 1 ms
volatile unsigned int pwm_close_time = 1000;//2500 PWM unit = 1 ms
//counters and flags
volatile unsigned int count = 0;
volatile int calibration_mode = 0;
// represents a flex sensor
typedef struct flex_sensor {
float peak;
float min;
float peaks[11];
float mins[11];
float peak_percents[11];
float min_percents[11];
int valid; //0 or 1
float offset;
}flex_sensor;
flex_sensor sensors[8];
// For each action, weights for absolute differences vs ratio differences
volatile float difference_weights[11];
volatile float ratio_weights[11];
#define num_valid_sensors 8
// Messages to be printed during calibration mode
char finger_msgs[12][25] = {"open hand, don't move", "close thumb", "open thumb", "close index", "open index",
"close middle+ring", "open middle+ring", "close pinky", "open pinky", "close hand", "open hand", "do nothing"};
// Lock between calculation and motor threads
int motor_lock = 0;
//Global variables accessed by calculation and motor threads
int min_index = 8;
int max_accuracy_index = 0;
// Helper function for float absolute value
float float_abs(float f) {
if (f < 0) return -1.0*f;
else return f;
}
// Interrupt service routine. This is where data collection occurs.
void __ISR(_TIMER_3_VECTOR, ipl2) Timer3Handler(void)
{
//If button is pressed, turn on calibration mode
if (mPORTAReadBits(BIT_0)) {
calibration_mode = 1;
}
//Counter - cycles through the valid flex sensors each time ISR runs
// Sets the mux select bits
if (count == num_valid_sensors-1) {
count = 0;
mPORTBClearBits(BIT_9 | BIT_8 | BIT_7);
}
else {
switch(count) {
case 0:
count = 1;
mPORTBSetBits(BIT_7); //001
break;
case 1:
count = 2;
mPORTBClearBits(BIT_7); //010
mPORTBSetBits(BIT_8);
break;
case 2:
count = 3;
mPORTBSetBits(BIT_7); //011
break;
case 3:
count = 4;
mPORTBClearBits(BIT_8 | BIT_7); //100
mPORTBSetBits(BIT_9);
break;
case 4:
count = 5;
mPORTBSetBits(BIT_7); //101
break;
case 5:
count = 6;
mPORTBClearBits(BIT_7); //110
mPORTBSetBits(BIT_8);
break;
case 6:
count = 7;
mPORTBSetBits(BIT_7); //111
break;
default:
count = 0;
mPORTBClearBits(BIT_9 | BIT_8 | BIT_7);
break;
}
}
// Read the ADC
adc_9 = ReadADC10(0);
AcquireADC10();
// Update peak and min values for the current sensor
if (sensors[count].valid) {
if (adc_9 > sensors[count].peak) sensors[count].peak = adc_9;
if (adc_9 < sensors[count].min) sensors[count].min = adc_9;
}
mT3ClearIntFlag();
// Output ADC on the DAC - useful for debugging to see what we are reading
DAC_data = adc_9*4;
// test for ready
while (TxBufFullSPI2());
mPORTBClearBits(BIT_4); // start transaction
// write to spi2
WriteSPI2(DAC_config_chan_A | (DAC_data & 0xfff));
// test for done
while (SPI2STATbits.SPIBUSY); // wait for end of transaction
// MUST read to clear buffer for port expander elsewhere in code
int junk;
junk = ReadSPI2();
// CS high
mPORTBSetBits(BIT_4); // end transaction
}
// === thread structures ============================================
static struct pt pt_motor, pt_calibration, pt_calculation;
// === Calibration Thread ==========================================
static PT_THREAD (protothread_calibration(struct pt *pt))
{
PT_BEGIN(pt);
while(1){
static int i, j, k, a, b;
#define num_trials 5
// Structures for storing intermediate data
static float possible_weights[5] = {0.0, 0.25, 0.5,0.75, 1.0};
static float peak_data[num_trials][num_valid_sensors][11];
static float min_data[num_trials][num_valid_sensors][11];
static float peak_ratio_data[num_trials][num_valid_sensors][11];
static float min_ratio_data[num_trials][num_valid_sensors][11];
if (calibration_mode) {
//Zero out arrays
for (i=0; i < num_valid_sensors; i++) {
for (j=0; j < 11; j++) {
sensors[i].peaks[j] = 0.0;
sensors[i].mins[j] = 0.0;
sensors[i].peak_percents[j] = 0.0;
sensors[i].min_percents[j] = 0.0;
}
}
//print start message
tft_fillRoundRect(0,0, 300, 380, 1, ILI9340_BLACK);
tft_setCursor(2, 180);
sprintf(buffer2,"%s", finger_msgs[0]);
tft_writeString(buffer2);
PT_YIELD_TIME_msec(1000) ;
// Repeat for every trial and action
for (i=0; i < num_trials; i++){
for (j=0; j<11; j++){
// Print a message for each action
tft_fillRoundRect(0,0, 300, 280, 1, ILI9340_BLACK);
tft_setCursor(2, 180);
sprintf(buffer2,"%s", finger_msgs[j+1]);
tft_writeString(buffer2);
PT_YIELD_TIME_msec(2000) ;
// For each sensor, save the peak and min values
for (k = 0; k < num_valid_sensors;k++)
{
peak_data[i][k][j] = float_abs(sensors[k].peak);
min_data[i][k][j] = float_abs(sensors[k].min);
peak_ratio_data[i][k][j] = float_abs(sensors[k].peak);
min_ratio_data[i][k][j] = float_abs(sensors[k].min);
// Sum up the values to later average
sensors[k].peaks[j] = sensors[k].peaks[j] + float_abs(sensors[k].peak);
sensors[k].mins[j] = sensors[k].mins[j] + float_abs(sensors[k].min);
sensors[k].peak_percents[j] = sensors[k].peak_percents[j] + float_abs(sensors[k].peak);
sensors[k].min_percents[j] = sensors[k].min_percents[j] + float_abs(sensors[k].min);
// Reset peak and min values
sensors[k].peak = -1024.0;
sensors[k].min = 1024.0;
}
}
}
// For each sensor and action
for (j = 0; j<num_valid_sensors; j++){
for (i =0; i < 11; i++)
{
// Average all the trials
sensors[j].peaks[i] = sensors[j].peaks[i]/ ((float) num_trials);
sensors[j].mins[i] = sensors[j].mins[i]/ ((float) num_trials);
sensors[j].peak_percents[i] = sensors[j].peak_percents[i]/ ((float) num_trials);
sensors[j].min_percents[i] = sensors[j].min_percents[i]/ ((float) num_trials);
sensors[j].offset = sensors[j].offset/num_trials;
}
}
static float peak_sum;
static float min_sum;
// Calculate ratios for each action by summing all peaks and mins
for (i =0; i < 11; i++) {
peak_sum = 0.0;
min_sum = 0.0;
for (j = 0; j<num_valid_sensors; j++){
sensors[j].peak_percents[i] = sensors[j].peak_percents[i] - sensors[j].offset;
sensors[j].min_percents[i] = sensors[j].min_percents[i] - sensors[j].offset;
peak_sum += sensors[j].peak_percents[i];
min_sum += sensors[j].min_percents[i];
}
for (j = 0; j<num_valid_sensors; j++){
sensors[j].peak_percents[i] = sensors[j].peak_percents[i]/ peak_sum;
sensors[j].min_percents[i] = sensors[j].min_percents[i]/ min_sum;
for (k = 0; k < num_trials; k++){
peak_ratio_data[k][j][i] = peak_ratio_data[k][j][i]/peak_sum;
min_ratio_data[k][j][i] = min_ratio_data[k][j][i]/min_sum;
}
}
}
// Determine the best weights for difference and ratio by minimizing
// the error over all trials
static float current_peaks[num_valid_sensors];
static float current_mins[num_valid_sensors];
static float errors[11][5];
static float expected;
static float curr_peak_error;
static float curr_min_error;
static float current_peak_sum;
static float current_min_sum;
static float current_peak_percents[num_valid_sensors];
static float current_min_percents[num_valid_sensors];
static float scaler = 100;
static int max_accuracy = 0;
// For each trial and possible weight
for (a = 0; a < num_trials; a++){
for (b =0; b < 5; b++){
//zero out errors array
for (j=0; j < 11; j++) {
errors[j][b] = 0.0;
}
// Calculate the error using this weight
for (i=0; i < num_valid_sensors; i++) {
for (j=0; j < 11; j++) {
expected = sensors[i].peaks[j];
curr_peak_error = float_abs(expected - peak_data[a][i][j])*possible_weights[b];
errors[j][b] = errors[j][b] + curr_peak_error;
expected = sensors[i].mins[j];
curr_min_error = float_abs(expected - min_data[a][i][j])*possible_weights[b];
errors[j][b] = errors[j][b] + curr_min_error;
expected = sensors[i].peak_percents[j];
curr_peak_error = float_abs(expected - peak_ratio_data[a][i][j])*scaler*(1-possible_weights[b]);
errors[j][b] = errors[j][b] + curr_peak_error;
expected = sensors[i].min_percents[j];
curr_min_error = float_abs(expected - min_ratio_data[a][i][j])*scaler*(1-possible_weights[b]);
errors[j][b] = errors[j][b] + curr_min_error;
}
}
}}
// Find the weight with the minimum error across trials
static float min;
static int min_weight_index;
for (j=0; j < 11; j++) {
min = errors[j][0];
min_weight_index= 0;
for(b=1; b<5; b++) {
if (errors[j][b] <= min) {
min = errors[j][b];
min_weight_index = b;
}
}
// Update global array with best weights
difference_weights[j] = possible_weights[min_weight_index];
ratio_weights[j] = 1 - difference_weights[j];
}
// Calibration is finished
calibration_mode = 0;
}
else PT_YIELD_TIME_msec(300) ; // If not calibrating, just yield
}
PT_END(pt);
}
// === Calculation Thread =================================================
static PT_THREAD (protothread_calculation(struct pt *pt))
{
PT_BEGIN(pt);
while(1) {
//Test motor lock
while (calibration_mode) PT_YIELD_TIME_msec(20) ;
while (motor_lock) PT_YIELD_TIME_msec(20) ;
static float current_peaks[num_valid_sensors];
static float current_mins[num_valid_sensors];
static float errors[11];
static int i,j;
static float expected;
static float curr_peak_error;
static float curr_min_error;
static float current_peak_sum;
static float current_min_sum;
static float current_peak_percents[num_valid_sensors];
static float current_min_percents[num_valid_sensors];
static float scaler = 100;
//zero out errors array
for (j=0; j < 11; j++) {
errors[j] = 0.0;
}
for (j=0; j < num_valid_sensors; j++) {
current_peak_percents[j] = 0.0;
current_min_percents[j] = 0.0;
}
current_peak_sum = 0.0;
current_min_sum = 0.0;
// For each sensor, calculate current peaks, mins and ratios
for (i=0; i < num_valid_sensors; i++) {
current_peaks[i] = float_abs(sensors[i].peak);
current_mins[i] = float_abs(sensors[i].min);
sensors[i].peak = sensors[i].peak - sensors[i].offset;
sensors[i].min = sensors[i].min - sensors[i].offset;
current_peak_sum += float_abs(sensors[i].peak);
current_min_sum += float_abs(sensors[i].min);
}
for (i=0; i < num_valid_sensors; i++) {
current_peak_percents[i] = float_abs(sensors[i].peak / current_peak_sum);
if (current_min_sum < 0.00001) current_min_percents[i] = 0.0;
else current_min_percents[i] = float_abs(sensors[i].min / current_min_sum);
}
// Calculate the error between saved data from calibration and what
// we just calculated, scaled by their corresponding weights
for (i=0; i < num_valid_sensors; i++) {
for (j=0; j < 11; j++) {
expected = sensors[i].peaks[j];
curr_peak_error = float_abs(expected - current_peaks[i])*difference_weights[j];
errors[j] = errors[j] + curr_peak_error;
expected = sensors[i].mins[j];
curr_min_error = float_abs(expected - current_mins[i])*difference_weights[j];
errors[j] = errors[j] + curr_min_error;
expected = sensors[i].peak_percents[j];
curr_peak_error = float_abs(expected - current_peak_percents[i])*scaler*ratio_weights[j];
errors[j] = errors[j] + curr_peak_error;
expected = sensors[i].min_percents[j];
curr_min_error = float_abs(expected - current_min_percents[i])*scaler*ratio_weights[j];
errors[j] = errors[j] + curr_min_error;
}
}
// Find the action with the minimum error - this is the prediction
static float min;
min = errors[0];
min_index = 0;
for (j=1; j < 11; j++) {
if (errors[j] <= min) {
min = errors[j];
min_index = j;
}
}
//reset peaks and mins
for (i=0; i < num_valid_sensors; i++) {
sensors[i].peak = -1024.0;
sensors[i].min = 1024.0;
}
PT_YIELD_TIME_msec(500) ;
}
PT_END(pt);
}
// === Motor Thread =================================================
static PT_THREAD (protothread_motor(struct pt *pt))
{
PT_BEGIN(pt);
while(1) {
// Don’t move hand during calibration
while (calibration_mode) PT_YIELD_TIME_msec(20) ;
// Temporarily block calculation thread
motor_lock = 1;
static int i;
switch(min_index) {
case 0: //close thumb
SetDCOC1PWM(pwm_close_time);
break;
case 1: //open thumb
SetDCOC1PWM(pwm_open_time);
break;
case 2: //close index
SetDCOC2PWM(pwm_close_time);
break;
case 3: //open index
SetDCOC2PWM(pwm_open_time);
break;
case 4: //close middle+ring
SetDCOC3PWM(pwm_close_time);
SetDCOC4PWM(pwm_close_time);
break;
case 5: //open middle+ring
SetDCOC3PWM(pwm_open_time);
SetDCOC4PWM(pwm_open_time);
break;
case 6: //close pinky
SetDCOC5PWM(pwm_close_time);
break;
case 7: //open pinky
SetDCOC5PWM(pwm_open_time);
break;
case 8: //close hand
SetDCOC1PWM(pwm_close_time);
SetDCOC2PWM(pwm_close_time);
SetDCOC3PWM(pwm_close_time);
SetDCOC4PWM(pwm_close_time);
SetDCOC5PWM(pwm_close_time);
break;
case 9: //open hand
SetDCOC1PWM(pwm_open_time);
SetDCOC2PWM(pwm_open_time);
SetDCOC3PWM(pwm_open_time);
SetDCOC4PWM(pwm_open_time);
SetDCOC5PWM(pwm_open_time);
break;
case 10:
default:
break;
}
// Calculation can now run
motor_lock = 0;
PT_YIELD_TIME_msec(20) ;
}
PT_END(pt);
} // motor thread
// === Main ======================================================
void main(void) {
//SYSTEMConfigPerformance(PBCLK);
PPSOutput(2, RPB5, SDO2);
// control CS for DAC
mPORTBSetPinsDigitalOut(BIT_4);
mPORTBSetBits(BIT_4);
mPORTASetPinsDigitalIn(BIT_0);//set pin RA0 to input
EnablePullDownA(BIT_0);
// divide Fpb by 2, configure the I/O ports. Not using SS in this example
// 16 bit transfer CKP=1 CKE=1
// possibles SPI_OPEN_CKP_HIGH; SPI_OPEN_SMP_END; SPI_OPEN_CKE_REV
// For any given peripherial, you will need to match these
// clk divider set to 4 for 10 MHz
SpiChnOpen(SPI_CHANNEL2, SPI_OPEN_ON | SPI_OPEN_MODE16 | SPI_OPEN_MSTEN |
SPI_OPEN_CKE_REV, 4);
// end DAC setup
ANSELA = 0; ANSELB = 0;
// the ADC ///////////////////////////////////////
// configure and enable the ADC
CloseADC10(); // ensure the ADC is off before setting the configuration
// define setup parameters for OpenADC10
// Turn module on | ouput in integer | trigger mode auto | enable autosample
// ADC_CLK_AUTO -- Internal counter ends sampling and starts conversion (Auto convert)
// ADC_AUTO_SAMPLING_ON -- Sampling begins immediately after last conversion completes; SAMP bit is
automatically set
// ADC_AUTO_SAMPLING_OFF -- Sampling begins with AcquireADC10();
#define PARAM1 ADC_FORMAT_INTG16 | ADC_CLK_AUTO | ADC_AUTO_SAMPLING_OFF //
// define setup parameters for OpenADC10
// ADC ref external | disable offset test | disable scan mode | do 1 sample | use single buf | alternate mode off
#define PARAM2 ADC_VREF_AVDD_AVSS | ADC_OFFSET_CAL_DISABLE | ADC_SCAN_OFF |
ADC_SAMPLES_PER_INT_1 | ADC_ALT_BUF_OFF | ADC_ALT_INPUT_OFF
//
// Define setup parameters for OpenADC10
// use peripherial bus clock | set sample time | set ADC clock divider
// ADC_CONV_CLK_Tcy2 means divide CLK_PB by 2 (max speed)
// ADC_SAMPLE_TIME_5 seems to work with a source resistance < 1kohm
#define PARAM3 ADC_CONV_CLK_PB | ADC_SAMPLE_TIME_5 | ADC_CONV_CLK_Tcy2
//ADC_SAMPLE_TIME_15| ADC_CONV_CLK_Tcy2
// define setup parameters for OpenADC10
// set AN11 and as analog inputs
#define PARAM4 ENABLE_AN11_ANA // pin 24
// define setup parameters for OpenADC10
// do not assign channels to scan
#define PARAM5 SKIP_SCAN_ALL
// use ground as neg ref for A | use AN11 for input A
// configure to sample AN11
SetChanADC10(ADC_CH0_NEG_SAMPLEA_NVREF | ADC_CH0_POS_SAMPLEA_AN11); // configure to
sample AN11
OpenADC10(PARAM1, PARAM2, PARAM3, PARAM4, PARAM5); // configure ADC using the parameters
defined above
EnableADC10(); // Enable the ADC
///////////////////////////////////////////////////////
OpenTimer2(T2_ON | T2_SOURCE_INT | T2_PS_1_16, 50000);
OpenTimer3(T3_ON | T3_SOURCE_INT | T3_PS_1_1, 50000);
// set up the timer interrupt with a priority of 2
ConfigIntTimer3(T3_INT_ON | T3_INT_PRIOR_2);
mT3ClearIntFlag(); // and clear the interrupt flag
//MUX CONTROL
mPORTBSetPinsDigitalOut(BIT_9 | BIT_8 | BIT_7); // Control signals for mux
// set up compare3 for PWM mode
// pwm only
OpenOC1(OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE , 0, 0);
PPSOutput(1, RPB3, OC1);
OpenOC2(OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE , 0, 0);
PPSOutput(2, RPA1, OC2);
OpenOC4(OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE , 0, 0);
PPSOutput(3, RPA2, OC4);
OpenOC3(OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE , 0, 0);
PPSOutput(4, RPA3, OC3);
OpenOC5(OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE , 0, 0);
PPSOutput(3, RPA4, OC5);
// === config threads ==========
PT_setup();
// === setup system wide interrupts ========
INTEnableSystemMultiVectoredInt();
// init the threads
PT_INIT(&pt_timer);
PT_INIT(&pt_motor);
// init the display
// NOTE that this init assumes SPI channel 1 connections
tft_init_hw();
tft_begin();
tft_fillScreen(ILI9340_BLACK);
//240x320 vertical display
tft_setRotation(0); // Use tft_setRotation(1) for 320x240
// Initialize arrays
int i,j;
for(i=0; i < num_valid_sensors; i++) {
sensors[i].valid = 1;
sensors[i].peak = -1024.0;
sensors[i].min = 1024.0;
sensors[i].offset = 0.0;
for (j=0; j < 11; j++) {
sensors[i].peaks[j] = 0.0;
sensors[i].mins[j] = 0.0;
sensors[i].peak_percents[j] = 0.0;
sensors[i].min_percents[j] = 0.0;
}
}
for (j=0; j < 11; j++) {
difference_weights[j] = 0.5;
ratio_weights[j] = 0.5;
}
// round-robin scheduler for threads
while (1){
PT_SCHEDULE(protothread_motor(&pt_motor));
PT_SCHEDULE(protothread_calibration(&pt_calibration));
PT_SCHEDULE(protothread_calculation(&pt_calculation));
}
} // main
// === end ======================================================
Schematics:
Circuit for a single flex sensor
Circuit for a single servo motor
Hand Assembly
Assembly of the 3D printed hand by Ryan
Gross. Screenshots taken from the hand
assembly video on
thingiverse:
https://www.thingiverse.com/th
ing:1691704
Cost details:
Part
Amount
Expense per part
Total
expense
Part number
PIC32
1
5
5
Big board
1
10
10
Power supply (board)
1
5
5
Power supply (bench)
1
5
5
MicroStick II
1
1
1
TFT LCD Screen
1
10
10
Whiteboard
3
6
18
Jumper cables
40
0.1
4
Flex piezo
8
3
24
MSP1006-ND (digikey)
Servo
4
0
0
3D printed hand
1
0
0
Fishing line
1
0
0
Paracord
1
3
3
Sock
1
0
0
Total
85
Parts with a cost of 0 were owned, created, or found by the group members.
Division of tasks:
Our group worked very closely together on all aspects of the project. There were very few tasks that were
the sole responsibility of one person, and what was split up was mainly experimental.
Caeli MacLennan
Cassandra Scarpa
Parth Saraswat
All Members
3D printed
hand
mounting
High level
design,
hardware
Serial data
collection
script
Results,
introduction,
conclusion
and appendix
sections of
report.
Machine
learning
model
Software
Design
section of
report
All other
hardware,
software and
design tasks
References:
Data Sheets:
https://www.ti.com/store/ti/en/p/product/?p=CD4051BE
https://www.digikey.com/product-detail/en/te-connectivity-measurement-specialties/1002794/MSP1006-
ND/279646
Designs:
https://www.thingiverse.com/thing:1691704/attribution_card
Research:
https://www.news.gatech.edu/2017/12/11/force-strong-amputee-controls-individual-prosthetic-fingers
https://teachmeanatomy.info/upper-limb/muscles/anterior-forearm/
https://openbionics.com/hero-arm/