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