Skip to content

Commit 5b81537

Browse files
songjunyisongjunyi
committed
Merge pull request #12 from dcorboy/movement
Movement from Dave Corboy
2 parents a4b64ac + 61dc85e commit 5b81537

3 files changed

Lines changed: 462 additions & 332 deletions

File tree

‎examples/movement/movement.ino‎

Lines changed: 233 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,233 @@
1+
/******************************************************************************************
2+
File Name : movement.ino
3+
Author : Dave Corboy
4+
Version : V1.0
5+
Date : 24 Feb, 2016
6+
Modified Date : 28 Feb, 2016
7+
Description : uArm Metal Movement Type Demo
8+
*******************************************************************************************/
9+
10+
#include <Wire.h>
11+
#include <uarm_library.h>
12+
13+
// states
14+
#define MOVEMENT 0
15+
#define SERVO_CACHE 1
16+
#define PATHING 2
17+
#define EASING 3
18+
#define HAND_REL 4
19+
20+
byte state;
21+
22+
void setup() {
23+
Wire.begin(); // join i2c bus (address optional for master)
24+
Serial.begin(9600); // start serial port at 9600 bps
25+
26+
// uArm init
27+
pinMode(STOPPER, INPUT_PULLUP); digitalWrite(STOPPER, HIGH);
28+
pinMode(BTN_D4, INPUT_PULLUP); digitalWrite(BTN_D4, HIGH);
29+
pinMode(BTN_D7, INPUT_PULLUP); digitalWrite(BTN_D7, HIGH);
30+
pinMode(BUZZER, OUTPUT); digitalWrite(BUZZER, LOW);
31+
pinMode(PUMP_EN, OUTPUT); digitalWrite(PUMP_EN, LOW);
32+
pinMode(VALVE_EN, OUTPUT); digitalWrite(VALVE_EN, LOW);
33+
// uArm init
34+
35+
alert(2);
36+
change_state(MOVEMENT);
37+
}
38+
39+
void loop() {
40+
if (Serial.available() > 0) {
41+
byte input = Serial.read();
42+
switch (state) {
43+
case MOVEMENT :
44+
if (input == 's') {
45+
change_state(SERVO_CACHE);
46+
} else if (input == 'p') {
47+
change_state(PATHING);
48+
} else if (input == 'e') {
49+
change_state(EASING);
50+
} else if (input == 'h') {
51+
change_state(HAND_REL);
52+
}
53+
break;
54+
case SERVO_CACHE :
55+
if (input == 'q') {
56+
change_state(MOVEMENT);
57+
} else if (input == 'b') {
58+
// detaching the servos after each move forces the cache to be recalculated, generating the errors
59+
many_xyz_start_points(true);
60+
move_home_position(.5);
61+
} else if (input == 'a') {
62+
// without detaching, the servo angle cache remains in place and initial movement has less jitter
63+
many_xyz_start_points(false);
64+
move_home_position(.5);
65+
}
66+
break;
67+
case PATHING :
68+
if (input == 'q') {
69+
change_state(MOVEMENT);
70+
} else if (input == 'l') {
71+
path_moves(PATH_LINEAR); // PATH_LINEAR creates a linear path between the start end and point
72+
} else if (input == 'a') {
73+
path_moves(PATH_ANGLES); // PATH_ANGLES instead interpolates the start and ending servos positions
74+
}
75+
break;
76+
case EASING :
77+
if (input == 'q') {
78+
change_state(MOVEMENT);
79+
} else if (input == 'c') {
80+
ease_moves(INTERP_EASE_INOUT_CUBIC);
81+
} else if (input == 'e') {
82+
ease_moves(INTERP_EASE_INOUT);
83+
} else if (input == 'i') {
84+
ease_moves(INTERP_EASE_IN);
85+
} else if (input == 'o') {
86+
ease_moves(INTERP_EASE_OUT);
87+
} else if (input == 'l') {
88+
ease_moves(INTERP_LINEAR);
89+
}
90+
break;
91+
case HAND_REL :
92+
if (input == 'q') {
93+
change_state(MOVEMENT);
94+
} else if (input == 'n') {
95+
hand_moves(F_ABSOLUTE);
96+
} else if (input == 'r') {
97+
hand_moves(F_HAND_ROT_REL);
98+
}
99+
break;
100+
}
101+
}
102+
}
103+
104+
void change_state(byte new_state) {
105+
switch (new_state) {
106+
case MOVEMENT :
107+
Serial.println(F("This sketch demonstrates the changes added to uArm movement functions."));
108+
Serial.println(F("Select an item for demonstration:"));
109+
Serial.println(F("(s) Servo caching"));
110+
Serial.println(F("(p) Path options"));
111+
Serial.println(F("(e) Easing of movement"));
112+
Serial.println(F("(h) Hand-relative orientation"));
113+
move_home_position(1);
114+
break;
115+
case SERVO_CACHE :
116+
Serial.println(F("Servo values are now cached to remove readAngle error introduced at the beginning of a move."));
117+
Serial.println(F("(b) Before servo caching"));
118+
Serial.println(F("(a) After servo caching"));
119+
Serial.println(F("(q) Quit to top menu"));
120+
break;
121+
case PATHING :
122+
Serial.println(F("In addition to the original linear path, a new option to interpolate"));
123+
Serial.println(F("servo angles can sometimes offer smoother, non-linear movement."));
124+
Serial.println(F("(l) Linear path demo"));
125+
Serial.println(F("(a) Angular path demo"));
126+
Serial.println(F("(q) Quit to top menu"));
127+
break;
128+
break;
129+
case EASING :
130+
Serial.println(F("Several new movement easing function have also been added."));
131+
Serial.println(F("(c) Original cubic easing"));
132+
Serial.println(F("(e) Quadratic ease-in/ease-out"));
133+
Serial.println(F("(i) Quadratic ease-in only"));
134+
Serial.println(F("(o) Quadratic ease-out only"));
135+
Serial.println(F("(l) Linear (no easing)"));
136+
Serial.println(F("(q) Quit to top menu"));
137+
break;
138+
break;
139+
case HAND_REL :
140+
Serial.println(F("Hand rotation can automatically maintain orientation through base rotation."));
141+
Serial.println(F("(n) No hand orientation"));
142+
Serial.println(F("(r) Rotation-relative hand"));
143+
Serial.println(F("(q) Quit to top menu"));
144+
break;
145+
}
146+
Serial.println();
147+
state = new_state;
148+
}
149+
150+
void alert(byte beeps) {
151+
uarm.alert(beeps, 50, 50);
152+
}
153+
154+
void move_home_position(float time) {
155+
uarm.moveToOpts(0, -21, 20, 90, F_ABSOLUTE, time, PATH_ANGLES, INTERP_EASE_INOUT);
156+
}
157+
158+
void many_xyz_start_points(bool recalc_servos) {
159+
// detaching the servos after each move forces the cache to be recalculated, generating the errors
160+
// this mimics the previous behavior where positions were always recalculated between moves
161+
if (recalc_servos) {
162+
uarm.detachAll();
163+
}
164+
uarm.moveToOpts(-14, -19, 20, 90, F_ABSOLUTE, .5, PATH_ANGLES, INTERP_EASE_INOUT);
165+
if (recalc_servos) {
166+
uarm.detachAll();
167+
}
168+
delay(500);
169+
uarm.moveToOpts(-7, -26, 14, 90, F_ABSOLUTE, .5, PATH_ANGLES, INTERP_EASE_INOUT);
170+
if (recalc_servos) {
171+
uarm.detachAll();
172+
}
173+
delay(500);
174+
uarm.moveToOpts(0, -19, 20, 90, F_ABSOLUTE, .5, PATH_ANGLES, INTERP_EASE_INOUT);
175+
if (recalc_servos) {
176+
uarm.detachAll();
177+
}
178+
delay(500);
179+
uarm.moveToOpts(7, -26, 14, 90, F_ABSOLUTE, .5, PATH_ANGLES, INTERP_EASE_INOUT);
180+
if (recalc_servos) {
181+
uarm.detachAll();
182+
}
183+
delay(500);
184+
uarm.moveToOpts(14, -19, 20, 90, F_ABSOLUTE, .5, PATH_ANGLES, INTERP_EASE_INOUT);
185+
if (recalc_servos) {
186+
uarm.detachAll();
187+
}
188+
delay(500);
189+
}
190+
191+
void path_moves(byte path_type) {
192+
// PATH_LINEAR creates a linear path between the start end and point
193+
// PATH_ANGLES instead interpolates the start and ending servos positions
194+
uarm.moveToOpts(-7, -14, 10, 90, F_ABSOLUTE, 1, path_type, INTERP_EASE_INOUT);
195+
delay(500);
196+
uarm.moveToOpts(15, -26, 20, 90, F_ABSOLUTE, 1, path_type, INTERP_EASE_INOUT);
197+
delay(500);
198+
uarm.moveToOpts(-15, -26, 20, 90, F_ABSOLUTE, 1, path_type, INTERP_EASE_INOUT);
199+
delay(500);
200+
uarm.moveToOpts(7, -14, 10, 90, F_ABSOLUTE, 1, path_type, INTERP_EASE_INOUT);
201+
delay(500);
202+
uarm.moveToOpts(0, -21, 20, 90, F_ABSOLUTE, 1, path_type, INTERP_EASE_INOUT);
203+
}
204+
205+
void ease_moves(byte ease_type) {
206+
uarm.moveToOpts(-10, -26, 15, 90, F_ABSOLUTE, 1, PATH_ANGLES, INTERP_EASE_INOUT);
207+
delay(750);
208+
uarm.moveToOpts(10, -26, 15, 90, F_ABSOLUTE, .75, PATH_ANGLES, ease_type);
209+
delay(500);
210+
uarm.moveToOpts(10, -14, 15, 90, F_ABSOLUTE, .75, PATH_ANGLES, ease_type);
211+
delay(500);
212+
uarm.moveToOpts(-10, -14, 15, 90, F_ABSOLUTE, .75, PATH_ANGLES, ease_type);
213+
delay(500);
214+
uarm.moveToOpts(-10, -26, 15, 90, F_ABSOLUTE, .75, PATH_ANGLES, ease_type);
215+
delay(750);
216+
uarm.moveToOpts(0, -21, 20, 90, F_ABSOLUTE, 1, PATH_ANGLES, INTERP_EASE_INOUT);
217+
}
218+
219+
void hand_moves(byte relative) {
220+
// F_HAND_ROT_REL will keep the hand orientation contstant through a move
221+
uarm.moveToOpts(-15, -15, 12, relative ? 0 : 90, relative, 1, PATH_ANGLES, INTERP_EASE_INOUT);
222+
delay(500);
223+
uarm.moveToOpts(-15, -15, 8, relative ? 0 : 90, relative, .5, PATH_ANGLES, INTERP_EASE_INOUT);
224+
delay(500);
225+
uarm.moveToOpts(-15, -15, 17, relative ? 0 : 90, relative, .5, PATH_ANGLES, INTERP_EASE_INOUT);
226+
delay(500);
227+
uarm.moveToOpts(15, -15, 17, relative ? 0 : 90, relative, 2, PATH_ANGLES, INTERP_EASE_INOUT);
228+
delay(500);
229+
uarm.moveToOpts(15, -15, 8, relative ? 0 : 90, relative, .5, PATH_ANGLES, INTERP_EASE_INOUT);
230+
delay(500);
231+
uarm.moveToOpts(0, -21, 20, 90, F_ABSOLUTE, 1, PATH_ANGLES, INTERP_EASE_INOUT);
232+
}
233+

0 commit comments

Comments
 (0)