|

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