|
楼主 |
发表于 2014-5-10 09:55:02
|
显示全部楼层
// our point structure to make things nice.* E {1 ] c8 N& Q) S2 |
struct LongPoint {
% K% d/ C5 v4 ` long x;% G" e0 o! D0 @- G
long y;
4 D1 A1 p N2 X: c- a% C! | long z;
4 i5 r" e2 t% Y8 [( j long u;, |. }& z; j" g& D! o" M. V! O5 E, R
};
5 o8 g8 F! o# p! W4 G% q; t" S8 {8 R$ |9 X% |, a: P4 d
struct FloatPoint {
4 d* T) m; y' l: b; F) a float x;
" h& A# U0 b8 V4 M, e* J+ ?0 a float y;
" Q4 O/ |2 j$ D/ V' { float z;
* N& L7 \! `# _ float u;
' F$ \9 O: D7 s+ T) n- q1 j) c3 q};
! j, i7 k: u+ {" \ d
+ S- {, P* B i3 V) zFloatPoint current_units;: _: B# g( d. [
FloatPoint target_units;
, D' R! }( @# i! tFloatPoint delta_units;
5 I, d. D8 J) T6 C* e
8 ~. p& H4 w" J* N+ V! o, aFloatPoint current_steps;
: I) b4 V+ c$ ?7 p6 e# yFloatPoint target_steps;" U# B* t: P! n
FloatPoint delta_steps;! H$ N3 A0 Q; {3 Q% J+ Q, B" K
; c- N( C7 W, U! H4 [
boolean abs_mode = false; //0 = 增量位置模式; 1 = 绝对位置模式
; N- T- A8 d- v- W- {8 C& q+ O- l' x" o, B& O
//default to inches for units. b ^! ?2 Y' l
float x_units = X_STEPS_PER_INCH;
3 W, t) V6 X: Q% }$ g3 K5 yfloat y_units = Y_STEPS_PER_INCH;6 W" m. g! N/ Z4 Z
float z_units = Z_STEPS_PER_INCH;
& F0 e+ a) d$ {, J; kfloat u_units = U_STEPS_PER_INCH;6 k3 x8 ~3 U3 x. T6 s# E, ]( F' L6 A; S
float curve_section = CURVE_SECTION_INCHES;" N& q: e) [( U8 w2 k: w
9 t- s% |( ~! J. i9 w//our direction vars
; ?0 |" z- U: h/ dbyte x_direction = 1;5 Q$ N5 r' w1 |
byte y_direction = 1;: @/ Q6 b! N: i6 G L
byte z_direction = 1;
! A4 P2 R8 e) d5 G* G! X$ Nbyte u_direction = 1;$ s. R5 N" N5 F, t% I
$ e7 n+ ~! n6 t7 a% R% S: y# A5 p
//初始化字符串处理
9 |0 O) q5 p' S- O" D- Bvoid init_process_string()
( b% r z0 A/ X+ s" f! t1 M{
5 ?6 M$ w6 Z- s$ x* W //init our command
) A; h. Z: `1 D* o: T for (byte i=0; i<COMMAND_SIZE; i++)
, k& |) |- f5 b word_old[i] = 0;
# C B% R/ Y; [1 g serial_count = 0;4 I) h* y9 p4 T, ]; m! c. ?4 u, H
}5 f- l8 J: z# m
: r4 s* t0 u+ k2 I, F" a5 R- P
//our feedrate variables.
* q/ u* \5 H Pfloat feedrate = 0.0;
* e- C; Z% s5 F0 e5 g6 h4 w. y4 a- blong feedrate_micros = 0;
: E: X. R( B- \2 x* J
' O' g: \" `' U+ `//读取并执行命令
; N% H# z$ Y" Zvoid process_string(char instruction[], int size)( R9 V% |/ M" w' T- I0 |) J& e
{
) u) |# H1 O- @8 w. S //the character / means delete block... used for comments and stuff.
; t* J' Y) P' E if (instruction[0] == '/')
0 t9 N! D. r2 B1 w. t {
& o6 ]- J! P, l) W3 }1 j" ~5 q9 a // SerialUSB.print("ok");4 `$ H: y" M4 \* z& ~2 C1 I2 S6 W/ P
// SerialUSB.print(byte(78));
k5 D4 h: v( M$ { [1 D return;9 Q* V+ j7 }2 E' p% L- V
}7 G8 C1 q( e% G# M
//init baby!+ N& [% |2 N% R8 _, t A
FloatPoint fp;
7 w% q8 I# Y$ t! R5 |0 E) h* p fp.x = 0.0;
& a, S; Z8 }2 i fp.y = 0.0;
% g) x# O9 x/ F, L* ?' o fp.z = 0.0;( b; h! h+ ~/ o0 h3 ]2 K ^
fp.u = 0.0;
5 V, L( h; B/ ~2 @) h/ H1 ~! Z* m( s8 p( @8 S+ ~/ z
byte code = 0;
) L4 d1 h$ V8 m. ]
1 V* n" ?8 b$ O& X$ _: q$ ^ //显示在处理的命令
0 f. D( r" H1 [- e% v#if (LCD_TYPE == 1604 )4 i8 d8 r9 ^2 a7 k3 @/ n
// lcd.setCursor(0, 4);
+ T4 a- v- V' r' z+ B // lcd.print(word_old);0 X) p) r" x) r( k2 u1 t. S$ Z$ @
#endif
) l2 ?% `9 {) a6 q1 N SerialUSB.println();
, H5 O( o5 ?) w8 ^. w& ` f0 N, n SerialUSB.print(instruction);
/ Y( N5 Y3 w+ B% @( @7 f: ` SerialUSB.print("\t");
* k' M' S* {) J' T0 d6 M9 o. Y9 x5 P. X6 }7 ]
//what line are we at?
6 b+ f5 x, S$ d: t // long line = -1;
6 K% X+ `; g2 s/ L8 u, t$ J& | // if (has_command('N', instruction, size))
9 e7 [) D2 a4 I$ E // line = (long)search_string('N', instruction, size);& _0 n6 i2 J7 L/ i4 K. W
3 G: B7 [& M* L4 k1 X8 c /*6 \* W) l8 @7 H8 G# D. \( T: |
Serial.print("line: ");8 O S$ a2 h7 f5 k
Serial.println(line);
4 Y3 X) H, R/ \8 M( q+ r Serial.println(instruction);- }$ u1 d+ {& r7 e0 r
*/
/ [' A( T0 ?3 }8 d8 I //判断是否读取了个 G代码?/ w7 }& V } B4 l
if (
9 V* H1 N8 {. @* I7 C% t has_command('G', instruction, size) ||/ d- G; }! g1 c0 P, N, r
has_command('X', instruction, size) ||
6 G! {" W9 Z2 D) _- ? has_command('Y', instruction, size) ||! Z* C. b1 |9 E8 E, E2 U
has_command('Z', instruction, size) ||* U" B& q$ m2 V* p" a" u
has_command('U', instruction, size)
3 P8 e. ?5 Y9 E# W0 u H )
( G1 Q0 r6 _8 k9 M4 L {
+ m* q1 p4 p' c# z4 a6 x" I0 L //which one?
, ^) s$ |! N' G" f code = (int)search_string('G', instruction, size);0 g" T7 b* [+ B" e9 q+ D1 `
// Get co-ordinates if required by the code type given
4 p" W5 C5 L! ^ switch (code)# d# m6 V1 l1 K
{
5 Q5 D" y- v9 V6 M& n case 0:
( l0 f" r' w3 z, n; @, C1 u case 1:
0 k- q8 W: q( ` case 2:1 a; j3 d0 w: ?. `8 v0 O i
case 3:+ i# A7 F' ?# j+ M2 m8 G
if(abs_mode)
/ l, [1 B+ b3 S/ ~% t1 c {6 n+ X. N# F; o c" i: z
//we do it like this to save time. makes curves better.& R: D, h8 e( Q
//eg. if only x and y are specified, we dont have to waste time looking up z.! r3 K% }2 \$ ~# X
if (has_command('X', instruction, size))
# s& ?. w- r' B1 { fp.x = search_string('X', instruction, size);+ j6 x! o: M: C& l. u
else
5 l% V' v6 \- T4 v fp.x = current_units.x;
+ M- w0 q# y5 h/ q* V
4 z3 m- F% [/ ^% _2 q6 y8 u if (has_command('Y', instruction, size))3 e8 S: `# t" }& O8 i8 `! U
fp.y = search_string('Y', instruction, size);
+ L& S3 K3 [4 K1 i else
4 U$ |4 I9 U3 s0 V3 Y; ~ fp.y = current_units.y;
& j& u+ C& f, F+ H1 H; R% X. d- Y8 P, |- Q- x: T
if (has_command('Z', instruction, size))
+ }" O3 C$ O* U. x7 F1 { fp.z = search_string('Z', instruction, size);
+ q( {* g1 \" A: Y0 p8 J else
0 \2 o+ |$ O' R" F1 t' i$ a fp.z = current_units.z;1 ^1 _+ U5 a" |8 E9 O: `) Z
$ c# J, X b) v& ^) x' r+ x if (has_command('U', instruction, size))
& p# |! u* ]2 L9 Q fp.u = search_string('U', instruction, size);2 o; O8 H" U7 i4 M
else
8 J/ u9 d$ X$ Y4 n( d0 y9 E fp.u = current_units.u;" f" E5 Y/ A" M" A) x2 z1 R
}
0 k; F3 Y9 k8 k else2 {. i, |* B8 [* W
{
3 m7 E2 ?/ Q' `- t# m w$ k fp.x = search_string('X', instruction, size) + current_units.x;
6 ^: j: D# Y. A9 j9 J fp.y = search_string('Y', instruction, size) + current_units.y;' T$ ~& `* [6 {/ F) W
fp.z = search_string('Z', instruction, size) + current_units.z;
- o$ g) o' I1 p8 _ fp.u = search_string('U', instruction, size) + current_units.u;" N! f0 n9 [1 w3 D
}1 K$ W' A5 B" e/ `. b3 h1 o" ?
break;& ?1 l! e( ]! a' M
}+ v( k6 E) i0 _& L9 h- _: L( S
//do something!
0 I5 F% ?. `/ a( @ switch (code)
6 j( B4 O/ l% T. z" x4 G {, T+ {, `5 z7 p6 Q5 E4 g$ P
//Rapid Positioning! L& `9 p9 p: M9 l( {" c: R$ x
//Linear Interpolation3 b" p) L* N& s: J" y. s8 I: i
//these are basically the same thing.
# g, [4 N8 g3 R3 Q/ N$ w1 Q case 0:
& ]% [1 j: U7 S' e! F- t' u1 C" S/ H$ o4 s case 1:
. S. |1 B3 i3 h, C% t6 K2 N //set our target.
3 l! k) s! V% n. F# V set_target(fp.x, fp.y, fp.z, fp.u);1 x5 X' _ h# w% r; x' b# I
//set_targeta( fp.a);: r( _0 Q1 ?8 {! E
//do we have a set speed?- D) X2 A1 j, u# g; m+ `4 h$ w& U
if (has_command('G', instruction, size))
: q! A h( C/ @8 u {9 }9 z3 R8 O9 F; D. c6 b" v1 e
//adjust if we have a specific feedrate.
/ V" s. g' w' |+ S2 q1 U if (code == 1)
4 t- K7 o" U/ H/ y; z/ w {
9 `! H+ N- N, l2 H //how fast do we move?
8 I) i, {; e: y1 B; R feedrate = search_string('F', instruction, size);
# z' T( f0 G { if (feedrate > 0)9 t, @# c6 ~1 Z% C( a
feedrate_micros = calculate_feedrate_delay(feedrate);; H( u% P; u/ B7 m3 M4 V; I9 P
//nope, no feedrate
/ b6 `; I" C4 K4 ` else7 b' V: v6 \3 i% ~2 ~
feedrate_micros = getMaxSpeed();+ C) j* v6 ~( ?/ h. `. T
}
' f* K5 G2 ~0 J* P& } //use our max for normal moves.0 z7 Y% b2 Z- f' w9 s, ?
else) ]6 [5 S, s9 Y2 V W9 O+ q/ K0 A
feedrate_micros = getMaxSpeed();
, \& U! F# B, t- T6 q' a }
' Q# d l# D, a0 h$ z //nope, just coordinates!
6 _9 O. d; L% v. S! @7 r7 u else
! o6 {" X$ r/ F& w {4 r2 N0 q, @/ S5 c9 H& R6 B3 k
//do we have a feedrate yet?
4 W" X) E2 K8 B1 S/ R3 s if (feedrate > 0)$ P" d* c+ s/ ~0 h
feedrate_micros = calculate_feedrate_delay(feedrate);
9 ^, n1 ^8 f" I/ M* g4 g. L //nope, no feedrate: F* ]9 [3 X' J4 v0 ^: l6 l% P- ?
else
# K: H9 o. e6 S0 D3 X: }2 S feedrate_micros = getMaxSpeed();7 X1 t) a8 i1 Z+ _" @% }- _
}3 R/ m, m7 H+ j8 }; |1 I. {8 ~' X
N0 o: g2 H0 i //finally move.
. y3 y0 `( Y; r dda_move(feedrate_micros);
% _. R" W/ {+ Z5 i, f! r' i if(stop_flag) return;
$ A) y; {' \' V, B( Z) o/ P break;
0 w5 d# q( K- l4 E
* H+ w2 }% X5 i" ~4 T n9 b //Clockwise arc- p* ~/ f, J6 O$ {7 `
case 2:, V3 t) C. s' H6 Y3 Z
//Counterclockwise arc) z- X9 e+ {+ A: x8 j% c# i
case 3:" |0 @; K7 ]. O# R
FloatPoint cent;% v; d8 m. p' Z8 ?! T* x
// Centre coordinates are always relative
; x( ~# N! `; L5 y( R cent.x = search_string('I', instruction, size) + current_units.x;
) U* b, c) z( k; a cent.y = search_string('J', instruction, size) + current_units.y;& `* m& _. d3 Q& u7 N, e9 j. S5 R
float angleA, angleB, angle, radius, length, aX, aY, bX, bY;% \+ X3 k( J; K1 Z- z! C
. @" K& z# H* [2 C7 d( l/ l6 @0 ?
aX = (current_units.x - cent.x);+ O6 \; Z8 q* _4 E0 j2 E1 g: h5 K
aY = (current_units.y - cent.y);, J$ c, ?+ m5 v4 E: }; ^1 u
bX = (fp.x - cent.x);
6 s/ y* @. ~3 y bY = (fp.y - cent.y);
) u0 [0 i/ V0 I s9 V3 J2 v2 v4 ]
0 x" q, t/ l- S& G2 x; E. X! o% e if (code == 2) { // Clockwise0 @3 W! G- S- c O* H3 N
angleA = atan2(bY, bX);
5 D1 `) Z0 M. h angleB = atan2(aY, aX); M R9 `! ] N& F) f% H k
}
4 M. N9 H5 k7 f; D# m1 H else { // Counterclockwise
$ I6 Q, K" y: m angleA = atan2(aY, aX);/ ~+ G; t( _ \! x6 v" C8 g
angleB = atan2(bY, bX);
! F& D0 Z3 K4 D6 f" {6 o/ o }2 C- H9 O" p' t9 z$ B0 a4 a# }! W
// Make sure angleB is always greater than angleA3 \" O. O; H# ]3 i/ ?
// and if not add 2PI so that it is (this also takes
j& p: M4 n+ W0 A- c; }6 n+ { // care of the special case of angleA == angleB,
4 d( |0 ^7 i) ^, u# l; ~ // ie we want a complete circle)
9 _/ b* v6 H9 O if (angleB <= angleA) angleB += 2 * M_PI;2 D3 D( A+ f, q8 u, ?: M% M& E: J! G
angle = angleB - angleA;6 A3 ^6 g7 Q4 C; @
5 V5 u; _) K7 z/ X radius = sqrt(aX * aX + aY * aY);6 c/ N2 B$ d* C. F3 }2 w Y
length = radius * angle;- w% I& v5 X% E# K# a6 l
int steps, s, step;
: t% X5 r" l9 O, {+ ^* I steps = (int) ceil(length / curve_section);
5 Q9 r$ W+ \9 t4 h" E! f" h# ~% |; W% ^
FloatPoint newPoint;
' F9 `/ Z8 q: E/ b& A9 V" d for (s = 1; s <= steps; s++) {' Z& F" `* k9 L d. N
step = (code == 3) ? s : steps - s; // Work backwards for CW
5 ?9 V; R+ l/ P1 e6 t+ W newPoint.x = cent.x + radius * cos(angleA + angle * ((float) step / steps));
- {! x* ~* c! G. K, _0 F2 ~ newPoint.y = cent.y + radius * sin(angleA + angle * ((float) step / steps));
9 j: `* R/ M9 g- [& O% \/ Z3 S. C set_target(newPoint.x, newPoint.y, fp.z, fp.u);
) Y* h* ^/ r; a2 h0 P/ S
6 S+ a( R5 S' {. s" @6 B // Need to calculate rate for each section of curve
) R0 N! l9 a1 A* w: G+ H if (feedrate > 0)0 T3 {" ]- ?" `8 I9 Y0 I
feedrate_micros = calculate_feedrate_delay(feedrate);- |/ m; G' F) G
else
7 V8 b" Y5 M! x) E: f0 A3 [ feedrate_micros = getMaxSpeed();
, ^8 C4 B( e3 _% b/ }5 [: R8 \. P: j: @1 m8 o% @, s
// Make step9 j% h; z6 l+ E! g/ T: l8 }- R6 Y
dda_move(feedrate_micros);
& H- _# @0 ^' b% \8 k' K5 }6 u if(stop_flag) return;/ E& Y" @3 B* _1 T5 j
}( |( f" C% ^, G! E, d* a/ X
) b/ J3 C- O9 ]) U5 B# D break;
1 R: B, e+ x6 }: c) Y
- J/ _4 K) J) F) d5 l //Dwell. z$ [1 d: U& a: W* K
case 4:0 Z1 C+ E! o+ p3 q% Q' }
delay((int)search_string('P', instruction, size));
8 O* G3 [0 I2 f5 C( S& E& J: k break;
7 J! x) U! l6 x. z8 U! ]- \
$ ^- n& i# x- J- z) I- f: L& p //Inches for Units9 C* @1 g$ L' A: F- }
case 20:
8 f3 F+ n' ^% u/ j x_units = X_STEPS_PER_INCH;& [! M" `" B0 P
y_units = Y_STEPS_PER_INCH;' O- l! v2 ^+ e5 t
z_units = Z_STEPS_PER_INCH;
1 V: k5 G5 P; u. d# I5 _( O! \ u_units = U_STEPS_PER_INCH;
! [7 S. D+ W" D, O2 s+ K( n% ` curve_section = CURVE_SECTION_INCHES;
1 O9 C2 w& C8 D/ E7 v& f calculate_deltas();
+ r- A" O& r. q0 i- P4 j: R break;. c6 g; \* }( b! }4 W0 R
. C+ a# t3 u8 J% k' Z6 ]
//mm for Units' E, d( Q$ O z1 K2 |/ G3 Q
case 21:* g/ N9 f+ G+ [+ i. | ?7 o
x_units = X_STEPS_PER_MM;; Q0 c0 [# Y$ Q6 ^1 R$ ?7 k
y_units = Y_STEPS_PER_MM;
/ _" ]9 A) G J# u R7 ? z_units = Z_STEPS_PER_MM;: _* ^1 m- S& c1 T/ l
u_units = U_STEPS_PER_MM;( w/ W& O9 I' k& q4 s2 O
curve_section = CURVE_SECTION_MM;8 C+ f4 n1 E+ C* [7 x) H- A) [
calculate_deltas();6 `. d. F! c: E3 o6 O, Z
break;
# H) V; F1 t# ?: O8 C; Z7 j0 m3 T
! c* V2 m5 N K# \$ \ A1 w, v; @ //go home.* z% B s( H# M4 F
case 28:
8 |2 m6 w7 H9 L) D/ [2 D set_target(0.0, 0.0, 0.0, 0.0);% ]% J0 v: p+ `' B$ O+ V9 G6 g
dda_move(getMaxSpeed());
^8 p9 t& ^+ c- D# Q! ? if(stop_flag) return;
3 M6 _+ \# R: ]5 j# a* j break;
, c. S4 V0 Q3 U: r- _6 Z9 c0 U& v9 ~8 R' l" y! B
//go home via an intermediate point.
: U( Q$ `% g1 b& D1 y case 30:
$ E7 }- ^. h X4 C fp.x = search_string('X', instruction, size);1 b$ r3 b9 P+ _' u. \% M5 M5 X( h
fp.y = search_string('Y', instruction, size);5 x9 w/ y6 m. p c
fp.z = search_string('Z', instruction, size);: ] X8 ~7 ]: ~
fp.u = search_string('U', instruction, size);2 i, C E4 B) `) M4 k) A+ A; [; `
//set our target.
" c3 d" G1 J# N( ~2 ^0 X6 E# { if(abs_mode)5 q7 D! ?! S3 z; \, E- q
{) d# b) g0 k- n0 K- S1 _) D% B
if (!has_command('X', instruction, size))
4 m/ \, i# V" z3 {; R fp.x = current_units.x;4 d" Q1 V0 q3 g3 K$ t, ~
if (!has_command('Y', instruction, size))
! v% @7 s* e# h% J, ^$ Q3 K fp.y = current_units.y; I2 r- o. d" O' M
if (!has_command('Z', instruction, size))4 {8 m. J2 I; V8 `6 A' {3 z0 n% Z
fp.z = current_units.z; x3 A5 v+ B2 V. i9 I# e# o
if (!has_command('U', instruction, size))
& q3 c2 x$ V5 @5 b# d* U: @ fp.u = current_units.u;. K& h9 x5 W% t! i) `
set_target(fp.x, fp.y, fp.z, fp.u);6 k9 ]4 ~3 q- \1 _$ q! i
$ W. i; d% N( o8 H: B9 s# u }0 q+ v$ l% e5 o3 x! l4 U
else G7 x/ L1 A& D; Q" g0 N
set_target(current_units.x + fp.x, current_units.y + fp.y, current_units.z + fp.z, current_units.u + fp.u );
O# S. v' H* B& d. l3 s& t
' S& ^+ b# p9 ?3 v4 S, M. S6 T //go there.
' Z- M8 ]# ]3 H dda_move(getMaxSpeed());6 w) ?2 \% v* Z1 p
if(stop_flag) return;
& X2 P5 v* ]1 H; j
/ O8 g/ w, ^1 @' u6 l2 V. Q //go home.: S& ~' V1 ^" F" e6 C
set_target(0.0, 0.0, 0.0, 0.0 );6 E6 c* \) \4 x
2 _! f* ]( u0 t# f* g" J* D dda_move(getMaxSpeed());: C4 I0 B! F4 t9 _
if(stop_flag) return;% ?4 F/ ~2 C+ h0 A' \$ {5 l
break;
6 o! j; o* g6 ~# s; a" M& D/ i' e& ]
# r5 } \9 _- b* h+ r' w* E //Absolute Positioning
/ W8 A' Q, K' ? case 90:
. E! l% M$ ?+ x# ]" e abs_mode = true;, n$ J' q/ O v2 i
break;
. V, O7 M& Q( E+ h: J7 a* S
Q) i" F: @" [; _" E; O, A) c8 ] //Incremental Positioning* {" o S; Y/ W: U6 k# S9 F
case 91:8 e1 t, b- M4 f
abs_mode = false;
% N% w3 P( g- z8 d6 o, \ break;
/ e6 W4 k' }+ T
/ s4 d: `$ U" c //Set as home! S- ^; ?( N4 C3 p' d
case 92:; A9 p/ H3 t4 Q% x1 ]
; [3 ~$ h, f+ Q6 p set_position(0.0, 0.0, 0.0, 0.0 );* ^" m2 l0 ?4 x( ?2 c3 _7 a& m+ c' T
- P* o* |0 |1 K break;
9 _1 m4 M* a. W' @, {0 D* [8 d! H4 M3 Z+ K$ @) a$ t+ m* h5 l t
/*
* A! F# l# m) t7 F //Inverse Time Feed Mode w2 o- I& I& z9 Q' q
case 93:: C+ k1 O4 V t$ r; P( Q$ s
! G$ P* |6 J- c3 S- E. r9 g$ j
break; //TODO: add this
& t9 Z- I$ c- i1 t
" w1 b' h X U4 ?6 | //Feed per Minute Mode. ^4 t9 \& L0 O; t+ Q, W# m
case 94:
6 L4 {" f- d5 D& p1 D" E - j4 m T, r7 ^8 n
break; //TODO: add this( A0 \8 V$ X+ T! N# |- R
*/8 V7 A5 [; u3 o( L9 V
2 u) p. Z; M4 [
default:
3 x( S" p3 L- j& r# [: a SerialUSB.print("huh? G");
* G1 R7 \! C% i& S) u; Q* L SerialUSB.println(code,DEC);
2 W9 n: A; L- T2 B4 V/ z }
) q }; \2 V, t% F& M2 Z# [2 \/ j6 a }' _8 I8 \3 i0 J- e3 W
8 T* L% j. X6 A0 E. S" a9 l //find us an m code.3 O* c1 V. m! r! _8 Y! C
if (has_command('M', instruction, size))
* i) y% B+ m# n6 C8 _6 ~# l {
% O5 D: a: n4 i4 w' _ code = search_string('M', instruction, size);: ^5 x. e9 t, I: p, h( Q
switch (code)4 S* V& e- F) M( z$ ]5 z
{
) P& Y% u, m3 d+ ?9 C! ^) C //TODO: this is a bug because search_string returns 0. gotta fix that.4 m; Y) D1 V) I
case 0:
+ D8 X0 N7 C' [1 c; {. w; }3 S true;
. h$ r6 ~8 R1 p: R! K break;
9 ~6 a. d/ p% D: [" |0 d; P5 z
8 u, }: l( A5 D- _( f; y case 100:
7 h; \2 }- i* \4 [5 c1 K" m break;
) i' r; n9 z. ?: H z' q8 e
8 |4 m3 a n0 T/ g5 G: | // fire camera relay
. H) j5 q( h1 ~4 g1 H case 101:+ \2 _ R; X1 Z
camera_shutter1();1 D: x, }0 z3 C* b
break;2 q- f2 E' T$ E) ^
8 `( n4 p9 N( E
// fire camera relay29 U" c9 @: D4 ~( h- @3 Y
case 102:2 b' u' U E, h" M' I
camera_shutter2();
! n; t! d+ ]1 }* n9 Z& M" | break;& I2 n+ M# W. i
/*
1 j& d; y7 x' `" k // turn aux 1 relay on
; i3 `4 b0 _- g( A case 103:% a8 s+ h4 f0 b% f
camera_aux1_on();- b* D E# Y& K0 v2 l( z$ P: _
break;: E1 G8 L; D e" W
% L4 Z- e T+ x, `5 T
// turn aux 1 relay off
+ w( x8 q+ l+ i' L c8 @$ S case 104:
- V; I" x' s1 b, M5 N5 \" b; \ camera_aux1_off();, W! v3 k4 f& [* F9 y3 j; {
break;& x/ C8 {6 z! Q$ p
7 g* L4 i8 j0 N4 K4 A! Y" N" Q
// turn aux 2 relay on
0 [9 I% y" n6 K) ]% r( r case 105:+ G3 _8 I6 f- h
camera_aux2_on();! F6 P7 B- n9 Z3 N- H
break;
# H6 k% ]6 G/ ?- ?7 [, j4 l4 _# W5 L O* U# I2 ^
// turn aux 2 relay off
6 w0 A% {8 z* K( G3 Z3 j case 106:
" f4 u7 D% }0 Z* O camera_aux2_off();
' A1 V" p$ ~- Z# ] r' C break;/ b. G: [4 D. r" @ H
7 B1 O. l! p# v; H' E2 O5 P, r
// turn aux 3 relay on
' q3 `; d* H9 M) B4 P case 107:
1 n! ?* t& D. f+ b- T7 F1 H camera_aux3_on();6 ^* q) ?( E$ I7 _1 [8 A$ o
break;
% `: t9 D: `3 o8 V, R5 R8 i# s5 }, l: p z6 A4 h' K
// turn aux 3 relay off c: g9 `7 ^. w. p! {
case 108:: B t( f7 P* X: C$ Q" e
camera_aux3_off();
( d- ~" w$ `% d break;
2 g. `8 c3 `3 h1 R. p0 ^2 j7 C+ Q
3 ?' b3 c! N" H' N$ h/ O {9 N // turn aux 4 relay on- v3 _- q# ]! p1 C
case 109:0 C% w+ L2 e5 r$ R$ o7 l$ N
camera_aux4_on();
* b6 s+ \+ p/ V! L C break;7 ~6 [" l: w0 d
- V& Y7 O0 w5 E) M! y. a
// turn aux 4 relay off
! G% r5 C- ]1 h, \5 V case 110:
5 K- I# f, M+ w0 i5 J7 @2 i camera_aux4_off();
2 m. S) e; S# A8 L6 z& @$ L' f break;
. v1 u7 R" J) [9 k*/0 W! I- j) W1 _! b% r- {
default:
3 w3 u* B6 M2 B1 T6 g, f+ W6 ]7 f% d) o
SerialUSB.print("Huh? M");& M' ^0 J4 D3 o/ ^9 U+ E4 o9 O
SerialUSB.println(code);
/ F; m$ }$ j9 m# d# } }
. Z1 w# f% Y) o9 \- a }& J6 n1 ~/ z1 B5 E& i0 v
1 T* e! }& B& D5 y4 ]
//tell our host we're done.+ e; c8 h# O! o) D4 A
SerialUSB.print(byte(78));
( I" x. R" F4 f
2 s% L: e3 J2 a. P/ r& t) X1 X# O}! O' _' b1 k8 @" }* j2 P" _
3 x8 n# ^9 [ q//look for the number that appears after the char key and return it
6 }3 n: W o) m, e7 m' y' Ydouble search_string(char key, char instruction[], int string_size). i2 p T) K+ B# ^
{
! Q L8 E' p% C8 P9 t. q$ N% I char temp[10] = " ";8 ]- I; p9 G0 `. u& G
for (byte i=0; i<string_size; i++)
; l# ~/ p" G4 b {
1 _- T* f8 }" K( V8 A; n if (instruction[i] == key)
$ i# d- C5 N4 U {
, }4 [" a* f2 [0 b4 X# k i++;
0 Q* S5 n% @9 H# V: [ int k = 0;
& Q# a+ f9 O) p* h while (i < string_size && k < 10)% x: a3 q5 E6 M+ L6 j1 Q; y
{
& p/ E* t0 f4 k# _! b if (instruction[i] == 0 || instruction[i] == ' ')
2 `4 {- M) c1 I# G- \ break; ]2 R) d M1 z9 Y, j
+ P+ J$ Y$ \; m8 q, o8 U& M
temp[k] = instruction[i];! G) h1 O( U/ Q6 X4 F4 ^3 b8 }
i++;/ B* z+ t5 b2 t$ D$ p) H! H
k++;
; t5 C0 i+ b& M% o# p% d" H } _# q( |, U+ n/ _. \
return strtod(temp, NULL);5 w P* V# P( Z( t) H* e: a1 ]
}
+ w( O! h" P1 T- ], K0 o }
`$ ^2 K k& h# e- P0 k6 d/ A$ A& r
return 0;( }1 Q8 ? X6 U+ v4 n/ V
}8 |9 k" _: a% P$ Y! n! X# }
1 I( G+ L1 w0 F! \4 m
//look for the command if it exists.4 B5 C' ~" R, s0 I
bool has_command(char key, char instruction[], int string_size)
$ N9 s- w% y, L- [! a{
. ?+ {: h+ n9 k( y# N: z9 u9 p for (byte i=0; i<string_size; i++)
& ^7 `' Y: ]2 f7 Y& b- V3 x {, F& `$ X C3 I. C& p/ S/ E
if (instruction[i] == key){
- l0 ~, }7 U" [5 p: \" G- w
" u* l C- r: }1 A: Q return true;
0 K4 q1 ]% G# M. D }5 P, i! o3 Z: t& G# v% _
}6 N; A0 D1 p8 y& W1 D. k% N0 B& x
* _, K# U) S$ ^8 x- p
return false;: r7 n1 @! @. ^' {7 p
}
) X; l4 s" O% I+ s8 H( `
l9 I8 n( m8 n) H" e; t. ~2 M% ?6 h: J/ B7 X' r# W7 g
& ]* l7 Q7 p8 P# t, y5 e: Y# \
|
|