|
7#
楼主 |
发表于 2014-5-10 09:55:02
|
只看该作者
// our point structure to make things nice./ |% b% C% [/ ^$ o2 |2 i( W% g
struct LongPoint {( L$ X+ m5 u' ]) g/ e$ l
long x;$ L7 O+ a) W( c/ W- Y
long y;0 c. s4 N( y5 q* N& x" w
long z;
- P6 P) L# a" }2 t& t4 z7 e4 i long u;
$ L. e, r p' O5 B7 B9 V Z; G6 ?}; Z8 h* m$ m/ }
1 L, c- a. C# n3 U
struct FloatPoint {
" y4 P! a' W8 m* a& Z float x;( v. n* \/ `2 N
float y;% ]6 Y/ n2 b: Z2 ~& L
float z;% i0 }9 e8 S: g* G2 |9 o6 T' m; n; Z
float u;
+ {% M$ v4 G8 @2 q; r};; e6 C* s4 {( n! q( J; Y$ K5 R
& k! h4 b% z) O$ b; f
FloatPoint current_units;
3 d. h# x3 l8 N* H2 HFloatPoint target_units;. f) {- j0 m, {% Q0 Q( m
FloatPoint delta_units;
, N( x5 Q M' J$ j. @5 ?% r0 Z3 p
FloatPoint current_steps;( F$ h% ^9 q+ i. ?5 X5 X
FloatPoint target_steps;3 K \; f% O9 a5 B# r* S6 b
FloatPoint delta_steps;* p# [$ E0 u* F3 ^
( k( L# i2 g+ m2 i4 A& X/ y6 Z
boolean abs_mode = false; //0 = 增量位置模式; 1 = 绝对位置模式, Z2 j- @" ]5 w' `* r( X! i
& w- S* v I u- s/ G
//default to inches for units
: I5 _' d- a- \6 f2 Wfloat x_units = X_STEPS_PER_INCH;% n3 E+ s+ d: M2 }
float y_units = Y_STEPS_PER_INCH;
0 @! a: Q9 `7 I' T$ a+ P9 `float z_units = Z_STEPS_PER_INCH;. L5 E0 Q# d8 M2 r) D, ^6 q$ [
float u_units = U_STEPS_PER_INCH;
( X5 x8 W% e4 Y7 B( ?+ t! L) Yfloat curve_section = CURVE_SECTION_INCHES;
' v/ H3 v& A2 n, t$ d, e t& i8 z1 E; ~# b, P
//our direction vars5 U) d( [$ M3 h4 Z s
byte x_direction = 1;
4 }$ Z- }0 Q9 m. F6 N. L5 b' Ibyte y_direction = 1;
7 z- V6 w$ ?: w, F3 Jbyte z_direction = 1;$ b3 G( m1 a1 P2 `
byte u_direction = 1;+ m+ ~6 ?# x" n0 V$ P) b7 z1 K
( \* s7 X3 R0 I. ~* l9 A5 B//初始化字符串处理
/ c; I, A2 u2 i6 E) T- Hvoid init_process_string()! {. x7 Z$ t$ V1 Y* C
{
% M' N& n) x O9 i! L0 Z //init our command6 }/ R7 p$ T( I/ C* j
for (byte i=0; i<COMMAND_SIZE; i++)) s) ?/ @' v, ]' l
word_old[i] = 0;" i* w2 O2 \3 T
serial_count = 0;4 L6 G, |5 T. a3 [) {
}% [# c [9 }( H3 t. L
2 Y `/ X. U$ P$ t# T# v7 |4 |//our feedrate variables.
4 u6 J' B. `$ ]4 qfloat feedrate = 0.0;$ d/ J) ~7 ~6 M p! g( a
long feedrate_micros = 0;$ @- U$ m* N+ {7 f
, l# ~/ A2 F$ C+ y- O% A//读取并执行命令
- B5 U' i) t) N+ q/ h0 J5 B5 Cvoid process_string(char instruction[], int size)
^# t+ |% J* r7 n! _9 r{
0 V+ T+ ?4 I; A; c //the character / means delete block... used for comments and stuff.$ @$ q( {3 v# C ] c
if (instruction[0] == '/')
1 | `- z! S5 J' x' ? {) h$ k, a- S+ q
// SerialUSB.print("ok");: I: A$ Q% o, C9 y
// SerialUSB.print(byte(78));$ {- }4 L Q5 t9 Q$ Y
return;' b4 k" S7 R( L) d+ b
}5 [5 L3 l$ i2 @! Q$ u* Z& e: B' S
//init baby!
0 b* S) K+ |5 n K1 D FloatPoint fp;& j% c$ t& g3 m" ~8 C
fp.x = 0.0; _, x0 B- b4 Z
fp.y = 0.0;
* v, \8 t5 u6 ^( ]' b& k fp.z = 0.0;0 `* @. u: v5 n& e0 c, |9 v: R; p
fp.u = 0.0;, ~5 B& Z$ }. W/ y" G3 ]
) q* S0 L/ ?2 q8 v
byte code = 0;
' \1 ^+ M) C1 V) c3 w1 W3 u; F4 s! _+ Z2 c; W/ x% y
//显示在处理的命令
) b0 W- Q4 `* F5 o& q#if (LCD_TYPE == 1604 )
1 ?+ F% B: O. B: H7 l) U5 E // lcd.setCursor(0, 4);
# A2 b; o, n9 I% _' S& O/ D // lcd.print(word_old);/ w* F# W: r! X' G$ L/ q. O2 E
#endif
+ D. F+ \! `' z9 V8 K SerialUSB.println();, n$ E$ ? L6 j
SerialUSB.print(instruction);
4 e0 x. y4 N9 s0 @. _7 d2 O7 h$ e SerialUSB.print("\t");; Z' ^7 r, g6 Y" e) k9 y/ k
# K2 q1 a; V7 t" Z# i1 u: M' H v
//what line are we at?, q. q& i; b L8 @7 A. ^: r
// long line = -1;, c" c( y1 u( d" y8 w
// if (has_command('N', instruction, size))4 l* g) i3 W" k7 w3 G+ {* M1 v$ X
// line = (long)search_string('N', instruction, size);
* z' {% m& ^+ D$ H% A) l# Y6 Q0 N
/*
, h' L3 M7 J1 L! `* Z2 r& C* |; _ Serial.print("line: ");2 E; M7 H8 U. V! i, R
Serial.println(line);" K3 o9 w: g5 _, m' c
Serial.println(instruction);
) L; O9 Y8 U( M */
% ]) G. e6 \& |: v //判断是否读取了个 G代码?8 V$ Y1 }0 x- u
if (
6 n% f0 A6 q2 n has_command('G', instruction, size) ||$ |8 t+ l6 \2 w& W$ Z
has_command('X', instruction, size) ||
) S: h4 v2 L1 x9 }9 }- j' L# f has_command('Y', instruction, size) ||
% V3 d. N' j8 p" ~+ ? has_command('Z', instruction, size) ||& b/ W6 c$ a" ~8 ?) i- j
has_command('U', instruction, size)& H9 {, b% z) W
)% n1 B7 Q- K6 Y" `; E s
{
% Y" b0 F0 M1 K$ K( i1 s: D //which one?
1 j* Z! {% V8 }. Z6 { code = (int)search_string('G', instruction, size);4 a9 s Y, F- O K! f
// Get co-ordinates if required by the code type given
! ?9 x) O1 b4 D) ^: I switch (code)
3 Y& ~' r6 e7 `" ] {- K& j4 v% |/ I2 H* w
case 0:
' a+ o! T3 E# ^ case 1:, E# b7 N8 P) M; T7 [+ [4 V
case 2:+ x( K+ P& ~6 P: O k4 r' t
case 3:( b% F- e `& ]1 w* H1 O, A' A
if(abs_mode)1 I" g3 i5 H) z7 Z
{" l# [5 I1 X+ Q* N8 ^! d
//we do it like this to save time. makes curves better.8 @- {4 H& A5 w" Z/ Y
//eg. if only x and y are specified, we dont have to waste time looking up z.; H- t* S% S0 h
if (has_command('X', instruction, size))
2 ?9 f5 o [' U. R* u1 W( p. K fp.x = search_string('X', instruction, size);" q4 B; R9 T8 e+ c7 B: T1 B
else U4 M0 E& g, Z+ P& h" v
fp.x = current_units.x;4 G7 k0 x. z$ ~
6 e" D3 h# r$ v- x% W0 I# @/ R
if (has_command('Y', instruction, size))/ B+ m: s- \2 k0 ]- h, {
fp.y = search_string('Y', instruction, size);
. V4 ]* J7 d0 ^ else
% T; k0 i1 s# Z" @8 A1 J! f6 G9 i fp.y = current_units.y;
( y# Z6 r5 v( T3 A$ Z7 X5 E, ]& m, U5 g% X/ Y4 q
if (has_command('Z', instruction, size))# k! F5 u6 d# q8 d& p! H
fp.z = search_string('Z', instruction, size);5 y v. y1 F J2 |$ K
else4 k/ Z& L" q/ ~* w
fp.z = current_units.z;
$ j7 D$ N S% h- ]; h% w* L ^ . x# w( W+ `, X4 L3 Y4 Q1 \* v9 ~, ?
if (has_command('U', instruction, size)). l& y; w4 i) d3 b* H/ D4 D: k& g; T
fp.u = search_string('U', instruction, size);+ m8 v7 H" N( n6 M& t# g! b" L7 }
else
$ x5 R1 K( H) y' u fp.u = current_units.u;) s8 r- a. c* O& m3 e
}
% L4 C: Y0 |4 z" X" K" x else. P6 H9 H7 S! ?
{
' n9 O; A `! D5 X/ n fp.x = search_string('X', instruction, size) + current_units.x;7 D7 |; x( y4 x2 d) @
fp.y = search_string('Y', instruction, size) + current_units.y;
. y- ?4 u+ W4 o; L fp.z = search_string('Z', instruction, size) + current_units.z;
6 k0 G' [6 B( l; f' e5 P fp.u = search_string('U', instruction, size) + current_units.u;4 Z# }( \/ j. Z# y' R
}$ q4 m. j5 C7 _, u5 A7 |6 }! ~; d& I
break;
4 T6 o: r; v* U# h7 y- X2 D+ `6 O }
3 o' P% z% [. ^; U: F( s3 K //do something!/ G: t1 q- g" d/ Y- `
switch (code)" c) `# h5 G% W( F( p
{6 ^" r: O: d% e/ \2 F( c R
//Rapid Positioning5 v$ }; \+ k7 V! X8 Z% U, ]
//Linear Interpolation( s; R6 {7 v, E+ B& K- @( u
//these are basically the same thing.- R' i8 A, H% E7 `0 W7 P6 l
case 0:/ |8 y8 f2 e5 Q
case 1:
0 k2 ~' b/ o& Y/ z8 y //set our target.
; W" M! w" T. t% q set_target(fp.x, fp.y, fp.z, fp.u);# I& p6 T5 s% D
//set_targeta( fp.a);
1 I! y2 m+ C6 _6 H6 F# D9 z W //do we have a set speed?% U8 a1 s; j# n1 C" Y
if (has_command('G', instruction, size))
. H8 M- o: [7 j/ Z1 l9 ?! Y( g2 z; r {! w( C3 o! M ] g! D
//adjust if we have a specific feedrate.+ B* M) D" H) J0 k2 B7 {; @
if (code == 1)
' m8 ?% @8 x0 c {; @" o+ ^& v A
//how fast do we move? f! t9 Y9 G8 M$ `* x7 j
feedrate = search_string('F', instruction, size);
+ s$ {% ^- z- S0 B/ q if (feedrate > 0)+ v* q. I, M$ ?* e& N8 u' o1 W
feedrate_micros = calculate_feedrate_delay(feedrate);
5 C* {8 v; c1 h4 `4 P //nope, no feedrate# Y" G# ?7 \1 p* i* P& l
else, r8 A' u8 B' t
feedrate_micros = getMaxSpeed(); ^2 a1 B% _7 p v
}+ J8 S1 s7 }' i$ l5 w. x/ Y
//use our max for normal moves.% Q$ t, W* \& D
else
: f. \4 ^' h2 U3 k; |+ i7 ] feedrate_micros = getMaxSpeed();
5 J; R/ e3 \# M }
% q- d8 h W$ ^8 e5 {$ w! { //nope, just coordinates!; n8 K/ _* u: s1 m; i4 o/ g
else
: c# H# V* c6 R% \8 _ {
4 G3 ^; `: J2 {7 T6 F0 C //do we have a feedrate yet?
: m! d& d, ^8 N; o* T if (feedrate > 0): `* }- g) C8 o
feedrate_micros = calculate_feedrate_delay(feedrate);- v$ G8 R3 s# v, N
//nope, no feedrate
+ [8 C2 p" k) T' H( {+ W; Z3 \8 a else
- r0 V6 s' ~2 p7 r- M feedrate_micros = getMaxSpeed();% M, p# a: F: F! w
}
9 C" v, d( l3 B- C* l' M0 \
" a' t7 v. h# k" c1 P //finally move.
: }, a) u1 W6 W, D2 g0 y( {# `8 m dda_move(feedrate_micros);
7 V, M, O1 v2 g3 @" G if(stop_flag) return;0 X) G; C7 A$ t f
break;
. d. a/ K! _; ^: Q6 m. V0 }1 G% A' O) x* J$ |' R) r; Q( I) N
//Clockwise arc
. I& C2 ~4 G2 _ case 2:
7 ~. n) w9 x% W& |; _( a8 V //Counterclockwise arc0 k/ Z7 _1 |+ w1 r. K5 O' m
case 3:
" I3 M- P! T* t& p3 H# v FloatPoint cent;( G! P4 E$ i% i: P' w! H3 y
// Centre coordinates are always relative3 `( B' J8 \& P; o, `$ B! S
cent.x = search_string('I', instruction, size) + current_units.x;2 T: { q. n0 `4 k& i' L
cent.y = search_string('J', instruction, size) + current_units.y;
' t2 a8 o8 q |, B0 D float angleA, angleB, angle, radius, length, aX, aY, bX, bY;
|: j1 g% I& \0 j3 c: a: Y5 E4 B% W( Z: L3 _5 O
aX = (current_units.x - cent.x);
7 M( B- J% b5 h) r aY = (current_units.y - cent.y);
" p# {0 Y2 _& w bX = (fp.x - cent.x);* }; [! b" J7 H5 ^: b% A& C
bY = (fp.y - cent.y);
3 Q7 v, M/ d2 O. D1 Y+ i, `7 v/ k8 z
if (code == 2) { // Clockwise
. r0 K: i5 y1 j2 @& W$ o6 p/ i angleA = atan2(bY, bX);/ w- x2 k% t7 X1 X
angleB = atan2(aY, aX);8 M- D5 h' n: v `
}
) s: z z* K7 u) K1 m) M' n0 z. A else { // Counterclockwise+ a+ o' ?2 A9 E+ W
angleA = atan2(aY, aX);) v3 R) }7 q/ f
angleB = atan2(bY, bX);
2 Z- s' Y3 S* @ }, H4 [2 ~2 D3 d! T4 J3 t
// Make sure angleB is always greater than angleA, k C P* T E
// and if not add 2PI so that it is (this also takes
0 w$ W; }, H' `0 C. d) R // care of the special case of angleA == angleB,3 G2 F7 \0 b0 j) J0 S9 D& J4 }
// ie we want a complete circle)
4 n: [6 {9 i. |$ a if (angleB <= angleA) angleB += 2 * M_PI;
4 _; {# t. E" @& Q0 B angle = angleB - angleA;4 r5 p9 n. T( d2 I
: K8 s* [, H: {( B1 n
radius = sqrt(aX * aX + aY * aY);
7 w, ]! d7 V. q, Y' q length = radius * angle;% n0 @6 N- z+ D
int steps, s, step;: _- c, N! {5 `* Z; [
steps = (int) ceil(length / curve_section);
4 G5 t" ]. e. r, B4 e) p& T X! D& ^: D, m8 S% q4 K! k+ B/ G
FloatPoint newPoint;; n- T+ q$ A& H! U. J' N
for (s = 1; s <= steps; s++) {
+ \8 S$ `9 G- g step = (code == 3) ? s : steps - s; // Work backwards for CW8 C$ ?8 S) ], ^; O. @
newPoint.x = cent.x + radius * cos(angleA + angle * ((float) step / steps));! z, x1 G/ \! w" n; m) Z
newPoint.y = cent.y + radius * sin(angleA + angle * ((float) step / steps));( x. _' j+ O4 l4 M" m0 V5 I7 Z! j( s
set_target(newPoint.x, newPoint.y, fp.z, fp.u);
: x# c; D7 B# Y3 P
( d) q- r* [" d7 r" q( h8 d0 U3 A! V // Need to calculate rate for each section of curve* }8 n+ l/ ^" h6 F/ m+ N/ H
if (feedrate > 0)4 k$ J# X5 F' k; m- r" ?0 O* N
feedrate_micros = calculate_feedrate_delay(feedrate);
; p- ?& x$ j8 R$ l2 W- C; ^ else i; q9 S& X. O/ U, A3 Q+ ?8 x
feedrate_micros = getMaxSpeed();
/ L6 q: ]+ F" I9 f, j6 \( f6 f; {% r' s) D' B. o- y( D
// Make step
* z% C7 E. P/ m# [3 ? dda_move(feedrate_micros);
) f+ j( n4 N6 v, C if(stop_flag) return;# j g* |! T- m, J2 c8 f: t0 D
}& T$ E5 _* C# @" f
& P5 A- @5 F% T1 t, [8 J break;7 c0 o, w/ W- F
Z I/ @6 I* i# p
//Dwell
% c: T7 e# n: T6 u" A# L case 4:- R1 t" D1 p. p" `$ o! Y' L% | j5 f( W& b
delay((int)search_string('P', instruction, size));0 I# m- o S( H& b6 ]
break;
1 ` y4 F* G* R" `, n h' c- Y$ m- \7 l
//Inches for Units0 q- d- i( m* W6 Z, X5 K$ J+ p
case 20:
0 q) V4 A. _5 Q/ ^" L* N) | x_units = X_STEPS_PER_INCH;
" t c( X2 W# k& }$ _ y_units = Y_STEPS_PER_INCH;6 c3 [! s- K5 B
z_units = Z_STEPS_PER_INCH;! J* o: u* J8 T' h4 G6 {
u_units = U_STEPS_PER_INCH;
8 Y4 _0 g# W2 Y3 Z1 M& t curve_section = CURVE_SECTION_INCHES;
% g( g8 v: [, ?4 y calculate_deltas();
3 ?5 s* H4 Y8 ]# g break;: r' [: x# S7 i* C0 Q) B' a: @
9 r- E- g( `/ `& ^
//mm for Units
Z% b1 I5 z' g: \( P case 21:
) Q G; }+ z, p# ?2 Q& R x_units = X_STEPS_PER_MM;
E ?! q# P: f* b E y_units = Y_STEPS_PER_MM;6 @9 u, A% F, {; `" [
z_units = Z_STEPS_PER_MM;
7 Y8 ?! Z& l! T9 i, m4 n! u+ @ u_units = U_STEPS_PER_MM;% {: I( H* y( l5 L% N5 q0 B
curve_section = CURVE_SECTION_MM;& C3 F- v3 f( u, n9 B
calculate_deltas();2 R! h4 `" q. e% B4 N$ e$ h1 H: @
break;
2 i- `, g2 b, @7 R; e' w6 I/ d# r& i
//go home.
0 h4 F$ c5 m9 c; X' L case 28:8 L/ H. j7 x, A- P! \
set_target(0.0, 0.0, 0.0, 0.0);/ j4 O/ Q8 r! v, ]# ?8 S# R
dda_move(getMaxSpeed());
7 n, u% w) q9 L" T5 u# l4 r if(stop_flag) return;
6 L" U! y/ d, t$ ~7 c4 n break;) {8 ?+ y e6 I) l# f+ S" ^! }
\, ?/ h2 o* p0 V) O
//go home via an intermediate point.
5 w! x: P5 N! _) N9 j case 30:% ?# h8 Q( o3 V* C
fp.x = search_string('X', instruction, size);, J: l$ x+ W0 w
fp.y = search_string('Y', instruction, size);. \4 M* T/ d7 L; N# v
fp.z = search_string('Z', instruction, size);8 T, h4 S$ h' g( _# {! h$ |# F
fp.u = search_string('U', instruction, size);3 j! ~6 Q- O, P5 c/ H; \. ~6 `! o
//set our target.
# O( c3 A3 M: }7 ^/ M3 z if(abs_mode)
( \6 E8 I8 V1 D# i4 v" X$ y, c {
% P Q- E" [) O! ~- @& u9 J" Y% X if (!has_command('X', instruction, size))
+ _9 L2 {; z/ r" w fp.x = current_units.x;
! \6 [- M. T& I. s# G+ L+ G- a/ p+ r. V if (!has_command('Y', instruction, size))
, t4 G5 ]* B: }- R; ~ fp.y = current_units.y;+ {5 W3 z" f7 }0 G, i9 N
if (!has_command('Z', instruction, size))7 Y# _' H; j8 j
fp.z = current_units.z;
- o3 x6 {- c! d& b9 w% t# [3 v if (!has_command('U', instruction, size))- j/ F8 w# E1 f9 ]
fp.u = current_units.u;
2 G; ?2 h- }. b: X3 v set_target(fp.x, fp.y, fp.z, fp.u);2 C6 V2 _* |1 s \! U3 @- k4 ?
# ]8 J/ ?5 i0 ^$ P: p }
! b0 {& } ]2 B" r2 f, O else
6 O1 K* C3 {5 o' W6 C4 v8 S5 ~- K set_target(current_units.x + fp.x, current_units.y + fp.y, current_units.z + fp.z, current_units.u + fp.u );
0 e* J; a! ~: q/ G2 P' R7 o
- L8 r! u# Q6 B* D/ d: y( D //go there.
0 b7 ], U0 i6 F5 ]* r; S" Q5 e dda_move(getMaxSpeed());4 Z6 k0 [6 Y- @9 e- Y/ _6 P
if(stop_flag) return;5 r- ?1 F0 A; S. l- x% X' S1 j
: U" b+ W, b' ]( e/ u5 ?' F
//go home.
4 g. x+ i6 L, B0 B set_target(0.0, 0.0, 0.0, 0.0 );
3 v& n/ u, ~6 \- u3 x ; i9 \( ~* `7 {' [ e9 D2 M& Y
dda_move(getMaxSpeed());
, j* k$ p1 z8 |; [# F& | if(stop_flag) return;
! R5 ]; D0 h8 u! ^$ |% g( c' K break;
% ^' L3 U& h) x8 U+ @' G, g! m r. D. Q
//Absolute Positioning
( ^$ n$ e: X$ G. R case 90:
! l7 x3 H, a* m9 G abs_mode = true;
6 K* z1 j% e) E3 x' x ~ break;+ |7 R% [9 A! W
! w& t' q$ C( o9 B q! E* G
//Incremental Positioning, c1 V/ s0 i8 K4 \
case 91:
( ^6 Z2 J# ?3 X- |3 Z& N abs_mode = false;$ ?4 E6 Y( i" | _% }
break;
9 U& j; B( ^" N/ @8 J6 F* [8 ~: u( `0 J& t2 y7 a* |
//Set as home* L1 E [- W' H4 J' u
case 92:) Q Q d+ H8 Q) j
" K5 G& F" Z& h; G! y7 l8 G! ~+ G set_position(0.0, 0.0, 0.0, 0.0 );2 w. d# [, q3 [6 x5 t4 @
, A- o( P9 P3 ~7 T3 |- J
break;
" R: Y7 K4 d! H f$ `
5 o ?: r1 K- ? e! Y /*
8 P" R% J. D7 d1 ^ //Inverse Time Feed Mode
9 |6 l% A' |. A+ K9 V. U case 93:2 ~, ^5 I: q; y! t" s8 w
$ |* C- c# }1 c% w6 g
break; //TODO: add this
* ]& e& n% Q+ z) a2 C: c0 p& x R
8 T: g$ O- J( x4 E E( a //Feed per Minute Mode: ?4 p x: ~& C6 N W
case 94:3 X2 @9 \. [- \ i. P2 N' T/ T" X9 q
9 }6 ?6 W* r4 |+ m1 d, G break; //TODO: add this- b" J {7 e& T" T$ G- B
*/; s- i3 |! g* q8 j3 I0 k7 I. O
0 ~+ w; m/ D2 b* \! _( u: I% E
default:* J3 E2 c. h. y, A8 n4 m0 [+ t. _1 e
SerialUSB.print("huh? G");
~4 I( y' i) e, D SerialUSB.println(code,DEC);
. L0 R" i8 p$ P5 M }! D# j! G$ u6 ?4 X! P
}, X& X9 g/ z# J) ~1 r6 t
% L+ j; }% [0 f5 A4 J8 S6 C
//find us an m code.
6 m6 k, O' b! \0 _& n! {6 i if (has_command('M', instruction, size))
) [9 p, j0 b4 i; _ { f2 S7 G. Z0 k8 E
code = search_string('M', instruction, size);2 B' E% N3 Y" y* }# E
switch (code)0 [" e9 r2 T* J: x, n$ F3 @
{
- A7 ?7 \" \4 g% ^. x$ D //TODO: this is a bug because search_string returns 0. gotta fix that.
T# u4 n* R1 N+ I9 N% N1 U3 N6 J case 0:
8 Z0 V8 L' [8 H3 M3 A( G/ e true;
6 s% W; H- A* U- ^6 I break;* O, M3 Q" [% \) ^
5 Y ~" l9 T/ I" G3 I& h9 { case 100:
6 A9 x5 ~2 A6 {& l9 x break;& U9 _! u/ }: S
3 M6 V7 Q* ]$ x( r. n // fire camera relay+ K1 a- p* f6 [" }4 y5 f
case 101:. f- f* Z! g6 j! @/ a
camera_shutter1();- w& o* i( \5 @* @1 |% E& _
break;
7 \" U5 l# `0 [. ]1 N* i1 `! m% c' m6 t% R4 o, Z% T- y" J
// fire camera relay2( Z: f( V6 k! r$ @: b% u: L
case 102:5 k6 y& d8 K9 O) ?0 }* }( \
camera_shutter2();
$ u$ c) m) q8 b break;
, V! T/ Q! B7 M( o/ [& R+ K/*
3 i) z% M2 z* ? // turn aux 1 relay on
! v+ T# K" B+ I7 Q- s$ U& c9 C case 103:; o4 r f+ e2 f9 i- g
camera_aux1_on();6 \ _' K$ h- V3 w8 m3 t( e# O
break;
/ W! h$ X1 p: O* T$ \7 k u8 s
// turn aux 1 relay off
& X- o! u& P! y case 104: Z% u% N5 @8 t, u1 f; V. j+ ?
camera_aux1_off();
$ s' k# C! M, l break;
9 @2 ~8 x( L( Q7 a5 h
6 T( {$ u7 K# S# i2 W" o! y // turn aux 2 relay on
7 \# L% g) @$ ?. b: ~6 s% m case 105:
# E0 G2 h8 {/ V( e! w camera_aux2_on();
7 |) E0 d" N! z- E! c% i break;
, k8 U7 w. o' a8 U8 ?: ]' h) m/ `, i* _- Y2 @
// turn aux 2 relay off# \/ F( f: T/ K: o$ h9 P
case 106:2 k8 _9 \& F% w( a( m/ ^' @7 `
camera_aux2_off();
2 y; P7 a+ Y" N2 s1 M! { break;1 J5 L! v3 o% B: F, N
% d5 E" n9 H; f# D; B
// turn aux 3 relay on% h& |- ~8 ~4 r9 d/ p* U6 B# `" d
case 107:
- J% f& m- F( P* U camera_aux3_on();
! T/ L) C; c. o- o' e1 n ^ break;
# p) [& \2 M$ Q8 [7 l3 L- F: H* ]- j5 X$ Q
// turn aux 3 relay off: h; ]& J3 o x( W
case 108:# @8 Q! B4 _. E6 D/ X3 Y) p$ Q1 t
camera_aux3_off();
. H! {% q5 @9 a8 ]! r: v break;
& j3 s( v. R1 e' q+ [; D Z6 {) t. w2 z
8 t a; k; C+ E9 I6 O6 s7 i2 m) C // turn aux 4 relay on
# i$ o8 |7 M k5 F# \7 L1 q% ~ case 109:
8 i f O; q! h6 Q3 q camera_aux4_on();
: O' ?: U1 S6 j; Y) k0 k break;
+ e: O' ~+ I: q+ Y% j+ a; t8 L5 W( t! k! ?* X; Z' ^& i, W) F
// turn aux 4 relay off! t# s" n) v. L; q: K4 d5 \5 ~) G
case 110:
: v2 e$ m- J$ U# |+ U* A7 b camera_aux4_off();: v% ? h/ i. ~+ _( e
break;$ Y2 g) P! @) {- V, `! f f; g
*/+ V W; B+ A- Z$ e2 g* T' t* E' ]
default:
1 O; n. @1 Q7 X4 c2 Y1 d
+ y- C# |1 X+ }3 u8 }. ?! q, r& ^% g SerialUSB.print("Huh? M");
; O. P- S6 p7 J8 ^' h9 Y! @. r+ S SerialUSB.println(code);
) w% h' v' P. P, T( p( G8 c6 M }4 C7 k4 \" {" u! F" k/ q
}
6 H3 _* d w3 J, L9 W/ S B( }$ M/ `, {0 l
//tell our host we're done.) Z- v5 W& ]/ B* \7 E ]+ @5 P
SerialUSB.print(byte(78));6 H, [- s6 L" {9 k& S8 n
' W Q, N5 |0 F0 Y7 f
} w" m/ W8 E! E f
: R9 o& \- \4 }7 ~//look for the number that appears after the char key and return it. s) a# H( P ?0 t( `
double search_string(char key, char instruction[], int string_size)
6 l( o1 n, B- a3 |{
' ]& S3 }+ W9 m y0 [. Y8 G4 I- a2 I* z char temp[10] = " ";
- ]3 y4 Y( j2 H f1 U- E for (byte i=0; i<string_size; i++)
$ |4 t, E T! O) \# N5 ~5 _ {7 L' ^. K4 U# Q: D) R. l9 ^
if (instruction[i] == key). ?6 H2 V5 i: p) S
{% j! d; w: N7 w0 W8 T% x
i++;
6 f3 `& m8 l' {) x. G int k = 0;( W9 b# j: f9 w: I/ W8 C$ C8 N; `
while (i < string_size && k < 10)
, {+ f: b- I2 K, q! z3 Q {
: L* Y, W5 Q8 N1 x7 C U3 f) r2 [ if (instruction[i] == 0 || instruction[i] == ' ')
3 o+ N3 V. A: j- b$ [0 k9 |, A- d break;
7 z o, S; }. ?# r) J! \( z( D* p1 w" G( b; c# j
temp[k] = instruction[i]; a2 H) \3 i2 Q& p8 O0 Z
i++;
9 h1 c9 p- X9 O. F) ` k++;4 {* m6 R( b9 ~0 h
}
( Q5 o+ H9 K- U( S return strtod(temp, NULL);# i- ^$ d% s1 E* E. C
}8 U% O( I0 q% I% O9 X0 z/ ~
}, Y, F4 ^1 G/ C9 B6 l
1 F# P1 _4 s/ g- k
return 0;
* J7 t& e) N/ R; m}
3 m* R- B: W9 y$ q z) I# b5 n( {0 C0 }# V
//look for the command if it exists.
7 | ^$ P: D; v3 Qbool has_command(char key, char instruction[], int string_size)
4 Y s; S- K( M8 }9 }{' T: z5 p6 w6 x
for (byte i=0; i<string_size; i++)
, k4 I1 |* }$ o# K# P; Z; P3 V! N: J {7 q; {8 q% ^, a4 i& u4 ^6 N
if (instruction[i] == key){; s5 l8 @$ H7 b
- u, w3 p9 z5 _$ X3 d8 @
return true;
& y* [$ S" t! A2 |" H6 N }& e+ M! d8 u- i; x. T' S+ Z% _
}
9 |# C+ P# o/ A- a2 Q
8 i A# w9 r1 }* h y+ r return false;
& L8 U- d& W5 f3 a} D+ W/ i" J5 w. W# W6 `6 w ]* \
4 w c6 J" |5 B( ?4 K( I5 R
8 j r. p% J( {" c; @% w5 U( i+ T* r( s
* I& H) C7 o, g1 F( s |
|
|