|
7#
樓主 |
發表于 2014-5-10 09:55:02
|
只看該作者
// our point structure to make things nice.+ ^ b7 _- Q; f& \1 H
struct LongPoint {$ O5 U6 ]# i; u* x2 ^. r
long x;* r: }3 Q& T) ^' o
long y;' [& }5 z0 Y% F* D' H# E/ j
long z;& e3 A: f0 B* Y, c* d
long u;
9 S: \+ q) u- E" R( P};
/ e. a; i( q0 s. ~6 k$ O4 Z2 ^, D; l! Q: @- v# I5 v
struct FloatPoint {
6 S% B; h$ t8 E float x;$ X% H0 w& Q; Q
float y;
/ \4 a J$ ^! }8 s/ Q7 n( U! w$ b float z;/ Y: z7 E) K1 W, M8 d7 y
float u;
, n5 `" ~) P8 N3 s7 P6 z, O3 T t};, b: M9 O) d5 S
6 x! U e: C3 T( I; d4 ^1 d$ yFloatPoint current_units;. v; @$ H6 ^9 d2 B T
FloatPoint target_units;8 D, P U, L& d$ R
FloatPoint delta_units;
2 J5 W4 B7 E7 |2 V! Z5 g/ i2 a2 j) ]1 T# e/ K
FloatPoint current_steps;5 J0 N9 V) g7 p. k/ D8 @! H
FloatPoint target_steps;
/ K: b: }7 W( I8 O; ^. x3 J1 F: zFloatPoint delta_steps;2 X! W( n: k. h. Z3 N9 U5 c
" m+ e# |% r/ m6 mboolean abs_mode = false; //0 = 增量位置模式; 1 = 絕對位置模式
. N& d- x' I* a# i7 @0 `, S" Z! T. {0 {( x
//default to inches for units _4 E D- Y O4 A% c$ h, |+ s# ~
float x_units = X_STEPS_PER_INCH;. e. c7 T; b! [% |1 R
float y_units = Y_STEPS_PER_INCH;: c' ~! \; l6 t5 z8 T: ?
float z_units = Z_STEPS_PER_INCH;' P; o0 d% m5 \# N. K1 @
float u_units = U_STEPS_PER_INCH;% h% Q7 z) R. T, k
float curve_section = CURVE_SECTION_INCHES;
: a8 |0 \( \% Y: z. P0 s5 x9 G) t* k% ? G. ]
//our direction vars
& V) W. `$ ?- r8 I9 y$ ^1 ^6 Gbyte x_direction = 1;
8 Q1 \3 m- ]# A9 E+ Gbyte y_direction = 1;
8 ~& }3 P; I6 U8 _byte z_direction = 1;
+ G0 L# A% j; A# y; a8 U% u6 Tbyte u_direction = 1;/ W4 M0 F9 w$ T! C4 x6 [
6 Z G$ A# J3 U* F4 [
//初始化字符串處理
1 j l( B Y* X) V" r, p6 Vvoid init_process_string()
$ ~7 p5 k& {( ?9 L `% j6 A{
# q8 k) p" G( y/ u P4 O //init our command
1 I q& I, `& u* L# O6 C2 ~ for (byte i=0; i<COMMAND_SIZE; i++)
5 z: G. R' P2 R. n0 O4 @8 ^ word_old[i] = 0;
0 z1 ]" Z4 S% E) r& s' Z serial_count = 0;
* D' {4 y& \$ ^* S, u' D}
5 q7 A u, H; H" P8 V! P! _ {$ P1 o6 H% H$ e- @
//our feedrate variables.
% ?" a+ E! C! B% l9 t$ C0 {. afloat feedrate = 0.0;5 O0 v# j+ C' M n0 q
long feedrate_micros = 0;
- M n7 N$ j# i( X6 H
: m2 o% A5 }% y+ s8 j0 |//讀取并執行命令
' [9 x. S/ ]( ]% I: O! T% u5 ]void process_string(char instruction[], int size)
L" q4 {" v. c9 n& T: v: S{
5 Q" u( \. M" e2 i( D //the character / means delete block... used for comments and stuff.
8 l$ {7 _" W* G4 O, R if (instruction[0] == '/')3 R) k( L) y2 q
{* x5 _& R i5 w( Y3 y' k4 i; Q
// SerialUSB.print("ok");+ o1 ]) }" N8 u, R% A
// SerialUSB.print(byte(78));
* h1 d2 P5 E8 { return;0 v( U) t4 C' y ]& S1 G
}
$ o, ] y0 I3 [* L# p$ k //init baby!/ d1 { t0 }0 E j+ B
FloatPoint fp;0 g/ @" e+ B3 D8 j, m9 t1 h0 X. N
fp.x = 0.0;
& I6 U/ O9 B# E* w fp.y = 0.0;
- V+ n% }9 }, X7 C4 S x1 @6 w fp.z = 0.0;3 |3 z/ U4 M% P% V! B/ L1 K
fp.u = 0.0;. m' X) t" O: X" o" u' X
. R$ H$ A, \8 g
byte code = 0;
$ V2 |) l" H4 S b7 T( o6 Z5 X- c, P% O. h% d) U* n& O
//顯示在處理的命令+ Y1 p+ d# `( i% h
#if (LCD_TYPE == 1604 )0 G- ?0 S' U* Q- i
// lcd.setCursor(0, 4);
( `+ f& k5 @* k+ R // lcd.print(word_old);
) ?- i# s9 q! S0 u+ L' i- x1 T#endif
# }% n V' P- x" q1 P1 w SerialUSB.println();1 p" R5 M! G4 [$ ]* g% G
SerialUSB.print(instruction);( M8 x6 i$ O0 R4 I" m, D- `5 d) j
SerialUSB.print("\t");5 l, {' @" X& J" f: k
" \% k1 R6 x1 a/ L
//what line are we at?, w: \1 t9 G+ g5 Z0 K6 f
// long line = -1;
! u: [: n0 g* c$ w3 v# ~ // if (has_command('N', instruction, size)): o7 [) n9 r: a+ R Q+ [$ B b
// line = (long)search_string('N', instruction, size);! ^5 k, V1 V0 S6 V
+ r1 q& k4 J: k /*. h- W8 e z1 m/ G. r' r
Serial.print("line: ");
1 @( M$ F8 b; S7 B Serial.println(line);& K8 z: e1 V4 e# E1 d
Serial.println(instruction);
Y( S8 r J! L, {6 j */
( L5 n* ], `3 P" n //判斷是否讀取了個 G代碼?# D6 L7 z" ?7 C2 v: K. z
if (
! f2 }* b: R6 |" ~ has_command('G', instruction, size) ||' T5 ^6 T6 r" G9 f; p
has_command('X', instruction, size) ||
( m% }5 g4 W/ m% t8 K n has_command('Y', instruction, size) ||
2 T$ `. c8 R; i9 N: r9 P- R has_command('Z', instruction, size) ||. R$ {6 X$ b j w- |" Y0 p/ \! N7 z
has_command('U', instruction, size)
: Z+ z; M1 g2 R# G )6 A. Z7 e$ F8 u1 @# ~$ |/ I3 `
{
- Y( z) ^1 p% H; t* v& Q d, U6 c //which one?
6 g" ^9 ~4 y. { J2 S code = (int)search_string('G', instruction, size);7 C) Q3 O5 }6 O `+ {3 ~/ J
// Get co-ordinates if required by the code type given4 I/ G/ _! a4 n
switch (code)
! m& s( s# @2 r( t: [' L {
4 \2 A: Z! ~! m" | case 0:: L* s6 M7 N; y* `6 ~) D2 b
case 1:
0 S4 N9 }' ~1 v6 \ |/ b$ o8 b6 ] case 2:3 B1 K; H) S6 ^1 A% y
case 3:( K; _+ O; m( |* v2 H" P
if(abs_mode)
! y: ?4 ?+ C' R i, X% Y9 {% @) ~ {% V& O# G' R% z
//we do it like this to save time. makes curves better.+ V7 M7 A H8 B& d0 }! |+ [
//eg. if only x and y are specified, we dont have to waste time looking up z.
7 ?1 M% s9 E- g- @ if (has_command('X', instruction, size))
# H# B0 |( H6 p' Y: A$ I5 `% r" E fp.x = search_string('X', instruction, size);
% U! `. A; j8 Y$ g6 T; x' U else
* x; Z/ F5 x3 z fp.x = current_units.x;
7 d# ~: e) S, d. s7 l& z
# Q# P, u6 O4 i& ]0 s; Q* @ if (has_command('Y', instruction, size))
4 T- w$ m; q* \. v4 @( Q2 ~3 R fp.y = search_string('Y', instruction, size); |( k1 R) W+ l2 v
else4 d( }& k0 h# h u+ X
fp.y = current_units.y;
1 |% [# V6 ?5 E2 P9 S
* J' T1 S& K; N, x3 c if (has_command('Z', instruction, size))
# U6 b3 p2 v# d9 z6 ^ fp.z = search_string('Z', instruction, size);& I6 {" i q) K- M: C
else5 K0 U J1 Q! [$ {/ a4 m
fp.z = current_units.z;1 e: y1 z4 C% e+ C
4 B3 {- l( Y% ]! T, ] if (has_command('U', instruction, size))2 a# {0 O' ^- Z" X5 b( d) H) M
fp.u = search_string('U', instruction, size);
" n9 R* x0 {! O5 Q$ } p else( y7 H# U. m0 g
fp.u = current_units.u;
- i. z" C" _ O3 W8 e. q: R }- O" a" R* i, R2 n0 _9 _& L
else
; Y# D$ p# ^6 T {# X' S8 o4 R1 ^9 ^
fp.x = search_string('X', instruction, size) + current_units.x;
- a3 j9 N( b7 [- S ~6 J3 ^ fp.y = search_string('Y', instruction, size) + current_units.y;
0 E4 w6 ]7 g. b# ` fp.z = search_string('Z', instruction, size) + current_units.z;
+ D5 O3 F, E2 L S& m fp.u = search_string('U', instruction, size) + current_units.u;$ ]- k4 X4 ^6 W
}6 {5 S, ` F4 @- I: n
break;
3 f: ^# ?/ b g) v1 T) u/ L( u }% f! M, z1 m5 M" l- x
//do something!( `8 H8 B- N! v/ s
switch (code)/ K' y8 |* h! _) ? R
{
, x% F& i/ ?, C; x: n( D: Z2 x //Rapid Positioning4 M3 I1 P0 y9 ~8 E h
//Linear Interpolation& k) l" l' P9 B" v) q
//these are basically the same thing.' F) ?- i4 u' E5 ^
case 0:- j4 o: d. y2 L/ X
case 1:
5 O. J Q z1 Y //set our target.
: X2 {, k7 p2 w& w4 I* a set_target(fp.x, fp.y, fp.z, fp.u);- _. @& E H& ^' Q" _, }- p( P" M
//set_targeta( fp.a);# Z, d/ u% h1 r, E; ]& N3 e& x
//do we have a set speed?' y, e/ X0 h6 A
if (has_command('G', instruction, size))
' p8 P# Z% Z. B, a6 r0 h { S1 D1 r' D. D/ u
//adjust if we have a specific feedrate.9 P/ ~5 `& M) e6 B9 ]
if (code == 1)
! x; a5 r; L y9 @% T {6 r# g6 w: h( A
//how fast do we move?
6 G3 w9 j9 J# S8 Z* b! F% g feedrate = search_string('F', instruction, size);
) V" m$ o) Z) E$ L# C1 k if (feedrate > 0)- ?1 N5 ?. C* f( Z5 ]; ~! g7 A1 r
feedrate_micros = calculate_feedrate_delay(feedrate);1 F. ]7 f0 T S- S
//nope, no feedrate
& @2 n! Z' B+ V7 t) s$ a else
% J, A4 w- [7 Z2 o5 A$ p/ ^ feedrate_micros = getMaxSpeed();* p8 _1 ^- [" F+ l' ^
} x( W1 p S/ k: l
//use our max for normal moves.& s4 n: B2 H6 r6 o
else
, u' M& i8 ^; f feedrate_micros = getMaxSpeed();
) e6 F' v1 o6 N% F }1 A( `7 r3 w9 H- S
//nope, just coordinates!: b* O4 H4 P. M1 i
else1 _( k. o) m+ y# q" x9 m/ b) T( [& Z
{ ], B0 v/ Y& i' x/ |
//do we have a feedrate yet?- G6 X" l# f- a5 l4 T2 _ a0 J
if (feedrate > 0)
% f6 T9 x6 d- m feedrate_micros = calculate_feedrate_delay(feedrate);! u! t8 r* O6 U
//nope, no feedrate
) H4 C- ^$ s6 J# r; c else
. {6 s7 Y" Y. E9 F! S! F feedrate_micros = getMaxSpeed();8 E/ I) O2 K( b, U ?& a
}% v0 H m# T- \- R. [5 c- C6 e
$ H% B% S+ A+ k, Q: F" Q/ z //finally move.& s0 }/ T: r" V
dda_move(feedrate_micros);
) D2 Z4 i2 n5 b4 O9 Y1 h if(stop_flag) return;
, z2 G B+ c5 ^ break;
! l0 N% [4 f6 s# ?; z- V4 d- g, i# _, q8 U9 z
//Clockwise arc% _8 Z. [( m g( u) j
case 2:+ K& v4 m& o6 S, F7 R$ r- ]
//Counterclockwise arc0 h: B* O3 I0 b8 p% v; ^
case 3:
: G& d* e4 J6 w! M FloatPoint cent;
3 M6 g, p9 p; t4 z- ^ // Centre coordinates are always relative
1 ~/ H! H* {: K( q cent.x = search_string('I', instruction, size) + current_units.x;
# ^1 ~! l- a3 L2 P cent.y = search_string('J', instruction, size) + current_units.y;
. X: f: t" A; \+ q! N9 v* m float angleA, angleB, angle, radius, length, aX, aY, bX, bY;, S9 `3 E/ i7 F1 ^ O
- S* l+ C$ A- n+ b" X" Z# I \
aX = (current_units.x - cent.x);* W. k2 c! K9 @& T6 t% U, q
aY = (current_units.y - cent.y);
$ j" N1 n. N: {: P T) ]; q: r bX = (fp.x - cent.x);
- N7 n7 |0 e. W5 O7 ] bY = (fp.y - cent.y);- H' m6 g+ [ m' K9 V8 U/ E
7 O6 j& t: P* C* g if (code == 2) { // Clockwise
m7 [ i: v4 {+ Q9 d; | angleA = atan2(bY, bX);
k3 n6 w& ^4 r) Y r' ` angleB = atan2(aY, aX);
8 u$ @, }. k+ k8 p, c } & W" I- b5 P2 G9 d8 i
else { // Counterclockwise
b# y* I6 o( m; l+ \% S angleA = atan2(aY, aX);
9 t# }8 i: ?4 c4 B& c2 V angleB = atan2(bY, bX);# W, Z! m! G: s, B
}3 {6 v" k) n# r3 z
// Make sure angleB is always greater than angleA; C( I! C, S, d, R0 D: y, y
// and if not add 2PI so that it is (this also takes
6 z6 u* b( ^' t // care of the special case of angleA == angleB,
4 |) j9 I- P& n) }/ i // ie we want a complete circle)8 y# ]3 m# F& a+ a5 H" Z# m
if (angleB <= angleA) angleB += 2 * M_PI;3 a0 C- o3 V) Q
angle = angleB - angleA;
$ D4 H0 W3 a* ^' {! V8 a# \- @" o$ J) z) {" b* \. }1 s; m+ c3 S
radius = sqrt(aX * aX + aY * aY);
# G" o) i6 I. A* I+ @ f2 N$ y length = radius * angle;& x, C j1 H; X9 c
int steps, s, step;5 x3 v2 d$ `+ N8 X1 @: Y9 T! ?% C
steps = (int) ceil(length / curve_section);, D: C0 J( k( f
9 E* e8 {* o+ [: M4 r! G FloatPoint newPoint;
) X* V' F+ k% B for (s = 1; s <= steps; s++) {4 F4 v& k' J6 u5 P7 x; K
step = (code == 3) ? s : steps - s; // Work backwards for CW$ E4 v+ l d4 B1 k% ~
newPoint.x = cent.x + radius * cos(angleA + angle * ((float) step / steps));
5 t. p* i0 W% [/ P8 C* V1 s2 } newPoint.y = cent.y + radius * sin(angleA + angle * ((float) step / steps));
; A3 J- T# b8 j) u& @0 Y! ^ set_target(newPoint.x, newPoint.y, fp.z, fp.u);* G; H7 }6 N; h; a; ~& G
" G* B! ^/ @/ _6 I // Need to calculate rate for each section of curve$ E4 c. [( M1 ]6 `$ ^8 {( \
if (feedrate > 0); y5 V6 @4 o/ t2 B( I8 ?
feedrate_micros = calculate_feedrate_delay(feedrate);; S7 j2 O, o5 L
else
1 b8 n) o. M" t. u feedrate_micros = getMaxSpeed();/ X1 f3 ^0 {! F5 ]! T+ M
; E8 N$ I' \) R: [
// Make step2 R2 s w. d n* Y
dda_move(feedrate_micros);# I; \; T; ^4 @7 o/ e+ A( F$ `
if(stop_flag) return;
2 A! s* q8 p; T; i$ d7 z& `/ e0 { }
5 c3 c) X6 N4 b' m, l* B# r$ q% \9 D
break;
' t6 L0 i8 E* V+ E" c2 Q5 p. J$ V3 z" F8 n2 e% P( ^
//Dwell
; ~8 F1 ~- G( L5 k/ p2 _ case 4:
$ D4 ~( u p- o! c7 [0 X delay((int)search_string('P', instruction, size));- o$ U @* u' t: }, V
break;
9 M7 y/ h4 }0 r; I2 O( G0 a1 ^4 {. j7 D, E
//Inches for Units
3 i' d0 h( u0 K# l1 _/ {: C9 Y case 20:
$ }% p9 G. ^- a( Z) ? x_units = X_STEPS_PER_INCH;+ C- w r7 G, G- y) e
y_units = Y_STEPS_PER_INCH;) R/ l& S4 s0 @
z_units = Z_STEPS_PER_INCH;2 k) {* v. h; P. j9 B" x. V7 Z+ P
u_units = U_STEPS_PER_INCH;# t3 X& U; G) m
curve_section = CURVE_SECTION_INCHES;
0 N) N& }7 q. y. n |9 @+ \: ~$ j calculate_deltas();
% u& H' V! O" I* D' ] break;
( L) V( q) v6 z& K# Z1 ^. q% @. I6 Y" P* {5 L* p
//mm for Units: T2 H+ W3 ~3 j$ }. @
case 21:
9 b! x3 q% b+ {* o3 [! x: x# m x_units = X_STEPS_PER_MM; O; l1 q- _3 S: C* F5 y
y_units = Y_STEPS_PER_MM;5 c. U H$ E- v
z_units = Z_STEPS_PER_MM;: C* d6 y; G1 d e6 B( E6 q
u_units = U_STEPS_PER_MM;* d$ @3 H- U+ t$ ]. [/ L) v
curve_section = CURVE_SECTION_MM;
5 Q7 z1 C3 F: R; i calculate_deltas();
8 s: z+ y. V; w& t' f4 p break;
$ N# ]- Y0 @1 E2 J, o7 J/ y) q; ?# l* H- d" |
//go home.- O$ t) W" S! r! }
case 28:
+ B9 ?9 j0 B N0 Y1 N8 m set_target(0.0, 0.0, 0.0, 0.0);0 ~) ~: P# z' l$ H# _4 _# V$ j9 G1 b
dda_move(getMaxSpeed());% n l9 f. l6 w9 ^% H
if(stop_flag) return;
: R9 l. V! i& [5 G0 y1 V break;! I' z( ^. c# Q; N, |
: }( p: I. }- v //go home via an intermediate point.6 N) g/ g" }: N
case 30:! r6 |" U* z% d% n6 q# I1 J* r
fp.x = search_string('X', instruction, size);
0 ]: N8 ]. ?. J, h. k1 @ z( T5 e- H fp.y = search_string('Y', instruction, size);! C: @8 }0 _ j2 u# X$ i
fp.z = search_string('Z', instruction, size);! C" I. `, W" `# b. ]4 N- q
fp.u = search_string('U', instruction, size);$ E! |+ N5 b$ ^( W
//set our target.% \8 I5 \) F2 ?* `! W: T/ i j" V
if(abs_mode)0 s, T! [( H+ C8 z. @" v. J
{
) Y0 v( a8 \( ]6 ]5 I4 @) y if (!has_command('X', instruction, size))4 u/ S, v. F3 G, G% O* S
fp.x = current_units.x;$ O, v9 W# m N1 T) r% Z- b- \6 Y
if (!has_command('Y', instruction, size))& ~: T' R8 |# S2 S
fp.y = current_units.y;1 H: y# H, U1 Z/ P
if (!has_command('Z', instruction, size))
a* e! v* e( E% C fp.z = current_units.z;+ ^ E0 V3 a1 Q D8 E
if (!has_command('U', instruction, size))
, ]$ c" N/ j! T; \% x fp.u = current_units.u;
9 g+ f" p1 T- J set_target(fp.x, fp.y, fp.z, fp.u);
& Z$ g* e& r/ O# L+ o: U0 B / G2 b1 F5 T) E' c y% R2 u
}
# h5 A& i& i; s3 X# ^ else
7 Q7 g8 T9 l4 ~ set_target(current_units.x + fp.x, current_units.y + fp.y, current_units.z + fp.z, current_units.u + fp.u );" P5 N) Z1 c6 ]! c; T
8 d A& e$ D7 k: ^2 L' ~
//go there.5 }, C' b$ H: d9 U7 u; J0 r1 r
dda_move(getMaxSpeed());, m/ Q1 Y8 s7 Q3 ~! z
if(stop_flag) return;
2 B, i/ }2 i3 t$ N7 d* W% a
8 _4 g2 _6 a: z2 c //go home.) w8 E& ~! V: y# q- `+ A# b
set_target(0.0, 0.0, 0.0, 0.0 );! \2 Y$ }7 q# V) f* a
. O! s1 B8 z: W, y# v1 G% |* [ dda_move(getMaxSpeed());, M% [* `6 t* q0 h
if(stop_flag) return;
. N" X* m& \5 h5 d1 I9 u* X break;
, S9 [+ n! d* p9 Y6 V2 K' R, N8 O$ i: `- ~
//Absolute Positioning X/ k' b5 q" ?( a% \3 w4 }1 M
case 90:2 b4 `9 |( `& c' ?2 [' X
abs_mode = true;
* n; a7 }& P2 U" [' o6 j break;
7 u5 `; u+ G9 p9 x% [3 X5 ?! ?* J T1 ^4 v" x5 _: D$ b
//Incremental Positioning0 G" h: p- l3 J2 Y! [
case 91:
" ]5 c; J9 M/ ^ abs_mode = false;) z& P0 @$ h+ D9 ?9 G3 m; Y
break;# X i9 A1 ^: L, ?
) f5 X: D6 d n4 x, Q2 V+ ^
//Set as home0 P' c+ u% j3 W- e) K' n
case 92:
) S( B% O; T* N' ~7 C , D |, t! M. }; i" U$ `+ j& \
set_position(0.0, 0.0, 0.0, 0.0 ); |5 X) D+ D9 Z; m2 T& M
& `. O! i4 U, h# M. f+ Y" }+ u break;4 s0 O7 k$ c3 a/ q/ A Q
, G0 j. q# o& }: I' O- A$ M, t6 z
/*
4 O- c. q' ^4 W3 a0 x0 W //Inverse Time Feed Mode2 |% g7 R, i$ j) ?% W- |
case 93: N+ X# V) H3 [ T2 S
5 i( k4 C& P( C9 b2 I" v& x- L
break; //TODO: add this* f1 T& j% Q) ?9 P# ]( `8 b- ^
+ u5 t- y" |+ `! {- D0 }$ E //Feed per Minute Mode: w& u) H9 S1 `3 @* G9 I# q6 W8 ?
case 94:( S# {$ @2 p0 t9 _* m7 p& I9 {
; Y/ L( e# U V$ ?' k5 d break; //TODO: add this) R6 {5 z' M: F. z d
*/
0 G# n& i0 P- A: G6 {) v# X% Z# C6 A. I- ~ e* r
default:
1 v; u8 C; u" r+ c9 _0 \. } SerialUSB.print("huh? G");0 N0 ?6 N3 l5 ?; n4 U) q" u& K# p
SerialUSB.println(code,DEC);
% k8 K$ O0 ? s: L- M }
2 k( @; A/ c! [4 p }
0 n; g+ ~( I5 W( k* }3 W( s( Z' [% k5 Q$ x6 ~, ^
//find us an m code.
: k) K7 \* ~9 a! d% _ if (has_command('M', instruction, size))3 `1 h" b; h% l% r4 A, p/ ]
{
+ |; N) G# m! ^$ I5 T0 C code = search_string('M', instruction, size);4 t" l6 N4 p; u! l% v: j! [
switch (code)- v( ^( J: ^0 B L1 s% y
{: s; q6 ~1 h) q9 x, W
//TODO: this is a bug because search_string returns 0. gotta fix that.' x5 M- b' x# B; `2 e. Y1 P
case 0:6 u5 f- d' t- o0 }9 S
true;: M, X- R- @3 r, t: e
break;
$ b8 d4 p4 _# R1 S
6 f' r, f. D$ k, g% n case 100:# m8 J+ I p( `! B+ \- k6 U* G
break;
' g( k/ \1 E+ o* H* q( h+ D5 u: z' ]5 x% g) P/ k) g% t
// fire camera relay
* T% O+ x; j& M3 _ case 101:
" M6 |: [8 A" w, I camera_shutter1();1 X$ J# E# c! B' g1 b8 t
break;5 N0 }* W) `+ l# y7 Z m9 u
) h3 X& ~; H. ~9 U; _3 }/ F7 s$ I
// fire camera relay2
2 H1 N( G; }# P case 102:' e3 ~; M6 D3 p& U" \. j
camera_shutter2();
* w: Y# ?- q1 a5 q break;) a: ]1 Q& A$ b9 N" s6 N
/*
1 q. v `$ L. | c5 U4 y // turn aux 1 relay on
5 d& K; c1 g' K1 \! Q case 103:
- c( D6 u8 d; j" y& `/ z: b6 F camera_aux1_on();1 O, J" C9 [6 [0 @9 w9 F5 m( l
break;
( N7 w) v2 _9 k; W$ g: q: p; N% s4 i8 P
// turn aux 1 relay off9 d$ p! i# a% m5 e0 O# `
case 104:
5 V6 R( f+ c9 V$ _7 \- S# Y* Q camera_aux1_off();
1 ^- G6 x- |# B% }- S break;
+ T5 ~. M& s9 C: ^) |* m5 n0 c: T, r9 d" s5 G+ W1 k# I
// turn aux 2 relay on
. k H9 w. e& \0 @) z4 ~ case 105:
) _3 K6 M3 s; S) @4 u$ B camera_aux2_on();/ w( ~! v) V2 q0 Z( m$ S
break;
2 U( b n9 [7 i- D; ^8 P7 u ^$ S( b, P' q# p! h& N: P4 a
// turn aux 2 relay off
2 Y) |2 a" r- ^+ d case 106: h+ o9 @) S/ c1 H3 ? _
camera_aux2_off();' l4 }" y7 P0 Z) u
break;& I, s& d6 f+ a( d6 x' u
, f' N" j9 ], h9 ]( j // turn aux 3 relay on8 n2 A& v- I6 E& c
case 107:
# h6 F, l1 ` G% Z9 g camera_aux3_on();% K0 R) M5 U7 `) A: E
break;
9 n' O' {1 k9 ]# P
6 Y# R1 ~% J5 Z' n5 q // turn aux 3 relay off
* w* N: x# C1 [( m( U0 f0 K case 108:0 R) O4 z' }, t6 [6 W) J- M
camera_aux3_off();' L }$ M1 \: M: S- g
break;# O8 C7 j8 h7 u9 `
# Q0 _( f' j* z- J& t
// turn aux 4 relay on# k9 a, L3 m* x8 U c4 f' ~( j
case 109:
|& B/ W' A7 W9 L# n camera_aux4_on();5 C, S6 f @$ b3 `
break;
/ ?+ D! G( p, U7 |4 m6 h1 g$ a) y8 w6 M3 f, Y8 }
// turn aux 4 relay off
7 e8 [! u1 Q X+ a case 110:
% m% @9 h) T4 t9 Y) m+ O camera_aux4_off();! X# f3 \; g- r4 B* p
break;
9 M/ q% S% N1 E# j6 e*/
) U' W: y: ?/ J' r8 n5 B default:
. S" h8 u" ]+ R- F" }
, e' V" [/ L* f SerialUSB.print("Huh? M");
: i& D( n7 C3 z4 e$ B) H. R& Q SerialUSB.println(code);8 o7 o8 [- ^# ?( s, }
}; h: s# E. n! U
}: j0 f( E( P. t" q- i0 i5 g" I
) h: ^* z* n$ \/ N6 {, ~! O
//tell our host we're done.
. Q$ E+ t6 Z6 m% U SerialUSB.print(byte(78));0 r; L% ]* ?' H* k0 ^
/ [' u& s. [. I3 a y1 j% @
}9 Y7 y! d5 O1 ^7 ?! f H$ X! y' h
: a/ t6 m% j* k
//look for the number that appears after the char key and return it. I) Q7 g; G4 ~* ]: u7 J$ E
double search_string(char key, char instruction[], int string_size)# V0 b: S5 v/ }- l
{+ ^7 w% C& n5 C/ D
char temp[10] = " ";
2 N7 b7 p: y p/ E% ~& b for (byte i=0; i<string_size; i++)
% w- o# \7 E$ H* K {
. i- v4 ^& H" y0 a if (instruction[i] == key)& ?- F( a2 {, i m
{
" Q+ n6 _4 [) t/ c4 u2 T1 G i++; 5 Z c* F) ? f: y
int k = 0;' ?+ H$ u6 V' V; E( b7 k7 ~- J7 B0 ?
while (i < string_size && k < 10)
) J, T, r: U( G8 k+ t {' W% d3 m9 w" }( g7 V
if (instruction[i] == 0 || instruction[i] == ' ')9 z/ i0 m7 @: Q+ u
break;
8 L7 g! O! Z8 n: W% A
2 }" i! q' x! x, u temp[k] = instruction[i];
/ Z9 z& Z' m/ |+ T5 {, G( o i++;
! w6 k2 ?: X6 m# I6 l: G/ p* }- K* h k++;0 h e, t C6 F, ~8 ?# E
}
3 {0 D, {9 n4 q7 J R return strtod(temp, NULL);# E: O3 \9 d( l
}
( _4 w$ Z- { K8 j+ O }5 ?* v7 `5 `7 c6 k1 U
% o' T9 z& z7 J9 C, p' L: g return 0;% ], u; B3 L- _# {1 }
}
& R5 U) d9 y; V4 j* k$ k
5 Q/ K% ?. k7 j3 P3 J( j" f2 A//look for the command if it exists.
6 Y$ p, H( y. x8 F+ ]; Ybool has_command(char key, char instruction[], int string_size)
6 }- z8 U3 W' a% T5 _0 t{
1 l8 T* q* `1 g% [* S8 r for (byte i=0; i<string_size; i++)+ {6 B5 q4 [) c- P) Q0 m7 \
{
; r P2 T2 N8 a) |9 m if (instruction[i] == key){" }: d% [- B& ]$ |6 z
5 {6 K1 L8 s' q return true;
8 j( K: [( ?! }# p }& Y( A K5 Y( \+ _6 d
}
7 f, z, \; }; ~. ~" _2 h5 G
: ]1 m7 z; x f' ?! [ return false;. g" ]" Z5 B- f$ ^5 |4 C
}2 q' o1 r9 g: u2 M" I9 Y
, v+ k# j" u: E4 f3 p4 b
9 L, K. j) ]1 i3 b( g8 C. z+ b
0 n2 F7 `4 i: o, b/ h% ]6 M |
|