|

楼主 |
发表于 2014-5-10 09:55:02
|
显示全部楼层
// our point structure to make things nice.
; D3 ~6 ^7 L# Vstruct LongPoint {. q' d9 {+ W5 [
long x;% J/ F5 q9 Z) Q7 E2 l# [' A
long y;
0 E8 S$ l! M* ^ long z;! d2 M& Q% y! R- F s
long u;4 P' P: n, E* Z- m) b4 x" v, G! m/ P
};: e+ C# u: h% o6 {
/ b/ d6 D' L' ]/ V ?& O- @: ustruct FloatPoint {1 p! C( V# C/ ?1 R' E- H" A
float x;% m+ X U: g3 J8 Z
float y;
6 ~5 w- h3 b# w+ ^5 F! A6 Q float z;
& [# o# m% l. G4 L! R5 u$ _4 h float u;
/ e- ~* A$ t9 _5 V! b};
5 w9 D* w2 Z" a; _' I+ A( u+ p# [2 D& ^# g' ]% Q! {$ \
FloatPoint current_units;
; y0 {3 f4 z" S# d& R* UFloatPoint target_units;6 ^# g1 D% D& z* [4 n$ W
FloatPoint delta_units;
3 n; S+ o/ U S% c6 j8 b$ u5 Q2 W+ y- b
FloatPoint current_steps;* x- J' h1 h1 y, r; s3 U
FloatPoint target_steps;
& i; v: o Y9 d( J5 W' o* @* cFloatPoint delta_steps;
1 _: q* |' w! r: J7 X5 F: @6 j0 @9 ]6 `8 o/ F. b9 `- _
boolean abs_mode = false; //0 = 增量位置模式; 1 = 绝对位置模式
: P; E! S$ f O; ]. j4 P
6 j& J8 p$ a: }. V( y% s//default to inches for units
% ~, G6 l, G6 wfloat x_units = X_STEPS_PER_INCH;
2 N* S7 g/ X3 L/ q" xfloat y_units = Y_STEPS_PER_INCH;) d* y6 ~+ @* q; _" V6 s
float z_units = Z_STEPS_PER_INCH;
1 F( L+ E. `3 I- Afloat u_units = U_STEPS_PER_INCH;% G& y: C3 ]$ Y6 q; L
float curve_section = CURVE_SECTION_INCHES;7 y3 X6 j* u% c, T
; f, n( j8 B% k7 h( E; M. Y) D
//our direction vars
8 n- ~/ K6 Z0 }# R8 N+ Nbyte x_direction = 1;" C5 Y$ a( \' H
byte y_direction = 1;
6 K* n7 E* ~; H3 ]; y) y1 ubyte z_direction = 1;
S& ~4 p7 f4 pbyte u_direction = 1;& q$ {. @9 T" s% A# V" Y+ m
. |# u7 D+ s* @2 d3 J3 C//初始化字符串处理7 K& m! r1 v4 ^5 @7 k9 W& x+ @
void init_process_string()
# h8 [0 d6 g& c/ _. A{
( J. x& Z( R1 q7 Z& _ //init our command1 z8 Z$ q2 j% G2 Y, H! m* m& s+ |' H
for (byte i=0; i<COMMAND_SIZE; i++)
' g' S' ?2 J6 g( z4 W word_old[i] = 0;
3 f5 z3 h- r i! u; ~/ a serial_count = 0;
+ R1 g! }% N% e4 S) K}
) y, g z# F1 E: L0 p/ }* a8 {# z/ @7 T7 \$ j8 K3 V/ _
//our feedrate variables.
6 }: ^; ]* B# X2 ffloat feedrate = 0.0;# T. p' ]: `2 K
long feedrate_micros = 0;# Y( L( ~9 A! q0 b
4 z( d( \% }* G//读取并执行命令
5 I4 Y5 W. h( w5 Z, O- `void process_string(char instruction[], int size)
; Q$ T) y& Q* W- ]6 O j# `, m3 [{6 x+ v( w1 |3 W; o( V
//the character / means delete block... used for comments and stuff.
6 B' V L& p* Q: ] if (instruction[0] == '/')
& `0 [9 e N( `( t2 P {
R5 x. U' P* d4 O# | a# U% f // SerialUSB.print("ok");
( E ^; v! \7 M3 p // SerialUSB.print(byte(78));! p* k9 g' v; R, p3 k
return;
; n# D, R1 L) a6 f4 Q. G+ P9 y }
0 A5 z: P% [5 q/ ~% S5 [ //init baby!# ~% w7 _7 _( G3 S s1 V
FloatPoint fp;
) C- p- W6 j7 } B$ M0 f fp.x = 0.0;
0 v1 y6 V" I) f% E; ^ fp.y = 0.0;4 w0 g0 V, w G7 z% f& g! `0 z
fp.z = 0.0;% X9 }8 ?: ?8 w7 r W
fp.u = 0.0;
. M- x7 ^7 c* T4 T7 b6 _7 e j5 S/ b
byte code = 0;
4 d% }- t% x2 T W1 ?& {. K9 N: j P# ]7 I/ s
//显示在处理的命令
* f- y0 ^9 b$ [/ y- i: d#if (LCD_TYPE == 1604 )+ h( b3 Y1 i" U t
// lcd.setCursor(0, 4);" K9 [7 L- N! Z, Y
// lcd.print(word_old);
& n2 j" p7 B2 ]( F! A% ]8 z#endif$ O: M* A3 b4 s) U, |2 ]% e
SerialUSB.println();
8 |( U2 q1 }' a9 {$ ] SerialUSB.print(instruction);
$ h: O! E' T) z) O SerialUSB.print("\t");
6 U+ K% L& X# X$ B+ f- H# m
# v) C6 f# c5 K- i* Y9 s9 V' \& P8 @ //what line are we at?8 `( e, N2 W9 {2 A2 [! A1 t
// long line = -1;2 g1 k" j; @6 y0 E; o6 w' [
// if (has_command('N', instruction, size)); s/ Y1 z, w1 A% ^1 s8 L
// line = (long)search_string('N', instruction, size); c! _- {7 l0 {$ }
- |- o5 g! R4 P* a; _5 _ /*4 S+ C/ S& O5 w5 q
Serial.print("line: ");- |+ a5 w9 q ^. k5 {- t6 A
Serial.println(line);
! _' b$ K& r6 ?- Q+ R Serial.println(instruction);4 j" N' n( z# A1 ^4 `
*/
& u% e: C+ N; w1 d6 n0 J //判断是否读取了个 G代码?+ r( n% h S0 m. [) R
if (
0 S$ U% |! B+ p has_command('G', instruction, size) ||5 d0 f8 \/ F- P: r5 \' q& ^
has_command('X', instruction, size) ||( f( {9 B: T3 ^$ d( N
has_command('Y', instruction, size) ||
2 N' r: G4 v1 A7 J( y" N2 Z# { has_command('Z', instruction, size) ||4 s1 \2 [; b2 x+ U( s. s% M2 `: M
has_command('U', instruction, size)
1 ]' N1 ^1 n6 v# j) e- p. D' a )5 d7 C& g+ |* t- m5 H. Z& O, `( `2 F
{
7 ~: ^1 s1 d1 T) [& R- n //which one?$ S7 [# R k( o) M) b8 ]
code = (int)search_string('G', instruction, size);9 x u) l5 ^8 T% S
// Get co-ordinates if required by the code type given
" K% Z: _ F( ^7 v9 A switch (code) ]. |& p8 z8 y' d% U2 Y+ e
{
2 T. p# H0 H# w2 p7 v) F. ~4 U1 A# Q case 0:! Y; g; t7 G3 [3 Y) F
case 1:) I9 d5 O. ^. e, u4 b! T" p
case 2:" L! G9 X. i, p/ Y6 {! z4 `
case 3:
: c7 U( U u$ ^- V" a2 _4 r% } if(abs_mode)
# M& m8 a( r' C$ ?5 w" z {
9 |, g8 e' B! a: C# @. | //we do it like this to save time. makes curves better.$ [* k. U/ x+ u4 @: a
//eg. if only x and y are specified, we dont have to waste time looking up z./ `1 @; T/ }5 d0 N9 o) F
if (has_command('X', instruction, size))
1 [. l" T% W, f2 ^/ ] fp.x = search_string('X', instruction, size);3 ?2 A3 c* f3 x5 M3 l5 c
else# |& w; g' k3 m% V( x2 W
fp.x = current_units.x;
1 g, F" L8 S* ~, ^0 i: E0 A% \
) W) P- f; X7 n s, A, k" X if (has_command('Y', instruction, size))4 K: t0 Z! `; n
fp.y = search_string('Y', instruction, size);
* I* X) D( K- v9 v else2 _' O& @* c0 A) W+ w& G
fp.y = current_units.y;
; O* b; M) H8 ?" H+ ~' s" {" Y& b7 n! G0 [8 m$ U
if (has_command('Z', instruction, size))
2 i% D3 ]% c3 [ fp.z = search_string('Z', instruction, size);
3 A0 _% W% F- C else0 M, F% ~; o* W6 N1 A) t4 {
fp.z = current_units.z;
% p8 ?/ E; K0 ^ ! a8 C5 E% b9 O8 H" x. I& z
if (has_command('U', instruction, size))
, ]1 [ n# j( O+ s fp.u = search_string('U', instruction, size);
' ^& r9 W; [) z4 v0 ?7 _* Q; K/ @ else- _$ X% d4 C- y6 g8 X
fp.u = current_units.u;
7 Q: j% w, @( c' ~& }) a }) S: G8 P' K: `% i) e0 |
else
" T+ v% F& _3 m- s {0 p Q+ a( x( T: `* m. |- V. w9 z- [
fp.x = search_string('X', instruction, size) + current_units.x;; `! J' K6 ? Z8 [
fp.y = search_string('Y', instruction, size) + current_units.y;. P3 v, t5 {' E" L$ F
fp.z = search_string('Z', instruction, size) + current_units.z;& l( H, l; h" j3 K
fp.u = search_string('U', instruction, size) + current_units.u;3 u% {* Z& V: _, ^
}7 U3 M( ?, q1 S" I- {4 K& k' ^1 ?
break;5 N- i, y+ v3 p. i" z
}% f+ C& W d* e3 P. o
//do something!2 d+ H9 `! S/ I1 J+ G8 ?- ?
switch (code)+ u) J8 U) i K1 t: v/ }& B
{5 [& s; @2 M1 P
//Rapid Positioning
9 E3 T- H# G5 ~, Z4 C- ^ //Linear Interpolation( }5 h. B2 }7 z& T7 I: k s
//these are basically the same thing.6 o \: ]! x- e) I( m7 |2 R+ ]
case 0:
7 [% A0 U, H4 k0 o' w8 ~ case 1:" U: e3 m0 W& O8 t% [$ I
//set our target.
1 e4 n( F/ M; m7 r set_target(fp.x, fp.y, fp.z, fp.u);3 K ^5 ^1 H2 _# d) G
//set_targeta( fp.a);& i+ u' [) j8 o" ~, U$ r
//do we have a set speed?
0 W- S5 A6 }0 H+ g- R' T if (has_command('G', instruction, size))' Q% `8 ^. D m T# Q
{; f: O9 }2 v. e9 ^7 g( Q- t
//adjust if we have a specific feedrate.
& Z* Q, o! k& @: }9 R4 S if (code == 1)
' _3 A1 E( {- A, W' ~ {
' T! k; C! Y; h: q1 O @4 X- I //how fast do we move?, W% A9 M, u; I
feedrate = search_string('F', instruction, size);4 B$ F/ n, ~0 t
if (feedrate > 0)! x7 l) l7 i. f* n
feedrate_micros = calculate_feedrate_delay(feedrate);. y5 o3 ?3 T6 r
//nope, no feedrate
5 E5 j4 d- c5 X else
$ `! o8 a9 }1 ?& R. {1 x feedrate_micros = getMaxSpeed();: [+ a6 F& o4 q4 G8 ^
}
6 X1 i" X, d# c1 g- ~' C //use our max for normal moves.
5 J9 o9 y y" W4 d- U else$ |7 C) T5 R# I$ e7 n; p% i0 }
feedrate_micros = getMaxSpeed();& l* _& o3 p `( T% J1 Q
}
: z- ?9 L" X) Q9 M //nope, just coordinates!
1 w( c- |" Q$ O. E' X9 l else
; q! W% P( J4 s( N& Y: U, X {+ ]) T7 Z2 q. f# v9 [5 M7 J
//do we have a feedrate yet?
# c/ _0 Y1 n, k, j. O+ H# U if (feedrate > 0)5 L( G' @9 }6 }" P
feedrate_micros = calculate_feedrate_delay(feedrate);
d( \6 | J! N' H8 p# {$ D: `" D" e //nope, no feedrate
" S( }9 k! e) K5 K else, @- H# u( ^# {: ~
feedrate_micros = getMaxSpeed();+ q& Q+ W. G. t: k0 f" t
}% u0 G1 P( N8 \1 q+ p
. R; W/ L& Z2 R0 | a% q0 V* O5 B
//finally move.5 `! Y7 ]4 E- X- d5 J
dda_move(feedrate_micros);" e& Q& K, u' {, {. Y
if(stop_flag) return;
/ U6 y* u1 D Z0 w' s, |( x- m( |1 D7 I' O break;
$ G: @& ]: O2 }! m) |, ^& s* H3 k9 }0 A0 S
//Clockwise arc
! @! `( ^* h! H- D! p6 u case 2:
) N% o1 ~# J; i //Counterclockwise arc' u7 Z/ @/ r, `, l; X( }
case 3:
* J* k( q, j6 L* l7 y) Y2 s FloatPoint cent;1 | o& W+ K* o1 R/ W
// Centre coordinates are always relative
: m! r! l7 k. s( W1 r cent.x = search_string('I', instruction, size) + current_units.x;- D2 A8 `8 F3 {. R9 K
cent.y = search_string('J', instruction, size) + current_units.y;
4 K; w( k- h; {6 A/ Q0 w8 w+ i, P float angleA, angleB, angle, radius, length, aX, aY, bX, bY;$ d Y! X$ \+ t, b; }: G8 t
: r+ w* a! }: [+ w6 X
aX = (current_units.x - cent.x);
- [* K s& W) b5 o, P aY = (current_units.y - cent.y);
$ O. h4 V: G- r+ R. I; H0 z bX = (fp.x - cent.x);
3 B2 ?7 l& {+ [# I" h- V: p bY = (fp.y - cent.y);* w9 ?/ W/ B, G ~
& k8 Z6 |% X: y: x2 Q
if (code == 2) { // Clockwise
# Y- N% w2 A" R, |' ^ angleA = atan2(bY, bX);; B! _ N0 v) d5 Z1 l! B
angleB = atan2(aY, aX);- M! ^+ Q3 A" T1 [) G. m) P0 H
}
' a6 g! l1 T5 I2 H* S else { // Counterclockwise4 `; U. p% \, x3 f: d
angleA = atan2(aY, aX);! W8 b/ s5 U8 i1 k2 e- Y- V
angleB = atan2(bY, bX);
2 R* b: ^9 D; A/ I2 t }
. |& T& @0 X$ V" `6 C5 A( Z // Make sure angleB is always greater than angleA
% z3 H2 N& K8 V9 ] // and if not add 2PI so that it is (this also takes
* Z1 Y/ N* Z- [* K0 M; E/ [ // care of the special case of angleA == angleB,- o0 P' p' g9 ^4 e% T
// ie we want a complete circle)
7 @# R& S6 ~4 I! k1 @/ N I if (angleB <= angleA) angleB += 2 * M_PI;
! P1 _7 O/ ?5 v$ Q3 K angle = angleB - angleA;0 Z# g+ _$ q" {! h$ A% s
( z2 j, d5 ?& ~
radius = sqrt(aX * aX + aY * aY);
7 N' w. C0 \+ e2 F length = radius * angle;% w/ e2 @& X# [* @) F& d
int steps, s, step;
! q* g$ B- T; C( g# _ steps = (int) ceil(length / curve_section);" E( H( J1 G# s7 Y0 {5 V
2 m. e. ~, B( a6 ]) J FloatPoint newPoint;
+ \5 h. e9 O" |( n" Y) Y* r* X for (s = 1; s <= steps; s++) {
4 Z5 S9 Y) Y5 P& X* _+ c) g step = (code == 3) ? s : steps - s; // Work backwards for CW
" q) _0 W# r" Y* ~7 N newPoint.x = cent.x + radius * cos(angleA + angle * ((float) step / steps));: F" d" j/ k, K# h2 T. v( L2 r
newPoint.y = cent.y + radius * sin(angleA + angle * ((float) step / steps));! w$ |! B3 ^/ @ W( g s' D: h
set_target(newPoint.x, newPoint.y, fp.z, fp.u);
9 }9 f. ^$ W2 _! \1 a) `
) p5 i( @) a) r$ J9 p( V // Need to calculate rate for each section of curve
5 \* F5 c+ Y/ B3 }/ Y: O4 |) d if (feedrate > 0)7 U9 n2 {0 t" d
feedrate_micros = calculate_feedrate_delay(feedrate);
3 S0 ~# }: ^) Z; A1 N% z e3 Z else
1 T: a- @8 f- Y+ x6 ] feedrate_micros = getMaxSpeed();
1 V; u) q" C6 Y, n8 S7 w
" ~& X# i3 K6 A6 A // Make step0 [8 z) n! ]( g7 R/ T
dda_move(feedrate_micros);
: \ \( t+ u, ?( p) Z, ? if(stop_flag) return;
, T2 T- o% ~% y# W- d }
2 U. Q" N0 Y3 d0 L2 X' U6 x9 G4 I" U+ b; u- w/ t' X
break;8 X3 B* V2 `) }: u# c
0 J4 ]9 P. B! U" c0 {: A //Dwell
7 i% g( m+ B* a" V case 4:! r2 z1 j2 E' C
delay((int)search_string('P', instruction, size));
" y8 s1 D* f* R' ]9 \# X7 Z2 H break;
' O* x, ~" M0 R' l; N H: D$ W% k$ V+ M1 L
//Inches for Units
- ?1 j& k3 A& h7 ?! U" j case 20:% T0 g; F) G! M: ?% ]. P! H
x_units = X_STEPS_PER_INCH;
2 X7 q- d# N: w3 P4 R y_units = Y_STEPS_PER_INCH;
5 V+ @& x: d; s" Z- Q! X/ ] z_units = Z_STEPS_PER_INCH;
+ x/ e6 {/ M0 Z4 X$ t u_units = U_STEPS_PER_INCH;+ b, f+ |+ }% j5 S+ e
curve_section = CURVE_SECTION_INCHES;
) o4 i- b3 C) o( y6 _. E+ i calculate_deltas();
! X1 X- H9 |# V2 Z3 j break; O. D, }/ F1 q1 c2 d4 n
7 ?9 K1 m: h" j" { //mm for Units6 E, r$ Q* O: q
case 21:. D2 P1 x# o% R2 Q0 y6 i) X6 O
x_units = X_STEPS_PER_MM;
D1 b9 V6 C8 S8 e4 P% X% F y_units = Y_STEPS_PER_MM;
' p/ T4 @0 h1 d% r7 g/ a6 P: Y6 ^ z_units = Z_STEPS_PER_MM;: L$ X1 z2 ]0 n( N; m; j( `
u_units = U_STEPS_PER_MM;
6 w+ E) a; m& Z1 G curve_section = CURVE_SECTION_MM;
% {" E) P2 Q# H2 s calculate_deltas();- O6 c, x) o l3 y$ g) A3 a
break;
6 c( n! X- s' R' Z( d+ R+ ?9 w
, l" l* l% @/ Y" b //go home.
. T$ |; L3 |9 {/ q1 C5 F case 28:
& f$ Z7 ]) `- z+ p- \ set_target(0.0, 0.0, 0.0, 0.0);
9 Z' \' w5 e% g# A# x dda_move(getMaxSpeed());4 N4 V# _* ]/ X; s' I' n) ]
if(stop_flag) return;
6 I& C: N! O* L0 S$ ]) L s break;
9 i; Q, p! x' V7 l4 ?! P1 ?6 D! u3 Q+ j
//go home via an intermediate point.
9 n. R" z( _" f$ |% _9 ?0 W case 30:
6 |6 C3 ?: O* P: t% g F* F fp.x = search_string('X', instruction, size);
! c9 e* z9 q* |/ V/ y& Y fp.y = search_string('Y', instruction, size);! L" c# j; Y! b" q" s" ^4 S& }% W
fp.z = search_string('Z', instruction, size);
0 Q; R- N+ w3 P" @% @ fp.u = search_string('U', instruction, size);4 {5 U2 U l1 b) ?. V1 \$ t3 N
//set our target.; h7 J% B! u S# G
if(abs_mode): T! k( p" k1 |" d8 C9 {
{
8 B- V, Y% P; q8 v5 I( f! @ if (!has_command('X', instruction, size))2 G4 K8 R6 X$ u% X- o: L7 |
fp.x = current_units.x;
! d: o, V; D3 f9 {4 y if (!has_command('Y', instruction, size))
, \& [# V' N$ i- u5 j8 E fp.y = current_units.y;
& w5 m. V( \' q N% V. W: B6 y if (!has_command('Z', instruction, size))
; i1 p. I8 Y- [8 `' a) N2 D) D fp.z = current_units.z;! }, N% _% c% D% M7 G" o& T$ o6 k4 T
if (!has_command('U', instruction, size))
1 U4 ` |. j ^1 N, \: h fp.u = current_units.u;+ C: u0 I Y8 L- D1 r* O
set_target(fp.x, fp.y, fp.z, fp.u);7 y+ ~3 E0 Z* p$ `/ T5 }9 C
* l. \; o, T2 q9 [
}
9 f. ~! ]( ^( _, y: C; J* X else
8 Q9 S* v( v& y H7 \ set_target(current_units.x + fp.x, current_units.y + fp.y, current_units.z + fp.z, current_units.u + fp.u );4 G' p( u0 T' `" {
% p. E* _) x7 D. T, i6 I //go there.* Z1 d5 ?/ v( A
dda_move(getMaxSpeed());
0 A. _! p1 v) l7 [* E) n if(stop_flag) return;
) O7 e+ u! [& W$ v @1 x7 M
6 o& w9 j. N# ]7 E8 E //go home.
1 P3 c* h) D2 \" D set_target(0.0, 0.0, 0.0, 0.0 );
% o' _/ c2 s( a; J( P # N7 b0 U! Z% K
dda_move(getMaxSpeed());
, I. B+ c- j( r7 E3 ` if(stop_flag) return;
3 u# j6 {! B2 O7 f break;# I! f) q" ]& X3 f, k# m
$ ^8 @6 |2 a$ a5 h9 ^ //Absolute Positioning
+ y7 Q' f1 [# k, _9 Z4 a0 \ case 90:
+ F6 B1 w: \4 T; d: y) l& B( U7 S abs_mode = true;% C! \7 w% R/ U
break;4 f8 X0 ?7 A) b1 e! \. E0 C
% _: @7 p: g+ y( O4 o" U
//Incremental Positioning
# I- U/ I6 G2 c- \' z4 ]4 V5 k case 91:& D; k0 r1 Q3 @
abs_mode = false;4 f+ I( h# d8 M3 }& B
break;: F8 \8 ]# h: s! r9 ^1 {9 i2 {
: \* d' R2 m' g* C+ [! | //Set as home
+ Z5 m# X! c5 P case 92:$ G8 }( q* g; M
5 f0 b, b! f- E7 y0 ?4 J
set_position(0.0, 0.0, 0.0, 0.0 );
. A ]) Q5 V9 t( x
- c! d% Q6 L9 p V0 P: y: ? break;
" Q6 V& `1 f" m! }6 s. s0 T
- \, j2 x6 w0 j /*" ~2 J4 E5 T: B; N I7 b+ \) N
//Inverse Time Feed Mode9 B- L; `# Z" P, p# c! ^
case 93:
3 F3 C( H: v( Z* v9 X 8 A( H3 n- r! }8 C5 t4 H% \
break; //TODO: add this
, n+ o! N8 B- v; M* I Z( j* k: G8 ?& K9 Y- [" ~
//Feed per Minute Mode5 Z/ d9 p2 \( v: q& Z9 n
case 94:
% r. v9 |- i, U$ P/ l5 M7 i z+ K# R
' O( S0 a0 r" x2 E' L break; //TODO: add this# f+ Y [7 E2 s5 ?& r
*/
F @! ~' T3 q; S
+ U9 T/ @- ^, I5 v7 A7 i1 D6 h default:# @" Z( g# t! p
SerialUSB.print("huh? G");
1 M4 \) Y8 q. p, a+ V SerialUSB.println(code,DEC);
6 n% {2 I5 V! f: D9 J. g5 } P }6 T% s3 H( M$ X: Y3 Y2 Y
}
# q. T: ^( r) j% }; ]: j1 a/ g
: q% a" _! e* `0 O //find us an m code.
% ^: z' _+ l4 n7 X# L# e3 p if (has_command('M', instruction, size))
* s6 E- W& G9 P+ E {" e$ k( ^% C: V: k- K" c& A) X
code = search_string('M', instruction, size);! v3 S6 ^5 K+ h: e) z
switch (code)
2 o: a7 `' }7 Z( R2 G5 @) L) o, j9 V: B {
; \! \9 _: Y5 r1 G //TODO: this is a bug because search_string returns 0. gotta fix that.* M2 s& D% |" W. [5 m
case 0:
9 z8 T* N. C; j9 k. W true;9 o/ M. ?7 x5 ^; {; I% N2 H3 k
break;1 c! _) X) w: n' E8 o
- O9 t! H- I- I) R+ |( e case 100:
0 Q* z- Z4 V& t: _/ ^ break;4 I8 c' {4 r$ X5 A4 r) e
0 a+ A+ F# j; U" l' ]0 G
// fire camera relay
7 S) b! W+ { U, y* A& X case 101:6 p& `3 T7 p. R
camera_shutter1();. g3 ?. K) X) P s
break;$ g* ]7 a: Z/ Q& A
2 z# q# V/ g. H4 T7 e$ a
// fire camera relay2
/ Y" `% B8 w. D( x7 a* j. @ case 102:
0 {: u' ]) `/ c I! J1 \ k6 k% K camera_shutter2();
8 |) t$ z$ b( S. j# `; d$ D; j3 B break;
0 p* f! i: u: @5 f7 h! Z/*7 ?5 |& G k' @6 p4 Q7 K: Y4 W
// turn aux 1 relay on
! {' W5 ?! T4 `' Z case 103:
. [9 H3 [3 e# J: ]7 _4 R: M3 w camera_aux1_on();
, s0 K% }$ N5 h6 z* S break;
$ Z+ b B0 ]1 o" ?# ~6 L
E0 N" I# ?! I' p# v // turn aux 1 relay off
+ J1 h' y# a# }5 V9 _1 ]3 y0 B case 104:6 }7 E3 P+ g, I5 a8 N$ U" H
camera_aux1_off();
; v0 B. \8 P* p% E break;- ~7 h; e: y+ f9 A% Y& R3 M: H2 x* u
; T: P3 H/ o) g5 [0 h8 S // turn aux 2 relay on# a+ d0 R; ]- H/ S9 i. w
case 105:: A. [) D- u1 A7 r3 c
camera_aux2_on();0 F) t+ r; h e9 p& f0 ^8 d8 O
break;7 W+ j7 J+ T s0 E" {. r2 |3 y
7 J5 W# w" z" R: y
// turn aux 2 relay off
% A: _9 i0 L+ w) q p7 ~ case 106:' p ^. }/ a% L1 f/ t
camera_aux2_off();
. K) b1 d: {4 y0 m. X3 c5 v break;
9 n) l- h0 U& f, B# n3 z; I
2 @/ J. B4 d- S8 A3 D( @ // turn aux 3 relay on
) Y7 Z2 u8 u* X! n8 L case 107:7 X. Y9 s* `, U' \- R# O
camera_aux3_on();
: a" x/ [- Z, o5 T: s! E. ^ break;/ f* O$ `3 _4 `* i* B6 j+ x1 R
( x- H3 o, t/ B4 n- {+ ~2 g; H // turn aux 3 relay off
6 D9 u" [1 j0 d9 T& q case 108:
3 r! r3 W" G4 M' E9 J, ` camera_aux3_off();
# O: J0 W1 H* r1 {- D" [ break;
. L0 m* I/ v- \" ~4 r, O x' F/ y! U* P, L: b
// turn aux 4 relay on4 S$ `6 o4 }% G5 C. r( d
case 109:5 b3 W, |1 c& d" x& U9 c2 I
camera_aux4_on();7 X, l( j. W2 R
break;
. l# u5 `3 _2 m" r
1 S! g$ g: O) Z& _: _/ j, b // turn aux 4 relay off
- w. [6 j l+ O G2 ^ D" Y case 110:8 O/ a. k7 {! R4 G
camera_aux4_off();, E" y- j7 ~- A" c
break;
. } a' K! F5 @# r \+ K*/
( I, `- H3 w7 N. i: r8 M* G default:; _' Z0 Z, ^6 C8 n2 f
]+ ]9 K y5 d, i
SerialUSB.print("Huh? M");
0 }) }0 [# o, ]3 n" t SerialUSB.println(code);+ I; I) i: B9 K* T2 q
}/ C3 ~! K$ C e, C
}
: @2 G6 Q+ `# N& Y9 W# p* K! v
//tell our host we're done.7 n5 Z) Q h. G
SerialUSB.print(byte(78));
5 L: P( l" z3 Q2 J2 }/ X0 ]. M' R8 B: s0 b& f
}
6 d; Y2 Q. s+ d+ A. d) B U k5 s& R1 `. }! H8 G( B( O
//look for the number that appears after the char key and return it4 N8 Q0 m. x- j' m6 y% Y5 g8 ?
double search_string(char key, char instruction[], int string_size)
4 ~& s- l8 F' m; A+ w0 Z7 _% }{9 f% g* s4 Y/ B0 A
char temp[10] = " ";
6 x1 S) `* T/ n; B- }% ^ for (byte i=0; i<string_size; i++)# J5 t' f$ D4 [" u
{$ d/ v+ W- T! N8 W. |+ W5 \$ v
if (instruction[i] == key)
# k6 N5 {* A- B {" |+ c" x3 |' R. R, a8 h
i++; 7 x5 U+ J) x! g+ T" m
int k = 0;' ?& Q2 V9 ]! S) o2 q4 I/ z
while (i < string_size && k < 10) K0 w) L1 X; V! _# W3 ?% M* m
{
a) C% U( w* ^% ` if (instruction[i] == 0 || instruction[i] == ' ')8 C6 ? Q! M. N, r+ N
break;
6 p |( W5 g% j. K$ {
, [6 P4 c3 h. a4 m2 V. S) I temp[k] = instruction[i];0 Q, v9 I5 J3 j. w' D0 k
i++;/ G* a; K( B( _' @5 s V
k++;
9 Q; N! e+ r. A, A }0 j( y2 @4 O$ [/ e- J, [" i; @( J
return strtod(temp, NULL);0 l% U: V+ ]5 Q
}4 \4 V" w5 d5 `. n b2 P
}; S5 w) Z( F/ z, a q) N, x
. V2 N7 D# v; c
return 0;' }- h7 k8 S4 e
}
( p& a$ W# [# F8 [; [, S$ g0 I: Q& C, Y& ]9 E
//look for the command if it exists.
, _( `' b3 k: y6 e" Ubool has_command(char key, char instruction[], int string_size)
) n( s- ]$ x" B7 o) G0 j{
! A5 R) S1 J# B4 X( t+ M for (byte i=0; i<string_size; i++)
/ v- `9 T' \+ H8 i) u7 w {
n# G8 G. x9 c( ~7 A4 p. e if (instruction[i] == key){
( I. P7 m1 t( V" @
9 K) V \2 Y6 F8 C5 p; M return true;
7 x% i# e/ o* G }% e+ N, s/ H" i; a. B( t0 X9 V# o
}2 c; m" R7 ~. \. a$ O
. U+ A/ a8 z Y& L4 n return false;' `- X; Q" M2 s4 r. X
}
- }$ [0 T; W! R6 X3 u5 q
& n! c- H+ l+ w! u7 `1 s8 b4 G6 }2 I, i8 N V
( ~% t0 z& u5 T6 x) w! n! O
|
|