-
Notifications
You must be signed in to change notification settings - Fork 9
/
InMoov.ino
159 lines (122 loc) · 3.49 KB
/
InMoov.ino
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
#include <Servo.h>
Servo servos[5];
int numFingers = 5;
int safeClosed = 135;
int safeOpen = 45;
int openThreshold[] = {
70, //thumb - 0 MIN
70, //pointer 0 MIN
135, //middle 0 MIN
70, //ring 0 MIN
70 //pinky 0 MIN
};
int closedThreshold[] = {
160, //thumb - 180 MAX
180, //pointer - 180 MAX
5, //middle - 180 MAX
170, //ring - 180 MAX
160 //pinky - 180 MAX
};
int pos[5]; //array of current finger positions ... this gets written to the hand
float normalizedPos;
int led = 3;
int targetValue = 0;
int actualValue = 0;
int fadeAmount = 5; // how many points to fade the LED by
int addToValue = 0;
float actualValueFloat = 0.0;
float numberToHand = 0.0;
void setup() {
Serial.begin(115200);
// for(int i = 0; i < numFingers; i++){
// servos[i].attach(i + 2);
// pos[i] = openThreshold[i]; //set all fingers to open by default
// }
servos[0].attach(A1);
servos[1].attach(A2);
servos[2].attach(A3);
servos[3].attach(A4);
servos[4].attach(A5);
pinMode(led, OUTPUT);
}
void loop() { // Loop through motion tests
// allOpen(); // Uncomment to use this
// delay(4000); // Uncomment to use this
// allClosed(); // Uncomment to use this
// delay(2000); // Uncomment to use this
//======= Basic Open/Close Animation ======= //
//close hand
for(float i = 0.0; i < 1.0; i+=0.01){
setAllFingers(i);
Serial.println(i);
delay(20);
}
delay(2000);
//open hand
for(float i = 1.0; i > 0.0; i-=0.01){
setAllFingers(i);
Serial.println(i);
delay(20);
}
delay(2000);
//------ Responding To OpenBCI EMG ------//
// addToValue = (int)(((float)targetValue - (float)actualValue)/10.0);
// actualValue += addToValue;
//
// if(actualValue > 255) actualValue = 255;
// if(actualValue < 0) actualValue = 0;
//
// analogWrite(led, actualValue);
//
// // actualValueFloat = (float)actualValue;
// numberToHand = float(map(actualValue, 0, 255, 0, 1000))/1000.0;
//
// // analogWrite(led, (int)numberToHand*255);
//
// setAllFingers(numberToHand);
// delay(20);
//----- For Resetting Servos to 90 ----- //
// setAllServos(90);
}
void serialEvent() {
while (Serial.available()) {
int newData = Serial.read();
targetValue = (int)newData;
}
}
// Motion to set the servo into "virtual" 0 position: alltovirtual
void allMiddle() {
for(int i = 0; i < numFingers; i++){
servos[i].write(90);
}
}
// Motion to set the servo into "rest" position: alltorest
void allOpen() {
for(int i = 0; i < numFingers; i++){
servos[i].write(openThreshold[i]);
}
}
// Motion to set the servo into "max" position: alltomax
void allClosed() {
for(int i = 0; i < numFingers; i++){
servos[i].write(closedThreshold[i]);
}
}
void setAllFingers(float _normalizedPos){
for(int i = 0; i < numFingers; i++){
pos[i] = (int)((_normalizedPos * (closedThreshold[i] - openThreshold[i])) + openThreshold[i]); //map the normalized value onto the full range or each finger
servos[i].write(pos[i]);
}
}
void setFinger (int _finger, float _normalizedPos){
pos[_finger] = (int)((_normalizedPos * (closedThreshold[_finger] - openThreshold[_finger])) + openThreshold[_finger]);
servos[_finger].write(pos[_finger]);
}
void setFingerDegree (int _finger, int _degree){
servos[_finger].write(_degree);
}
void setAllServosTo(int _degree){ //set all servos to the middle
for(int i = 0; i < numFingers; i++){
servos[i].write(_degree);
}
}